airspeed_selector: new ASPD_SEL_EN to control enabling/disabling module

2026-06-06 22:20:06 +08:00 · 2023-07-06 11:46:44 -04:00
255 changed files with 11245 additions and 13325 deletions
@@ -9,7 +9,7 @@ pipeline {
        script {
          def build_nodes = [:]
          def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2023-06-26",
+            armhf: "px4io/px4-dev-armhf:2022-08-12",
            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,8 +40,6 @@ 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",
@@ -55,10 +53,8 @@ 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",
@@ -76,7 +72,7 @@ pipeline {
                     "matek_h743_default",
                     "modalai_fc-v1_default",
                     "modalai_fc-v2_default",
-                     "mro_ctrl-zero-classic_default",
+                     "modalai_voxl2-io_default",
                     "mro_ctrl-zero-f7_default",
                     "mro_ctrl-zero-f7-oem_default",
                     "mro_ctrl-zero-h7-oem_default",
@@ -89,7 +85,6 @@ 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",
@@ -169,7 +164,7 @@ pipeline {
  }
  options {
    buildDiscarder(logRotator(numToKeepStr: '5', artifactDaysToKeepStr: '14'))
-    timeout(time: 120, unit: 'MINUTES')
+    timeout(time: 90, unit: 'MINUTES')
  }
 }

@@ -12,7 +12,7 @@ pipeline {
            stage("build cubepilot_cubeorange_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  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:2022-08-12'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  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:2022-08-12'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  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:2022-08-12'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  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:2022-08-12'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  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" --ignore-stdout-errors'
+                    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 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:2022-08-12'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  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" --ignore-stdout-errors'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman"'
                  }
                }
                stage("status") {
@@ -486,7 +486,7 @@ pipeline {
            stage("build px4_fmu-v5_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  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:2022-08-12'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  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:2022-08-12",
+	"image": "px4io/px4-dev-nuttx-focal:2021-09-08",

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

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

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

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

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

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

        stage('failsafe docs') {
          agent {
-            docker { image 'px4io/px4-dev-nuttx-focal:2022-08-12' }
+            docker { image 'px4io/px4-dev-nuttx-focal:2021-08-18' }
          }
          steps {
            sh '''#!/bin/bash -l
@@ -125,7 +125,7 @@ pipeline {
        stage('uORB graphs') {
          agent {
            docker {
-              image 'px4io/px4-dev-nuttx-focal:2022-08-12'
+              image 'px4io/px4-dev-nuttx-focal:2021-08-18'
              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
            }
          }
@@ -1,122 +0,0 @@
-#!/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,8 +81,6 @@ 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,7 +17,6 @@
 # @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_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
+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

@@ -13,7 +13,6 @@
 # @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,7 +13,6 @@
 # @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,7 +21,11 @@ control_allocator start
 fw_rate_control start
 fw_att_control start
 fw_pos_control start
-airspeed_selector start
+
+if param greater -s ASPD_SEL_EN 0
+then
+	airspeed_selector start
+fi

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

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

 vtol_att_control start

@@ -330,12 +330,6 @@ 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, ignore_stdout_errors=False):
+def do_nsh_cmd(port_url, baudrate, cmd):
    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, ignore_stdout_errors=False):
            if success_cmd in serial_line:
                sys.exit(return_code)
            else:
-                if "ERROR " in serial_line and not ignore_stdout_errors:
+                if "ERROR " in serial_line:
                    return_code = -1

                print_line(serial_line)
@@ -148,8 +148,6 @@ 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())
@@ -157,7 +155,7 @@ def main():

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

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

 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:2022-08-12"
+		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
 	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:2023-06-26"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
 	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
 		# scumaker_pilotpi_arm64
-		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:2022-08-12"
+		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest"
 	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then
 		# posix_rpi_cross, posix_bebop_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2023-06-26"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
 	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:2022-08-12"
+	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
 fi

 # docker hygiene
@@ -61,19 +61,18 @@ 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 = 'px4io/px4-dev-base-focal:2021-09-08'
+            container = 'base-focal'
            if toolchain:
                if toolchain.startswith('aarch64'):
-                    container = 'px4io/px4-dev-aarch64:2022-08-12'
+                    container = 'aarch64'
                elif toolchain == 'arm-linux-gnueabihf':
-                    container = 'px4io/px4-dev-armhf:2023-06-26'
+                    container = 'armhf'
                else:
-                    if verbose: print(f'unmatched toolchain: {toolchain}')
+                    if verbose: print(f'possibly unmatched toolchain: {toolchain}')
        elif platform == 'nuttx':
-            container = 'px4io/px4-dev-nuttx-focal:2022-08-12'
-        else:
-            if verbose: print(f'unmatched platform: {platform}')
+            container = 'nuttx-focal'

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

@@ -114,3 +113,4 @@ extra_args = {}
 if args.pretty:
    extra_args['indent'] = 2
 print(json.dumps(github_action_config, **extra_args))
+
@@ -1,15 +0,0 @@
-<?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>
@@ -1,8 +0,0 @@
-<?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,7 +11,6 @@
    <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,6 +291,7 @@ 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,7 +140,15 @@

 #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,7 +141,15 @@

 #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,8 +23,6 @@ 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
@@ -38,6 +36,7 @@ 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,6 +14,10 @@ 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
@@ -25,11 +29,13 @@ 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
@@ -48,6 +54,7 @@ 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,7 +32,6 @@ 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_DRIVERS_UWB_UWB_SR150=y
+CONFIG_COMMON_UWB=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -28,7 +28,6 @@ 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,6 +1,4 @@
 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,6 +28,7 @@ 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
@@ -44,4 +45,3 @@ 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,7 +22,6 @@ 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
@@ -99,6 +98,8 @@ 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,14 +1,7 @@
-# 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
@@ -18,5 +11,3 @@ 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,8 +2,6 @@ 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,7 +41,6 @@ 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,6 +291,7 @@ 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,8 +60,6 @@ set(msg_files
 	CollisionReport.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
-	DatamanRequest.msg
-	DatamanResponse.msg
 	DebugArray.msg
 	DebugKeyValue.msg
 	DebugValue.msg
@@ -111,7 +109,6 @@ set(msg_files
 	HeaterStatus.msg
 	HomePosition.msg
 	HoverThrustEstimate.msg
-	InputCapture.msg
 	InputRc.msg
 	InternalCombustionEngineStatus.msg
 	IridiumsbdStatus.msg
@@ -181,7 +178,6 @@ set(msg_files
 	SensorSelection.msg
 	SensorsStatus.msg
 	SensorsStatusImu.msg
-	SensorUwb.msg
 	SystemPower.msg
 	TakeoffStatus.msg
 	TaskStackInfo.msg
@@ -198,6 +194,8 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
+	UwbDistance.msg
+	UwbGrid.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg
@@ -1,8 +0,0 @@
-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
@@ -1,15 +0,0 @@
-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,8 +127,3 @@ 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
@@ -1,5 +0,0 @@
-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,7 +3,3 @@ 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

-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
+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)

 float32 s_variance_m_s		# GPS speed accuracy estimate, (metres/sec)
 float32 c_variance_rad		# GPS course accuracy estimate, (radians)
@@ -1,34 +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 	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)
@@ -0,0 +1,15 @@
+# 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)
@@ -0,0 +1,25 @@
+# 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,8 +6,13 @@ uint64 armed_time # Arming timestamp (microseconds)
 uint64 takeoff_time # Takeoff timestamp (microseconds)

 uint8 arming_state
-uint8 ARMING_STATE_DISARMED = 1
-uint8 ARMING_STATE_ARMED    = 2
+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 latest_arming_reason
 uint8 latest_disarming_reason
@@ -116,3 +121,4 @@ 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_mount_procfs < 0) {
+	if (ret < 0) {
 		syslog(LOG_ERR, "ERROR: Failed to mount procfs at /proc: %d\n", ret_mount_procfs);
 	}

@@ -75,8 +75,6 @@ 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 = (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 };
+			geo.latitude = gps.lat;
+			geo.longitude = gps.lon;
+			geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast<double>(gps.alt) };

 			uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];

@@ -195,13 +195,6 @@ 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,6 +45,5 @@
 * @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,11 +61,41 @@ __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,5 +0,0 @@
-menuconfig DRIVERS_INPUT_CAPTURE
-	bool "input_capture"
-	default n
-	---help---
-		Enable support for input_capture
@@ -1,456 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}
@@ -1,150 +0,0 @@
-/****************************************************************************
- *
- *   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);
-
-};
@@ -320,10 +320,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet)

 			sensor_gps.fix_type = gpsFix;

-			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.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.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: 1000 }
+        disarmed: { min: 800, max: 2200, default: 900 }
        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;           // centimeters since MSP API 1.39, meters before
+	int16_t  alt;           // meters
 	int16_t  groundSpeed;   // cm/s
-	int16_t  groundCourse;  // unit: degree x 100, centidegrees
+	int16_t  groundCourse;  // unit: degree x 10
 	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 = 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);
+		raw_gps.lat = vehicle_gps_position.lat;
+		raw_gps.lon = vehicle_gps_position.lon;
+		raw_gps.alt =  vehicle_gps_position.alt / 10;

 		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 = static_cast<int32_t>(vehicle_gps_position.altitude_msl_m * 100.0);	// cm
+		altitude.estimatedActualPosition = vehicle_gps_position.alt / 10;

 	} else {
 		altitude.estimatedActualPosition = 0;
 	}

 	if (estimator_status.solution_status_flags & (1 << 5)) {
-		altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s
+		altitude.estimatedActualVelocity = -vehicle_local_position.vz * 10; //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: 1000 }
+        disarmed: { min: 800, max: 2200, default: 900 }
        min: { min: 800, max: 1400, default: 1000 }
        max: { min: 1600, max: 2200, default: 2000 }
        failsafe: { min: 800, max: 2200 }
@@ -218,55 +218,42 @@ void PWMOut::update_params()

 	updateParams();

-	// Automatically set PWM configuration when a channel is first assigned
+	// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
 	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) {
-					// 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 (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 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: 1000 }
+        disarmed: { min: 800, max: 2200, default: 900 }
        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: 1000 }
+        disarmed: { min: 800, max: 2200, default: 900 }
        min: { min: 800, max: 1400, default: 1000 }
        max: { min: 1600, max: 2200, default: 2000 }
        failsafe: { min: 800, max: 2200 }
@@ -702,48 +702,36 @@ 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) {
-						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 (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 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 = static_cast<int32_t>(round(sensor_gps.latitude_deg * 1e7));
-					int32_t longitude = static_cast<int32_t>(round(sensor_gps.longitude_deg * 1e7));
+					int32_t latitude = sensor_gps.lat;
+					int32_t longitude = sensor_gps.lon;
 					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 = static_cast<int16_t>(sensor_gps.altitude_msl_m * 1e3) + 1000;
+					uint16_t altitude = sensor_gps.alt + 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 = 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));
+	int32_t latitude = vehicle_gps_position.lat;
+	int32_t longitude = vehicle_gps_position.lon;
 	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 = static_cast<int16_t>(round(vehicle_gps_position.altitude_msl_m + 1.0));
+	uint16_t altitude = vehicle_gps_position.alt + 1000;
 	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 = 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
+	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

 	return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
 }
@@ -65,8 +65,6 @@ 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(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.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.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().longitude_deg * 1e7);
+	uint32_t iLon =  6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lon);

 	iLon |= (1 << 31);

-	if (s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg < 0) { iLon |= (1 << 30); }
+	if (s_port_subscription_data->vehicle_gps_position_sub.get().lon < 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().latitude_deg * 1e7);
+	uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lat);

-	if (s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg < 0) { iLat |= (1 << 30); }
+	if (s_port_subscription_data->vehicle_gps_position_sub.get().lat < 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 = static_cast<uint32_t>(s_port_subscription_data->vehicle_gps_position_sub.get().altitude_msl_m * 1e2);
+	uint32_t iAlt = s_port_subscription_data->vehicle_gps_position_sub.get().alt / 10;
 	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 = gps.latitude_deg;
+		double lat = ((double)(gps.lat)) * 1e-7d;

 		/* 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 = gps.longitude_deg;
+		double lon = ((double)(gps.lon)) * 1e-7d;

 		/* 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)(round(gps.altitude_msl_m) + 500.0);
+		uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f);
 		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.latitude_deg, _gps.longitude_deg,
+		const float distance = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE,
 				       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 = (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,
+	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,
 			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,8 +745,7 @@ void SagetechMXS::send_operating_msg()
 	mxs_state.op.altRes25 =
 		!mxs_state.inst.altRes100;                                // Host Altitude Resolution from install

-	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.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT;                       // Height above sealevel in feet

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

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

 	// Get Vehicle Longitude and Latitude and Convert to string
-	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 int32_t longitude = _gps.lon;
+	const int32_t latitude =  _gps.lat;
 	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,
@@ -837,7 +836,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 = (float)_gps.altitude_ellipsoid_m;
+	gps.height = _gps.alt_ellipsoid * 1E-3;

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

-	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.altitude = _gps.alt * SAGETECH_SCALE_MM_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 double SAGETECH_SCALE_M_TO_FT{3.28084};
+	static constexpr float SAGETECH_SCALE_MM_TO_FT{0.00328084F};
 	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,6 +156,7 @@ 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,8 +64,6 @@ 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.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;
+	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;

 	if (valid_pos_cov) {
 		// Horizontal position uncertainty
@@ -177,8 +177,6 @@ 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,12 +147,6 @@ 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();
-
 	}
 }

@@ -190,11 +184,6 @@ 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);
 	}
@@ -511,37 +500,6 @@ 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
@@ -551,95 +509,21 @@ 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.

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

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

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

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

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

-		// // 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;
+	return 0;
 }

 bool CanIface::waitCCCRBitStateChange(uint32_t mask, bool target_state)
@@ -762,8 +646,7 @@ 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_BOE;   // bus off state
+		   | FDCAN_IE_RF1FE;  // Rx FIFO 1 FIFO full

 	// Keep Rx interrupts on Line 0; move Tx to Line 1
 	can_->ILS = FDCAN_ILS_TCL;  // TC interrupt on line 1
@@ -798,8 +681,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;
@@ -871,24 +754,6 @@ 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.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.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.status = gps.fix_type;
 			fix2.ned_velocity[0] = gps.vel_n_m_s;
 			fix2.ned_velocity[1] = gps.vel_e_m_s;
@@ -4,11 +4,12 @@ serial_config:
    port_config_param:
      name: UWB_PORT_CFG
      group: UWB
-      default: "TEL2"
+      default: ""

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

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

-        UWB_SENS_ROT:
+        UWB_INIT_OFF_YAW:
          description:
-            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
+            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
+
+
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -63,130 +63,128 @@
 // 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 *port):
+UWB_SR150::UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug):
 	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"))
 {
-	/* store port name */
-	strncpy(_port, port, sizeof(_port) - 1);
-	/* enforce null termination */
-	_port[sizeof(_port) - 1] = '\0';
+	_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"); }
 }

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

-bool UWB_SR150::init()
+void UWB_SR150::run()
 {
-	// execute Run() on every sensor_accel publication
-	if (!_sensor_uwb_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
+	// 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);
 	}

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

-	return true;
+	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));
+	}
 }

-void UWB_SR150::start()
+void UWB_SR150::grid_info_read(position_t *grid)
 {
-	/* schedule a cycle to start things */
-	ScheduleNow();
-}
+	//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");

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

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

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

-		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);
-		}
+	} 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");
 	}

-	/* 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;
-	}
+	fclose(file);
 }

 int UWB_SR150::custom_command(int argc, char *argv[])
@@ -216,20 +214,43 @@ $ 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 = UWB_DEFAULT_PORT;
+	const char *device_name = nullptr;
+	bool error_flag = false;
 	int baudrate = 0;
+	bool uwb_pos_debug = false; // Display UWB position calculation debug Messages

-	while ((ch = px4_getopt(argc, argv, "d:b", &option_index, &option_arg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "d:b:p", &option_index, &option_arg)) != EOF) {
 		switch (ch) {
 		case 'd':
 			device_name = option_arg;
@@ -239,54 +260,47 @@ int UWB_SR150::task_spawn(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;
 		}
 	}

-	UWB_SR150 *instance = new UWB_SR150(device_name);
+	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;
+	}

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

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

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

 	} else {
-		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();
+		PX4_INFO("Constructing UWB_SR150. Device: %s", device_name);
+		return new UWB_SR150(device_name, baudrate, uwb_pos_debug);
 	}
 }

-int UWB_SR150::collectData()
+int UWB_SR150::distance()
 {
+	_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);
@@ -336,54 +350,366 @@ int UWB_SR150::collectData()
 	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) (36 bytes)
+	//  - Size of message == sizeof(distance_msg_t) (51 bytes)
 	//  - status == 0x00
-	bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b);
+	//  - 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)
+	//);

 	if (ok) {

 		/* Ranging Message*/
-		_sensor_uwb.timestamp 		= hrt_absolute_time();
+		_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.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;
+		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.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;
+		// Algorithm goes here
+		UWB_POS_ERROR_CODES UWB_POS_ERROR = UWB_SR150::localization();

+		_uwb_distance.status = 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;
+		if (UWB_OK == UWB_POS_ERROR) {

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

-		/*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;
+		} 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;

-		/* 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_LIN_DEP_FOR_THREE:
+					PX4_INFO("UWB localization: linear dependent with 3 Anchors");
+					break;

-		_sensor_uwb_pub.publish(_sensor_uwb);
+				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);

 	} 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-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -38,62 +38,100 @@
 #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 <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
+#include <perf/perf_counter.h>
 #include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionCallback.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/sensor_uwb.h>
+#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/SubscriptionInterval.hpp>
+#include <uORB/topics/vehicle_attitude.h>
 #include <matrix/math.hpp>
-
-#define UWB_DEFAULT_PORT "/dev/ttyS1"
+#include <matrix/Matrix.hpp>

 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 {
-	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
+	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
 } __attribute__((packed)) UWB_range_meas_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 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
+	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
 } __attribute__((packed)) distance_msg_t;

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

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

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

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

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

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

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

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

 	void parameters_update();

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

-	// Publications
-	uORB::Publication<sensor_uwb_s> _sensor_uwb_pub{ORB_ID(sensor_uwb)};
+	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
+	)

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

-	// 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};
+	int _uart;
 	fd_set _uart_set;
 	struct timeval _uart_timeout {};
+	bool _uwb_pos_debug;

-	unsigned _consecutive_fail_count;
-	int _interval{100000};
+	uORB::Publication<uwb_grid_s> _uwb_grid_pub{ORB_ID(uwb_grid)};
+	uwb_grid_s _uwb_grid{};

-	distance_msg_t 		_distance_result_msg{};
+	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;
+
+	perf_counter_t _read_count_perf;
+	perf_counter_t _read_err_perf;
 };
+
 #endif //PX4_RDDRONE_H
@@ -35,12 +35,12 @@

 using namespace time_literals;

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

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

 	sensor_gps_s sensor_gps{};
 	sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954;
-	sensor_gps.latitude_deg = _latitude;
-	sensor_gps.longitude_deg = _longitude;
-	sensor_gps.altitude_msl_m = _altitude;
-	sensor_gps.altitude_ellipsoid_m = _altitude;
+	sensor_gps.lat = _latitude;
+	sensor_gps.lon = _longitude;
+	sensor_gps.alt = _altitude;
+	sensor_gps.alt_ellipsoid = _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, double altitude_m = 30.1);
+	FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, float altitude_m = 30.1f);

 	~FakeGps() override = default;

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

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

-	double _latitude{29.6603018};   // Latitude in degrees
-	double _longitude{-82.3160500}; // Longitude in degrees
-	double _altitude{30.1};         // Altitude in meters above MSL, (millimetres)
+	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)
 };
@@ -42,5 +42,4 @@ 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.latitude_deg;
-				const double lon = gps.longitude_deg;
+				const double lat = gps.lat / 1.e7;
+				const double lon = gps.lon / 1.e7;

 				// 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,7 +45,6 @@ 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,6 +198,7 @@ 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),
@@ -1,34 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 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
-# 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.
-#
-############################################################################
-
-px4_add_library(dataman_client DatamanClient.cpp)
@@ -1,644 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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
- * 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 DatamanClient.cpp
- */
-
-#include <dataman_client/DatamanClient.hpp>
-
-DatamanClient::DatamanClient()
-{
-	_dataman_request_pub.advertise();
-	_dataman_response_sub = orb_subscribe(ORB_ID(dataman_response));
-
-	if (_dataman_response_sub < 0) {
-		PX4_ERR("Failed to subscribe (%i)", errno);
-
-	} else {
-		// make sure we don't get any stale response by doing an orb_copy
-		dataman_response_s response{};
-		orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response);
-
-		_fds.fd = _dataman_response_sub;
-		_fds.events = POLLIN;
-
-		hrt_abstime timestamp = hrt_absolute_time();
-
-		dataman_request_s request;
-		request.timestamp = timestamp;
-		request.request_type = DM_GET_ID;
-		request.client_id = CLIENT_ID_NOT_SET;
-
-		bool success = syncHandler(request, response, timestamp, 1000_ms);
-
-		if (success && (response.client_id > CLIENT_ID_NOT_SET)) {
-
-			_client_id = response.client_id;
-
-		} else {
-			PX4_ERR("Failed to get client ID!");
-		}
-	}
-}
-
-DatamanClient::~DatamanClient()
-{
-	if (_dataman_response_sub >= 0) {
-		orb_unsubscribe(_dataman_response_sub);
-	}
-}
-
-bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_response_s &response,
-				const hrt_abstime &start_time, hrt_abstime timeout)
-{
-	bool response_received = false;
-	int32_t ret = 0;
-	hrt_abstime time_elapsed = hrt_elapsed_time(&start_time);
-	_dataman_request_pub.publish(request);
-
-	while (!response_received && (time_elapsed < timeout)) {
-
-		uint32_t timeout_ms = 100;
-		ret = px4_poll(&_fds, 1, timeout_ms);
-
-		if (ret < 0) {
-			PX4_ERR("px4_poll returned error: %" PRIu32, ret);
-			break;
-
-		} else if (ret == 0) {
-
-			// No response received, send new request
-			_dataman_request_pub.publish(request);
-
-		} else {
-
-			bool updated = false;
-			orb_check(_dataman_response_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response);
-
-				if (response.client_id == request.client_id) {
-
-					if ((response.request_type == request.request_type) &&
-					    (response.item == request.item) &&
-					    (response.index == request.index)) {
-						response_received = true;
-						break;
-					}
-
-				} else if (request.client_id == CLIENT_ID_NOT_SET) {
-
-					// validate timestamp from response.data
-					if (0 == memcmp(&(request.timestamp), &(response.data), sizeof(hrt_abstime))) {
-						response_received = true;
-						break;
-					}
-				}
-			}
-		}
-
-		time_elapsed = hrt_elapsed_time(&start_time);
-	}
-
-	if (!response_received && ret >= 0) {
-		PX4_ERR("timeout after %" PRIu32 " ms!", static_cast<uint32_t>(timeout / 1000));
-	}
-
-	return response_received;
-}
-
-bool DatamanClient::readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout)
-{
-	if (length > g_per_item_size[item]) {
-		PX4_ERR("Length  %" PRIu32 " can't fit in data size for item  %" PRIi8, length, static_cast<uint8_t>(item));
-		return false;
-	}
-
-	bool success = false;
-	hrt_abstime timestamp = hrt_absolute_time();
-
-	dataman_request_s request;
-	request.timestamp = timestamp;
-	request.index = index;
-	request.data_length = length;
-	request.client_id = _client_id;
-	request.request_type = DM_READ;
-	request.item = static_cast<uint8_t>(item);
-
-	dataman_response_s response{};
-	success = syncHandler(request, response, timestamp, timeout);
-
-	if (success) {
-
-		if (response.status != dataman_response_s::STATUS_SUCCESS) {
-
-			success = false;
-			PX4_ERR("readSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32,
-				response.status, static_cast<uint8_t>(item), index, length);
-
-		} else {
-			memcpy(buffer, response.data, length);
-		}
-	}
-
-	return success;
-}
-
-bool DatamanClient::writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout)
-{
-	if (length > g_per_item_size[item]) {
-		PX4_ERR("Length  %" PRIu32 " can't fit in data size for item  %" PRIi8, length, static_cast<uint8_t>(item));
-		return false;
-	}
-
-	bool success = false;
-	hrt_abstime timestamp = hrt_absolute_time();
-
-	dataman_request_s request;
-	request.timestamp = timestamp;
-	request.index = index;
-	request.data_length = length;
-	request.client_id = _client_id;
-	request.request_type = DM_WRITE;
-	request.item = static_cast<uint8_t>(item);
-
-	memcpy(request.data, buffer, length);
-
-	dataman_response_s response{};
-	success = syncHandler(request, response, timestamp, timeout);
-
-	if (success) {
-
-		if (response.status != dataman_response_s::STATUS_SUCCESS) {
-
-			success = false;
-			PX4_ERR("writeSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32,
-				response.status, static_cast<uint8_t>(item), index, length);
-		}
-	}
-
-	return success;
-}
-
-bool DatamanClient::clearSync(dm_item_t item, hrt_abstime timeout)
-{
-	bool success = false;
-	hrt_abstime timestamp = hrt_absolute_time();
-
-	dataman_request_s request;
-	request.timestamp = timestamp;
-	request.client_id = _client_id;
-	request.request_type = DM_CLEAR;
-	request.item = static_cast<uint8_t>(item);
-
-	dataman_response_s response{};
-	success = syncHandler(request, response, timestamp, timeout);
-
-	if (success) {
-
-		if (response.status != dataman_response_s::STATUS_SUCCESS) {
-
-			success = false;
-			PX4_ERR("clearSync failed! status=%" PRIu8 ", item=%" PRIu8,
-				response.status, static_cast<uint8_t>(item));
-		}
-	}
-
-	return success;
-}
-
-bool DatamanClient::readAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length)
-{
-	if (length > g_per_item_size[item]) {
-		PX4_ERR("Length  %" PRIu32 " can't fit in data size for item  %" PRIi8, length, static_cast<uint8_t>(item));
-		return false;
-	}
-
-	bool success = false;
-
-	if (_state == State::Idle) {
-
-		hrt_abstime timestamp = hrt_absolute_time();
-
-		dataman_request_s request;
-		request.timestamp = timestamp;
-		request.index = index;
-		request.data_length = length;
-		request.client_id = _client_id;
-		request.request_type = DM_READ;
-		request.item = static_cast<uint8_t>(item);
-
-		_active_request.timestamp = timestamp;
-		_active_request.request_type = DM_READ;
-		_active_request.item = item;
-		_active_request.index = index;
-		_active_request.buffer = buffer;
-		_active_request.length = length;
-
-		_state = State::RequestSent;
-
-		_dataman_request_pub.publish(request);
-
-		success = true;
-	}
-
-	return success;
-}
-
-bool DatamanClient::writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length)
-{
-	if (length > g_per_item_size[item]) {
-		PX4_ERR("Length  %" PRIu32 " can't fit in data size for item  %" PRIi8, length, static_cast<uint8_t>(item));
-		return false;
-	}
-
-	bool success = false;
-
-	if (_state == State::Idle) {
-
-		hrt_abstime timestamp = hrt_absolute_time();
-
-		dataman_request_s request;
-		request.timestamp = timestamp;
-		request.index = index;
-		request.data_length = length;
-		request.client_id = _client_id;
-		request.request_type = DM_WRITE;
-		request.item = static_cast<uint8_t>(item);
-
-		memcpy(request.data, buffer, length);
-
-		_active_request.timestamp = timestamp;
-		_active_request.request_type = DM_WRITE;
-		_active_request.item = item;
-		_active_request.index = index;
-		_active_request.buffer = buffer;
-		_active_request.length = length;
-
-		_state = State::RequestSent;
-
-		_dataman_request_pub.publish(request);
-
-		success = true;
-	}
-
-	return success;
-}
-
-bool DatamanClient::clearAsync(dm_item_t item)
-{
-	bool success = false;
-
-	if (_state == State::Idle) {
-
-		hrt_abstime timestamp = hrt_absolute_time();
-
-		dataman_request_s request;
-		request.timestamp = timestamp;
-		request.client_id = _client_id;
-		request.request_type = DM_CLEAR;
-		request.item = static_cast<uint8_t>(item);
-		request.index = 0;
-
-		_active_request.timestamp = timestamp;
-		_active_request.request_type = DM_CLEAR;
-		_active_request.item = item;
-		_active_request.index = request.index;
-		_state = State::RequestSent;
-
-		_dataman_request_pub.publish(request);
-
-		success = true;
-	}
-
-	return success;
-}
-
-void DatamanClient::update()
-{
-	if (_state == State::RequestSent) {
-
-		bool updated = false;
-		orb_check(_dataman_response_sub, &updated);
-
-		dataman_response_s response;
-
-		if (updated) {
-			orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response);
-
-			if ((response.client_id == _client_id) &&
-			    (response.request_type == _active_request.request_type) &&
-			    (response.item == _active_request.item) &&
-			    (response.index == _active_request.index)) {
-
-				if (response.request_type == DM_READ) {
-					memcpy(_active_request.buffer, response.data, _active_request.length);
-				}
-
-				_response_status = response.status;
-
-				if (_response_status != dataman_response_s::STATUS_SUCCESS) {
-
-					PX4_ERR("Async request type %" PRIu8 " failed! status=%" PRIu8 " item=%" PRIu8 " index=%" PRIu32,
-						response.request_type, response.status, static_cast<uint8_t>(_active_request.item), _active_request.index);
-				}
-
-				_state = State::ResponseReceived;
-			}
-		}
-
-		if (_state == State::RequestSent) {
-
-			/* Retry the request if there is no answer */
-			if (((_active_request.request_type != DM_CLEAR) && (hrt_elapsed_time(&_active_request.timestamp) > 100_ms)) ||
-			    (hrt_elapsed_time(&_active_request.timestamp) > 1000_ms)
-			   ) {
-
-				hrt_abstime timestamp = hrt_absolute_time();
-
-				_active_request.timestamp = timestamp;
-
-				dataman_request_s request;
-				request.timestamp = timestamp;
-				request.index = _active_request.index;
-				request.data_length = _active_request.length;
-				request.client_id = _client_id;
-				request.request_type = static_cast<uint8_t>(_active_request.request_type);
-				request.item = static_cast<uint8_t>(_active_request.item);
-
-				if (_active_request.request_type == DM_WRITE) {
-					memcpy(request.data, _active_request.buffer, _active_request.length);
-				}
-
-				_dataman_request_pub.publish(request);
-
-				_state = State::RequestSent;
-			}
-		}
-	}
-}
-
-bool DatamanClient::lastOperationCompleted(bool &success)
-{
-	bool completed = false;
-	success = false;
-
-	if (_state == State::ResponseReceived) {
-
-		if (_response_status == dataman_response_s::STATUS_SUCCESS) {
-			success = true;
-		}
-
-		_state = State::Idle;
-		completed = true;
-	}
-
-	return completed;
-}
-
-void DatamanClient::abortCurrentOperation()
-{
-	_state = State::Idle;
-}
-
-DatamanCache::DatamanCache(const char *cache_miss_perf_counter_name, uint32_t num_items)
-	: _cache_miss_perf(perf_alloc(PC_COUNT, cache_miss_perf_counter_name))
-{
-	_items = new Item[num_items] {};
-
-	if (_items != nullptr) {
-		_num_items = num_items;
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-}
-
-DatamanCache::~DatamanCache()
-{
-	delete[] _items;
-	perf_free(_cache_miss_perf);
-}
-
-void DatamanCache::resize(uint32_t num_items)
-{
-	Item *new_items = new Item[num_items] {};
-
-	if (new_items != nullptr) {
-		uint32_t num_min = num_items < _num_items ? num_items : _num_items;
-
-		for (uint32_t i = 0; i < num_min; ++i) {
-			new_items[i] = _items[i];
-		}
-
-		delete[] _items;
-		_items = new_items;
-		_num_items = num_items;
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-}
-
-bool DatamanCache::load(dm_item_t item, uint32_t index)
-{
-	if (!_items) {
-		return false;
-	}
-
-	bool success = false;
-	bool duplicate = false;
-
-	//Prevent duplicates
-	for (uint32_t i = 0; i < _num_items; ++i) {
-
-		if (_items[i].cache_state != State::Idle &&
-		    _items[i].cache_state != State::Error &&
-		    _items[i].response.item == item &&
-		    _items[i].response.index == index) {
-			duplicate = true;
-			break;
-		}
-	}
-
-	if (!duplicate && (_item_counter < _num_items)) {
-
-		_items[_load_index].cache_state = State::RequestPrepared;
-		_items[_load_index].response.item = item;
-		_items[_load_index].response.index = index;
-
-		_load_index = (_load_index + 1) % _num_items;
-
-		++_item_counter;
-
-		success = true;
-	}
-
-	return success;
-}
-
-bool DatamanCache::loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout)
-{
-	if (length > g_per_item_size[item]) {
-		PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast<uint8_t>(item));
-		return false;
-	}
-
-	if (!_items) {
-		return false;
-	}
-
-	bool success = false;
-	bool item_found = false;
-
-	for (uint32_t i = 0; i < _num_items; ++i) {
-		if ((_items[i].response.item == item) &&
-		    (_items[i].response.index == index)) {
-			item_found = true;
-
-			if (_items[i].cache_state == State::ResponseReceived) {
-				memcpy(buffer, _items[i].response.data, length);
-				success = true;
-				break;
-			}
-		}
-	}
-
-	if (!success && (timeout > 0)) {
-		perf_count(_cache_miss_perf);
-		success = _client.readSync(item, index, buffer, length, timeout);
-
-		// Cache the item if not found already (it could be in the process of being loaded)
-		if (success && !item_found && _item_counter < _num_items) {
-			_items[_load_index].cache_state = State::ResponseReceived;
-			_items[_load_index].response.item = item;
-			_items[_load_index].response.index = index;
-			memcpy(_items[_load_index].response.data, buffer, length);
-
-			_load_index = (_load_index + 1) % _num_items;
-
-			++_item_counter; // Still increase the counter here
-		}
-	}
-
-	return success;
-}
-
-void DatamanCache::update()
-{
-	if (_item_counter > 0) {
-
-		_client.update();
-
-		bool success = false;
-		bool response_success = false;
-
-		switch (_items[_update_index].cache_state) {
-		case State::Idle:
-			break;
-
-		case State::ResponseReceived:
-			// Skip it
-			changeUpdateIndex();
-			break;
-
-		case State::RequestPrepared:
-
-			success = _client.readAsync(static_cast<dm_item_t>(_items[_update_index].response.item),
-						    _items[_update_index].response.index,
-						    _items[_update_index].response.data,
-						    g_per_item_size[_items[_update_index].response.item]);
-
-			if (success) {
-				_items[_update_index].cache_state = State::RequestSent;
-
-			} else {
-				_items[_update_index].cache_state = State::Error;
-			}
-
-			break;
-
-		case State::RequestSent:
-
-			if (_client.lastOperationCompleted(response_success)) {
-
-				if (response_success) {
-
-					_items[_update_index].cache_state = State::ResponseReceived;
-					changeUpdateIndex();
-
-				} else {
-					_items[_update_index].cache_state = State::Error;
-				}
-			}
-
-			break;
-
-		case State::Error:
-			// Handled below
-			break;
-		}
-
-		if (_items[_update_index].cache_state == State::Error) {
-			PX4_ERR("Caching: item %" PRIu8 ", index %" PRIu32", status %" PRIu8,
-				_items[_update_index].response.item, _items[_update_index].response.index,
-				_items[_update_index].response.status);
-
-			changeUpdateIndex();
-		}
-
-	}
-}
-
-void DatamanCache::invalidate()
-{
-	for (uint32_t i = 0; i < _num_items; ++i) {
-		_items[i].cache_state = State::Idle;
-	}
-
-	_update_index = 0;
-	_item_counter = 0;
-	_load_index = 0;
-	_client.abortCurrentOperation();
-}
-
-inline void DatamanCache::changeUpdateIndex()
-{
-	_update_index = (_update_index + 1) % _num_items;
-
-	if (_item_counter > 0) {
-		--_item_counter;
-	}
-}
--- a/Show More
+++ b/Show More