Remove ppk capture from input capture

F
Add input capture module
2026-06-04 04:50:05 +08:00 · 2023-08-14 15:24:36 +02:00 · 2023-08-14 15:16:11 +02:00 · 2023-07-28 17:14:25 -04:00 · 2023-07-28 17:12:01 -04:00 · 2023-07-28 09:31:44 -04:00
255 changed files with 13325 additions and 11245 deletions
@@ -9,7 +9,7 @@ pipeline {
        script {
          def build_nodes = [:]
          def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2022-08-12",
+            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",
@@ -40,6 +40,8 @@ pipeline {
                     "ark_can-flow_default",
                     "ark_can-gps_canbootloader",
                     "ark_can-gps_default",
+                     "ark_cannode_canbootloader",
+                     "ark_cannode_default",
                     "ark_can-rtk-gps_canbootloader",
                     "ark_can-rtk-gps_default",
                     "ark_fmu-v6x_bootloader",
@@ -53,8 +55,10 @@ pipeline {
                     "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",
@@ -72,7 +76,7 @@ pipeline {
                     "matek_h743_default",
                     "modalai_fc-v1_default",
                     "modalai_fc-v2_default",
-                     "modalai_voxl2-io_default",
+                     "mro_ctrl-zero-classic_default",
                     "mro_ctrl-zero-f7_default",
                     "mro_ctrl-zero-f7-oem_default",
                     "mro_ctrl-zero-h7-oem_default",
@@ -85,6 +89,7 @@ pipeline {
                     "nxp_fmuk66-v3_default",
                     "nxp_fmuk66-v3_socketcan",
                     "nxp_fmurt1062-v1_default",
+                     "nxp_mr-canhubk3_default",
                     "nxp_ucans32k146_canbootloader",
                     "nxp_ucans32k146_default",
                     "omnibus_f4sd_default",
@@ -164,7 +169,7 @@ pipeline {
  }
  options {
    buildDiscarder(logRotator(numToKeepStr: '5', artifactDaysToKeepStr: '14'))
-    timeout(time: 90, unit: 'MINUTES')
+    timeout(time: 120, unit: 'MINUTES')
  }
 }

@@ -12,7 +12,7 @@ pipeline {
            stage("build cubepilot_cubeorange_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -91,7 +91,7 @@ pipeline {
            stage("build cuav_x7pro_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -165,7 +165,7 @@ pipeline {
            stage("build px4_fmu-v4_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -238,7 +238,7 @@ pipeline {
            stage("build px4_fmu-v4pro_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -312,7 +312,7 @@ pipeline {
            stage("build px4_fmu-v5_debug") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -362,7 +362,7 @@ pipeline {
                    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"'
+                    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"'
@@ -403,7 +403,7 @@ pipeline {
            stage("build px4_fmu-v5_stackcheck") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -449,7 +449,7 @@ pipeline {
                    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"'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors'
                  }
                }
                stage("status") {
@@ -486,7 +486,7 @@ pipeline {
            stage("build px4_fmu-v5_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -560,7 +560,7 @@ pipeline {
            stage("build nxp_fmuk66-v3_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -2,7 +2,7 @@
 // https://github.com/microsoft/vscode-dev-containers/tree/v0.134.0/containers/cpp
 {
 	"name": "px4-dev-nuttx",
-	"image": "px4io/px4-dev-nuttx-focal:2021-09-08",
+	"image": "px4io/px4-dev-nuttx-focal:2022-08-12",

 	"runArgs": [ "--cap-add=SYS_PTRACE", "--security-opt", "seccomp=unconfined" ],

@@ -28,7 +28,7 @@ jobs:
          "parameters_metadata",
        ]
    container:
-      image: px4io/px4-dev-nuttx-focal:2021-09-08
+      image: px4io/px4-dev-nuttx-focal:2022-08-12
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -1,9 +1,12 @@
-name: Linux Targets
+name: Compile Linux Targets

 on:
  push:
    branches:
    - 'main'
+    - 'stable'
+    - 'beta'
+    - 'release/*'
  pull_request:
    branches:
    - '*'
@@ -1,9 +1,12 @@
-name: Linux ARM64 Targets
+name: Compile Linux ARM64 Targets

 on:
  push:
    branches:
    - 'main'
+    - 'stable'
+    - 'beta'
+    - 'release/*'
  pull_request:
    branches:
    - '*'
@@ -11,7 +14,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-aarch64:2021-09-08
+    container: px4io/px4-dev-aarch64:2022-08-12
    strategy:
      matrix:
        config: [
@@ -10,13 +10,12 @@ on:

 jobs:
  build:
-    runs-on: macos-10.15
+    runs-on: macos-latest
    strategy:
      matrix:
        config: [
          px4_fmu-v5_default,
          px4_sitl
-          #tests, # includes px4_sitl
          ]
    steps:
    - uses: actions/checkout@v1
@@ -1,9 +1,12 @@
-name: Nuttx Targets
+name: Compile Nuttx Targets

 on:
  push:
    branches:
    - 'main'
+    - 'stable'
+    - 'beta'
+    - 'release/*'
  pull_request:
    branches:
    - '*'
@@ -11,7 +14,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-09-08
+    container: px4io/px4-dev-nuttx-focal:2022-08-12
    strategy:
      fail-fast: false
      matrix:
@@ -24,12 +24,15 @@ jobs:
    needs: enumerate_targets
    strategy:
      matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}}
-    container: px4io/px4-dev-${{ matrix.container }}:2021-09-08
+    container: ${{ matrix.container }}
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{secrets.ACCESS_TOKEN}}

+    - name: ownership workaround
+      run: git config --system --add safe.directory '*'
+
    - name: make ${{matrix.target}}
      run: make ${{matrix.target}}

@@ -21,7 +21,7 @@ jobs:
          "failsafe_web",
        ]
    container:
-      image: px4io/px4-dev-nuttx-focal:2021-09-08
+      image: px4io/px4-dev-nuttx-focal:2022-08-12
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -103,7 +103,7 @@ jobs:

  uorb_graph:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-09-08
+    container: px4io/px4-dev-nuttx-focal:2022-08-12
    steps:
    - uses: actions/checkout@v1
      with:
@@ -94,7 +94,7 @@ pipeline {

        stage('failsafe docs') {
          agent {
-            docker { image 'px4io/px4-dev-nuttx-focal:2021-08-18' }
+            docker { image 'px4io/px4-dev-nuttx-focal:2022-08-12' }
          }
          steps {
            sh '''#!/bin/bash -l
@@ -125,7 +125,7 @@ pipeline {
        stage('uORB graphs') {
          agent {
            docker {
-              image 'px4io/px4-dev-nuttx-focal:2021-08-18'
+              image 'px4io/px4-dev-nuttx-focal:2022-08-12'
              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
            }
          }
@@ -0,0 +1,122 @@
+#!/bin/sh
+#
+# @name 6DoF Omnicopter SITL
+#
+# @type Quadrotor Wide
+#
+# @maintainer Jaeyoung Lim <jalim@ethz.ch>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=omnicopter}
+
+param set-default CA_AIRFRAME 0
+
+param set-default CA_ROTOR_COUNT 8
+param set-default CA_R_REV 255
+
+param set-default CA_ROTOR0_PX 0.14435
+param set-default CA_ROTOR0_PY -0.14435
+param set-default CA_ROTOR0_PZ -0.14435
+param set-default CA_ROTOR0_KM 0.05 # CCW
+param set-default CA_ROTOR0_AX -0.788675
+param set-default CA_ROTOR0_AY -0.211325
+param set-default CA_ROTOR0_AZ -0.57735
+
+param set-default CA_ROTOR1_PX -0.14435
+param set-default CA_ROTOR1_PY -0.14435
+param set-default CA_ROTOR1_PZ -0.14435
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_AX 0.211325
+param set-default CA_ROTOR1_AY -0.788675
+param set-default CA_ROTOR1_AZ 0.57735
+
+param set-default CA_ROTOR2_PX 0.14435
+param set-default CA_ROTOR2_PY 0.14435
+param set-default CA_ROTOR2_PZ -0.14435
+param set-default CA_ROTOR2_KM 0.05
+param set-default CA_ROTOR2_AX -0.211325
+param set-default CA_ROTOR2_AY 0.788675
+param set-default CA_ROTOR2_AZ 0.57735
+
+param set-default CA_ROTOR3_PX -0.14435
+param set-default CA_ROTOR3_PY 0.14435
+param set-default CA_ROTOR3_PZ -0.14435
+param set-default CA_ROTOR3_KM 0.05
+param set-default CA_ROTOR3_AX 0.788675
+param set-default CA_ROTOR3_AY 0.211325
+param set-default CA_ROTOR3_AZ -0.57735
+
+param set-default CA_ROTOR4_PX 0.14435
+param set-default CA_ROTOR4_PY -0.14435
+param set-default CA_ROTOR4_PZ 0.14435
+param set-default CA_ROTOR4_KM 0.05
+param set-default CA_ROTOR4_AX 0.788675
+param set-default CA_ROTOR4_AY 0.211325
+param set-default CA_ROTOR4_AZ -0.57735
+
+param set-default CA_ROTOR5_PX -0.14435
+param set-default CA_ROTOR5_PY -0.14435
+param set-default CA_ROTOR5_PZ 0.14435
+param set-default CA_ROTOR5_KM 0.05
+param set-default CA_ROTOR5_AX -0.211325
+param set-default CA_ROTOR5_AY 0.788675
+param set-default CA_ROTOR5_AZ 0.57735
+
+param set-default CA_ROTOR6_PX 0.14435
+param set-default CA_ROTOR6_PY 0.14435
+param set-default CA_ROTOR6_PZ 0.14435
+param set-default CA_ROTOR6_KM 0.05
+param set-default CA_ROTOR6_AX 0.211325
+param set-default CA_ROTOR6_AY -0.788675
+param set-default CA_ROTOR6_AZ 0.57735
+
+param set-default CA_ROTOR7_PX -0.14435
+param set-default CA_ROTOR7_PY 0.14435
+param set-default CA_ROTOR7_PZ 0.14435
+param set-default CA_ROTOR7_KM 0.05
+param set-default CA_ROTOR7_AX -0.788675
+param set-default CA_ROTOR7_AY -0.211325
+param set-default CA_ROTOR7_AZ -0.57735
+
+param set-default SIM_GZ_EN 1
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+
+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_FUNC5 105
+param set-default SIM_GZ_EC_FUNC6 106
+param set-default SIM_GZ_EC_FUNC7 107
+param set-default SIM_GZ_EC_FUNC8 108
+
+param set-default SIM_GZ_EC_MIN1 0
+param set-default SIM_GZ_EC_MIN2 0
+param set-default SIM_GZ_EC_MIN3 0
+param set-default SIM_GZ_EC_MIN4 0
+param set-default SIM_GZ_EC_MIN5 0
+param set-default SIM_GZ_EC_MIN6 0
+param set-default SIM_GZ_EC_MIN7 0
+param set-default SIM_GZ_EC_MIN8 0
+
+param set-default SIM_GZ_EC_MAX1 1100
+param set-default SIM_GZ_EC_MAX2 1100
+param set-default SIM_GZ_EC_MAX3 1100
+param set-default SIM_GZ_EC_MAX4 1100
+param set-default SIM_GZ_EC_MAX5 1100
+param set-default SIM_GZ_EC_MAX6 1100
+param set-default SIM_GZ_EC_MAX7 1100
+param set-default SIM_GZ_EC_MAX8 1100
+# disable MC desaturation which improves attitude tracking
+param set-default CA_METHOD 0
+
+# disable attitude failure detection
+param set-default FD_FAIL_P 0
+param set-default FD_FAIL_R 0
+
@@ -81,6 +81,8 @@ px4_add_romfs_files(
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

+	8011_gz_omnicopter
+
 	10015_gazebo-classic_iris
 	10016_none_iris
 	10017_jmavsim_iris
@@ -59,7 +59,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 	fi

 	# look for running ${gz_command} gazebo world
-	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )

 	# shellcheck disable=SC2153
 	if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
@@ -17,6 +17,7 @@
 # @maintainer Andreas Antener <andreas@uaventure.com>
 #
 # @board bitcraze_crazyflie exclude
+# @board diatone_mamba-f405-mk2 exclude
 #

 . ${R}etc/init.d/rc.fw_defaults
@@ -34,9 +34,9 @@ param set-default CA_ROTOR3_PX -0.25
 param set-default CA_ROTOR3_PY 0.25
 param set-default CA_ROTOR3_KM -0.05

-param set-default PWM_AUX_FUNC1 101
-param set-default PWM_AUX_FUNC2 102
-param set-default PWM_AUX_FUNC3 103
-param set-default PWM_AUX_FUNC4 104
-param set-default PWM_AUX_TIM0 -4
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_TIM0 -4

@@ -13,6 +13,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board diatone_mamba-f405-mk2 exclude
 #
 . ${R}etc/init.d/rc.mc_defaults

@@ -13,6 +13,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board diatone_mamba-f405-mk2 exclude
 #
 . ${R}etc/init.d/rc.mc_defaults

@@ -21,11 +21,7 @@ control_allocator start
 fw_rate_control start
 fw_att_control start
 fw_pos_control start
-
-if param greater -s ASPD_SEL_EN 0
-then
-	airspeed_selector start
-fi
+airspeed_selector start

 #
 # Start attitude control auto-tuner
@@ -20,10 +20,7 @@ ekf2 start &
 #
 control_allocator start

-if param greater -s ASPD_SEL_EN 0
-then
-	airspeed_selector start
-fi
+airspeed_selector start

 vtol_att_control start

@@ -330,6 +330,12 @@ else
 		fi
 	fi

+	# PPS capture driver
+	if param greater -s INPUT_CAP_ENABLE 0
+	then
+		input_capture start
+	fi
+
 	#
 	# Sensors System (start before Commander so Preflight checks are properly run).
 	# Commander needs to be this early for in-air-restarts.
@@ -39,7 +39,7 @@ def print_line(line):
        print('{0}'.format(line), end='')


-def do_nsh_cmd(port_url, baudrate, cmd):
+def do_nsh_cmd(port_url, baudrate, cmd, ignore_stdout_errors=False):
    ser = serial.serial_for_url(url=port_url, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=1, xonxoff=False, rtscts=False, dsrdtr=False, inter_byte_timeout=1)

    timeout_start = time.monotonic()
@@ -106,7 +106,7 @@ def do_nsh_cmd(port_url, baudrate, cmd):
            if success_cmd in serial_line:
                sys.exit(return_code)
            else:
-                if "ERROR " in serial_line:
+                if "ERROR " in serial_line and not ignore_stdout_errors:
                    return_code = -1

                print_line(serial_line)
@@ -148,6 +148,8 @@ def main():
    parser.add_argument('--device', "-d", nargs='?', default=default_device, help='', required=device_required)
    parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="serial port baud rate (default=57600)", default=57600)
    parser.add_argument("--cmd", "-c", dest="cmd", help="Command to run")
+    parser.add_argument('--ignore-stdout-errors', action='store_true',
+                        help='Ignore errors printed to stdout')
    args = parser.parse_args()

    tmp_file = "{0}/pyserial_spy_file.txt".format(tempfile.gettempdir())
@@ -155,7 +157,7 @@ def main():

    print("pyserial url: {0}".format(port_url))

-    do_nsh_cmd(port_url, args.baudrate, args.cmd)
+    do_nsh_cmd(port_url, args.baudrate, args.cmd, args.ignore_stdout_errors)

 if __name__ == "__main__":
   main()
@@ -4,16 +4,16 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then
 	echo "guessing PX4_DOCKER_REPO based on input";
 	if [[ $@ =~ .*px4_fmu.* ]]; then
 		# nuttx-px4fmu-v{1,2,3,4,5}
-		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
+		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2022-08-12"
 	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
 		# beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2023-06-26"
 	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
 		# scumaker_pilotpi_arm64
-		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest"
+		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:2022-08-12"
 	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then
 		# posix_rpi_cross, posix_bebop_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2023-06-26"
 	elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then
 		# clang tools
 		PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04"
@@ -27,7 +27,7 @@ fi

 # otherwise default to nuttx
 if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
+	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2022-08-12"
 fi

 # docker hygiene
@@ -61,18 +61,19 @@ def process_target(px4board_file, target_name):

    if platform not in excluded_platforms:
        # get the container based on the platform and toolchain
-        container = platform
        if platform == 'posix':
-            container = 'base-focal'
+            container = 'px4io/px4-dev-base-focal:2021-09-08'
            if toolchain:
                if toolchain.startswith('aarch64'):
-                    container = 'aarch64'
+                    container = 'px4io/px4-dev-aarch64:2022-08-12'
                elif toolchain == 'arm-linux-gnueabihf':
-                    container = 'armhf'
+                    container = 'px4io/px4-dev-armhf:2023-06-26'
                else:
-                    if verbose: print(f'possibly unmatched toolchain: {toolchain}')
+                    if verbose: print(f'unmatched toolchain: {toolchain}')
        elif platform == 'nuttx':
-            container = 'nuttx-focal'
+            container = 'px4io/px4-dev-nuttx-focal:2022-08-12'
+        else:
+            if verbose: print(f'unmatched platform: {platform}')

        ret = {'target': target_name, 'container': container}

@@ -113,4 +114,3 @@ extra_args = {}
 if args.pretty:
    extra_args['indent'] = 2
 print(json.dumps(github_action_config, **extra_args))
-
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>Omnicopter</name>
+  <version>1.0</version>
+  <sdf version='1.9'>model.sdf</sdf>
+
+  <author>
+   <name>Jaeyoung Lim</name>
+   <email>jalim@ethz.ch</email>
+  </author>
+
+  <description>
+    Omnicopter model for over actuated system
+  </description>
+</model>
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <include>
+      <name>omnicopter</name>
+      <pose>0 0 0 0 0 0</pose>
+      <uri>https://fuel.gazebosim.org/1.0/PX4/models/Omnicopter</uri>
+    </include>
+</sdf>
@@ -11,6 +11,7 @@
    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
    <plugin name='gz::sim::systems::AirPressure' filename='gz-sim-air-pressure-system'/>
+    <plugin name='gz::sim::systems::ApplyLinkWrench' filename='gz-sim-apply-link-wrench-system'/>
    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
      <render_engine>ogre2</render_engine>
    </plugin>
@@ -291,7 +291,6 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
-CONFIG_UART7_RXBUFSIZE=1500
 CONFIG_UART5_RXDMA=y
 CONFIG_UART5_TXBUFSIZE=10000
 CONFIG_UART5_TXDMA=y
@@ -140,15 +140,7 @@

 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
-#define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
-#define PWM_DEFAULT_MIN 20
-#define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
-#define PWM_DEFAULT_MAX 255
-#define PWM_LOWEST_MAX 255
-#define PWM_DEFAULT_TRIM 1500
-

 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */
@@ -141,15 +141,7 @@

 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
-#define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
-#define PWM_DEFAULT_MIN 20
-#define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
-#define PWM_DEFAULT_MAX 255
-#define PWM_LOWEST_MAX 255
-#define PWM_DEFAULT_TRIM 1500
-

 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */
@@ -23,6 +23,8 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -36,7 +38,6 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_DMESG=y
-CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MTD=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -33,6 +33,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_TOP=y
-CONFIG_SYSTEMCMDS_DMESG=y
@@ -14,10 +14,6 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
-# CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
-# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
-# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
-# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
@@ -29,13 +25,11 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-# CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
@@ -54,7 +48,6 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
-# CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -32,6 +32,7 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -22,7 +22,7 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_COMMON_UWB=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -28,6 +28,7 @@ CONFIG_UAVCANNODE_RTK_DATA=y
 CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -1,4 +1,6 @@
 CONFIG_DRIVERS_ADC_ADS1115=n
+CONFIG_DRIVERS_CAMERA_CAPTURE=n
+CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
@@ -28,7 +28,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
-CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -45,3 +44,4 @@ CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
 CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
 CONFIG_DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L=y
 CONFIG_DRIVERS_TEST_PPM=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
@@ -22,6 +22,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_INPUT_CAPTURE=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
@@ -98,8 +99,6 @@ CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SD_STRESS=y
-CONFIG_SYSTEMCMDS_SERIAL_TEST=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
@@ -1,7 +1,14 @@
+# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
+CONFIG_COMMON_HYGROMETERS=n
+CONFIG_COMMON_OSD=n
 CONFIG_DRIVERS_ADC_ADS1115=n
+CONFIG_DRIVERS_BATT_SMBUS=n
+CONFIG_DRIVERS_CAMERA_CAPTURE=n
+CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
+CONFIG_DRIVERS_UAVCAN=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
@@ -11,3 +18,5 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
@@ -2,6 +2,8 @@ CONFIG_COMMON_DISTANCE_SENSOR=n
 CONFIG_COMMON_INS=n
 CONFIG_COMMON_TELEMETRY=n
 CONFIG_DRIVERS_ADC_ADS1115=n
+CONFIG_DRIVERS_CAMERA_CAPTURE=n
+CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
@@ -7,11 +7,11 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
+CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
-CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
@@ -41,6 +41,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
@@ -291,7 +291,6 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
-CONFIG_UART7_RXBUFSIZE=1500
 CONFIG_UART5_RXDMA=y
 CONFIG_UART5_TXBUFSIZE=10000
 CONFIG_UART5_TXDMA=y
@@ -60,6 +60,8 @@ set(msg_files
 	CollisionReport.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
+	DatamanRequest.msg
+	DatamanResponse.msg
 	DebugArray.msg
 	DebugKeyValue.msg
 	DebugValue.msg
@@ -109,6 +111,7 @@ set(msg_files
 	HeaterStatus.msg
 	HomePosition.msg
 	HoverThrustEstimate.msg
+	InputCapture.msg
 	InputRc.msg
 	InternalCombustionEngineStatus.msg
 	IridiumsbdStatus.msg
@@ -178,6 +181,7 @@ set(msg_files
 	SensorSelection.msg
 	SensorsStatus.msg
 	SensorsStatusImu.msg
+	SensorUwb.msg
 	SystemPower.msg
 	TakeoffStatus.msg
 	TaskStackInfo.msg
@@ -194,8 +198,6 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
-	UwbDistance.msg
-	UwbGrid.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg
@@ -0,0 +1,8 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint8 client_id
+uint8 request_type	# id/read/write/clear
+uint8 item			# dm_item_t
+uint32 index
+uint8[56] data
+uint32 data_length
@@ -0,0 +1,15 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint8 client_id
+uint8 request_type	# id/read/write/clear
+uint8 item			# dm_item_t
+uint32 index
+uint8[56] data
+
+uint8 STATUS_SUCCESS = 0
+uint8 STATUS_FAILURE_ID_ERR = 1
+uint8 STATUS_FAILURE_NO_DATA = 2
+uint8 STATUS_FAILURE_READ_FAILED = 3
+uint8 STATUS_FAILURE_WRITE_FAILED = 4
+uint8 STATUS_FAILURE_CLEAR_FAILED = 5
+uint8 status
@@ -127,3 +127,8 @@ uint32 mag_device_id
 # legacy local position estimator (LPE) flags
 uint8 health_flags		# Bitmask to indicate sensor health states (vel, pos, hgt)
 uint8 timeout_flags		# Bitmask to indicate timeout flags (vel, pos, hgt)
+
+float32 mag_inclination_deg
+float32 mag_inclination_ref_deg
+float32 mag_strength_gs
+float32 mag_strength_ref_gs
@@ -0,0 +1,5 @@
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample # the timestamp of the raw data (microseconds)
+uint32 seq		# Capture sequence number
+
+uint32 ORB_QUEUE_LENGTH = 2
@@ -3,3 +3,7 @@ uint8 dataman_id	# default 0, there are two offboard storage places in the datam

 uint16 count		# count of the missions stored in the dataman
 int32 current_seq	# default -1, start at the one changed latest
+
+int16 mission_update_counter # indicates updates to the mission, reload from dataman if increased
+int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased
+int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased
@@ -5,10 +5,10 @@ uint64 timestamp_sample

 uint32 device_id                # unique device ID for the sensor that does not change between power cycles

-int32 lat			# Latitude in 1E-7 degrees
-int32 lon			# Longitude in 1E-7 degrees
-int32 alt			# Altitude in 1E-3 meters above MSL, (millimetres)
-int32 alt_ellipsoid 		# Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
+float64 latitude_deg		# Latitude in degrees, allows centimeter level RTK precision
+float64 longitude_deg		# Longitude in degrees, allows centimeter level RTK precision
+float64 altitude_msl_m		# Altitude above MSL, meters
+float64 altitude_ellipsoid_m	# Altitude above Ellipsoid, meters

 float32 s_variance_m_s		# GPS speed accuracy estimate, (metres/sec)
 float32 c_variance_rad		# GPS course accuracy estimate, (radians)
@@ -0,0 +1,34 @@
+# UWB distance contains the distance information measured by an ultra-wideband positioning system,
+# such as Pozyx or NXP Rddrone.
+
+uint64 	timestamp		# time since system start (microseconds)
+
+uint32 	sessionid		# UWB SessionID
+uint32 	time_offset		# Time between Ranging Rounds in ms
+uint32 	counter			# Number of Ranges since last Start of Ranging
+uint16 	mac			# MAC adress of Initiator (controller)
+
+uint16 	mac_dest		# MAC adress of Responder (Controlee)
+uint16 	status			# status feedback #
+uint8 	nlos			# None line of site condition y/n
+float32 distance		# distance in m to the UWB receiver
+
+
+#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees
+float32 aoa_azimuth_dev	# Angle of arrival of first incomming RX msg
+float32 aoa_elevation_dev	# Angle of arrival of first incomming RX msg
+float32 aoa_azimuth_resp	# Angle of arrival of first incomming RX msg at the responder
+float32 aoa_elevation_resp	# Angle of arrival of first incomming RX msg at the responder
+
+# Figure of merit for the angle measurements
+uint8 aoa_azimuth_fom		# AOA Azimuth FOM
+uint8 aoa_elevation_fom		# AOA Elevation FOM
+uint8 aoa_dest_azimuth_fom	# AOA Azimuth FOM
+uint8 aoa_dest_elevation_fom	# AOA Elevation FOM
+
+# Initiator physical configuration
+uint8 orientation		# Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
+				# Standard configuration is Antennas facing down and azimuth aligened in forward direction
+float32 offset_x		# UWB initiator offset in X axis (NED drone frame)
+float32 offset_y		# UWB initiator offset in Y axis (NED drone frame)
+float32 offset_z		# UWB initiator offset in Z axis (NED drone frame)
@@ -1,15 +0,0 @@
-# UWB distance contains the distance information measured by an ultra-wideband positioning system,
-# such as Pozyx or NXP Rddrone.
-
-uint64 	timestamp		# time since system start (microseconds)
-uint32 	time_uwb_ms		# Time of UWB device in ms
-uint32 	counter			# Number of Ranges since last Start of Ranging
-uint32 	sessionid		# UWB SessionID
-uint32 	time_offset		# Time between Ranging Rounds in ms
-uint16 	status			# status feedback #
-
-uint16[12] anchor_distance	# distance in cm to each UWB Anchor (UWB Device which takes part in Ranging)
-bool[12] nlos			# Visual line of sight yes/no
-float32[12] aoafirst		# Angle of arrival of first incoming RX msg
-
-float32[3] position		# Position of the Landing point in relation to the Drone (x,y,z in Meters NED)
@@ -1,25 +0,0 @@
-# UWB report message contains the grid information measured by an ultra-wideband positioning system,
-# such as Pozyx or NXP Rddrone.
-
-uint64 	timestamp	# time since system start (microseconds)
-uint16 	initator_time	# time to synchronize clocks (microseconds)
-uint8 	num_anchors	# Number of anchors
-
-float64[4] target_gps	# GPS position of target of the UWB system (Lat / Lon / Alt / Yaw Offset to true North)
-
-# Grid position information
-# Position in x,y,z in  (x,y,z in centimeters NED)
-# staring with POI and Anchor 0 up to Anchor 11
-int16[3] target_pos 	# Point of Interest, mostly landing Target x,y,z
-int16[3] anchor_pos_0
-int16[3] anchor_pos_1
-int16[3] anchor_pos_2
-int16[3] anchor_pos_3
-int16[3] anchor_pos_4
-int16[3] anchor_pos_5
-int16[3] anchor_pos_6
-int16[3] anchor_pos_7
-int16[3] anchor_pos_8
-int16[3] anchor_pos_9
-int16[3] anchor_pos_10
-int16[3] anchor_pos_11
@@ -6,13 +6,8 @@ uint64 armed_time # Arming timestamp (microseconds)
 uint64 takeoff_time # Takeoff timestamp (microseconds)

 uint8 arming_state
-uint8 ARMING_STATE_INIT = 0
-uint8 ARMING_STATE_STANDBY = 1
-uint8 ARMING_STATE_ARMED = 2
-uint8 ARMING_STATE_STANDBY_ERROR = 3
-uint8 ARMING_STATE_SHUTDOWN = 4
-uint8 ARMING_STATE_IN_AIR_RESTORE = 5
-uint8 ARMING_STATE_MAX = 6
+uint8 ARMING_STATE_DISARMED = 1
+uint8 ARMING_STATE_ARMED    = 2

 uint8 latest_arming_reason
 uint8 latest_disarming_reason
@@ -121,4 +116,3 @@ bool rc_calibration_in_progress
 bool calibration_enabled

 bool pre_flight_checks_pass		# true if all checks necessary to arm pass
-
@@ -163,7 +163,7 @@ int px4_platform_init()
 #if defined(CONFIG_FS_PROCFS)
 	int ret_mount_procfs = mount(nullptr, "/proc", "procfs", 0, nullptr);

-	if (ret < 0) {
+	if (ret_mount_procfs < 0) {
 		syslog(LOG_ERR, "ERROR: Failed to mount procfs at /proc: %d\n", ret_mount_procfs);
 	}

@@ -75,6 +75,8 @@ ModalIo::ModalIo() :
 		_esc_status.esc[i].esc_power       = 0;
 	}

+	_esc_status_pub.advertise();
+
 	qc_esc_packet_init(&_fb_packet);
 	qc_esc_packet_init(&_uart_bridge_packet);

@@ -66,9 +66,9 @@ public:
 			size_t payload_size = reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;

 			reg_udral_physics_kinematics_geodetic_Point_0_1 geo {};
-			geo.latitude = gps.lat;
-			geo.longitude = gps.lon;
-			geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast<double>(gps.alt) };
+			geo.latitude = (int64_t)(gps.latitude_deg / 1e7);
+			geo.longitude = (int64_t)(gps.longitude_deg / 1e7);
+			geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = gps.altitude_msl_m };

 			uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];

@@ -195,6 +195,13 @@ int LightwareLaser::init()
 		_type = Type::LW20c;
 		break;

+	case 7:
+		/* SF/LW30/d (200m 49-20'000Hz) */
+		_px4_rangefinder.set_min_distance(0.2f);
+		_px4_rangefinder.set_max_distance(200.0f);
+		_conversion_interval = 20409;
+		break;
+
 	default:
 		PX4_ERR("invalid HW model %" PRId32 ".", hw_model);
 		return ret;
@@ -45,5 +45,6 @@
 * @value 4 SF11/c
 * @value 5 SF/LW20/b
 * @value 6 SF/LW20/c
+ * @value 7 SF/LW30/d
 */
 PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
@@ -61,41 +61,11 @@ __BEGIN_DECLS
 */
 #define PWM_LOWEST_MIN 90

-/**
- * Default value for a shutdown motor
- */
-#define PWM_MOTOR_OFF	900
-
-/**
- * Default minimum PWM in us
- */
-#define PWM_DEFAULT_MIN 1000
-
-/**
- * Highest PWM allowed as the minimum PWM
- */
-#define PWM_HIGHEST_MIN 1600
-
 /**
 * Highest maximum PWM in us
 */
 #define PWM_HIGHEST_MAX 2500

-/**
- * Default maximum PWM in us
- */
-#define PWM_DEFAULT_MAX 2000
-
-/**
- * Default trim PWM in us
- */
-#define PWM_DEFAULT_TRIM 0
-
-/**
- * Lowest PWM allowed as the maximum PWM
- */
-#define PWM_LOWEST_MAX 200
-
 #endif // not PX4_PWM_ALTERNATE_RANGES

 /**
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018 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,14 +31,19 @@
 #
 ############################################################################

+set(PARAM_PREFIX PWM_MAIN)

-px4_add_library(ArmStateMachine
-	ArmStateMachine.cpp
+if(CONFIG_BOARD_IO)
+	set(PARAM_PREFIX PWM_AUX)
+endif()
+
+px4_add_module(
+	MODULE drivers__input_capture
+	MAIN input_capture
+	COMPILE_FLAGS
+		-DPARAM_PREFIX="${PARAM_PREFIX}"
+	SRCS
+		input_capture.cpp
+	DEPENDS
+		arch_io_pins
 )
-target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_link_libraries(ArmStateMachine PUBLIC health_and_arming_checks)
-
-px4_add_functional_gtest(SRC ArmStateMachineTest.cpp
-	LINKLIBS ArmStateMachine health_and_arming_checks hysteresis sensor_calibration ArmAuthorization mode_util
-	)
-
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_INPUT_CAPTURE
+	bool "input_capture"
+	default n
+	---help---
+		Enable support for input_capture
@@ -0,0 +1,456 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file input_capture.cpp
+ *
+ * Online and offline geotagging from camera feedback
+ *
+ * @author Mohammed Kabir <kabir@uasys.io>
+ * @author Jaeyoung Lim <jalim@ethz.ch>
+ */
+
+#include "input_capture.hpp"
+#include <px4_platform_common/events.h>
+#include <systemlib/mavlink_log.h>
+
+#include <board_config.h>
+
+#define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
+
+using namespace time_literals;
+
+namespace camera_capture
+{
+InputCapture *g_input_capture{nullptr};
+}
+
+struct work_s InputCapture::_work_publisher;
+
+InputCapture::InputCapture() :
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+	memset(&_work_publisher, 0, sizeof(_work_publisher));
+
+	// Capture Parameters
+	_p_strobe_delay = param_find("CAM_CAP_DELAY");
+	param_get(_p_strobe_delay, &_strobe_delay);
+
+	_p_camera_capture_mode = param_find("CAM_CAP_MODE");
+	param_get(_p_camera_capture_mode, &_camera_capture_mode);
+
+	_p_camera_capture_edge = param_find("CAM_CAP_EDGE");
+	param_get(_p_camera_capture_edge, &_camera_capture_edge);
+
+	// get the capture channel from function configuration params
+	_capture_channel = -1;
+
+	for (unsigned i = 0; i < 16 && _capture_channel == -1; ++i) {
+		char param_name[17];
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
+		param_t function_handle = param_find(param_name);
+		int32_t function;
+
+		if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
+			if (function == 2032) { // Camera_Capture
+				_capture_channel = i;
+			}
+		}
+	}
+
+	_capture_pub.advertise();
+}
+
+InputCapture::~InputCapture()
+{
+	camera_capture::g_input_capture = nullptr;
+}
+
+void
+InputCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
+{
+	// Maximum acceptable rate is 5kHz
+	if ((edge_time - _trigger.hrt_edge_time) < 200_us) {
+		++_trigger_rate_exceeded_counter;
+
+		if (_trigger_rate_exceeded_counter > 100) {
+
+			// Trigger rate too high, stop future interrupts
+			up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr);
+			_trigger_rate_failure.store(true);
+		}
+
+	} else if (_trigger_rate_exceeded_counter > 0) {
+		--_trigger_rate_exceeded_counter;
+	}
+
+	_trigger.chan_index = chan_index;
+	_trigger.hrt_edge_time = edge_time;
+	_trigger.edge_state = edge_state;
+	_trigger.overflow = overflow;
+
+	work_queue(HPWORK, &_work_publisher, (worker_t)&InputCapture::publish_trigger_trampoline, this, 0);
+}
+
+int
+InputCapture::gpio_interrupt_routine(int irq, void *context, void *arg)
+{
+	InputCapture *dev = static_cast<InputCapture *>(arg);
+
+	dev->_trigger.chan_index = 0;
+	dev->_trigger.hrt_edge_time = hrt_absolute_time();
+	dev->_trigger.edge_state = 0;
+	dev->_trigger.overflow = 0;
+
+	work_queue(HPWORK, &_work_publisher, (worker_t)&InputCapture::publish_trigger_trampoline, dev, 0);
+
+	return PX4_OK;
+}
+
+void
+InputCapture::publish_trigger_trampoline(void *arg)
+{
+	InputCapture *dev = static_cast<InputCapture *>(arg);
+
+	dev->publish_trigger();
+}
+
+void
+InputCapture::publish_trigger()
+{
+	bool publish = false;
+
+	if (_trigger_rate_failure.load()) {
+		mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: Input capture disabled\t");
+		events::send(events::ID("input_capture_trigger_rate_exceeded"),
+			     events::Log::Error, "Hardware fault: Input capture disabled");
+		_trigger_rate_failure.store(false);
+	}
+
+	input_capture_s trigger{};
+
+	// MODES 1 and 2 are not fully tested
+	if (_camera_capture_mode == 0 || _gpio_capture) {
+		trigger.timestamp = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+		trigger.seq = _capture_seq++;
+		_last_trig_time = trigger.timestamp;
+
+		publish = true;
+
+	} else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high)
+		if (_trigger.edge_state == 1) {
+			_last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+
+		} else if (_trigger.edge_state == 0 && _last_trig_begin_time > 0) {
+			trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
+			trigger.seq = _capture_seq++;
+			_last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
+			_last_trig_time = trigger.timestamp;
+			publish = true;
+			_capture_seq++;
+		}
+
+	} else { // Get timestamp of mid-exposure (active low)
+		if (_trigger.edge_state == 0) {
+			_last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+
+		} else if (_trigger.edge_state == 1 && _last_trig_begin_time > 0) {
+			trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
+			trigger.seq = _capture_seq++;
+			_last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
+			_last_trig_time = trigger.timestamp;
+			publish = true;
+		}
+
+	}
+
+	_capture_overflows = _trigger.overflow;
+
+	if (!publish) {
+		return;
+	}
+
+	_capture_pub.publish(trigger);
+}
+
+void
+InputCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
+				 uint32_t overflow)
+{
+	camera_capture::g_input_capture->capture_callback(chan_index, edge_time, edge_state, overflow);
+}
+
+void
+InputCapture::Run()
+{
+	// Command handling
+	vehicle_command_s cmd{};
+
+	if (_command_sub.update(&cmd)) {
+
+		// TODO : this should eventuallly be a capture control command
+		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
+
+			// Enable/disable signal capture
+			if (commandParamToInt(cmd.param1) == 1) {
+				set_capture_control(true);
+
+			} else if (commandParamToInt(cmd.param1) == 0) {
+				set_capture_control(false);
+
+			}
+
+			// Reset capture sequence
+			if (commandParamToInt(cmd.param2) == 1) {
+				reset_statistics(true);
+
+			}
+
+			// Acknowledge the command
+			vehicle_command_ack_s command_ack{};
+
+			command_ack.timestamp = hrt_absolute_time();
+			command_ack.command = cmd.command;
+			command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			command_ack.target_system = cmd.source_system;
+			command_ack.target_component = cmd.source_component;
+
+			_command_ack_pub.publish(command_ack);
+		}
+	}
+}
+
+void
+InputCapture::set_capture_control(bool enabled)
+{
+// a board can define BOARD_CAPTURE_GPIO to use a separate capture pin. It's used if no channel is configured
+#if defined(BOARD_CAPTURE_GPIO)
+
+	if (_capture_channel == -1) {
+		px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, true, false, true, &InputCapture::gpio_interrupt_routine, this);
+		_capture_enabled = enabled;
+		_gpio_capture = true;
+		reset_statistics(false);
+	}
+
+#endif
+
+	if (!_gpio_capture) {
+		if (_capture_channel == -1) {
+			PX4_WARN("No capture channel configured");
+			_capture_enabled = false;
+
+		} else {
+			capture_callback_t callback = nullptr;
+			void *context = nullptr;
+
+			if (enabled) {
+				callback = &InputCapture::capture_trampoline;
+				context = this;
+			}
+
+			int ret = up_input_capture_set_callback(_capture_channel, callback, context);
+
+			if (ret == 0) {
+				_capture_enabled = enabled;
+				_gpio_capture = false;
+
+			} else {
+				PX4_ERR("Unable to set capture callback for chan %" PRIu8 " (%i)", _capture_channel, ret);
+				_capture_enabled = false;
+			}
+
+			reset_statistics(false);
+		}
+	}
+
+}
+
+void
+InputCapture::reset_statistics(bool reset_seq)
+{
+	if (reset_seq) {
+		_capture_seq = 0;
+	}
+
+	_last_trig_begin_time = 0;
+	_last_exposure_time = 0;
+	_last_trig_time = 0;
+	_capture_overflows = 0;
+}
+
+int
+InputCapture::start()
+{
+	if (!_gpio_capture && _capture_channel != -1) {
+		input_capture_edge edge = Both;
+
+		if (_camera_capture_mode == 0) {
+			edge = _camera_capture_edge ? Rising : Falling;
+		}
+
+		int ret = up_input_capture_set(_capture_channel, edge, 0, nullptr, nullptr);
+
+		if (ret != 0) {
+			PX4_ERR("up_input_capture_set failed (%i)", ret);
+			return ret;
+		}
+	}
+
+	// run every 100 ms (10 Hz)
+	ScheduleOnInterval(100000, 10000);
+
+	return PX4_OK;
+}
+
+void
+InputCapture::stop()
+{
+	ScheduleClear();
+
+	work_cancel(HPWORK, &_work_publisher);
+
+	if (camera_capture::g_input_capture != nullptr) {
+		delete (camera_capture::g_input_capture);
+	}
+}
+
+void
+InputCapture::status()
+{
+	PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO");
+	PX4_INFO("Frame sequence : %" PRIu32, _capture_seq);
+
+	if (_last_trig_time != 0) {
+		PX4_INFO("Last trigger timestamp : %" PRIu64 " (%i ms ago)", _last_trig_time,
+			 (int)(hrt_elapsed_time(&_last_trig_time) / 1000));
+
+	} else {
+		PX4_INFO("No trigger yet");
+	}
+
+	if (_camera_capture_mode != 0) {
+		PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0);
+	}
+
+	PX4_INFO("Number of overflows : %" PRIu32, _capture_overflows);
+
+	if (_gpio_capture) {
+		PX4_INFO("Using board GPIO pin");
+
+	} else if (_capture_channel == -1) {
+		PX4_INFO("No Capture channel configured");
+
+	} else {
+		input_capture_stats_t stats;
+		int ret =  up_input_capture_get_stats(_capture_channel, &stats, false);
+
+		if (ret != 0) {
+			PX4_ERR("Unable to get stats for chan %" PRIu8 " (%i)", _capture_channel, ret);
+
+		} else {
+			PX4_INFO("Status chan: %" PRIu8 " edges: %" PRIu32 " last time: %" PRIu64 " last state: %" PRIu32
+				 " overflows: %" PRIu32 " latency: %" PRIu16,
+				 _capture_channel,
+				 stats.edges,
+				 stats.last_time,
+				 stats.last_edge,
+				 stats.overflows,
+				 stats.latency);
+		}
+	}
+}
+
+static int usage()
+{
+	PX4_INFO("usage: camera_capture {start|stop|on|off|reset|status}\n");
+	return 1;
+}
+
+extern "C" __EXPORT int input_capture_main(int argc, char *argv[]);
+
+int input_capture_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		return usage();
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (camera_capture::g_input_capture != nullptr) {
+			PX4_WARN("already running");
+			return 0;
+		}
+
+		camera_capture::g_input_capture = new InputCapture();
+
+		if (camera_capture::g_input_capture == nullptr) {
+			PX4_WARN("alloc failed");
+			return 1;
+		}
+
+		if (!camera_capture::g_input_capture->start()) {
+			return 0;
+
+		} else {
+			return 1;
+		}
+
+	}
+
+	if (camera_capture::g_input_capture == nullptr) {
+		PX4_WARN("not running");
+		return 1;
+
+	} else if (!strcmp(argv[1], "stop")) {
+		camera_capture::g_input_capture->stop();
+
+	} else if (!strcmp(argv[1], "status")) {
+		camera_capture::g_input_capture->status();
+
+	} else if (!strcmp(argv[1], "on")) {
+		camera_capture::g_input_capture->set_capture_control(true);
+
+	} else if (!strcmp(argv[1], "off")) {
+		camera_capture::g_input_capture->set_capture_control(false);
+
+	} else if (!strcmp(argv[1], "reset")) {
+		camera_capture::g_input_capture->set_capture_control(false);
+		camera_capture::g_input_capture->reset_statistics(true);
+
+	} else {
+		return usage();
+	}
+
+	return 0;
+}
@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 input_capture.hpp
+ *
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_input_capture.h>
+#include <drivers/drv_pwm_output.h>
+#include <lib/parameters/param.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/workqueue.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/input_capture.h>
+#include <uORB/topics/pps_capture.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+class InputCapture : public px4::ScheduledWorkItem
+{
+public:
+	/**
+	 * Constructor
+	 */
+	InputCapture();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~InputCapture();
+
+	/**
+	 * Start the task.
+	 */
+	int			start();
+
+	/**
+	 * Stop the task.
+	 */
+	void			stop();
+
+	void 			status();
+
+	// Low-rate command handling loop
+	void			Run() override;
+
+	static void		capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
+			uint32_t overflow);
+
+	void 			set_capture_control(bool enabled);
+
+	void			reset_statistics(bool reset_seq);
+
+	void			publish_trigger();
+
+
+	static struct work_s	_work_publisher;
+
+private:
+	int _capture_channel = 5; ///< by default, use FMU output 6
+
+	// Publishers
+	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<input_capture_s>		_capture_pub{ORB_ID(input_capture)};
+
+	// Subscribers
+	uORB::Subscription				_command_sub{ORB_ID(vehicle_command)};
+
+	// Trigger Buffer
+	struct _trig_s {
+		uint32_t chan_index;
+		hrt_abstime hrt_edge_time;
+		uint32_t edge_state;
+		uint32_t overflow;
+	} _trigger{};
+
+	bool			_capture_enabled{false};
+	bool			_gpio_capture{false};
+
+	// Parameters
+	param_t 		_p_strobe_delay{PARAM_INVALID};
+	float			_strobe_delay{0.0f};
+	param_t			_p_camera_capture_mode{PARAM_INVALID};
+	int32_t			_camera_capture_mode{0};
+	param_t			_p_camera_capture_edge{PARAM_INVALID};
+	int32_t			_camera_capture_edge{0};
+
+	// Signal capture statistics
+	uint32_t		_capture_seq{0};
+	hrt_abstime		_last_trig_begin_time{0};
+	hrt_abstime		_last_exposure_time{0};
+	hrt_abstime		_last_trig_time{0};
+	uint32_t 		_capture_overflows{0};
+
+	hrt_abstime	_pps_hrt_timestamp{0};
+	uint64_t		_pps_rtc_timestamp{0};
+	uint8_t			_trigger_rate_exceeded_counter{0};
+	px4::atomic<bool> _trigger_rate_failure{false};
+
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	// Signal capture callback
+	void			capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
+
+	// GPIO interrupt routine
+	static int		gpio_interrupt_routine(int irq, void *context, void *arg);
+
+	// Signal capture publish
+	static void		publish_trigger_trampoline(void *arg);
+
+};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017 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,45 +31,23 @@
 *
 ****************************************************************************/

-#include <matrix/math.hpp>
+/**
+ * @file input_capture_params.c
+ * Input capture parameters
+ *
+ * @author Mohammed Kabir <kabir@uasys.io>
+ * @author Jaeyoung Lim <jalim@ethz.ch>
+ */

-#ifndef EKF_UTILS_HPP
-#define EKF_UTILS_HPP
-
-// Use Kahan summation algorithm to get the sum of "sum_previous" and "input".
-// This function relies on the caller to be responsible for keeping a copy of
-// "accumulator" and passing this value at the next iteration.
-// Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm
-inline float kahanSummation(float sum_previous, float input, float &accumulator)
-{
-	const float y = input - accumulator;
-	const float t = sum_previous + y;
-	accumulator = (t - sum_previous) - y;
-	return t;
-}
-
-namespace ecl
-{
-inline float powf(float x, int exp)
-{
-	float ret;
-
-	if (exp > 0) {
-		ret = x;
-
-		for (int count = 1; count < exp; count++) {
-			ret *= x;
-		}
-
-		return ret;
-
-	} else if (exp < 0) {
-		return 1.0f / ecl::powf(x, -exp);
-	}
-
-	return 1.0f;
-}
-
-} // namespace ecl
-
-#endif // EKF_UTILS_HPP
+/**
+ * Input Capture Enable
+ *
+ * Enables the PPS capture module.
+ * This switches mode of FMU channel 7 to be the
+ * PPS input channel.
+ *
+ * @boolean
+  * @group Camera Control
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(INPUT_CAP_ENABLE, 0);
@@ -320,10 +320,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet)

 			sensor_gps.fix_type = gpsFix;

-			sensor_gps.lat = positionGpsLla.c[0] * 1e7;
-			sensor_gps.lon = positionGpsLla.c[1] * 1e7;
-			sensor_gps.alt = positionGpsLla.c[2] * 1e3;
-			sensor_gps.alt_ellipsoid = sensor_gps.alt;
+			sensor_gps.latitude_deg = positionGpsLla.c[0];
+			sensor_gps.longitude_deg = positionGpsLla.c[1];
+			sensor_gps.altitude_msl_m = positionGpsLla.c[2];
+			sensor_gps.altitude_ellipsoid_m = sensor_gps.altitude_msl_m;

 			sensor_gps.vel_ned_valid = true;
 			sensor_gps.vel_n_m_s = velocityGpsNed.c[0];
@@ -4,7 +4,7 @@ actuator_output:
    - param_prefix: PWM_MAIN
      channel_label: 'Channel'
      standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
        min: { min: 800, max: 1400, default: 1000 }
        max: { min: 1600, max: 2200, default: 2000 }
        failsafe: { min: 800, max: 2200 }
@@ -386,9 +386,9 @@ struct msp_raw_gps_t {
 	uint8_t  numSat;
 	int32_t  lat;           // 1 / 10000000 deg
 	int32_t  lon;           // 1 / 10000000 deg
-	int16_t  alt;           // meters
+	int16_t  alt;           // centimeters since MSP API 1.39, meters before
 	int16_t  groundSpeed;   // cm/s
-	int16_t  groundCourse;  // unit: degree x 10
+	int16_t  groundCourse;  // unit: degree x 100, centidegrees
 	uint16_t hdop;
 } __attribute__((packed));

@@ -314,9 +314,9 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
 	msp_raw_gps_t raw_gps {0};

 	if (vehicle_gps_position.fix_type >= 2) {
-		raw_gps.lat = vehicle_gps_position.lat;
-		raw_gps.lon = vehicle_gps_position.lon;
-		raw_gps.alt =  vehicle_gps_position.alt / 10;
+		raw_gps.lat = static_cast<int32_t>(vehicle_gps_position.latitude_deg * 1e7);
+		raw_gps.lon = static_cast<int32_t>(vehicle_gps_position.longitude_deg * 1e7);
+		raw_gps.alt = static_cast<int16_t>(vehicle_gps_position.altitude_msl_m * 100.0);

 		float course = math::degrees(vehicle_gps_position.cog_rad);

@@ -432,14 +432,14 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
 	msp_altitude_t altitude {0};

 	if (vehicle_gps_position.fix_type >= 2) {
-		altitude.estimatedActualPosition = vehicle_gps_position.alt / 10;
+		altitude.estimatedActualPosition = static_cast<int32_t>(vehicle_gps_position.altitude_msl_m * 100.0);	// cm

 	} else {
 		altitude.estimatedActualPosition = 0;
 	}

 	if (estimator_status.solution_status_flags & (1 << 5)) {
-		altitude.estimatedActualVelocity = -vehicle_local_position.vz * 10; //m/s to cm/s
+		altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s

 	} else {
 		altitude.estimatedActualVelocity = 0;
@@ -4,7 +4,7 @@ actuator_output:
    - param_prefix: PCA9685
      channel_label: 'Channel'
      standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
        min: { min: 800, max: 1400, default: 1000 }
        max: { min: 1600, max: 2200, default: 2000 }
        failsafe: { min: 800, max: 2200 }
@@ -218,42 +218,55 @@ void PWMOut::update_params()

 	updateParams();

-	// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
+	// Automatically set PWM configuration when a channel is first assigned
 	if (!_first_update_cycle) {
 		for (size_t i = 0; i < _num_outputs; i++) {
 			if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
 				int32_t output_function;

-				if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
-				    && output_function >= (int)OutputFunction::Servo1
-				    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
-					int32_t val = 1500;
-					PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
-					param_set(_mixing_output.disarmedParamHandle(i), &val);
+				if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
+					// Servos need PWM rate 50Hz and disramed value 1500us
+					if (output_function >= (int)OutputFunction::Servo1
+					    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+						int32_t val = 1500;
+						PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
+						param_set(_mixing_output.disarmedParamHandle(i), &val);

-					// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
-					for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+						// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+						for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {

-						uint32_t channels = io_timer_get_group(timer);
+							uint32_t channels = io_timer_get_group(timer);

-						if ((channels & (1u << i)) == 0) {
-							continue;
-						}
+							if ((channels & (1u << i)) == 0) {
+								continue;
+							}

-						if ((channels & previously_set_functions) == 0) { // None of the channels was set
-							char param_name[17];
-							snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+							if ((channels & previously_set_functions) == 0) { // None of the channels was set
+								char param_name[17];
+								snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);

-							int32_t tim_config = 0;
-							param_t handle = param_find(param_name);
+								int32_t tim_config = 0;
+								param_t handle = param_find(param_name);

-							if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
-								tim_config = 50;
-								PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
-								param_set(handle, &tim_config);
+								if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+									tim_config = 50;
+									PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
+									param_set(handle, &tim_config);
+								}
 							}
 						}
 					}
+
+					// Motors need a minimum value that idles the motor and have a deadzone at the top of the range
+					if (output_function >= (int)OutputFunction::Motor1
+					    && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
+						int32_t val = 1100;
+						PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
+						param_set(_mixing_output.minParamHandle(i), &val);
+						val = 1900;
+						PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
+						param_set(_mixing_output.maxParamHandle(i), &val);
+					}
 				}
 			}
 		}
@@ -5,7 +5,7 @@ actuator_output:
      param_prefix: '${PWM_MAIN_OR_AUX}'
      channel_labels: ['${PWM_MAIN_OR_AUX}', '${PWM_MAIN_OR_AUX_CAP}']
      standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
        min: { min: 800, max: 1400, default: 1000 }
        max: { min: 1600, max: 2200, default: 2000 }
        failsafe: { min: 800, max: 2200 }
@@ -7,7 +7,7 @@ actuator_output:
      channel_label_module_name_prefix: false
      timer_config_file: "boards/px4/io-v2/src/timer_config.cpp"
      standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
        min: { min: 800, max: 1400, default: 1000 }
        max: { min: 1600, max: 2200, default: 2000 }
        failsafe: { min: 800, max: 2200 }
@@ -702,36 +702,48 @@ void PX4IO::update_params()
 				if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
 					int32_t output_function;

-					if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
-					    && output_function >= (int)OutputFunction::Servo1
-					    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
-						int32_t val = 1500;
-						PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
-						param_set(_mixing_output.disarmedParamHandle(i), &val);
+					if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
+						if (output_function >= (int)OutputFunction::Servo1
+						    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+							int32_t val = 1500;
+							PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
+							param_set(_mixing_output.disarmedParamHandle(i), &val);

-						// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
-						for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
+							// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+							for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {

-							uint32_t channels = _group_channels[timer];
+								uint32_t channels = _group_channels[timer];

-							if ((channels & (1u << i)) == 0) {
-								continue;
-							}
+								if ((channels & (1u << i)) == 0) {
+									continue;
+								}

-							if ((channels & previously_set_functions) == 0) { // None of the channels was set
-								char param_name[17];
-								snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+								if ((channels & previously_set_functions) == 0) { // None of the channels was set
+									char param_name[17];
+									snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);

-								int32_t tim_config = 0;
-								param_t handle = param_find(param_name);
+									int32_t tim_config = 0;
+									param_t handle = param_find(param_name);

-								if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
-									tim_config = 50;
-									PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
-									param_set(handle, &tim_config);
+									if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+										tim_config = 50;
+										PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
+										param_set(handle, &tim_config);
+									}
 								}
 							}
 						}
+
+						// Motors need a minimum value that idles the motor
+						if (output_function >= (int)OutputFunction::Motor1
+						    && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
+							int32_t val = 1100;
+							PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
+							param_set(_mixing_output.minParamHandle(i), &val);
+							val = 1900;
+							PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
+							param_set(_mixing_output.maxParamHandle(i), &val);
+						}
 					}
 				}
 			}
@@ -241,11 +241,11 @@ void CrsfRc::Run()
 				sensor_gps_s sensor_gps;

 				if (_vehicle_gps_position_sub.update(&sensor_gps)) {
-					int32_t latitude = sensor_gps.lat;
-					int32_t longitude = sensor_gps.lon;
+					int32_t latitude = static_cast<int32_t>(round(sensor_gps.latitude_deg * 1e7));
+					int32_t longitude = static_cast<int32_t>(round(sensor_gps.longitude_deg * 1e7));
 					uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f;
 					uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f;
-					uint16_t altitude = sensor_gps.alt + 1000;
+					uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m * 1e3) + 1000;
 					uint8_t num_satellites = sensor_gps.satellites_used;
 					this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites);
 				}
@@ -96,11 +96,11 @@ bool CRSFTelemetry::send_gps()
 		return false;
 	}

-	int32_t latitude = vehicle_gps_position.lat;
-	int32_t longitude = vehicle_gps_position.lon;
+	int32_t latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));
+	int32_t longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));
 	uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
 	uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
-	uint16_t altitude = vehicle_gps_position.alt + 1000;
+	uint16_t altitude = static_cast<int16_t>(round(vehicle_gps_position.altitude_msl_m + 1.0));
 	uint8_t num_satellites = vehicle_gps_position.satellites_used;

 	return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed,
@@ -110,9 +110,9 @@ bool GHSTTelemetry::send_gps1_status()
 		return false;
 	}

-	int32_t latitude = vehicle_gps_position.lat;				// 1e-7 degrees
-	int32_t longitude = vehicle_gps_position.lon;				// 1e-7 degrees
-	uint16_t altitude = vehicle_gps_position.alt / 1000;			// mm -> m
+	int32_t latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));        // 1e-7 degrees
+	int32_t longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));      // 1e-7 degrees
+	uint16_t altitude = static_cast<int16_t>(round(vehicle_gps_position.altitude_msl_m));           // meters

 	return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
 }
@@ -65,6 +65,8 @@ int TAP_ESC::init()
 		return ret;
 	}

+	_esc_feedback_pub.advertise();
+
 	/* Respect boot time required by the ESC FW */
 	hrt_abstime uptime_us = hrt_absolute_time();

@@ -268,9 +268,9 @@ void BST::RunImpl()
 		if (gps.fix_type >= 3 && gps.eph < 50.0f) {
 			BSTPacket<BSTGPSPosition> bst_gps = {};
 			bst_gps.type = 0x02;
-			bst_gps.payload.lat = swap_int32(gps.lat);
-			bst_gps.payload.lon = swap_int32(gps.lon);
-			bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000);
+			bst_gps.payload.lat = swap_int32(static_cast<int32_t>(round(gps.latitude_deg * 1e7)));
+			bst_gps.payload.lon = swap_int32(static_cast<int32_t>(round(gps.longitude_deg * 1e7)));
+			bst_gps.payload.alt = swap_int16(static_cast<int16_t>(round(gps.altitude_msl_m)) + 1000);
 			bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f);
 			bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F);
 			bst_gps.payload.sats = gps.satellites_used;
@@ -233,11 +233,11 @@ void sPort_send_GPS_LON(int uart)
 	/* send longitude */
 	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
 	/* precision is approximately 0.1m */
-	uint32_t iLon =  6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lon);
+	uint32_t iLon =  6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg * 1e7);

 	iLon |= (1 << 31);

-	if (s_port_subscription_data->vehicle_gps_position_sub.get().lon < 0) { iLon |= (1 << 30); }
+	if (s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg < 0) { iLon |= (1 << 30); }

 	sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon);
 }
@@ -246,9 +246,9 @@ void sPort_send_GPS_LAT(int uart)
 {
 	/* send latitude */
 	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */
-	uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lat);
+	uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg * 1e7);

-	if (s_port_subscription_data->vehicle_gps_position_sub.get().lat < 0) { iLat |= (1 << 30); }
+	if (s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg < 0) { iLat |= (1 << 30); }

 	sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat);
 }
@@ -256,7 +256,7 @@ void sPort_send_GPS_LAT(int uart)
 void sPort_send_GPS_ALT(int uart)
 {
 	/* send altitude */
-	uint32_t iAlt = s_port_subscription_data->vehicle_gps_position_sub.get().alt / 10;
+	uint32_t iAlt = static_cast<uint32_t>(s_port_subscription_data->vehicle_gps_position_sub.get().altitude_msl_m * 1e2);
 	sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt);
 }

@@ -242,7 +242,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;

 		/* Get latitude in degrees, minutes and seconds */
-		double lat = ((double)(gps.lat)) * 1e-7d;
+		double lat = gps.latitude_deg;

 		/* Set the N or S specifier */
 		msg.latitude_ns = 0;
@@ -265,7 +265,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;

 		/* Get longitude in degrees, minutes and seconds */
-		double lon = ((double)(gps.lon)) * 1e-7d;
+		double lon = gps.longitude_deg;

 		/* Set the E or W specifier */
 		msg.longitude_ew = 0;
@@ -285,7 +285,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;

 		/* Altitude */
-		uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f);
+		uint16_t alt = (uint16_t)(round(gps.altitude_msl_m) + 500.0);
 		msg.altitude_L = (uint8_t)alt & 0xff;
 		msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;

@@ -455,7 +455,7 @@ void SagetechMXS::determine_furthest_aircraft()
 			continue;
 		}

-		const float distance = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE,
+		const float distance = get_distance_to_next_waypoint(_gps.latitude_deg, _gps.longitude_deg,
 				       vehicle_list[index].lat,
 				       vehicle_list[index].lon);

@@ -492,8 +492,8 @@ void SagetechMXS::handle_vehicle(const transponder_report_s &vehicle)
 	// needs to handle updating the vehicle list, keeping track of which vehicles to drop
 	// and which to keep, allocating new vehicles, and publishing to the transponder_report topic
 	uint16_t index = list_size_allocated + 1; // Make invalid to start with.
-	const bool my_loc_is_zero = (_gps.lat == 0) && (_gps.lon == 0);
-	const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE,
+	const bool my_loc_is_zero = (fabs(_gps.latitude_deg) < DBL_EPSILON) && (fabs(_gps.longitude_deg) < DBL_EPSILON);
+	const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.latitude_deg, _gps.longitude_deg,
 			vehicle.lat, vehicle.lon);
 	const bool is_tracked_in_list = find_index(vehicle, &index);
 	// const bool is_special = is_special_vehicle(vehicle.icao_address);
@@ -745,7 +745,8 @@ void SagetechMXS::send_operating_msg()
 	mxs_state.op.altRes25 =
 		!mxs_state.inst.altRes100;                                // Host Altitude Resolution from install

-	mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT;                       // Height above sealevel in feet
+	mxs_state.op.altitude = static_cast<int32_t>(_gps.altitude_msl_m *
+				SAGETECH_SCALE_M_TO_FT);   // Height above sealevel in feet

 	mxs_state.op.identOn = _adsb_ident.get();

@@ -806,8 +807,8 @@ void SagetechMXS::send_gps_msg()
 	}

 	// Get Vehicle Longitude and Latitude and Convert to string
-	const int32_t longitude = _gps.lon;
-	const int32_t latitude =  _gps.lat;
+	const int32_t longitude = static_cast<int32_t>(_gps.longitude_deg * 1e7);
+	const int32_t latitude =  static_cast<int32_t>(_gps.latitude_deg * 1e7);
 	const double lon_deg = longitude * 1.0E-7 * (longitude < 0 ? -1 : 1);
 	const double lon_minutes = (lon_deg - int(lon_deg)) * 60;
 	snprintf((char *)&gps.longitude, 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes,
@@ -836,7 +837,7 @@ void SagetechMXS::send_gps_msg()
 	snprintf((char *)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min,
 		 tm->tm_sec + (_gps.time_utc_usec % 1000000) * 1.0e-6);

-	gps.height = _gps.alt_ellipsoid * 1E-3;
+	gps.height = (float)_gps.altitude_ellipsoid_m;

 	// checkGPSInputs(&gps);
 	last.msg.type = SG_MSG_TYPE_HOST_GPS;
@@ -1284,7 +1285,8 @@ void SagetechMXS::auto_config_operating()
 	mxs_state.op.altHostAvlbl = false;
 	mxs_state.op.altRes25 = true;                                // Host Altitude Resolution from install

-	mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT;                       // Height above sealevel in feet
+	mxs_state.op.altitude = static_cast<int32_t>(_gps.altitude_msl_m *
+				SAGETECH_SCALE_M_TO_FT);     // Height above sealevel in feet

 	mxs_state.op.identOn = false;

@@ -142,7 +142,7 @@ private:
 	static constexpr float SAGETECH_SCALE_KNOTS_TO_M_PER_SEC{0.514444F};
 	static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_KNOTS{1.94384F};
 	static constexpr float SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC{0.00508F};
-	static constexpr float SAGETECH_SCALE_MM_TO_FT{0.00328084F};
+	static constexpr double SAGETECH_SCALE_M_TO_FT{3.28084};
 	static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN{196.85F};
 	static constexpr uint8_t ADSB_ALTITUDE_TYPE_PRESSURE_QNH{0};
 	static constexpr uint8_t ADSB_ALTITUDE_TYPE_GEOMETRIC{1};
@@ -156,7 +156,6 @@ private:
 	static constexpr uint16_t INVALID_SQUAWK{7777};
 	static constexpr unsigned BAUD_460800{0010004};			// B460800 not defined in MacOS termios
 	static constexpr unsigned BAUD_921600{0010007};			// B921600 not defined in MacOS termios
-	static constexpr double GPS_SCALE{1.0E-7};

 	// Stored variables
 	uint64_t _loop_count;
@@ -64,6 +64,8 @@ UavcanEscController::init()
 		return res;
 	}

+	_esc_status_pub.advertise();
+
 	return res;
 }

@@ -337,10 +337,10 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	 */
 	report.timestamp = hrt_absolute_time();

-	report.lat           = msg.latitude_deg_1e8 / 10;
-	report.lon           = msg.longitude_deg_1e8 / 10;
-	report.alt           = msg.height_msl_mm;
-	report.alt_ellipsoid = msg.height_ellipsoid_mm;
+	report.latitude_deg         = msg.latitude_deg_1e8 / 1e8;
+	report.longitude_deg        = msg.longitude_deg_1e8 / 1e8;
+	report.altitude_msl_m       = msg.height_msl_mm / 1e3;
+	report.altitude_ellipsoid_m = msg.height_ellipsoid_mm / 1e3;

 	if (valid_pos_cov) {
 		// Horizontal position uncertainty
@@ -177,6 +177,8 @@ public:
 	void handleTxInterrupt(uavcan::uint64_t utc_usec);
 	void handleRxInterrupt(uavcan::uint8_t fifo_index);

+	void handleBusOff();
+
 	/**
 	 * This method is used to count errors and abort transmission on error if necessary.
 	 * This functionality used to be implemented in the SCE interrupt handler, but that approach was
@@ -147,6 +147,12 @@ inline void handleTxInterrupt(uavcan::uint8_t iface_index)
 		}

 		ifaces[iface_index]->handleTxInterrupt(utc_usec);
+
+	} else if ((fdcan::Can[iface_index]->IR & FDCAN_IR_BO) != 0) {
+
+		fdcan::Can[iface_index]->IR  = FDCAN_IR_BO;
+		ifaces[iface_index]->handleBusOff();
+
 	}
 }

@@ -184,6 +190,11 @@ inline void handleRxInterrupt(uavcan::uint8_t iface_index)
 		fdcan::Can[iface_index]->IR = (FDCAN_IR_RF1N | FDCAN_IR_RF1F);
 		ifaces[iface_index]->handleRxInterrupt(1);

+	} else if ((IR & FDCAN_IR_BO) != 0) {
+
+		fdcan::Can[iface_index]->IR  = FDCAN_IR_BO;
+		ifaces[iface_index]->handleBusOff();
+
 	} else {
 		UAVCAN_ASSERT(0);
 	}
@@ -500,6 +511,37 @@ uavcan::int16_t CanIface::receive(uavcan::CanFrame &out_frame, uavcan::Monotonic
 uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter_configs,
 		uavcan::uint16_t num_configs)
 {
+
+	/*
+	* Software initialization is started by setting INIT bit in FDCAN_CCCR register, either by
+	* software or by a hardware reset, or by going Bus_Off. While INIT bit in FDCAN_CCCR
+	* register is set, message transfer from and to the CAN bus is stopped, the status of the CAN
+	* bus output FDCAN_TX is recessive (high). The counters of the error management logic
+	* (EML) are unchanged. Setting INIT bit in FDCAN_CCCR does not change any configuration
+	* register. Clearing INIT bit in FDCAN_CCCR finishes the software initialization. Afterwards
+	* the bit stream processor (BSP) synchronizes itself to the data transfer on the CAN bus by
+	* waiting for the occurrence of a sequence of 11 consecutive recessive bits (Bus_Idle) before
+	* it can take part in bus activities and start the message transfer.
+	*/
+
+	/*
+	* Access to the FDCAN configuration registers is only enabled when both INIT bit in
+	* FDCAN_CCCR register and CCE bit in FDCAN_CCCR register are set
+	*/
+
+	/*
+	* CCE bit in FDCAN_CCCR register can only be set/cleared while INIT bit in FDCAN_CCCR
+	* is set. CCE bit in FDCAN_CCCR register is automatically cleared when INIT bit in
+	* FDCAN_CCCR is cleared
+	*/
+
+	/*
+	 * Up to 128 filter elements can be configured for 11-bit standard IDs. When accessing a
+	 * standard message ID filter element, its address is the filter list standard start address
+	 * FDCAN_SIDFC.FLSSA plus the index of the filter element (0 ... 127
+	*/
+
+
 	/*
 	 * The FDCAN controller handles standard ID and extended ID filters separately.
 	 * We must scan through the requested filter configurations, and group them by
@@ -509,21 +551,95 @@ uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter
 	// Filter config registers are protected; write access is only available
 	// when bit CCE and bit INIT of FDCAN_CCCR register are set to 1.

-	// CriticalSectionLocker lock;
+	uint32_t num_of_sid_filter = 0;
+	uint32_t num_of_xid_filter = 0;

-	// // Request Init mode, then wait for completion
-	// can_->CCCR |= FDCAN_CCCR_INIT;
-	// while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {}
+	if (num_configs <= NumFilters) {

-	// // Configuration Chane Enable
-	// can_->CCCR |= FDCAN_CCCR_CCE;
+		CriticalSectionLocker lock;

-	// /// TODO -------------------------
+		// // Request Init mode, then wait for completion
+		can_->CCCR |= FDCAN_CCCR_INIT;

-	// // Leave Init mode
-	// can_->CCCR &= ~FDCAN_CCCR_INIT;
+		while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {};

-	return 0;
+		// // Configuration Chane Enable
+		can_->CCCR |= FDCAN_CCCR_CCE;
+
+		for (uint8_t i = 0; i < NumFilters; i++) {
+
+			if (i < num_configs) {
+
+				// determine what type of filter is this:
+				// and add to the number of filter
+				const uavcan::CanFilterConfig *const cfg = filter_configs + i;
+
+				// extended message
+				if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) {
+
+					volatile uint32_t *xid_filter_address = (uint32_t *)((can_->XIDFC | FDCAN_XIDFC_FLESA_Msk) + 2 * num_of_xid_filter);
+					num_of_xid_filter += 1;
+
+					uint32_t f0 = 0;
+					uint32_t f1 = 0;
+
+					// bit 31:29 EFEC[2:0], extended filter element configuration -> set Priority
+					f0 |= 4 << 29;
+
+					// bit 28:0 EFID[28:0], Extended Filter ID
+					f0 |= cfg->id;
+
+					// bit 31:30 EFEC[2:0], extended filter type -> classic filter
+					f1 |= 2 << 30;
+
+					// bit 28:0 EFID2[28:18], Extended Filter ID2
+					f1 |= cfg->mask;
+
+					*xid_filter_address = f0;
+					xid_filter_address += 1;
+					*(xid_filter_address) = f1;
+				}
+
+				// standard message
+				else {
+					volatile uint32_t *sid_filter_address = (uint32_t *)((can_->SIDFC | FDCAN_SIDFC_FLSSA_Msk) + num_of_sid_filter);
+
+					num_of_sid_filter += 1;
+
+					uint32_t filter = 0;
+
+					// bit 31:30 SFT[1:0], standard filter type, -> classic
+					filter |= 2 << 30;
+
+					// bit 29:27 SFEC[2:0], standard filter element configuration, -> Set priority
+					filter |= (4 << 27);
+
+					// bit 26:16 SFID1[10:0], Standard Filter ID1
+					filter |= (cfg->id << 16);
+
+					// bit 15:0 SFID2[15:10], Standard Filter ID2
+					filter |= (cfg->mask << 10);
+
+					*sid_filter_address = filter;
+
+
+				}
+			}
+
+		}
+
+		// set the number of SID filters
+		can_->SIDFC |= (num_of_sid_filter << FDCAN_SIDFC_LSS_Pos);
+		// set the number of XID filters
+		can_->XIDFC |= (num_of_xid_filter << FDCAN_XIDFC_LSE_Pos);
+
+		// // Leave Init mode
+		can_->CCCR &= ~FDCAN_CCCR_INIT;
+		return 0;
+	}
+
+
+	return -ErrFilterNumConfigs;
 }

 bool CanIface::waitCCCRBitStateChange(uint32_t mask, bool target_state)
@@ -646,7 +762,8 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
 		   | FDCAN_IE_RF0NE   // Rx FIFO 0 new message
 		   | FDCAN_IE_RF0FE   // Rx FIFO 0 FIFO full
 		   | FDCAN_IE_RF1NE   // Rx FIFO 1 new message
-		   | FDCAN_IE_RF1FE;  // Rx FIFO 1 FIFO full
+		   | FDCAN_IE_RF1FE  // Rx FIFO 1 FIFO full
+		   | FDCAN_IE_BOE;   // bus off state

 	// Keep Rx interrupts on Line 0; move Tx to Line 1
 	can_->ILS = FDCAN_ILS_TCL;  // TC interrupt on line 1
@@ -681,8 +798,8 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
 	 * factor of 4 necessary in the address relative to the SA register values.
 	 */

-	// Location of this interface's message RAM - address in CPU memory address
-	// and relative address (in words) used for configuration
+// Location of this interface's message RAM - address in CPU memory address
+// and relative address (in words) used for configuration
 	const uint32_t iface_ram_base = (2560 / 2) * self_index_;
 	const uint32_t gl_ram_base = SRAMCAN_BASE;
 	uint32_t ram_offset = iface_ram_base;
@@ -754,6 +871,24 @@ void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec)
 	pollErrorFlagsFromISR();
 }

+
+void CanIface::handleBusOff()
+{
+
+	/*
+	 * The bus off recovery sequence consists of 128 occurrences of 11 consecutive recessive bits. MCAN controllers
+	 * start sensing the bus looking for the recovery sequence when the INIT bit of control register (CCCR) is reset by
+	 * the user. The bus off recovery sequence cannot be shortened by setting or resetting CCCR[INIT].
+	 * Summarizing, if the device raises a bus off condition, CCCR[INIT] is set stopping all bus activities. Once
+	 * CCCR[INIT] has been cleared again by the software, the device will then wait for 129 occurrences of bus idle
+	 * (129 x 11 consecutive recessive bits) before resuming on normal operation. At the end of the bus off recovery
+	 * sequence, the error management counters will be reset, and so PSR.BO, ECR.TEC, and ECR.REC.
+	*/
+
+	can_->CCCR &= ~FDCAN_CCCR_INIT;
+
+}
+
 void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index)
 {
 	UAVCAN_ASSERT(fifo_index < 2);
@@ -81,10 +81,10 @@ public:

 			fix2.gnss_time_standard = fix2.GNSS_TIME_STANDARD_UTC;
 			fix2.gnss_timestamp.usec = gps.time_utc_usec;
-			fix2.latitude_deg_1e8 = (int64_t)gps.lat * 10;
-			fix2.longitude_deg_1e8 = (int64_t)gps.lon * 10;
-			fix2.height_msl_mm = gps.alt;
-			fix2.height_ellipsoid_mm = gps.alt_ellipsoid;
+			fix2.latitude_deg_1e8 = (int64_t)(gps.latitude_deg * 1e8);
+			fix2.longitude_deg_1e8 = (int64_t)(gps.longitude_deg * 1e8);
+			fix2.height_msl_mm = (int32_t)(gps.altitude_msl_m * 1e3);
+			fix2.height_ellipsoid_mm = (int32_t)(gps.altitude_ellipsoid_m * 1e3);
 			fix2.status = gps.fix_type;
 			fix2.ned_velocity[0] = gps.vel_n_m_s;
 			fix2.ned_velocity[1] = gps.vel_e_m_s;
@@ -4,12 +4,11 @@ serial_config:
    port_config_param:
      name: UWB_PORT_CFG
      group: UWB
-      default: ""
+      default: "TEL2"

 parameters:
    - group: UWB
      definitions:
-
        UWB_INIT_OFF_X:
          description:
            short: UWB sensor X offset in body frame
@@ -32,7 +31,7 @@ parameters:

        UWB_INIT_OFF_Z:
          description:
-            short: UWB sensor Y offset in body frame
+            short: UWB sensor Z offset in body frame
            long: UWB sensor positioning in relation to Drone in NED. Z offset.
          type: float
          unit: m
@@ -40,14 +39,52 @@ parameters:
          increment: 0.01
          default: 0.00

-        UWB_INIT_OFF_YAW:
+        UWB_SENS_ROT:
          description:
-            short: UWB sensor YAW offset in body frame
-            long: UWB sensor positioning in relation to Drone in NED. Yaw rotation in relation to direction of FMU.
-          type: float
-          unit: deg
-          decimal: 1
-          increment: 0.1
-          default: 0.00
-
-
+            short: UWB sensor orientation
+            long: The orientation of the sensor relative to the forward direction of the body frame. Look up table in src/lib/conversion/rotation.h
+                  Default position is the antannaes downward facing, UWB board parallel with body frame.
+          type: enum
+          values:
+            0: ROTATION_NONE
+            1: ROTATION_YAW_45
+            2: ROTATION_YAW_90
+            3: ROTATION_YAW_135
+            4: ROTATION_YAW_180
+            5: ROTATION_YAW_225
+            6: ROTATION_YAW_270
+            7: ROTATION_YAW_315
+            8: ROTATION_ROLL_180
+            9: ROTATION_ROLL_180_YAW_45
+            10: ROTATION_ROLL_180_YAW_90
+            11: ROTATION_ROLL_180_YAW_135
+            12: ROTATION_PITCH_180
+            13: ROTATION_ROLL_180_YAW_225
+            14: ROTATION_ROLL_180_YAW_270
+            15: ROTATION_ROLL_180_YAW_315
+            16: ROTATION_ROLL_90
+            17: ROTATION_ROLL_90_YAW_45
+            18: ROTATION_ROLL_90_YAW_90
+            19: ROTATION_ROLL_90_YAW_135
+            20: ROTATION_ROLL_270
+            21: ROTATION_ROLL_270_YAW_45
+            22: ROTATION_ROLL_270_YAW_90
+            23: ROTATION_ROLL_270_YAW_135
+            24: ROTATION_PITCH_90
+            25: ROTATION_PITCH_270
+            26: ROTATION_PITCH_180_YAW_90
+            27: ROTATION_PITCH_180_YAW_270
+            28: ROTATION_ROLL_90_PITCH_90
+            29: ROTATION_ROLL_180_PITCH_90
+            30: ROTATION_ROLL_270_PITCH_90
+            31: ROTATION_ROLL_90_PITCH_180
+            32: ROTATION_ROLL_270_PITCH_180
+            33: ROTATION_ROLL_90_PITCH_270
+            34: ROTATION_ROLL_180_PITCH_270
+            35: ROTATION_ROLL_270_PITCH_270
+            36: ROTATION_ROLL_90_PITCH_180_YAW_90
+            37: ROTATION_ROLL_90_YAW_270
+            38: ROTATION_ROLL_90_PITCH_68_YAW_293
+            39: ROTATION_PITCH_315
+            40: ROTATION_ROLL_90_PITCH_315
+          default: 0
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -63,128 +63,130 @@
 // The default baudrate of the uwb_sr150 module before configuration
 #define DEFAULT_BAUD B115200

-const uint8_t CMD_RANGING_STOP[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_STOP};
-const uint8_t CMD_RANGING_START[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_START};
-const uint8_t CMD_APP_START[UWB_CMD_LEN ] = {0x01, 0x00, 0x02, UWB_APP_START, UWB_PRECNAV_APP};
-const uint8_t CMD_APP_STOP[0x04 ] = {0x01, 0x00, 0x01, UWB_APP_STOP};
-
 extern "C" __EXPORT int uwb_sr150_main(int argc, char *argv[]);

-UWB_SR150::UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug):
+UWB_SR150::UWB_SR150(const char *port):
 	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
 	_read_count_perf(perf_alloc(PC_COUNT, "uwb_sr150_count")),
 	_read_err_perf(perf_alloc(PC_COUNT, "uwb_sr150_err"))
 {
-	_uwb_pos_debug = uwb_pos_debug;
-	// start serial port
-	_uart = open(device_name, O_RDWR | O_NOCTTY);
-
-	if (_uart < 0) { err(1, "could not open %s", device_name); }
-
-	int ret = 0;
-	struct termios uart_config {};
-	ret = tcgetattr(_uart, &uart_config);
-
-	if (ret < 0) { err(1, "failed to get attr"); }
-
-	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
-	ret = cfsetispeed(&uart_config, baudrate);
-
-	if (ret < 0) { err(1, "failed to set input speed"); }
-
-	ret = cfsetospeed(&uart_config, baudrate);
-
-	if (ret < 0) { err(1, "failed to set output speed"); }
-
-	ret = tcsetattr(_uart, TCSANOW, &uart_config);
-
-	if (ret < 0) { err(1, "failed to set attr"); }
+	/* store port name */
+	strncpy(_port, port, sizeof(_port) - 1);
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
 }

 UWB_SR150::~UWB_SR150()
 {
 	printf("UWB: Ranging Stopped\t\n");
-	int written = write(_uart, &CMD_APP_STOP, sizeof(CMD_APP_STOP));
-
-	if (written < (int) sizeof(CMD_APP_STOP)) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_APP_STOP));
-	}

+	// stop{}; will be implemented when this is changed to a scheduled work task
 	perf_free(_read_err_perf);
 	perf_free(_read_count_perf);

 	close(_uart);
 }

-void UWB_SR150::run()
+bool UWB_SR150::init()
 {
-	// Subscribe to parameter_update message
-	parameters_update();
-
-	//TODO replace with BLE grid configuration
-	grid_info_read(&_uwb_grid_info.target_pos);
-	_uwb_grid_info.num_anchors = 4;
-	_uwb_grid_info.initator_time = hrt_absolute_time();
-	_uwb_grid_info.mac_mode = 0x00;
-
-	/* Grid Info Message*/
-	_uwb_grid.timestamp = hrt_absolute_time();
-	_uwb_grid.initator_time = _uwb_grid_info.initator_time;
-	_uwb_grid.num_anchors = _uwb_grid_info.num_anchors;
-
-	memcpy(&_uwb_grid.target_pos, &_uwb_grid_info.target_pos, sizeof(position_t) * 5); //write Grid positions
-
-	_uwb_grid_pub.publish(_uwb_grid);
-
-	/* Ranging  Command */
-	int written = write(_uart, CMD_RANGING_START, UWB_CMD_LEN);
-
-	if (written <  UWB_CMD_LEN) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(uint8_t) *  UWB_CMD_LEN);
+	// execute Run() on every sensor_accel publication
+	if (!_sensor_uwb_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
 	}

-	while (!should_exit()) {
-		written = UWB_SR150::distance(); //evaluate Ranging Messages until Stop
-	}
+	// alternatively, Run on fixed interval
+	// ScheduleOnInterval(5000_us); // 2000 us interval, 200 Hz rate

-	if (!written) { printf("ERROR: Distance Failed"); }
-
-	// Automatic Stop. This should not be reachable
-	written = write(_uart, &CMD_RANGING_STOP, UWB_CMD_LEN);
-
-	if (written < (int) sizeof(CMD_RANGING_STOP)) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_RANGING_STOP));
-	}
+	return true;
 }

-void UWB_SR150::grid_info_read(position_t *grid)
+void UWB_SR150::start()
 {
-	//place holder, until UWB initiator can respond with Grid info
-	/*	This Reads the position of each Anchor in the Grid.
-		Right now the Grid configuration is saved on the SD card.
-		In the future, we would like, that the Initiator responds with the Grid Information (Including Position). */
-	PX4_INFO("Reading UWB GRID from SD... \t\n");
-	FILE *file;
-	file = fopen(UWB_GRID_CONFIG, "r");
+	/* schedule a cycle to start things */
+	ScheduleNow();
+}

-	int  bread = 0;
+void UWB_SR150::stop()
+{
+	ScheduleClear();
+}

-	for (int i = 0; i < 5; i++) {
-		bread += fscanf(file, "%hd,%hd,%hd\n", &grid[i].x, &grid[i].y, &grid[i].z);
+void UWB_SR150::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
 	}

-	if (bread == 5 * 3) {
-		PX4_INFO("GRID INFO READ! bytes read: %d \t\n", bread);
+	if (_uart < 0) {
+		/* open fd */
+		_uart = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);

-	} else { //use UUID from Grid survey
-		PX4_INFO("GRID INFO Missing! bytes read: %d \t\n Using standrd Grid \t\n", bread);
-		position_t grid_setup[5] = {{0x0, 0x0, 0x0}, {(int16_t)0xff68, 0x0, 0x0a}, {0x99, 0x0, 0x0a}, {0x0, 0x96, 0x64}, {0x0, (int16_t)0xff6a, 0x63}};
-		memcpy(grid, &grid_setup, sizeof(grid_setup));
-		PX4_INFO("Insert \"uwb_grid_config.csv\" containing gridinfo in cm at \"/fs/microsd/etc/\".\t\n");
-		PX4_INFO("n + 1 Anchor Positions starting with Landing Target. Int16 Format:  \"x,y,z\" \t\n");
+		if (_uart < 0) {
+			PX4_ERR("open failed (%i)", errno);
+			return;
+		}
+
+		struct termios uart_config;
+
+		int termios_state;
+
+		/* fill the struct for the new configuration */
+		tcgetattr(_uart, &uart_config);
+
+		/* clear ONLCR flag (which appends a CR for every LF) */
+		uart_config.c_oflag &= ~ONLCR;
+
+		//TODO: should I keep this?
+		/* no parity, one stop bit */
+		uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+		unsigned speed = DEFAULT_BAUD;
+
+		/* set baud rate */
+		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d ISPD", termios_state);
+		}
+
+		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d OSPD", termios_state);
+		}
+
+		if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) {
+			PX4_ERR("baud %d ATTR", termios_state);
+		}
 	}

-	fclose(file);
+	/* perform collection */
+	int collect_ret = collectData();
+
+	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) {
+
+		/* we know the sensor needs about four seconds to initialize */
+		if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+			PX4_ERR("collection error #%u", _consecutive_fail_count);
+		}
+
+		_consecutive_fail_count++;
+
+		/* restart the measurement state machine */
+		start();
+		return;
+
+	} else {
+		/* apparently success */
+		_consecutive_fail_count = 0;
+	}
 }

 int UWB_SR150::custom_command(int argc, char *argv[])
@@ -214,43 +216,20 @@ $ uwb start -d /dev/ttyS2
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Name of device for serial communication with UWB", false);
 	PRINT_MODULE_USAGE_PARAM_STRING('b', nullptr, "<int>", "Baudrate for serial communication", false);
-	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "<int>", "Position Debug: displays errors in Multilateration", false);
 	PRINT_MODULE_USAGE_COMMAND("stop");
 	PRINT_MODULE_USAGE_COMMAND("status");
 	return 0;
 }

 int UWB_SR150::task_spawn(int argc, char *argv[])
-{
-	int task_id = px4_task_spawn_cmd(
-			      "uwb_driver",
-			      SCHED_DEFAULT,
-			      SCHED_PRIORITY_DEFAULT,
-			      2048,
-			      &run_trampoline,
-			      argv
-		      );
-
-	if (task_id < 0) {
-		return -errno;
-
-	} else {
-		_task_id = task_id;
-		return 0;
-	}
-}
-
-UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[])
 {
 	int ch;
 	int option_index = 1;
 	const char *option_arg;
-	const char *device_name = nullptr;
-	bool error_flag = false;
+	const char *device_name = UWB_DEFAULT_PORT;
 	int baudrate = 0;
-	bool uwb_pos_debug = false; // Display UWB position calculation debug Messages

-	while ((ch = px4_getopt(argc, argv, "d:b:p", &option_index, &option_arg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "d:b", &option_index, &option_arg)) != EOF) {
 		switch (ch) {
 		case 'd':
 			device_name = option_arg;
@@ -260,47 +239,54 @@ UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[])
 			px4_get_parameter_value(option_arg, baudrate);
 			break;

-		case 'p':
-
-			uwb_pos_debug = true;
-			break;
-
 		default:
 			PX4_WARN("Unrecognized flag: %c", ch);
-			error_flag = true;
 			break;
 		}
 	}

-	if (!error_flag && device_name == nullptr) {
-		print_usage("Device name not provided. Using default Device: TEL1:/dev/ttyS4 \n");
-		device_name = "TEL2";
-		error_flag = true;
-	}
+	UWB_SR150 *instance = new UWB_SR150(device_name);

-	if (!error_flag && baudrate == 0) {
-		printf("Baudrate not provided. Using default Baud: 115200 \n");
-		baudrate = B115200;
-	}
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;

-	if (!error_flag && uwb_pos_debug == true) {
-		printf("UWB Position algorithm Debugging \n");
-	}
+		instance->ScheduleOnInterval(5000_us);

-	if (error_flag) {
-		PX4_WARN("Failed to start UWB driver. \n");
-		return nullptr;
+		if (instance->init()) {
+			return PX4_OK;
+		}

 	} else {
-		PX4_INFO("Constructing UWB_SR150. Device: %s", device_name);
-		return new UWB_SR150(device_name, baudrate, uwb_pos_debug);
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int uwb_sr150_main(int argc, char *argv[])
+{
+	return UWB_SR150::main(argc, argv);
+}
+
+void UWB_SR150::parameters_update()
+{
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		// If any parameter updated, call updateParams() to check if
+		// this class attributes need updating (and do so).
+		updateParams();
 	}
 }

-int UWB_SR150::distance()
+int UWB_SR150::collectData()
 {
-	_uwb_init_offset_v3f = matrix::Vector3f(_uwb_init_off_x.get(), _uwb_init_off_y.get(),
-						_uwb_init_off_z.get()); //set offset at the start
 	uint8_t *buffer = (uint8_t *) &_distance_result_msg;

 	FD_ZERO(&_uart_set);
@@ -350,366 +336,54 @@ int UWB_SR150::distance()
 	perf_count(_read_count_perf);

 	// All of the following criteria must be met for the message to be acceptable:
-	//  - Size of message == sizeof(distance_msg_t) (51 bytes)
+	//  - Size of message == sizeof(distance_msg_t) (36 bytes)
 	//  - status == 0x00
-	//  - Values of all 3 position measurements are reasonable
-	//      (If one or more anchors is missed, then position might be an unreasonably large number.)
-	bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b); //||
-	//(buffer_location == sizeof(grid_msg_t) && _distance_result_msg.stop == 0x1b)
-	//);
+	bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b);

 	if (ok) {

 		/* Ranging Message*/
-		_uwb_distance.timestamp = hrt_absolute_time();
-		_uwb_distance.time_uwb_ms = _distance_result_msg.time_uwb_ms;
-		_uwb_distance.counter = _distance_result_msg.seq_ctr;
-		_uwb_distance.sessionid = _distance_result_msg.sessionId;
-		_uwb_distance.time_offset = _distance_result_msg.range_interval;
+		_sensor_uwb.timestamp 		= hrt_absolute_time();

-		for (int i = 0; i < 4; i++) {
-			_uwb_distance.anchor_distance[i] = _distance_result_msg.measurements[i].distance;
-			_uwb_distance.nlos[i] = _distance_result_msg.measurements[i].nLos;
-			_uwb_distance.aoafirst[i] = float(_distance_result_msg.measurements[i].aoaFirst) /
-						    128; // Angle of Arrival has Format Q9.7; dividing by 2^7 results in the correct value
-		}
+		_sensor_uwb.sessionid 		= _distance_result_msg.sessionId;
+		_sensor_uwb.time_offset 	= _distance_result_msg.range_interval;
+		_sensor_uwb.counter 		= _distance_result_msg.seq_ctr;
+		_sensor_uwb.mac			= _distance_result_msg.MAC;

-		// Algorithm goes here
-		UWB_POS_ERROR_CODES UWB_POS_ERROR = UWB_SR150::localization();
+		_sensor_uwb.mac_dest		= _distance_result_msg.measurements.MAC;
+		_sensor_uwb.status		= _distance_result_msg.measurements.status;
+		_sensor_uwb.nlos 		= _distance_result_msg.measurements.nLos;
+		_sensor_uwb.distance 		= double(_distance_result_msg.measurements.distance) / 100;

-		_uwb_distance.status = UWB_POS_ERROR;

-		if (UWB_OK == UWB_POS_ERROR) {
+		/*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/
+		_sensor_uwb.aoa_azimuth_dev 	= - double(_distance_result_msg.measurements.aoa_azimuth) / 128;
+		_sensor_uwb.aoa_elevation_dev 	= - double(_distance_result_msg.measurements.aoa_elevation) / 128;
+		_sensor_uwb.aoa_azimuth_resp 	= - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128;
+		_sensor_uwb.aoa_elevation_resp 	= - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128;

-			_uwb_distance.position[0] = _rel_pos(0);
-			_uwb_distance.position[1] = _rel_pos(1);
-			_uwb_distance.position[2] = _rel_pos(2);

-		} else {
-			//only print the error if debug is enabled
-			if (_uwb_pos_debug) {
-				switch (UWB_POS_ERROR) { //UWB POSITION ALGORItHM Errors
-				case UWB_ANC_BELOW_THREE:
-					PX4_INFO("UWB not enough anchors for doing localization");
-					break;
+		/*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/
+		_sensor_uwb.aoa_azimuth_fom 		= - double(_distance_result_msg.measurements.aoa_azimuth) / 128;
+		_sensor_uwb.aoa_elevation_fom 		= - double(_distance_result_msg.measurements.aoa_elevation) / 128;
+		_sensor_uwb.aoa_dest_azimuth_fom	= - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128;
+		_sensor_uwb.aoa_dest_elevation_fom	= - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128;

-				case UWB_LIN_DEP_FOR_THREE:
-					PX4_INFO("UWB localization: linear dependent with 3 Anchors");
-					break;
+		/* Sensor physical offset*/ //for now we propagate the physical configuration via Uorb
+		_sensor_uwb.orientation		= _sensor_rot.get();
+		_sensor_uwb.offset_x		= _offset_x.get();
+		_sensor_uwb.offset_y		= _offset_y.get();
+		_sensor_uwb.offset_z		= _offset_z.get();

-				case UWB_ANC_ON_ONE_LEVEL:
-					PX4_INFO("UWB localization: Anchors are on a X,Y Plane and there are not enought Anchors");
-					break;
-
-				case UWB_LIN_DEP_FOR_FOUR:
-					PX4_INFO("UWB localization: linear dependent with four or more Anchors");
-					break;
-
-				case UWB_RANK_ZERO:
-					PX4_INFO("UWB localization: rank is zero");
-					break;
-
-				default:
-					PX4_INFO("UWB localization: Unknown failure in Position Algorithm");
-					break;
-				}
-			}
-		}
-
-		_uwb_distance_pub.publish(_uwb_distance);
+		_sensor_uwb_pub.publish(_sensor_uwb);

 	} else {
-		//PX4_ERR("Read %d bytes instead of %d.", (int) buffer_location, (int) sizeof(distance_msg_t));
 		perf_count(_read_err_perf);

 		if (buffer_location == 0) {
 			PX4_WARN("UWB module is not responding.");
-
-			//TODO add retry Ranging Start Message. Sometimes the UWB devices Crashes. (Check Power)
 		}
 	}

 	return 1;
 }
-
-UWB_POS_ERROR_CODES UWB_SR150::localization()
-{
-// 			WIP
-	/******************************************************
-	 ****************** 3D Localization *******************
-	 *****************************************************/
-
-	/*!@brief: This function calculates the 3D position of the initiator from the anchor distances and positions using least squared errors.
-	 *	 	   The function expects more than 4 anchors. The used equation system looks like follows:\n
-	 \verbatim
-	  		    -					-
-	  		   | M_11	M_12	M_13 |	 x	  b[0]
-	  		   | M_12	M_22	M_23 | * y	= b[1]
-	  		   | M_23	M_13	M_33 |	 z	  b[2]
-	  		    -					-
-	 \endverbatim
-	 * @param distances_cm_in_pt: 			Pointer to array that contains the distances to the anchors in cm (including invalid results)
-	 * @param no_distances: 				Number of valid distances in distance array (it's not the size of the array)
-	 * @param anchor_pos: 	Pointer to array that contains anchor positions in cm (including positions related to invalid results)
-	 * @param no_anc_positions: 			Number of valid anchor positions in the position array (it's not the size of the array)
-	 * @param position_result_pt: 			Pointer toposition. position_t variable that holds the result of this calculation
-	 * @return: The function returns a status code. */
-
-	/* 		Algorithm used:
-	 *		Linear Least Sqaures to solve Multilateration
-	 * 		with a Special case if there are only 3 Anchors.
-	 * 		Output is the Coordinates of the Initiator in relation to Anchor 0 in NEU (North-East-Up) Framing
-	 * 		In cm
-	 */
-
-	/* Resulting Position Vector*/
-	int64_t x_pos = 0;
-	int64_t y_pos = 0;
-	int64_t z_pos = 0;
-	/* Matrix components (3*3 Matrix resulting from least square error method) [cm^2] */
-	int64_t M_11 = 0;
-	int64_t M_12 = 0;																						// = M_21
-	int64_t M_13 = 0;																						// = M_31
-	int64_t M_22 = 0;
-	int64_t M_23 = 0;																						// = M_23
-	int64_t M_33 = 0;
-
-	/* Vector components (3*1 Vector resulting from least square error method) [cm^3] */
-	int64_t b[3] = {0};
-
-	/* Miscellaneous variables */
-	int64_t temp = 0;
-	int64_t temp2 = 0;
-	int64_t nominator = 0;
-	int64_t denominator = 0;
-	bool	anchors_on_x_y_plane = true;																		// Is true, if all anchors are on the same height => x-y-plane
-	bool	lin_dep = true;																						// All vectors are linear dependent, if this variable is true
-	uint8_t ind_y_indi =
-		0;	//numberr of independet vectors																					// First anchor index, for which the second row entry of the matrix [(x_1 - x_0) (x_2 - x_0) ... ; (y_1 - x_0) (y_2 - x_0) ...] is non-zero => linear independent
-
-	/* Arrays for used distances and anchor positions (without rejected ones) */
-	uint8_t 	no_distances = _uwb_grid_info.num_anchors;
-	uint32_t 	distances_cm[no_distances];
-	position_t 	anchor_pos[no_distances]; //position in CM
-	uint8_t		no_valid_distances = 0;
-
-	/* Reject invalid distances (including related anchor position) */
-	for (int i = 0; i < no_distances; i++) {
-		if (_distance_result_msg.measurements[i].distance != 0xFFFFu) {
-			//excludes any distance that is 0xFFFFU (int16 Maximum Value)
-			distances_cm[no_valid_distances] 		= _distance_result_msg.measurements[i].distance;
-			anchor_pos[no_valid_distances] 	= _uwb_grid_info.anchor_pos[i];
-			no_valid_distances++;
-
-		}
-	}
-
-	/* Check, if there are enough valid results for doing the localization at all */
-	if (no_valid_distances < 3) {
-		return UWB_ANC_BELOW_THREE;
-	}
-
-	/* Check, if anchors are on the same x-y plane */
-	for (int i = 1; i < no_valid_distances; i++) {
-		if (anchor_pos[i].z != anchor_pos[0].z) {
-			anchors_on_x_y_plane = false;
-			break;
-		}
-	}
-
-	/**** Check, if there are enough linear independent anchor positions ****/
-
-	/* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) ... | has rank 2
-	 * 			|(y_1 - y_0) (y_2 - y_0) ... | 				*/
-
-	for (ind_y_indi = 2; ((ind_y_indi < no_valid_distances) && (lin_dep == true)); ind_y_indi++) {
-		temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].x -
-				(int64_t)anchor_pos[0].x);
-		temp2 = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].x -
-				(int64_t)anchor_pos[0].x);
-
-		if ((temp - temp2) != 0) {
-			lin_dep = false;
-			break;
-		}
-	}
-
-	/* Leave function, if rank is below 2 */
-	if (lin_dep == true) {
-		return UWB_LIN_DEP_FOR_THREE;
-	}
-
-	/* If the anchors are not on the same plane, three vectors must be independent => check */
-	if (!anchors_on_x_y_plane) {
-		/* Check, if there are enough valid results for doing the localization */
-		if (no_valid_distances < 4) {
-			return UWB_ANC_ON_ONE_LEVEL;
-		}
-
-		/* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) (x_3 - x_0) ... | has rank 3 (Rank y, y already checked)
-		 * 			|(y_1 - y_0) (y_2 - y_0) (y_3 - y_0) ... |
-		 * 			|(z_1 - z_0) (z_2 - z_0) (z_3 - z_0) ... |											*/
-		lin_dep = true;
-
-		for (int i = 2; ((i < no_valid_distances) && (lin_dep == true)); i++) {
-			if (i != ind_y_indi) {
-				/* (x_1 - x_0)*[(y_2 - y_0)(z_n - z_0) - (y_n - y_0)(z_2 - z_0)] */
-				temp 	= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z -
-						(int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z -
-						(int64_t)anchor_pos[0].z);
-				temp2 	= ((int64_t)anchor_pos[1].x - (int64_t)anchor_pos[0].x) * temp;
-
-				/* Add (x_2 - x_0)*[(y_n - y_0)(z_1 - z_0) - (y_1 - y_0)(z_n - z_0)] */
-				temp 	= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z - (int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z - (int64_t)anchor_pos[0].z);
-				temp2 	+= ((int64_t)anchor_pos[ind_y_indi].x - (int64_t)anchor_pos[0].x) * temp;
-
-				/* Add (x_n - x_0)*[(y_1 - y_0)(z_2 - z_0) - (y_2 - y_0)(z_1 - z_0)] */
-				temp 	= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z -
-						(int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z -
-						(int64_t)anchor_pos[0].z);
-				temp2 	+= ((int64_t)anchor_pos[i].x - (int64_t)anchor_pos[0].x) * temp;
-
-				if (temp2 != 0) { lin_dep = false; }
-			}
-		}
-
-		/* Leave function, if rank is below 3 */
-		if (lin_dep == true) {
-			return UWB_LIN_DEP_FOR_FOUR;
-		}
-	}
-
-	/************************************************** Algorithm ***********************************************************************/
-
-	/* Writing values resulting from least square error method (A_trans*A*x = A_trans*r; row 0 was used to remove x^2,y^2,z^2 entries => index starts at 1) */
-	for (int i = 1; i < no_valid_distances; i++) {
-		/* Matrix (needed to be multiplied with 2, afterwards) */
-		M_11 += sq((int64_t)(anchor_pos[i].x - anchor_pos[0].x));
-		M_12 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].y - anchor_pos[0].y));
-		M_13 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-		M_22 += sq((int64_t)(anchor_pos[i].y - anchor_pos[0].y));
-		M_23 += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-		M_33 += sq((int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-
-		/* Vector */
-		temp = sq(distances_cm[0]) - sq(distances_cm[i])
-		       + sq(anchor_pos[i].x) + sq(anchor_pos[i].y)
-		       + sq(anchor_pos[i].z) - sq(anchor_pos[0].x)
-		       - sq(anchor_pos[0].y) - sq(anchor_pos[0].z);
-
-		b[0] += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * temp);
-		b[1] += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * temp);
-		b[2] += (int64_t)((int64_t)(anchor_pos[i].z - anchor_pos[0].z) * temp);
-	}
-
-	M_11 = 2 * M_11;
-	M_12 = 2 * M_12;
-	M_13 = 2 * M_13;
-	M_22 = 2 * M_22;
-	M_23 = 2 * M_23;
-	M_33 = 2 * M_33;
-
-	/* Calculating the z-position, if calculation is possible (at least one anchor at z != 0) */
-	if (anchors_on_x_y_plane == false) {
-		nominator = b[0] * (M_12 * M_23 - M_13 * M_22) + b[1] * (M_12 * M_13 - M_11 * M_23) + b[2] *
-			    (M_11 * M_22 - M_12 * M_12);			// [cm^7]
-		denominator = M_11 * (M_33 * M_22 - M_23 * M_23) + 2 * M_12 * M_13 * M_23 - M_33 * M_12 * M_12 - M_22 * M_13 *
-			      M_13;				// [cm^6]
-
-		/* Check, if denominator is zero (Rank of matrix not high enough) */
-		if (denominator == 0) {
-			return UWB_RANK_ZERO;
-		}
-
-		z_pos = ((nominator * 10) / denominator + 5) / 10;	// [cm]
-	}
-
-	/* Else prepare for different calculation approach (after x and y were calculated) */
-	else {
-		z_pos = 0;
-	}
-
-	/* Calculating the y-position */
-	nominator = b[1] * M_11 - b[0] * M_12 - (z_pos * (M_11 * M_23 - M_12 * M_13));	// [cm^5]
-	denominator = M_11 * M_22 - M_12 * M_12;// [cm^4]
-
-	/* Check, if denominator is zero (Rank of matrix not high enough) */
-	if (denominator == 0) {
-		return UWB_RANK_ZERO;
-	}
-
-	y_pos = ((nominator * 10) / denominator + 5) / 10;	// [cm]
-
-	/* Calculating the x-position */
-	nominator = b[0] - z_pos * M_13 - y_pos * M_12;	// [cm^3]
-	denominator = M_11;	// [cm^2]
-
-	x_pos = ((nominator * 10) / denominator + 5) / 10;// [cm]
-
-	/* Calculate z-position form x and y coordinates, if z can't be determined by previous steps (All anchors at z_n = 0) */
-	if (anchors_on_x_y_plane == true) {
-		/* Calculate z-positon relative to the anchor grid's height */
-		for (int i = 0; i < no_distances; i++) {
-			/* z² = dis_meas_n² - (x - x_anc_n)² - (y - y_anc_n)² */
-			temp = (int64_t)((int64_t)pow(distances_cm[i], 2)
-					 - (int64_t)pow((x_pos - (int64_t)anchor_pos[i].x), 2)
-					 - (int64_t)pow((y_pos - (int64_t)anchor_pos[i].y), 2));
-
-			/* z² must be positive, else x and y must be wrong => calculate positive sqrt and sum up all calculated heights, if positive */
-			if (temp >= 0) {
-				z_pos += (int64_t)sqrt(temp);
-
-			} else {
-				z_pos = 0;
-			}
-		}
-
-		z_pos = z_pos / no_distances;										// Divide sum by number of distances to get the average
-
-		/* Add height of the anchor grid's height */
-		z_pos += anchor_pos[0].z;
-	}
-
-	//Output is the Coordinates of the Initiator in relation to 0,0,0 in NEU (North-East-Up) Framing
-	// The end goal of this math is to get the position relative to the landing point in the NED frame.
-	_current_position_uwb_init = matrix::Vector3f(x_pos, y_pos, z_pos);
-
-	// Construct the rotation from the UWB_R4frame to the NWU frame.
-	// The UWB_SR150 frame is just NWU, rotated by some amount about the Z (up) axis. (Parameter Yaw offset)
-	// To get back to NWU, just rotate by negative this amount about Z.
-	_uwb_init_to_nwu = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f,
-					-(_uwb_init_off_yaw.get() * M_PI_F / 180.0f))); //
-	// The actual conversion:
-	//  - Subtract _landing_point to get the position relative to the landing point, in UWB_R4 frame
-	//  - Rotate by _rddrone_to_nwu to get into the NWU frame
-	//  - Rotate by _nwu_to_ned to get into the NED frame
-	_current_position_ned = _nwu_to_ned * _uwb_init_to_nwu * _current_position_uwb_init;
-
-	// Now the position is the landing point relative to the vehicle.
-	// so the only thing left is to convert cm to Meters and to add the Initiator offset
-	_rel_pos =  _current_position_ned / 100 + _uwb_init_offset_v3f;
-
-	// The UWB report contains the position of the vehicle relative to the landing point.
-
-	return UWB_OK;
-}
-
-int uwb_sr150_main(int argc, char *argv[])
-{
-	return UWB_SR150::main(argc, argv);
-}
-
-void UWB_SR150::parameters_update()
-{
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		// If any parameter updated, call updateParams() to check if
-		// this class attributes need updating (and do so).
-		updateParams();
-	}
-}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -38,101 +38,63 @@
 #include <poll.h>
 #include <sys/select.h>
 #include <sys/time.h>
+#include <perf/perf_counter.h>
+#include <lib/conversion/rotation.h>

 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/module.h>
-#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
 #include <uORB/Publication.hpp>
-#include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/uwb_grid.h>
-#include <uORB/topics/uwb_distance.h>
-#include <uORB/topics/parameter_update.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_uwb.h>
+#include <uORB/topics/parameter_update.h>
+
 #include <matrix/math.hpp>
-#include <matrix/Matrix.hpp>
+
+#define UWB_DEFAULT_PORT "/dev/ttyS1"

 using namespace time_literals;

-#define UWB_CMD  0x8e
-#define UWB_CMD_START  0x01
-#define UWB_CMD_STOP  0x00
-#define UWB_CMD_RANGING  0x0A
-#define STOP_B 0x0A
-
-#define UWB_PRECNAV_APP   0x04
-#define UWB_APP_START     0x10
-#define UWB_APP_STOP      0x11
-#define UWB_SESSION_START 0x22
-#define UWB_SESSION_STOP  0x23
-#define UWB_RANGING_START 0x01
-#define UWB_RANGING_STOP  0x00
-#define UWB_DRONE_CTL     0x0A
-
-#define UWB_CMD_LEN  0x05
-#define UWB_CMD_DISTANCE_LEN 0x21
-#define UWB_MAC_MODE 2
-#define MAX_ANCHORS 12
-#define UWB_GRID_CONFIG "/fs/microsd/etc/uwb_grid_config.csv"
-
-typedef struct {  //needs higher accuracy?
-	float lat, lon, alt, yaw; //offset to true North
-} gps_pos_t;
-
 typedef struct {
-	int16_t x, y, z; //axis in cm
-} position_t; // Position of a device or target in 3D space
-
-enum UWB_POS_ERROR_CODES {
-	UWB_OK,
-	UWB_ANC_BELOW_THREE,
-	UWB_LIN_DEP_FOR_THREE,
-	UWB_ANC_ON_ONE_LEVEL,
-	UWB_LIN_DEP_FOR_FOUR,
-	UWB_RANK_ZERO
-};
-
-typedef struct {
-	uint8_t MAC[2];		// MAC Adress of UWB device
-	uint8_t status;		// Status of Measurement
-	uint16_t distance; 	// Distance in cm
-	uint8_t nLos; 		// line of sight y/n
-	uint16_t aoaFirst;	// Angle of Arrival of incoming msg
+	uint16_t MAC;					// MAC address of UWB device
+	uint8_t status;					// Status of Measurement
+	uint16_t distance; 				// Distance in cm
+	uint8_t nLos; 					// line of sight y/n
+	int16_t aoa_azimuth;				// AOA of incoming msg for Azimuth antenna pairing
+	int16_t aoa_elevation;				// AOA of incoming msg for Altitude antenna pairing
+	int16_t aoa_dest_azimuth;			// AOA destination Azimuth
+	int16_t aoa_dest_elevation; 			// AOA destination elevation
+	uint8_t aoa_azimuth_FOM;			// AOA Azimuth FOM
+	uint8_t aoa_elevation_FOM;			// AOA Elevation FOM
+	uint8_t aoa_dest_azimuth_FOM;			// AOA Azimuth FOM
+	uint8_t aoa_dest_elevation_FOM;			// AOA Elevation FOM
 } __attribute__((packed)) UWB_range_meas_t;

 typedef struct {
-	uint32_t initator_time;  	//timestamp of init
-	uint32_t sessionId;	// Session ID of UWB session
-	uint8_t	num_anchors;	//number of anchors
-	gps_pos_t target_gps; //GPS position of Landing Point
-	uint8_t  mac_mode;	// MAC adress mode, either 2 Byte or 8 Byte
-	uint8_t MAC[UWB_MAC_MODE][MAX_ANCHORS];
-	position_t target_pos; //target position
-	position_t anchor_pos[MAX_ANCHORS]; // Position of each anchor
-	uint8_t stop; 		// Should be 27
-} grid_msg_t;
-
-typedef struct {
-	uint8_t cmd;      	// Should be 0x8E for distance result message
-	uint16_t len; 		// Should be 0x30 for distance result message
-	uint32_t time_uwb_ms;	// Timestamp of UWB device in ms
-	uint32_t seq_ctr;	// Number of Ranges since last Start of Ranging
-	uint32_t sessionId;	// Session ID of UWB session
-	uint32_t range_interval;	// Time between ranging rounds
-	uint8_t  mac_mode;	// MAC adress mode, either 2 Byte or 8 Byte
-	uint8_t  no_measurements;	// MAC adress mode, either 2 Byte or 8 Byte
-	UWB_range_meas_t measurements[4]; //Raw anchor_distance distances in CM 2*9
-	uint8_t stop; 		// Should be 0x1B
+	uint8_t cmd;      				// Should be 0x8E for distance result message
+	uint16_t len; 					// Should be 0x30 for distance result message
+	uint32_t seq_ctr;				// Number of Ranges since last Start of Ranging
+	uint32_t sessionId;				// Session ID of UWB session
+	uint32_t range_interval;			// Time between ranging rounds
+	uint16_t MAC;					// MAC address of UWB device
+	UWB_range_meas_t measurements; 			//Raw anchor_distance distances in CM 2*9
+	uint8_t stop; 					// Should be 0x1B
 } __attribute__((packed)) distance_msg_t;

-class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams
+class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug);
-
+	UWB_SR150(const char *port);
 	~UWB_SR150();

+	/**
+	 * @see ModuleBase::task_spawn
+	 */
+	static int task_spawn(int argc, char *argv[]);
+
 	/**
 	 * @see ModuleBase::custom_command
 	 */
@@ -143,67 +105,51 @@ public:
 	 */
 	static int print_usage(const char *reason = nullptr);

-	/**
-	 * @see ModuleBase::Multilateration
-	 */
-	UWB_POS_ERROR_CODES localization();
+	bool init();

-	/**
-	 * @see ModuleBase::Distance Result
-	 */
-	int distance();
+	void start();

-	/**
-	 * @see ModuleBase::task_spawn
-	 */
-	static int task_spawn(int argc, char *argv[]);
+	void stop();

-	static UWB_SR150 *instantiate(int argc, char *argv[]);
-
-	void run() override;
+	int collectData();

 private:
-	static constexpr int64_t sq(int64_t x) { return x * x; }

 	void parameters_update();

-	void grid_info_read(position_t *grid);
+	void Run() override;

-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::UWB_INIT_OFF_X>) _uwb_init_off_x,
-		(ParamFloat<px4::params::UWB_INIT_OFF_Y>) _uwb_init_off_y,
-		(ParamFloat<px4::params::UWB_INIT_OFF_Z>) _uwb_init_off_z,
-		(ParamFloat<px4::params::UWB_INIT_OFF_YAW>) _uwb_init_off_yaw
-	)
+	// Publications
+	uORB::Publication<sensor_uwb_s> _sensor_uwb_pub{ORB_ID(sensor_uwb)};

+	// Subscriptions
+	uORB::SubscriptionCallbackWorkItem _sensor_uwb_sub{this, ORB_ID(sensor_uwb)};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

-	int _uart;
-	fd_set _uart_set;
-	struct timeval _uart_timeout {};
-	bool _uwb_pos_debug;
-
-	uORB::Publication<uwb_grid_s> _uwb_grid_pub{ORB_ID(uwb_grid)};
-	uwb_grid_s _uwb_grid{};
-
-	uORB::Publication<uwb_distance_s> _uwb_distance_pub{ORB_ID(uwb_distance)};
-	uwb_distance_s _uwb_distance{};
-
-	uORB::Publication<landing_target_pose_s> _landing_target_pub{ORB_ID(landing_target_pose)};
-	landing_target_pose_s _landing_target{};
-
-	grid_msg_t _uwb_grid_info{};
-	distance_msg_t _distance_result_msg{};
-	matrix::Vector3f _rel_pos;
-
-	matrix::Dcmf _uwb_init_to_nwu;
-	matrix::Dcmf _nwu_to_ned{matrix::Eulerf(M_PI_F, 0.0f, 0.0f)};
-	matrix::Vector3f _current_position_uwb_init;
-	matrix::Vector3f _current_position_ned;
-	matrix::Vector3f _uwb_init_offset_v3f;
-
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::UWB_PORT_CFG>) 			_uwb_port_cfg,
+		(ParamFloat<px4::params::UWB_INIT_OFF_X>) 		_offset_x,
+		(ParamFloat<px4::params::UWB_INIT_OFF_Y>) 		_offset_y,
+		(ParamFloat<px4::params::UWB_INIT_OFF_Z>) 		_offset_z,
+		(ParamInt<px4::params::UWB_SENS_ROT>) 			_sensor_rot
+	)
+	// Performance (perf) counters
 	perf_counter_t _read_count_perf;
 	perf_counter_t _read_err_perf;
-};

+	sensor_uwb_s _sensor_uwb{};
+
+	char _port[20] {};
+	hrt_abstime param_timestamp{0};
+
+	int _uart{-1};
+	fd_set _uart_set;
+	struct timeval _uart_timeout {};
+
+	unsigned _consecutive_fail_count;
+	int _interval{100000};
+
+	distance_msg_t 		_distance_result_msg{};
+};
 #endif //PX4_RDDRONE_H
@@ -35,12 +35,12 @@

 using namespace time_literals;

-FakeGps::FakeGps(double latitude_deg, double longitude_deg, float altitude_m) :
+FakeGps::FakeGps(double latitude_deg, double longitude_deg, double altitude_m) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
-	_latitude(latitude_deg * 10e6),
-	_longitude(longitude_deg * 10e6),
-	_altitude(altitude_m * 10e2f)
+	_latitude(latitude_deg),
+	_longitude(longitude_deg),
+	_altitude(altitude_m)
 {
 }

@@ -60,10 +60,10 @@ void FakeGps::Run()

 	sensor_gps_s sensor_gps{};
 	sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954;
-	sensor_gps.lat = _latitude;
-	sensor_gps.lon = _longitude;
-	sensor_gps.alt = _altitude;
-	sensor_gps.alt_ellipsoid = _altitude;
+	sensor_gps.latitude_deg = _latitude;
+	sensor_gps.longitude_deg = _longitude;
+	sensor_gps.altitude_msl_m = _altitude;
+	sensor_gps.altitude_ellipsoid_m = _altitude;
 	sensor_gps.s_variance_m_s = 0.3740f;
 	sensor_gps.c_variance_rad = 0.6737f;
 	sensor_gps.eph = 2.1060f;
@@ -45,7 +45,7 @@
 class FakeGps : public ModuleBase<FakeGps>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, float altitude_m = 30.1f);
+	FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, double altitude_m = 30.1);

 	~FakeGps() override = default;

@@ -67,7 +67,7 @@ private:

 	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};

-	int32_t _latitude{296603018};   // Latitude in 1e-7 degrees
-	int32_t _longitude{-823160500}; // Longitude in 1e-7 degrees
-	int32_t _altitude{30100};       // Altitude in 1e-3 meters above MSL, (millimetres)
+	double _latitude{29.6603018};   // Latitude in degrees
+	double _longitude{-82.3160500}; // Longitude in degrees
+	double _altitude{30.1};         // Altitude in meters above MSL, (millimetres)
 };
@@ -42,4 +42,5 @@ px4_add_module(
 		drivers_magnetometer
 		geo
 		px4_work_queue
+		world_magnetic_model
 )
@@ -66,8 +66,8 @@ void FakeMagnetometer::Run()
 		if (_vehicle_gps_position_sub.copy(&gps)) {
 			if (gps.eph < 1000) {

-				const double lat = gps.lat / 1.e7;
-				const double lon = gps.lon / 1.e7;
+				const double lat = gps.latitude_deg;
+				const double lon = gps.longitude_deg;

 				// magnetic field data returned by the geo library using the current GPS position
 				const float mag_declination_gps = get_mag_declination_radians(lat, lon);
@@ -45,6 +45,7 @@ add_subdirectory(controllib EXCLUDE_FROM_ALL)
 add_subdirectory(conversion EXCLUDE_FROM_ALL)
 add_subdirectory(crc EXCLUDE_FROM_ALL)
 add_subdirectory(crypto EXCLUDE_FROM_ALL)
+add_subdirectory(dataman_client EXCLUDE_FROM_ALL)
 add_subdirectory(drivers EXCLUDE_FROM_ALL)
 add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL)
 add_subdirectory(geo EXCLUDE_FROM_ALL)
@@ -198,7 +198,6 @@ TEST_F(ObstacleAvoidanceTest, oa_desired)

 	// WHEN: we subscribe to the uORB message out of the interface
 	uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)};
-	_sub_traj_wp_avoidance_desired.update();

 	// THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2
 	EXPECT_FLOAT_EQ(pos_sp(0),
--- a/Show More
+++ b/Show More