Works

Disable lockstep
Run indi control by default
2026-06-09 01:10:05 +08:00 · 2025-08-02 05:56:38 +02:00 · 2025-08-02 05:38:36 +02:00 · 2025-08-02 05:31:21 +02:00 · 2023-11-21 13:43:09 +01:00 · 2023-11-17 15:29:36 +01:00
265 changed files with 9129 additions and 252 deletions
@@ -18,7 +18,7 @@ jobs:
          "check_format",
          "tests",
          "tests_coverage",
-          "px4_fmu-v2_default stack_check",
+          # "px4_fmu-v2_default stack_check",
          "validate_module_configs",
          "shellcheck_all",
          "NO_NINJA_BUILD=1 px4_fmu-v5_default",
@@ -15,23 +15,23 @@ jobs:
    strategy:
      matrix:
        config: [
-          airmind_mindpx-v2_default,
-          ark_can-flow_canbootloader,
-          ark_can-flow_debug,
-          ark_can-flow_default,
-          av_x-v1_default,
-          bitcraze_crazyflie21_default,
-          bitcraze_crazyflie_default,
-          cuav_can-gps-v1_canbootloader,
-          cuav_can-gps-v1_debug,
-          cuav_can-gps-v1_default,
-          cuav_nora_default,
-          cuav_x7pro_default,
-          cubepilot_cubeorange_console,
-          cubepilot_cubeorange_default,
-          cubepilot_cubeyellow_console,
-          cubepilot_cubeyellow_default,
-          cubepilot_io-v2_default,
+          # airmind_mindpx-v2_default,
+          # ark_can-flow_canbootloader,
+          # ark_can-flow_debug,
+          # ark_can-flow_default,
+          # av_x-v1_default,
+          # bitcraze_crazyflie21_default,
+          # bitcraze_crazyflie_default,
+          # cuav_can-gps-v1_canbootloader,
+          # cuav_can-gps-v1_debug,
+          # cuav_can-gps-v1_default,
+          # cuav_nora_default,
+          # cuav_x7pro_default,
+          # cubepilot_cubeorange_console,
+          # cubepilot_cubeorange_default,
+          # cubepilot_cubeyellow_console,
+          # cubepilot_cubeyellow_default,
+          # cubepilot_io-v2_default,
          holybro_can-gps-v1_canbootloader,
          holybro_can-gps-v1_debug,
          holybro_can-gps-v1_default,
@@ -39,28 +39,28 @@ jobs:
          holybro_kakutef7_default,
          holybro_pix32v5_default,
          modalai_fc-v1_default,
-          mro_ctrl-zero-f7-oem_default,
-          mro_ctrl-zero-f7_default,
-          mro_ctrl-zero-h7-oem_default,
-          mro_ctrl-zero-h7_default,
-          mro_pixracerpro_default,
-          mro_x21-777_default,
-          mro_x21_default,
-          nxp_fmuk66-e_default,
-          nxp_fmuk66-e_rtps,
-          nxp_fmuk66-e_socketcan,
-          nxp_fmuk66-v3_default,
-          nxp_fmuk66-v3_rtps,
-          nxp_fmuk66-v3_socketcan,
-          nxp_fmurt1062-v1_default,
-          nxp_ucans32k146_canbootloader,
-          nxp_ucans32k146_default,
+          # mro_ctrl-zero-f7-oem_default,
+          # mro_ctrl-zero-f7_default,
+          # mro_ctrl-zero-h7-oem_default,
+          # mro_ctrl-zero-h7_default,
+          # mro_pixracerpro_default,
+          # mro_x21-777_default,
+          # mro_x21_default,
+          # nxp_fmuk66-e_default,
+          # nxp_fmuk66-e_rtps,
+          # nxp_fmuk66-e_socketcan,
+          # nxp_fmuk66-v3_default,
+          # nxp_fmuk66-v3_rtps,
+          # nxp_fmuk66-v3_socketcan,
+          # nxp_fmurt1062-v1_default,
+          # nxp_ucans32k146_canbootloader,
+          # nxp_ucans32k146_default,
          omnibus_f4sd_default,
-          px4_fmu-v2_default,
-          px4_fmu-v2_fixedwing,
-          px4_fmu-v2_multicopter,
-          px4_fmu-v2_rover,
-          px4_fmu-v2_test,
+          # px4_fmu-v2_default,
+          # px4_fmu-v2_fixedwing,
+          # px4_fmu-v2_multicopter,
+          # px4_fmu-v2_rover,
+          # px4_fmu-v2_test,
          px4_fmu-v3_default,
          px4_fmu-v4_cannode,
          px4_fmu-v4_default,
@@ -76,14 +76,14 @@ jobs:
          px4_fmu-v5_stackcheck,
          px4_fmu-v5_uavcanv0periph,
          px4_fmu-v5_uavcanv1,
-          px4_fmu-v5x_base_phy_DP83848C,
-          px4_fmu-v5x_default,
+          # px4_fmu-v5x_base_phy_DP83848C,
+          # px4_fmu-v5x_default,
          px4_fmu-v6u_default,
          px4_fmu-v6u_test,
          px4_fmu-v6x_default,
          px4_io-v2_default,
          spracing_h7extreme_default,
-          uvify_core_default
+          # uvify_core_default
          ]
    steps:
    - uses: actions/checkout@v1
@@ -18,7 +18,7 @@ jobs:
    - name: Install Python3
      run: sudo apt-get install python3 python3-setuptools python3-pip -y
    - name: Install tools
-      run: pip3 install --user mypy flake8
+      run: pip3 install --user mypy types-requests flake8
    - name: Check MAVSDK test scripts with mypy
      run: $HOME/.local/bin/mypy --strict test/mavsdk_tests/*.py
    - name: Check MAVSDK test scripts with flake8
@@ -0,0 +1,55 @@
+name: RotorS PX4 Build Test
+
+on:
+  push:
+    branches:
+    - 'master'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        container:
+        - 'px4io/px4-dev-simulation-bionic:2020-11-18' # Gazebo 9
+    container:
+      image: ${{ matrix.container }}
+      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
+    steps:
+    - uses: actions/checkout@v1
+      with:
+        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: Prepare ccache timestamp
+      id: ccache_cache_timestamp
+      shell: cmake -P {0}
+      run: |
+        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
+        message("::set-output name=timestamp::${current_date}")
+    - name: ccache cache files
+      uses: actions/cache@v2
+      with:
+        path: ~/.ccache
+        key: rotors_tests-RelWithDebInfo-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
+        restore-keys: rotors_tests-RelWithDebInfo-ccache-
+    - name: setup ccache
+      run: |
+          mkdir -p ~/.ccache
+          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
+          echo "compression = true" >> ~/.ccache/ccache.conf
+          echo "compression_level = 5" >> ~/.ccache/ccache.conf
+          echo "max_size = 100M" >> ~/.ccache/ccache.conf
+          ccache -s
+          ccache -z
+    - name: Install glog
+      run: apt update && apt install -y libgoogle-glog-dev libyaml-cpp-dev
+    - name: Build PX4 RotorS
+      env:
+        PX4_CMAKE_BUILD_TYPE: RelWithDebInfo
+        DONT_RUN: 1
+      run: make px4_sitl rotors
+    - name: ccache post-run px4/firmware
+      run: ccache -s
@@ -0,0 +1,22 @@
+# This pipeline keeps the master branch up to date with upstream master
+name: Upstream Synchronization
+
+on:
+  schedule:
+  - cron:  "*/15 * * * *"
+  workflow_dispatch:
+
+jobs:
+  repo-sync:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v2
+      with:
+        persist-credentials: false
+    - name: repo-sync
+      uses: repo-sync/github-sync@v2
+      with:
+        source_repo: "https://github.com/PX4/PX4-Autopilot.git"
+        source_branch: "master"
+        destination_branch: "master"
+        github_token: ${{ secrets.PAT }}
@@ -1,6 +1,6 @@
 [submodule "mavlink/include/mavlink/v2.0"]
 	path = mavlink/include/mavlink/v2.0
-	url = https://github.com/mavlink/c_library_v2.git
+	url = https://github.com/ethz-asl/fw_mavlink_c_library_v2.git
 	branch = master
 [submodule "src/drivers/uavcan/libuavcan"]
 	path = src/drivers/uavcan/libuavcan
@@ -63,3 +63,6 @@
 [submodule "src/drivers/uavcannode_gps_demo/libcanard"]
 	path = src/drivers/uavcannode_gps_demo/libcanard
 	url = https://github.com/UAVCAN/libcanard
+[submodule "Tools/rotors_simulator"]
+	path = Tools/rotors_simulator
+	url = https://github.com/ethz-asl/rotors_simulator.git
@@ -137,5 +137,13 @@
        "workbench.settings.enableNaturalLanguageSearch": false,
        "yaml.schemas": {
                "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
-        }
+        },
+        "python.autoComplete.extraPaths": [
+                "/home/jaeyoung/catkin_ws/devel/lib/python3/dist-packages",
+                "/opt/ros/noetic/lib/python3/dist-packages"
+        ],
+        "python.analysis.extraPaths": [
+                "/home/jaeyoung/catkin_ws/devel/lib/python3/dist-packages",
+                "/opt/ros/noetic/lib/python3/dist-packages"
+        ]
 }
@@ -0,0 +1,10 @@
+#!/bin/sh
+#
+# @name Plane Dynamic Soaring SITL
+#
+
+. ${R}etc/init.d-posix/airframes/1030_plane
+
+
+set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix
+set MIXER custom
@@ -0,0 +1,54 @@
+#!/bin/sh
+#
+# @name Plane SITL
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+param set-default EKF2_ARSP_THR 8
+param set-default EKF2_FUSE_BETA 1
+param set-default EKF2_MAG_ACCLIM 0
+param set-default EKF2_MAG_YAWLIM 0
+
+param set-default FW_LND_AIRSPD_SC 1
+param set-default FW_LND_ANG 8
+param set-default FW_THR_LND_MAX 0
+
+param set-default FW_L1_PERIOD 12
+
+param set-default FW_MAN_P_MAX 30
+
+param set-default FW_PR_I 0.4
+param set-default FW_PR_P 0.9
+param set-default FW_PR_FF 0.2
+param set-default FW_PSP_OFF 2
+param set-default FW_P_LIM_MAX 32
+param set-default FW_P_LIM_MIN -15
+
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_P 0.3
+
+param set-default FW_THR_MAX 0.6
+param set-default FW_THR_MIN 0.05
+param set-default FW_THR_CRUISE 0.25
+
+param set-default FW_T_ALT_TC 2
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+param set-default FW_T_TAS_TC 2
+
+param set-default FW_W_EN 1
+
+param set-default MIS_LTRMIN_ALT 30
+param set-default MIS_TAKEOFF_ALT 30
+
+param set-default NAV_ACC_RAD 15
+param set-default NAV_DLL_ACT 2
+param set-default NAV_LOITER_RAD 50
+
+param set-default RWTO_TKOFF 1
+
+set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix
+set MIXER custom
@@ -54,6 +54,7 @@ px4_add_romfs_files(
 	1020_uuv_generic
 	1021_uuv_hippocampus
 	1022_uuv_bluerov2_heavy
+	1029_plane_dynamicsoaring
 	1030_plane
 	1031_plane_cam
 	1032_plane_catapult
@@ -62,6 +63,7 @@ px4_add_romfs_files(
 	1034_rascal-electric
 	1035_techpod
 	1036_malolo
+	1037_believer
 	1040_standard_vtol
 	1041_tailsitter
 	1042_tiltrotor
@@ -0,0 +1,54 @@
+#!/bin/sh
+#
+# @name ASL_EasyGlider
+#
+# @type Standard Plane
+# @class Plane
+#
+# @output MAIN1 aileron
+# @output MAIN2 aileron
+# @output MAIN3 elevator
+# @output MAIN4 rudder
+# @output MAIN5 throttle
+# @output MAIN6 wheel
+# @output MAIN7 flaps
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Philipp Oettershagen <philipp.oettershagen@mavt.ethz.ch>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+if [ $AUTOCNF == yes ]
+then
+    param set FW_AIRSPD_MIN 10
+    param set FW_AIRSPD_TRIM 15
+    param set FW_AIRSPD_MAX 20
+
+    param set FW_MAN_P_MAX 45
+    param set FW_MAN_R_MAX 45
+    param set FW_R_LIM 45
+
+    param set FW_WR_FF 0.2
+    param set FW_WR_I 0.2
+    param set FW_WR_IMAX 0.8
+    param set FW_WR_P 1
+    param set FW_W_RMAX 0
+
+    # set disarmed value for the ESC
+    param set PWM_DISARMED 1000
+
+    param set-default SENS_EN_ADIS164X 1
+    param set SENS_EN_ADIS164X 4
+fi
+
+set MIXER asl_easyglider
+
+# use PWM parameters for throttle channel
+set PWM_OUT 5
@@ -0,0 +1,52 @@
+#!/bin/sh
+#
+# @name ASL_Techpod
+#
+# @type Plane A-Tail
+# @class Plane
+#
+# @output MAIN1 aileron right
+# @output MAIN2 aileron left
+# @output MAIN3 v-tail right
+# @output MAIN4 v-tail left
+# @output MAIN5 throttle
+# @output MAIN6 wheel
+# @output MAIN7 flaps right
+# @output MAIN8 flaps left
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Philipp Oettershagen <philipp.oettershagen@mavt.ethz.ch>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+if [ $AUTOCNF == yes ]
+then
+    param set FW_AIRSPD_MIN 10
+    param set FW_AIRSPD_TRIM 15
+    param set FW_AIRSPD_MAX 20
+
+    param set FW_MAN_P_MAX 55
+    param set FW_MAN_R_MAX 55
+    param set FW_R_LIM 55
+
+    param set FW_WR_FF 0.2
+    param set FW_WR_I 0.2
+    param set FW_WR_IMAX 0.8
+    param set FW_WR_P 1
+    param set FW_W_RMAX 0
+
+    # set disarmed value for the ESC
+    param set PWM_DISARMED 1000
+fi
+
+set MIXER asl_techpod
+
+# use PWM parameters for throttle channel
+set PWM_OUT 5
@@ -0,0 +1,52 @@
+#!/bin/sh
+#
+# @name ASL_SenseSoar2
+#
+# @type Plane A-Tail
+# @class Plane
+#
+# @output MAIN1 aileron right
+# @output MAIN2 aileron left
+# @output MAIN3 v-tail right
+# @output MAIN4 v-tail left
+# @output MAIN5 throttle
+# @output MAIN6 wheel
+# @output MAIN7 flaps right
+# @output MAIN8 flaps left
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Philipp Oettershagen <philipp.oettershagen@mavt.ethz.ch>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+if [ $AUTOCNF == yes ]
+then
+    param set FW_AIRSPD_MIN 12
+    param set FW_AIRSPD_TRIM 15
+    param set FW_AIRSPD_MAX 20
+
+    param set FW_MAN_P_MAX 45
+    param set FW_MAN_R_MAX 45
+    param set FW_R_LIM 55
+
+    param set FW_WR_FF 0.2
+    param set FW_WR_I 0.2
+    param set FW_WR_IMAX 0.8
+    param set FW_WR_P 1
+    param set FW_W_RMAX 0
+
+    # set disarmed value for the ESC
+    param set PWM_DISARMED 1000
+fi
+
+set MIXER asl_sensoar2
+
+# use PWM parameters for throttle channel
+set PWM_OUT 5
@@ -0,0 +1,54 @@
+#!/bin/sh
+#
+# @name Autonomous Systems Lab, ETH Zurich Believer
+#
+# @type Plane V-Tail
+# @class Plane
+#
+# @output MAIN1 aileron right
+# @output MAIN2 aileron left
+# @output MAIN3 v-tail right
+# @output MAIN4 v-tail left
+# @output MAIN5 throttle
+# @output MAIN6 wheel
+# @output MAIN7 flaps right
+# @output MAIN8 flaps left
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Jaeyoung Lim <jalim@ethz.ch>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+param set-default FW_AIRSPD_MIN 10
+param set-default FW_AIRSPD_TRIM 15
+param set-default FW_AIRSPD_MAX 20
+
+param set-default FW_MAN_P_MAX 55
+param set-default FW_MAN_R_MAX 55
+param set-default FW_R_LIM 55
+
+param set-default FW_WR_FF 0.2
+param set-default FW_WR_I 0.2
+param set-default FW_WR_IMAX 0.8
+param set-default FW_WR_P 1
+param set-default FW_W_RMAX 0
+
+# set disarmed value for the ESC
+param set-default PWM_MAIN_DISARM 1500
+param set-default PWM_MAIN_DIS5 900
+param set-default PWM_MAIN_DIS6 900
+
+# The Mini Talon does not have a wheel and
+# no flaps. I leave them here because the mixer
+# computes also wheel and flap controls.
+set MIXER AAVVTTFF_vtail
+
+# use PWM parameters for throttle channel
+set PWM_OUT 56
@@ -157,6 +157,11 @@ px4_add_romfs_files(
 	18001_TF-B1

 	# [22000, 22999] Reserve for custom models
+	# ETHZ ASL Vehicles
+	22000_asl_easyglider
+	22001_asl_techpod
+	22002_asl_sensesoar2
+	22003_asl_believer

 	24001_dodeca_cox

@@ -15,8 +15,12 @@ ekf2 start &
 #
 fw_att_control start
 fw_pos_control_l1 start
+fw_dyn_soar_control start
+fw_dyn_soar_estimator start
 airspeed_selector start
 #
 # Start Land Detector.
 #
 land_detector start fixedwing
+
+
@@ -21,6 +21,23 @@ fi
 #                           Begin Optional drivers                            #
 ###############################################################################

+# ADIS16448 spi external IMU
+if param compare -s SENS_EN_ADIS164X 1
+then
+	if param compare -s SENS_OR_ADIS164X 0
+	then
+		adis16448 -S start
+	fi
+	if param compare -s SENS_OR_ADIS164X 4
+	then
+		adis16448 -S start -R 4
+	fi
+
+	# add a sleep to make sure the mag is the priority one
+	usleep 1000000
+fi
+
+
 if param compare -s SENS_EN_BATT 1
 then
 	batt_smbus start -X
@@ -108,12 +125,57 @@ then
 	vl53l1x start -X
 fi

-# ADIS16448 spi external IMU
-if param compare -s SENS_EN_ADIS164X 1
+# Hall effect sensors si7210
+# Potentially remove the -k option if possible and improve the startup if possible
+if param greater CAL_AV_AOA_ID -1
 then
-	adis16448 -S start
+	set AOA_I2C_ID 0
+	if param compare CAL_AV_AOA_ID 48
+	then
+		set AOA_I2C_ID 48
+	fi
+	if param compare CAL_AV_AOA_ID 49
+	then
+		set AOA_I2C_ID 49
+	fi
+	if param compare CAL_AV_AOA_ID 50
+	then
+		set AOA_I2C_ID 50
+	fi
+	if param compare CAL_AV_AOA_ID 51
+	then
+		set AOA_I2C_ID 51
+	fi
+
+	si7210 start -X -k -a ${AOA_I2C_ID}
+	unset AOA_I2C_ID
 fi

+if param greater CAL_AV_SLIP_ID -1
+then
+	set SLIP_I2C_ID 0
+	if param compare CAL_AV_SLIP_ID 48
+	then
+		set SLIP_I2C_ID 48
+	fi
+	if param compare CAL_AV_SLIP_ID 49
+	then
+		set SLIP_I2C_ID 49
+	fi
+	if param compare CAL_AV_SLIP_ID 50
+	then
+		set SLIP_I2C_ID 50
+	fi
+	if param compare CAL_AV_SLIP_ID 51
+	then
+		set SLIP_I2C_ID 51
+	fi
+
+	si7210 start -X -k -a ${SLIP_I2C_ID}
+	unset SLIP_I2C_ID
+fi
+
+
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then
@@ -155,5 +217,3 @@ fi
 ###############################################################################
 #                            End Optional drivers                             #
 ###############################################################################
-
-sensors start
@@ -332,6 +332,8 @@ else
 		#
 		# board sensors: rc.sensors
 		#
+		. ${R}etc/init.d/rc.sensors
+
 		set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
 		if [ -f $BOARD_RC_SENSORS ]
 		then
@@ -340,7 +342,7 @@ else
 		fi
 		unset BOARD_RC_SENSORS

-		. ${R}etc/init.d/rc.sensors
+		sensors start

 		if param compare -s BAT1_SOURCE 2
 		then
@@ -0,0 +1,61 @@
+Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU
+=======================================================
+
+This file defines mixers suitable for controlling a fixed wing aircraft with
+aileron, v-tail (rudder, elevator), throttle, steerable wheel and flaps
+using PX4FMU.
+The configuration assumes the aileron servos are connected to PX4FMU servo
+output 0 and 1, the tail servos to output 2 and 3, the throttle
+to output 4, the wheel to output 5 and the flaps to output 6 and 7.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
+
+Aileron mixer (roll + flaperon)
+---------------------------------
+
+This mixer assumes that the aileron servos are set up mechanically reversed.
+
+M: 2
+S: 0 0 -10000 -10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 2
+S: 0 0 -10000 -10000      0 -10000  10000
+S: 0 6 -10000 -10000      0 -10000  10000
+
+V-tail mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+M: 2
+S: 0 2   7000   7000      0 -10000  10000
+S: 0 1  -8000  -8000      0 -10000  10000
+
+M: 2
+S: 0 2   7000   7000      0 -10000  10000
+S: 0 1   8000   8000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+S: 0 3      0  20000 -10000 -10000  10000
+
+M: 1
+S: 0 3      0  20000 -10000 -10000  10000
+
+Flaps mixer
+------------
+Flap servos are physically reversed.
+
+M: 1
+S: 0 4      0   5000 -10000 -10000  10000
+
+M: 1
+S: 0 4      0  -5000  10000 -10000  10000
+
@@ -35,6 +35,7 @@ px4_add_romfs_files(
 	AAERTWF.main.mix
 	AAVVTWFF.main.mix
 	AAVVTWFF_vtail.main.mix
+	AAVVTTFF_vtail.main.mix
 	AERT.main.mix
 	AETRFG.main.mix
 	babyshark.main.mix
@@ -95,4 +96,8 @@ px4_add_romfs_files(
 	vtol_delta.aux.mix
 	vtol_tailsitter_duo.main.mix
 	wingwing.main.mix
+	#ETHZ ASL Custom Mixers
+	asl_easyglider.main.mix
+	asl_sensesoar2.main.mix
+	asl_techpod.main.mix
 )
@@ -0,0 +1,70 @@
+ASL EasyGlider mixer
+=============================
+
+Documentation: https://dev.px4.io/en/concept/mixing.html
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps).
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
+
+Right Aileron mixer
+---------------------------------
+
+This mixer assumes that the aileron servos are set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0   6000  10000      0 -10000  10000
+S: 0 4      0 -10000      0 -10000  10000
+
+Elevator mixer
+------------
+Two scalers total (output, roll).
+
+This mixer assumes that the elevator servo is set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 1  -10000  -10000      0 -10000  10000
+
+Rudder mixer
+------------
+Two scalers total (output, yaw).
+
+This mixer assumes that the rudder servo is set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 1
+O:     -10000 -10000      0 -10000  10000
+S: 0 2  10000  10000      0 -10000  10000
+
+Left Aileron mixer
+---------------------------------
+
+This mixer assumes that the aileron servos are set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0 -10000  -6000      0 -10000  10000
+S: 0 4      0 -10000      0 -10000  10000
@@ -0,0 +1,61 @@
+ASL SenseSoar2 mixer for PX4IO
+=============================
+
+Documentation: https://dev.px4.io/en/concept/mixing.html
+
+Note1: CH2 AilR is down for pos PWM signal change, CH5 Ail L the other way around.
+Note2: The mixer defines a certain aileron differential, but the phyiscal differential
+       is stronger because of the servo mechanics (70% mixer diff -> ca. 60% physical).
+Note3: In the extras.txt on the sd-card, set pwm min/max such that the ailerons have
+       2.5cm upwards and 2.0cm downwards travel at FULL (u=+/- 1.0) mixer output. The
+       downwards travel with the mixer differential is then obviously less.
+
+=============================
+
+### Motor
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
+
+
+### Right aileron (with differential)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0  -7000 -10000      0 -10000  10000
+
+
+### Right ruddervator
+M: 2
+O:       6000   6000      0 -10000  10000
+S: 0 1  -6500  -6500      0 -10000  10000
+S: 0 2   7500   7500      0 -10000  10000
+
+
+### Left ruddervator
+M: 2
+O:       6000   6000      0 -10000  10000
+S: 0 1   6500   6500      0 -10000  10000
+S: 0 2   7500   7500      0 -10000  10000
+
+
+### Left aileron (with differential)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0 -10000  -7000      0 -10000  10000
+
+
+### Right flap
+#M: 1
+#O:      10000  10000      0 -10000   4000
+#S: 3 4  10000  10000      0 -10000  10000
+
+### Right flap
+M: 1
+O:      10000  10000      0 -10000   4000
+S: 0 4      0  20000 -10000 -10000  10000
+
+
+### Left flap
+M: 1
+O:      10000  10000      0  -4000  10000
+S: 0 4      0 -20000  10000 -10000  10000
@@ -0,0 +1,47 @@
+ASL Techpod mixer for PX4IO
+=============================
+
+Documentation: https://dev.px4.io/en/concept/mixing.html
+
+=============================
+
+### Motor
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
+
+
+### Right aileron (with differential)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0 -7500  -10000      0 -10000  10000
+
+
+### Elevator
+M: 1
+O:       6000   6000      0 -10000  10000
+S: 0 1   6000   6000      0  -6000   6000
+
+
+### Rudder
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 2  -6000  -6000      0  -6000   6000
+
+
+### Left aileron (with differential)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0 -10000  -7500      0 -10000  10000
+
+
+### Right flap
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0  -4000   4000
+
+
+### Left flap
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0  -4000   4000
@@ -159,8 +159,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10})
 		}),
 		initSPIBusExternal(SPI::Bus::SPI4, {
-			initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}),
 			initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}),
 		}),
 	}),

@@ -41,6 +41,7 @@ px4_add_board(
 		magnetometer # all available magnetometer drivers
 		optical_flow # all available optical flow drivers
 		osd
+		si7210
 		pca9685
 		pca9685_pwm_out
 		#power_monitor/ina226
@@ -159,8 +159,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10})
 		}),
 		initSPIBusExternal(SPI::Bus::SPI4, {
-			initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}),
 			initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}),
 		}),
 	}),

@@ -50,6 +50,7 @@ px4_add_board(
 		roboclaw
 		rpm
 		safety_button
+        si7210
 		telemetry # all available telemetry drivers
 		test_ppm
 		tone_alarm
@@ -66,6 +67,8 @@ px4_add_board(
 		events
 		flight_mode_manager
 		fw_att_control
+		fw_dyn_soar_control
+		fw_dyn_soar_estimator
 		fw_pos_control_l1
 		gyro_calibration
 		gyro_fft
@@ -86,10 +89,10 @@ px4_add_board(
 		sensors
 		sih
 		temperature_compensation
-		uuv_att_control
-		uuv_pos_control
+		#uuv_att_control
+		#uuv_pos_control
 		vmount
-		vtol_att_control
+		#vtol_att_control
 	SYSTEMCMDS
 		bl_update
 		dmesg
@@ -36,6 +36,8 @@ px4_add_board(
 		flight_mode_manager
 		fw_att_control
 		fw_pos_control_l1
+		fw_dyn_soar_control
+		fw_dyn_soar_estimator
 		gyro_calibration
 		gyro_fft
 		land_detector
@@ -57,8 +59,8 @@ px4_add_board(
 		#sih
 		simulator
 		temperature_compensation
-		uuv_att_control
-		uuv_pos_control
+		#uuv_att_control
+		#uuv_pos_control
 		vmount
 		vtol_att_control
 	SYSTEMCMDS
@@ -118,5 +120,5 @@ if(REPLAY_FILE)
 	message(STATUS "Building without lockstep for replay")
 	set(ENABLE_LOCKSTEP_SCHEDULER no)
 else()
-	set(ENABLE_LOCKSTEP_SCHEDULER yes)
+	set(ENABLE_LOCKSTEP_SCHEDULER no)
 endif()
@@ -34,6 +34,8 @@ px4_add_board(
 		events
 		flight_mode_manager
 		fw_att_control
+		fw_dyn_soar_control
+		fw_dyn_soar_estimator
 		fw_pos_control_l1
 		gyro_calibration
 		gyro_fft
@@ -36,6 +36,8 @@ px4_add_board(
 		flight_mode_manager
 		fw_att_control
 		fw_pos_control_l1
+		fw_dyn_soar_control
+		fw_dyn_soar_estimator
 		gyro_calibration
 		gyro_fft
 		land_detector
@@ -39,6 +39,8 @@ set(msg_files
 	actuator_controls.msg
 	actuator_outputs.msg
 	adc_report.msg
+	airflow_aoa.msg
+	airflow_slip.msg
 	airspeed.msg
 	airspeed_validated.msg
 	airspeed_wind.msg
@@ -98,6 +100,7 @@ set(msg_files
 	mount_orientation.msg
 	multirotor_motor_limits.msg
 	navigator_mission_item.msg
+    npfg_status.msg
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg
@@ -128,6 +131,7 @@ set(msg_files
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
+	sensor_hall.msg
 	sensor_gps.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
@@ -136,6 +140,12 @@ set(msg_files
 	sensor_preflight_mag.msg
 	sensor_selection.msg
 	sensors_status_imu.msg
+	soaring_controller_heartbeat.msg
+	soaring_controller_position_setpoint.msg
+	soaring_controller_position.msg
+	soaring_controller_status.msg
+	soaring_controller_wind.msg
+	soaring_estimator_shear.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg
@@ -0,0 +1,3 @@
+uint64 timestamp		# time since system start (microseconds)
+float32 aoa_rad # angle of attack in radians
+bool valid # true if measurement is within sensor/calibration range, false otherwise
@@ -0,0 +1,3 @@
+uint64 timestamp		# time since system start (microseconds)
+float32 slip_rad # sideslip angle in radians
+bool valid # true if measurement is within sensor/calibration range, false otherwise
@@ -0,0 +1,16 @@
+uint64 timestamp                # time since system start (microseconds)
+
+uint8 wind_est_valid            # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid)
+float32 lat_accel               # resultant lateral acceleration reference [m/s^2]
+float32 lat_accel_ff            # lateral acceleration demand only for maintaining curvature [m/s^2]
+float32 bearing_feas            # bearing feasibility [0,1]
+float32 bearing_feas_on_track   # on-track bearing feasibility [0,1]
+float32 signed_track_error      # signed track error [m]
+float32 track_error_bound       # track error bound [m]
+float32 airspeed_ref            # airspeed reference [m/s]
+float32 bearing                 # bearing angle [rad]
+float32 heading_ref             # heading angle reference [rad]
+float32 min_ground_speed_ref    # minimum forward ground speed reference [m/s]
+float32 adapted_period          # adapted period (if auto-tuning enabled) [s]
+float32 p_gain                  # controller proportional gain [rad/s]
+float32 time_const              # controller time constant [s]
@@ -7,3 +7,4 @@ bool velocity
 bool acceleration
 bool attitude
 bool body_rate
+bool actuator
@@ -0,0 +1,7 @@
+uint64 timestamp			# time since system start (microseconds)
+
+int8 instance	            # sensor instance
+
+float32 mag_t	            # magnetic field measurement in Tesla
+
+float32 temp_c	            # temperature measurement in deg C
@@ -0,0 +1,5 @@
+# SOARING CONTROLLER STATE, SERVES AS A HEARTBEAT IF CONTROLLER IS STILL PUBLISHING
+
+uint64 timestamp			# time since system start (microseconds)
+
+uint64 heartbeat            # time since system start (microseconds) of the last run of soaring controller
@@ -0,0 +1,8 @@
+# SOARING CONTROLLER POSITION IN ENU SOARING FRAME
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32[3] pos                            # POSITION VECTOR IN SOARING ENU FRAME
+float32[3] vel                            # VELOCITY VECTOR IN SOARING ENU FRAME
+float32[3] acc                            # ACCELERATION VECTOR IN SOARING ENU FRAME
+float32[4] att                            # UNIT QUATERNION DESCRIBING BODY FRAME POSE TO ENU
@@ -0,0 +1,12 @@
+# SOARING CONTROLLER POSITION SETPOINT
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32[3] pos                             # position in ENU frame
+float32[3] vel                             # velocity in ENU frame
+float32[3] acc                             # acceleration in ENU frame
+float32[3] f_command                       # force command inside controller
+float32[3] m_command                       # moment command inside controller
+float32[3] w_err                           # rotation vector to target pose
+
+
@@ -0,0 +1,6 @@
+# SOARING CONTROLLER STATE, SERVES AS FLAG IF CONTROLLER IS STILL PUBLISHING
+
+uint64 timestamp			# time since system start (microseconds)
+
+bool soaring_controller_running     # if true, the soaring controller is expected to publish
+bool timeout_detected               # if true, the soaring controller has crashed
@@ -0,0 +1,11 @@
+# SOARING CONTROLLER WIND ESTIMATE, USED FOR INDI CONTROL
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32[3] wind_estimate                            # WIND ESTIMATE IN ENU FRAME
+float32[3] wind_estimate_filtered                   # LP-FILTERED WIND ESTIMATE IN ENU FRAME, USED BY INDI CONTROLLER
+float32[3] position                                 # position of the current estimate in the soaring frame
+float32    airspeed                                 # current airspeed
+
+bool valid                                          # tell, if estimate is valid for shear estimator
+bool lock_params                                    # tell, if the shear estimator shall lock shear param values for soaring
@@ -0,0 +1,30 @@
+# SOARING ESTIMATOR WIND ESTIMATE, USED FOR SELECTING THE CORRECT TRAJECTORY
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32 vx                  # maximum wind in x-direction
+float32 vy                  # maximum wind in y-direction
+float32 bx                  # wind offset in x-direction
+float32 by                  # wind offset in y-direction
+float32 h                   # vertical position of shear layer in soaring frame
+float32 a                   # shear strength
+
+float32 sigma_vx            # covariance of vx
+float32 sigma_vy            # covariance of vy
+float32 sigma_bx            # covariance of bx
+float32 sigma_by            # covariance of by
+float32 sigma_h             # covariance of h
+float32 sigma_a             # covariance of a
+
+float32 coeff_0             # offset vertical wind
+float32 coeff_1             # linear coeff vertical wind
+float32 coeff_2             # quadratic coeff vertical wind
+
+float32 v_max               # discrete value of v_max (shear velocity), identifier for appropriate trajectrory
+float32 alpha               # discrete value of alpha (shear strength), identifier for appropriate trajectrory
+float32 h_ref               # discrete value of h_ref (shear location)
+float32 psi                 # discrete value of shear heading
+float32 aspd                # airspeed identifier for appropriate trajectrory
+
+bool soaring_feasible       # plausibility check
+uint64 reset_counter        # filter reset counter
@@ -342,6 +342,14 @@ rtps:
    id: 158
  - msg: estimator_event_flags
    id: 159
+  - msg: sensor_hall
+    id: 160
+  - msg: airflow_aoa
+    id: 161
+  - msg: airflow_slip
+    id: 162
+  - msg: npfg_status
+    id: 163
  ########## multi topics: begin ##########
  - msg: actuator_controls_0
    id: 170
@@ -85,11 +85,29 @@ ExternalProject_Add(jsbsim_bridge
 	BUILD_ALWAYS 1
 )

+px4_add_git_submodule(TARGET git_rotors PATH "${PX4_SOURCE_DIR}/Tools/rotors_simulator")
+ExternalProject_Add(rotors_simulator
+	SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/rotors_simulator/rotors_gazebo_plugins
+	CMAKE_ARGS
+		-DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+		-DNO_ROS=true
+	BINARY_DIR ${PX4_BINARY_DIR}/build_rotors
+	INSTALL_COMMAND ""
+	DEPENDS
+		git_rotors
+	USES_TERMINAL_CONFIGURE true
+	USES_TERMINAL_BUILD true
+	EXCLUDE_FROM_ALL true
+	BUILD_ALWAYS 1
+	BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j1
+)
+
 # create targets for each viewer/model/debugger combination
 set(viewers
 	none
 	jmavsim
 	gazebo
+	rotors
 )

 set(debuggers
@@ -102,6 +120,7 @@ set(debuggers

 set(models
 	none
+	believer
 	boat
 	cloudship
 	if750a
@@ -120,6 +139,7 @@ set(models
 	plane
 	plane_cam
 	plane_catapult
+	plane_dynamicsoaring
 	plane_lidar
 	r1_rover
 	rover
@@ -178,6 +198,8 @@ foreach(viewer ${viewers})
 						add_dependencies(${_targ_name} px4 sitl_gazebo)
 					elseif(viewer STREQUAL "jmavsim")
 						add_dependencies(${_targ_name} px4 git_jmavsim)
+					elseif(viewer STREQUAL "rotors")
+						add_dependencies(${_targ_name} px4 rotors_simulator)
 					endif()
 				else()
 					if(viewer STREQUAL "gazebo")
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 drv_hall.h
+ *
+ * Hall Effect Magnetic Sensor driver interface.
+ */
+
+#ifndef _DRV_HALL_H
+#define _DRV_HALL_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define HALL_BASE_DEVICE_PATH	"/dev/hall"
+#define HALL0_DEVICE_PATH		"/dev/hall0"
+#define HALL1_DEVICE_PATH		"/dev/hall1"
+#define HALL2_DEVICE_PATH		"/dev/hall2"
+#define HALL3_DEVICE_PATH		"/dev/hall3"
+
+
+#endif /* _DRV_HALL_H */
@@ -177,6 +177,8 @@
 #define DRV_DIST_DEVTYPE_SIM 0x9a
 #define DRV_DIST_DEVTYPE_SRF05 0x9b

+#define DRV_HALL_DEVTYPE_SI7210       0x9c
+
 #define DRV_DEVTYPE_UNUSED		0xff

 #endif /* _DRV_SENSOR_H */
@@ -105,14 +105,15 @@ ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
 	_px4_gyro(get_device_id(), rotation),
 	_px4_mag(get_device_id(), rotation)
 {
+	_debug_enabled = true;
 }

 ADIS16448::~ADIS16448()
 {
 	perf_free(_reset_perf);
-	perf_free(_perf_crc_bad);
 	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
+	perf_free(_perf_crc_bad);
 }

 int ADIS16448::init()
@@ -147,9 +148,9 @@ void ADIS16448::print_status()
 	I2CSPIDriverBase::print_status();

 	perf_print_counter(_reset_perf);
-	perf_print_counter(_perf_crc_bad);
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_perf_crc_bad);
 }

 int ADIS16448::probe()
@@ -159,25 +160,24 @@ int ADIS16448::probe()
 		PX4_WARN("Power-On Start-Up Time is 205 ms");
 	}

-	const uint16_t PROD_ID = RegisterRead(Register::PROD_ID);
+	for (int attempt = 0; attempt < 3; attempt++) {
+		const uint16_t PROD_ID = RegisterReadVerified(Register::PROD_ID);

-	if (PROD_ID != Product_identification) {
-		DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID);
-		return PX4_ERROR;
+		if (PROD_ID == Product_identification) {
+			const uint16_t SERIAL_NUM = RegisterReadVerified(Register::SERIAL_NUM);
+			const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1);
+			const uint16_t LOT_ID2 = RegisterReadVerified(Register::LOT_ID2);
+
+			PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2);
+
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID);
+		}
 	}

-	const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM);
-	const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1);
-	const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2);
-
-	PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2);
-
-	// Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection)
-	if (LOT_ID1 == 0x1824) {
-		_check_crc = true;
-	}
-
-	return PX4_OK;
+	return PX4_ERROR;
 }

 void ADIS16448::RunImpl()
@@ -198,7 +198,7 @@ void ADIS16448::RunImpl()
 	case STATE::WAIT_FOR_RESET:

 		if (_self_test_passed) {
-			if ((RegisterRead(Register::PROD_ID) == Product_identification)) {
+			if ((RegisterReadVerified(Register::PROD_ID) == Product_identification)) {
 				// if reset succeeded then configure
 				_state = STATE::CONFIGURE;
 				ScheduleNow();
@@ -208,24 +208,44 @@ void ADIS16448::RunImpl()
 				if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
 					PX4_DEBUG("Reset failed, retrying");
 					_state = STATE::RESET;
-					ScheduleDelayed(100_ms);
+					ScheduleDelayed(1000_ms);

 				} else {
-					PX4_DEBUG("Reset not complete, check again in 10 ms");
-					ScheduleDelayed(10_ms);
+					PX4_DEBUG("Reset not complete, check again in 100 ms");
+					ScheduleDelayed(100_ms);
 				}
 			}

 		} else {
 			RegisterWrite(Register::MSC_CTRL, MSC_CTRL_BIT::Internal_self_test);
 			_state = STATE::SELF_TEST_CHECK;
-			ScheduleDelayed(45_ms); // Automatic Self-Test Time 45 ms
+			ScheduleDelayed(90_ms); // Automatic Self-Test Time > 45 ms
 		}

 		break;

 	case STATE::SELF_TEST_CHECK: {
-			const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);
+			const uint16_t MSC_CTRL = RegisterReadVerified(Register::MSC_CTRL);
+
+			if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) {
+				// self test not finished, check again
+				if (hrt_elapsed_time(&_reset_timestamp) < 1000_ms) {
+					ScheduleDelayed(45_ms);
+					PX4_DEBUG("self test not complete, check again in 45 ms");
+					return;
+
+				} else {
+					// still not cleared, fail self test
+					_self_test_passed = false;
+					_state = STATE::RESET;
+					ScheduleDelayed(1000_ms);
+					return;
+				}
+			}
+
+			bool test_passed = true;
+
+			const uint16_t DIAG_STAT = RegisterReadVerified(Register::DIAG_STAT);

 			if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) {
 				PX4_ERR("self test failed");
@@ -248,6 +268,7 @@ void ADIS16448::RunImpl()

 				if (gyro_x_fail || gyro_y_fail || gyro_z_fail) {
 					PX4_ERR("gyroscope self-test failure");
+					test_passed = false;
 				}

 				// Accelerometer
@@ -257,18 +278,20 @@ void ADIS16448::RunImpl()

 				if (accel_x_fail || accel_y_fail || accel_z_fail) {
 					PX4_ERR("accelerometer self-test failure");
+					test_passed = false;
 				}
+			}

-				_self_test_passed = false;
-				_state = STATE::RESET;
-				ScheduleDelayed(1000_ms);
-
-			} else {
+			if (test_passed) {
 				PX4_DEBUG("self test passed");
 				_self_test_passed = true;
-				_state = STATE::RESET;
-				ScheduleNow();
+
+			} else {
+				_self_test_passed = false;
 			}
+
+			_state = STATE::RESET;
+			ScheduleDelayed(10_ms);
 		}

 		break;
@@ -364,18 +387,38 @@ void ADIS16448::RunImpl()
 					_px4_accel.set_temperature(temperature);
 					_px4_gyro.set_temperature(temperature);

+					bool imu_updated = false;
+
 					// sensor's frame is +x forward, +y left, +z up
 					//  flip y & z to publish right handed with z down (x forward, y right, z down)
 					const int16_t accel_x = buffer.XACCL_OUT;
 					const int16_t accel_y = (buffer.YACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.YACCL_OUT;
 					const int16_t accel_z = (buffer.ZACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZACCL_OUT;

+					if (accel_x != _accel_prev[0] || accel_y != _accel_prev[1] || accel_z != _accel_prev[2]) {
+						imu_updated = true;
+
+						_accel_prev[0] = accel_x;
+						_accel_prev[1] = accel_y;
+						_accel_prev[2] = accel_z;
+					}
+
 					const int16_t gyro_x = buffer.XGYRO_OUT;
 					const int16_t gyro_y = (buffer.YGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.YGYRO_OUT;
 					const int16_t gyro_z = (buffer.ZGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZGYRO_OUT;

-					_px4_accel.update(now, accel_x, accel_y, accel_z);
-					_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
+					if (gyro_x != _gyro_prev[0] || gyro_y != _gyro_prev[1] || gyro_z != _gyro_prev[2]) {
+						imu_updated = true;
+
+						_gyro_prev[0] = gyro_x;
+						_gyro_prev[1] = gyro_y;
+						_gyro_prev[2] = gyro_z;
+					}
+
+					if (imu_updated) {
+						_px4_accel.update(now, accel_x, accel_y, accel_z);
+						_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
+					}

 					// DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT
 					if (buffer.DIAG_STAT & DIAG_STAT_BIT::New_data_xMAGN_OUT_BARO_OUT) {
@@ -435,6 +478,27 @@ void ADIS16448::RunImpl()

 bool ADIS16448::Configure()
 {
+	const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1);
+
+	// Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection)
+	if (LOT_ID1 == 0x1824) {
+		_check_crc = true;
+
+		if (_perf_crc_bad == nullptr) {
+			_perf_crc_bad = perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad");
+		}
+
+	} else {
+		_check_crc = false;
+
+		for (auto &reg_cfg : _register_cfg) {
+			if (reg_cfg.reg == Register::MSC_CTRL) {
+				reg_cfg.set_bits = reg_cfg.set_bits & ~MSC_CTRL_BIT::CRC16_for_burst;
+				break;
+			}
+		}
+	}
+
 	// first set and clear all configured register bits
 	for (const auto &reg_cfg : _register_cfg) {
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
@@ -540,7 +604,7 @@ bool ADIS16448::RegisterCheck(const register_config_t &reg_cfg)
 {
 	bool success = true;

-	const uint16_t reg_value = RegisterRead(reg_cfg.reg);
+	const uint16_t reg_value = RegisterReadVerified(reg_cfg.reg);

 	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
 		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
@@ -565,10 +629,27 @@ uint16_t ADIS16448::RegisterRead(Register reg)
 	transferhword(cmd, nullptr, 1);
 	px4_udelay(SPI_STALL_PERIOD);
 	transferhword(nullptr, cmd, 1);
+	px4_udelay(SPI_STALL_PERIOD);

 	return cmd[0];
 }

+uint16_t ADIS16448::RegisterReadVerified(Register reg)
+{
+	// 3 attempts
+	for (int i = 0; i < 3; i++) {
+		uint16_t read1 = RegisterRead(reg);
+		uint16_t read2 = RegisterRead(reg);
+
+		if (read1 == read2) {
+			return read1;
+		}
+	}
+
+	// failed
+	return 0;
+}
+
 void ADIS16448::RegisterWrite(Register reg, uint16_t value)
 {
 	set_frequency(SPI_SPEED);
@@ -580,11 +661,12 @@ void ADIS16448::RegisterWrite(Register reg, uint16_t value)
 	transferhword(cmd, nullptr, 1);
 	px4_udelay(SPI_STALL_PERIOD);
 	transferhword(cmd + 1, nullptr, 1);
+	px4_udelay(SPI_STALL_PERIOD);
 }

 void ADIS16448::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits)
 {
-	const uint16_t orig_val = RegisterRead(reg);
+	const uint16_t orig_val = RegisterReadVerified(reg);

 	uint16_t val = (orig_val & ~clearbits) | setbits;

@@ -94,6 +94,7 @@ private:
 	bool RegisterCheck(const register_config_t &reg_cfg);

 	uint16_t RegisterRead(Register reg);
+	uint16_t RegisterReadVerified(Register reg);
 	void RegisterWrite(Register reg, uint16_t value);
 	void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits);

@@ -105,9 +106,9 @@ private:
 	PX4Magnetometer _px4_mag;

 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+	perf_counter_t _perf_crc_bad{nullptr};

 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
@@ -118,6 +119,9 @@ private:

 	bool _self_test_passed{false};

+	int16_t _accel_prev[3] {};
+	int16_t _gyro_prev[3] {};
+
 	enum class STATE : uint8_t {
 		RESET,
 		WAIT_FOR_RESET,
@@ -127,12 +131,11 @@ private:
 	} _state{STATE::RESET};

 	uint8_t _checked_register{0};
-	static constexpr uint8_t size_register_cfg{4};
+	static constexpr uint8_t size_register_cfg{3};
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register               | Set bits, Clear bits
 		{ Register::MSC_CTRL,     MSC_CTRL_BIT::CRC16_for_burst, 0 },
 		{ Register::SMPL_PRD,     SMPL_PRD_BIT::internal_sampling_clock, SMPL_PRD_BIT::decimation_rate },
-		{ Register::SENS_AVG,     SENS_AVG_BIT::Measurement_range_1000_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B },
-		{ Register::GPIO_CTRL,    GPIO_CTRL_BIT::GPIO2_DIRECTION | GPIO_CTRL_BIT::GPIO1_DIRECTION, 0},
+		{ Register::SENS_AVG,     SENS_AVG_BIT::Measurement_range_1000_set | SENS_AVG_BIT::Filter_Size_Variable_B_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B_clear },
 	};
 };
@@ -65,7 +65,7 @@ namespace Analog_Devices_ADIS16448
 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000;       // 2 MHz SPI serial interface
 static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read

-static constexpr uint32_t SPI_STALL_PERIOD = 9; // 9 us Stall period between data
+static constexpr uint32_t SPI_STALL_PERIOD = 11; // 9 us Stall period between data

 static constexpr uint16_t DIR_WRITE = 0x80;

@@ -128,8 +128,8 @@ enum GLOB_CMD_BIT : uint16_t {

 // SMPL_PRD
 enum SMPL_PRD_BIT : uint16_t {
-	// [12:8] D, decimation rate setting, binomial,
-	decimation_rate = Bit12 | Bit11 | Bit10 | Bit9, // disable
+	// [12:8] D, decimation rate setting, binomial
+	decimation_rate = Bit12 | Bit11 | Bit10 | Bit9 | Bit8, // disable

 	internal_sampling_clock = Bit0, // 1 = internal sampling clock, 819.2 SPS
 };
@@ -141,8 +141,8 @@ enum SENS_AVG_BIT : uint16_t {
 	Measurement_range_1000_clear = Bit9 | Bit8,

 	// [2:0] Filter Size Variable B
-	Filter_Size_Variable_B = Bit2 | Bit1 | Bit0, // disable
-
+	Filter_Size_Variable_B_set   = Bit1,
+	Filter_Size_Variable_B_clear = Bit2 | Bit0,
 };

 // GPIO_CTRL
@@ -36,7 +36,7 @@ px4_add_module(
 	MAIN adis16448
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
-		#-DDEBUG_BUILD
+		-DDEBUG_BUILD
 	SRCS
 		ADIS16448.cpp
 		ADIS16448.hpp
@@ -42,3 +42,15 @@
 * @value 1 Enabled
  */
 PARAM_DEFINE_INT32(SENS_EN_ADIS164X, 0);
+
+/**
+ * Analog Devices ADIS16448 IMU Orientation(external SPI)
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 101
+ * @group Sensors
+ * @value 0 ROTATION_NONE
+ * @value 4 ROTATION_YAW_180
+  */
+PARAM_DEFINE_INT32(SENS_OR_ADIS164X, 0);
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__si7210
+	MAIN si7210
+	COMPILE_FLAGS
+	SRCS
+		si7210_main.cpp
+		si7210.cpp
+	DEPENDS
+		drivers__vane
+		mathlib
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
@@ -0,0 +1,313 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 si7210.cpp
+ * @author: Amir Melzer  <amir.melzer@mavt.ethz.ch>
+ *
+ * Driver for the SI7210 connected via I2C.
+ */
+
+#include "si7210.h"
+
+using namespace time_literals;
+
+/* Get registers value */
+int
+SI7210::get_regs(uint8_t ptr, uint8_t *regs)
+{
+	uint8_t data;
+
+	if (OK != transfer(&ptr, 1, &data, 1)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	*regs  = data;
+
+	return OK;
+}
+
+/* Set registers value */
+int
+SI7210::set_regs(uint8_t ptr, uint8_t value)
+{
+	uint8_t data[2];
+
+	data[0] = ptr;
+	data[1] = value;
+
+	if (OK != transfer(&data[0], 2, nullptr, 0)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	/* read back the reg and verify */
+
+	if (OK != transfer(&ptr, 1, &data[1], 1)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	if (data[1] != value) {
+		//return -EIO;
+	}
+
+	return OK;
+}
+
+/* Get measurement value */
+int
+SI7210::get_measurement(uint8_t ptr, uint16_t *value)
+{
+	uint8_t data[2];
+
+	if (OK != transfer(&ptr, 1, &data[0], 2)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	*value = (uint16_t)((data[1] & 0xff) + ((data[0] & 0xff) << 8));
+
+	return OK;
+}
+
+/* Get sensor data */
+int
+SI7210::get_sensor_data(uint8_t otpAddr, int8_t *data)
+{
+	uint8_t optCtrl;
+	uint8_t reg;
+
+	if (OK != get_regs(OTP_CTRL, &optCtrl)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	if (OK != (optCtrl & OTP_CTRL_OPT_BUSY_MASK)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	reg = otpAddr;
+
+	if (OK != set_regs(OTP_ADDR, reg)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	reg = OTP_CTRL_OPT_READ_EN_MASK;
+
+	if (OK != set_regs(OTP_CTRL, reg)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	if (OK != get_regs(OTP_DATA, (uint8_t *) data)) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+
+int SI7210::write_command(uint16_t command)
+{
+	uint8_t cmd[2];
+	cmd[0] = static_cast<uint8_t>(command >> 8);
+	cmd[1] = static_cast<uint8_t>(command & 0xff);
+	return transfer(&cmd[0], 2, nullptr, 0);
+}
+
+bool
+SI7210::init_si7210()
+{
+	return configure() == 0;
+}
+
+int
+SI7210::configure()
+{
+	uint8_t reg;
+	int ret = get_regs(HREVID, &reg);
+	bool config_failed = false;
+
+	if (((reg & 0xf0) >> 4) != IDCHIPID) {
+		config_failed = true;
+	}
+
+	if ((reg & 0x0f) != REVID) {
+		config_failed = true;
+	}
+
+	if ((ret != PX4_OK) || config_failed) {
+		perf_count(_comms_errors);
+		DEVICE_DEBUG("config failed");
+		_state = State::RequireConfig;
+		return ret;
+	}
+
+	_state = State::Configuring;
+
+	return ret;
+}
+
+void SI7210::start()
+{
+	// make sure to wait 10ms after configuring the measurement mode
+	ScheduleDelayed(10_ms);
+}
+
+int SI7210::collect()
+{
+	perf_begin(_sample_perf);
+
+	_collect_phase = false;
+
+	uint8_t     reg;
+
+	uint16_t    magRawData;
+	uint16_t    tempRawData;
+
+	int8_t 		otpTempOffsetData;
+	int8_t 		otpTempGainData;
+
+	/* capture the magnetic field measurements */
+	reg = ARAUTOINC_ARAUTOINC_MASK;
+
+	if (OK != set_regs(ARAUTOINC, reg)) {
+		return -EIO;
+	}
+
+	reg = DSPSIGSEL_MAG_VAL_SEL;
+
+	if (OK != set_regs(DSPSIGSEL, reg)) {
+		return -EIO;
+	}
+
+	reg = POWER_CTRL_ONEBURST_MASK;
+
+	if (OK != set_regs(POWER_CTRL, reg)) {
+		return -EIO;
+	}
+
+	if (OK != get_measurement(DSPSIGM, &magRawData)) {
+		return -EIO;
+	}
+
+	/* capture the temperate measurements */
+	reg = DSPSIGSEL_TEMP_VAL_SEL;
+
+	if (OK != set_regs(DSPSIGSEL, reg)) {
+		return -EIO;
+	}
+
+	reg = POWER_CTRL_ONEBURST_MASK;
+
+	if (OK != set_regs(POWER_CTRL, reg)) {
+		return -EIO;
+	}
+
+	if (OK != get_measurement(DSPSIGM, &tempRawData)) {
+		return -EIO;
+	}
+
+	if (OK != get_sensor_data(OTP_TEMP_OFFSET, &otpTempOffsetData)) {
+		return -EIO;
+	}
+
+	if (OK != get_sensor_data(OTP_TEMP_GAIN, &otpTempGainData)) {
+		return -EIO;
+	}
+
+	float _tempOffset = (float)otpTempOffsetData / 16;
+	float _tempGain = 1 + (float)otpTempGainData / 2048;
+
+	if (OK == ((magRawData & 0x8000) && (tempRawData & 0x8000))) {
+		return -EIO;
+	}
+
+	sensor_hall_s report{};
+
+	/* generate a new report */
+	report.timestamp = hrt_absolute_time();
+	report.instance = _i2c_address;
+	report.mag_t = (float)(magRawData - MAG_BIAS) * MAG_CONV;
+	report.temp_c = (float)((tempRawData & ~0x8000) >> 3);
+	report.temp_c = _tempGain * (TEMP_CONV_A2 * report.temp_c * report.temp_c + TEMP_CONV_A1 * report.temp_c + TEMP_CONV_A0
+				     + VDD_CONV *
+				     VDD) +
+			_tempOffset;
+
+	_vane_pub.publish(report);
+
+	perf_end(_sample_perf);
+
+	return PX4_OK;
+}
+
+void
+SI7210::RunImpl()
+{
+	switch (_state) {
+	case State::RequireConfig:
+		if (configure() == PX4_OK) {
+			ScheduleDelayed(10_ms);
+
+		} else {
+			// periodically retry to configure
+			ScheduleDelayed(300_ms);
+		}
+
+		break;
+
+	case State::Configuring:
+		_state = State::Running;
+
+		ScheduleDelayed(10_ms);
+		break;
+
+	case State::Running:
+		int ret = collect();
+
+		if (ret != 0 && ret != -EAGAIN) {
+			_sensor_ok = false;
+			DEVICE_DEBUG("measure error");
+			_state = State::RequireConfig;
+		}
+
+		ScheduleDelayed(SI7210_CONVERSION_INTERVAL);
+		break;
+	}
+}
@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 si7210.h
+ * @author: Amir Melzer  <amir.melzer@mavt.ethz.ch>
+ *
+ * Driver for the SI7210 connected via I2C.
+ */
+
+#ifndef SI7210_HPP_
+#define SI7210_HPP_
+
+#include <drivers/drv_hall.h>
+#include <drivers/vane/vane.h>
+#include <math.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/topics/sensor_hall.h>
+
+#define I2C_ADDRESS_SI7210   0x30    /* Default SI7210 I2C address */
+
+#define SI7210_MAX_DATA_RATE     50
+
+#define SI7210_BUS_SPEED                     1000*100
+
+/* This value is set based on Max output data rate value */
+#define SI7210_CONVERSION_INTERVAL      (1000000 / 100) /* microseconds */
+
+#define IDCHIPID 					   0x01
+#define REVID						   0x04
+
+#define ARAUTOINC_ARAUTOINC_MASK       0x01
+#define OTP_CTRL_OPT_BUSY_MASK         0x01
+#define OTP_CTRL_OPT_READ_EN_MASK      0x02
+#define POWER_CTRL_SLEEP_MASK          0x01
+#define POWER_CTRL_STOP_MASK           0x02
+#define POWER_CTRL_ONEBURST_MASK       0x04
+#define POWER_CTRL_USESTORE_MASK       0x08
+#define POWER_CTRL_MEAS_MASK           0x80
+#define DSPSIGSEL_MAG_VAL_SEL          0
+#define DSPSIGSEL_TEMP_VAL_SEL         1
+
+/** I2C registers for Si72xx */
+#define OTP_TEMP_OFFSET  0x1D
+#define OTP_TEMP_GAIN    0x1E
+#define HREVID           0xC0
+#define DSPSIGM          0xC1
+#define DSPSIGL          0xC2
+#define DSPSIGSEL        0xC3
+#define POWER_CTRL       0xC4
+#define ARAUTOINC        0xC5
+#define CTRL1            0xC6
+#define CTRL2            0xC7
+#define SLTIME           0xC8
+#define CTRL3            0xC9
+#define A0               0xCA
+#define A1               0xCB
+#define A2               0xCC
+#define CTRL4            0xCD
+#define A3               0xCE
+#define A4               0xCF
+#define A5               0xD0
+#define OTP_ADDR         0xE1
+#define OTP_DATA         0xE2
+#define OTP_CTRL         0xE3
+#define TM_FG            0xE4
+
+/** temperature conv for Si72xx */
+#define TEMP_CONV_A2     -3.83e-6F
+#define TEMP_CONV_A1     0.16094F
+#define TEMP_CONV_A0     -279.80F
+
+#define VDD              3.30F
+#define VDD_CONV         -0.222F
+
+/** Magnetic conv for Si72xx */
+#define MAG_BIAS         0xC000
+#define MAG_CONV         0.00125F
+
+class SI7210 : public Vane, public I2CSPIDriver<SI7210>
+{
+public:
+	SI7210(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_SI7210,
+	       bool keep_retrying = false) :
+		Vane(bus, bus_frequency, address, SI7210_CONVERSION_INTERVAL),
+		I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address),
+		_keep_retrying{keep_retrying},
+		_i2c_address{address}
+	{
+	}
+
+	virtual ~SI7210() = default;
+
+	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+					     int runtime_instance);
+	static void print_usage();
+
+	void	RunImpl();
+
+	void start();
+
+private:
+	enum class State {
+		RequireConfig,
+		Configuring,
+		Running
+	};
+
+	int	measure() override { return 0; }
+	int	collect() override;
+	int	configure();
+
+	bool init_si7210();
+
+	/**
+	 * Get registers values
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int				get_regs(uint8_t ptr, uint8_t *regs);
+
+	/**
+	 * Set registers values
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int				set_regs(uint8_t ptr, uint8_t value);
+
+	/**
+	 * Get measurement values
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int				get_measurement(uint8_t ptr, uint16_t *value);
+
+	/**
+	 * Get si7210 data
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int				get_sensor_data(uint8_t otpAddr, int8_t *data);
+
+	/**
+	 * Write a command in Sensirion specific logic
+	 */
+	int write_command(uint16_t command);
+
+	uint16_t _scale{0};
+	const bool _keep_retrying;
+	State _state{State::RequireConfig};
+	int _i2c_address;
+};
+
+#endif /* SI7210_HPP_ */
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 si7210_main.cpp
+ * Driver for the SI7210 connected via I2C.
+ *
+ * Author: Amir Melzer  <amir.melzer@mavt.ethz.ch>
+ */
+
+
+#include "si7210.h"
+
+extern "C" __EXPORT int si7210_main(int argc, char *argv[]);
+
+I2CSPIDriverBase *SI7210::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				      int runtime_instance)
+{
+	SI7210 *instance = new SI7210(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address,
+				      cli.keep_running);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (instance->init() != PX4_OK) {
+		delete instance;
+		return nullptr;
+	}
+
+	instance->start();
+	return instance;
+}
+
+
+void
+SI7210::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("si7210", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(I2C_ADDRESS_SI7210);
+	PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG();
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+int
+si7210_main(int argc, char *argv[])
+{
+	using ThisDriver = SI7210;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 100000;
+	cli.i2c_address = I2C_ADDRESS_SI7210;
+	cli.support_keep_running = true;
+
+	int ch;
+
+	while ((ch = cli.getopt(argc, argv, "")) != EOF) {}
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_HALL_DEVTYPE_SI7210);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -56,6 +56,7 @@ add_subdirectory(mathlib)
 add_subdirectory(mixer)
 add_subdirectory(mixer_module)
 add_subdirectory(motion_planning)
+add_subdirectory(npfg)
 add_subdirectory(output_limit)
 add_subdirectory(perf)
 add_subdirectory(pid)
@@ -40,3 +40,4 @@ add_subdirectory(led)
 add_subdirectory(magnetometer)
 add_subdirectory(rangefinder)
 add_subdirectory(smbus)
+add_subdirectory(vane)
@@ -0,0 +1,35 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+px4_add_library(drivers__vane vane.cpp)
+target_link_libraries(drivers__vane PRIVATE drivers__device)
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 airspeed.cpp
+ * @author Simon Wilks <simon@px4.io>
+ * @author Lorenz Meier <lorenz@px4.io>
+ *
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <drivers/device/device.h>
+
+#include <drivers/device/i2c.h>
+
+#include <systemlib/err.h>
+#include <parameters/param.h>
+#include <perf/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_hall.h>
+
+#include <drivers/vane/vane.h>
+
+Vane::Vane(int bus, int bus_frequency, int address, unsigned conversion_interval) :
+	I2C(0, "Vane", bus, address, bus_frequency),
+	_sensor_ok(false),
+	_measure_interval(conversion_interval),
+	_collect_phase(false),
+	_vane_orb_class_instance(-1),
+	_class_instance(-1),
+	_conversion_interval(conversion_interval),
+	_sample_perf(perf_alloc(PC_ELAPSED, "vane_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "vane_com_err"))
+{
+}
+
+Vane::~Vane()
+{
+	if (_class_instance != -1) {
+		unregister_class_devname(HALL_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+Vane::init()
+{
+	/* do I2C init (and probe) first */
+	if (I2C::init() != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	/* register alternate interfaces if we have to */
+	_class_instance = register_class_devname(HALL_BASE_DEVICE_PATH);
+
+	/* advertise sensor topic, measure manually to initialize valid report */
+	measure();
+
+	return PX4_OK;
+}
+
+int
+Vane::probe()
+{
+	/* on initial power up the device may need more than one retry
+	   for detection. Once it is running the number of retries can
+	   be reduced
+	*/
+	_retries = 4;
+	int ret = measure();
+
+	// drop back to 2 retries once initialised
+	_retries = 2;
+	return ret;
+}
+
+int
+Vane::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <string.h>
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hall.h>
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <perf/perf_counter.h>
+#include <uORB/topics/sensor_hall.h>
+#include <uORB/PublicationMulti.hpp>
+
+class __EXPORT Vane : public device::I2C
+{
+public:
+	Vane(int bus, int bus_frequency, int address, unsigned conversion_interval);
+	virtual ~Vane();
+
+	int	init() override;
+
+	int	ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
+
+private:
+	Vane(const Vane &) = delete;
+	Vane &operator=(const Vane &) = delete;
+
+protected:
+	int	probe() override;
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	virtual int	measure() = 0;
+	virtual int	collect() = 0;
+
+	bool			_sensor_ok;
+	int			_measure_interval;
+	bool			_collect_phase;
+
+	uORB::PublicationMulti<sensor_hall_s>	_vane_pub{ORB_ID(sensor_hall)};
+
+	int			_vane_orb_class_instance;
+
+	int			_class_instance;
+
+	unsigned		_conversion_interval;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+};
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2018-2020 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(npfg
+	npfg.cpp
+	npfg.hpp
+)
+
+add_dependencies(npfg git_ecl)
+target_link_libraries(npfg PRIVATE ecl_geo)
@@ -0,0 +1,654 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2021 Autonomous Systems Lab, ETH Zurich. 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 npfg.cpp
+ * Implementation of a lateral-directional nonlinear path following guidance
+ * law with excess wind handling.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "npfg.hpp"
+#include <lib/ecl/geo/geo.h>
+#include <px4_platform_common/defines.h>
+#include <float.h>
+
+using matrix::Vector2d;
+using matrix::Vector2f;
+
+void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector2f &unit_path_tangent,
+		    float signed_track_error, const float path_curvature, bool in_front_of_wp /*= false*/,
+		    const Vector2f &bearing_vec_to_point /*= Vector2f{0.0f, 0.0f}*/)
+{
+	const float ground_speed = ground_vel.norm();
+
+	Vector2f air_vel = ground_vel - wind_vel;
+	const float airspeed = air_vel.norm();
+
+	const float wind_speed = wind_vel.norm();
+
+	float track_error = fabsf(signed_track_error);
+
+	// track error bound is dynamic depending on ground speed
+	track_error_bound_ = trackErrorBound(ground_speed, time_const_);
+	float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
+
+	// look ahead angle based purely on track proximity
+	float look_ahead_ang = lookAheadAngle(normalized_track_error);
+
+	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
+
+	// on-track wind triangle projections
+	float wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
+	float wind_dot_upt = wind_vel.dot(unit_path_tangent);
+
+	// calculate the bearing feasibility on the track at the current closest point
+	feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
+
+	// update control parameters considering upper and lower stability bounds (if enabled)
+	// must be called before trackErrorBound() as it updates time_const_
+	adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
+				      path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
+	p_gain_ = pGain(adapted_period_, damping_);
+	time_const_ = timeConst(adapted_period_, damping_);
+
+	// specific waypoint logic complications... handles case where we are following
+	// waypoints and are in front of the first of the segment.
+	// TODO: find a way to get this out of the main eval method!
+	if (in_front_of_wp && unit_path_tangent.dot(bearing_vec_) < unit_path_tangent.dot(bearing_vec_to_point)) {
+		// we are in front of the first waypoint and the bearing to the point is
+		// shallower than that to the line. reset path params to fly directly to
+		// the first waypoint.
+
+		// TODO: probably better to blend these instead of hard switching (could
+		// effect the adaptive tuning if we switch between these cases with wind
+		// gusts)
+
+		// pretend we are on track, recalculate necessary quantities
+
+		signed_track_error = 0.0f;
+		normalized_track_error = 0.0f;
+		unit_path_tangent = bearing_vec_to_point;
+
+		bearing_vec_ = bearing_vec_to_point;
+		look_ahead_ang = 0.0f;
+
+		wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
+		wind_dot_upt = wind_vel.dot(unit_path_tangent);
+		feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
+
+		adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
+					      path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
+		p_gain_ = pGain(adapted_period_, damping_);
+		time_const_ = timeConst(adapted_period_, damping_);
+	}
+
+	track_proximity_ = trackProximity(look_ahead_ang);
+
+	// wind triangle projections
+	const float wind_cross_bearing = cross2D(wind_vel, bearing_vec_);
+	const float wind_dot_bearing = wind_vel.dot(bearing_vec_);
+
+	// continuous representation of the bearing feasibility
+	feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed);
+
+	// we consider feasibility of both the current bearing as well as that on the track at the current closest point
+	const float feas_combined = feas_ * feas_on_track_;
+
+	min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_combined);
+
+	// reference air velocity with directional feedforward effect for following
+	// curvature in wind and magnitude incrementation depending on minimum ground
+	// speed violations and/or high wind conditions in general
+	air_vel_ref_ = refAirVelocity(wind_vel, bearing_vec_, wind_cross_bearing,
+				      wind_dot_bearing, wind_speed, min_ground_speed_ref_);
+	airspeed_ref_ = air_vel_ref_.norm();
+
+	// lateral acceleration demand based on heading error
+	const float lateral_accel = lateralAccel(air_vel, air_vel_ref_, airspeed);
+
+	// lateral acceleration needed to stay on curved track (assuming no heading error)
+	lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt,
+					   wind_cross_upt, airspeed, wind_speed, signed_track_error, path_curvature);
+
+	// total lateral acceleration to drive aircaft towards track as well as account
+	// for path curvature. The full effect of the feed-forward acceleration is smoothly
+	// ramped in as the vehicle approaches the track and is further smoothly
+	// zeroed out as the bearing becomes infeasible.
+	lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_;
+} // evaluate
+
+float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
+			const float track_error, const float path_curvature, const Vector2f &wind_vel,
+			const Vector2f &unit_path_tangent, const float feas_on_track) const
+{
+	float period = period_;
+	const float air_turn_rate = fabsf(path_curvature * airspeed);
+	const float wind_factor = windFactor(airspeed, wind_speed);
+
+	if (en_period_lb_) {
+		// lower bound for period not considering path curvature
+		const float period_lb_zero_curvature = periodLB(0.0f, wind_factor, feas_on_track) * PERIOD_SAFETY_FACTOR;
+
+		// lower bound for period *considering path curvature
+		float period_lb = periodLB(air_turn_rate, wind_factor, feas_on_track) * PERIOD_SAFETY_FACTOR;
+
+		// recalculate the time constant and track error bound considering the zero
+		// curvature, lower-bounded period and subsequently recalculate the normalized
+		// track error
+		const float time_const = timeConst(period_lb_zero_curvature, damping_);
+		const float track_error_bound = trackErrorBound(ground_speed, time_const);
+		const float normalized_track_error = normalizedTrackError(track_error, track_error_bound);
+
+		// calculate nominal track proximity with lower bounded time constant
+		// (only a numerical solution can find corresponding track proximity
+		// and adapted gains simultaneously)
+		const float look_ahead_ang = lookAheadAngle(normalized_track_error);
+		const float track_proximity = trackProximity(look_ahead_ang);
+
+		// ramp in curvature dependent lower bound
+		period_lb = (ramp_in_adapted_period_) ? period_lb * track_proximity + (1.0f - track_proximity) *
+			    period_lb_zero_curvature :
+			    period_lb;
+
+		// lower bounded period
+		period = math::max(period_lb, period);
+
+		// only allow upper bounding ONLY if lower bounding is enabled (is otherwise
+		// dangerous to allow period decrements without stability checks).
+		// NOTE: if the roll time constant is not accurately known, lower-bound
+		// checks may be too optimistic and reducing the period can still destabilize
+		// the system! enable this feature at your own risk.
+		if (en_period_ub_) {
+
+			const float period_ub = periodUB(air_turn_rate, wind_factor, feas_on_track);
+
+			if (en_period_ub_ && PX4_ISFINITE(period_ub) && period > period_ub) {
+				// NOTE: if the roll time constant is not accurately known, reducing
+				// the period here can destabilize the system!
+				// enable this feature at your own risk!
+
+				// upper bound the period (for track keeping stability), prefer lower bound if violated
+				const float period_adapted = math::max(period_lb, period_ub);
+
+				// transition from the nominal period to the adapted period as we get
+				// closer to the track
+				period = (ramp_in_adapted_period_) ? period_adapted * track_proximity + (1.0f - track_proximity) * period :
+					 period_adapted;
+			}
+		}
+	}
+
+	return period;
+} // adaptPeriod
+
+float NPFG::normalizedTrackError(const float track_error, const float track_error_bound) const
+{
+	return math::constrain(track_error / track_error_bound, 0.0f, 1.0f);
+}
+
+float NPFG::windFactor(const float airspeed, const float wind_speed) const
+{
+	// See [TODO: include citation] for definition/elaboration of this approximation.
+	if (wind_speed > airspeed || airspeed < EPSILON) {
+		return 2.0f;
+
+	} else {
+		return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed)));
+	}
+} // windFactor
+
+float NPFG::periodUB(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
+{
+	if (air_turn_rate * wind_factor > EPSILON) {
+		// multiply air turn rate by feasibility on track to zero out when we anyway
+		// should not consider the curvature
+		return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track);
+	}
+
+	return INFINITY;
+} // periodUB
+
+float NPFG::periodLB(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
+{
+	// this method considers a "conservative" lower period bound, i.e. a constant
+	// worst case bound for any wind ratio, airspeed, and path curvature
+
+	// the lower bound for zero curvature and no wind OR damping ratio < 0.5
+	const float period_lb = M_PI_F * roll_time_const_ / damping_;
+
+	if (air_turn_rate * wind_factor < EPSILON || damping_ < 0.5f) {
+		return period_lb;
+
+	} else {
+		// the lower bound for tracking a curved path in wind with damping ratio > 0.5
+		const float period_windy_curved_damped = 4.0f * M_PI_F * roll_time_const_ * damping_;
+
+		// blend the two together as the bearing on track becomes less feasible
+		return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb;
+	}
+} // periodLB
+
+float NPFG::trackProximity(const float look_ahead_ang) const
+{
+	const float sin_look_ahead_ang = sinf(look_ahead_ang);
+	return sin_look_ahead_ang * sin_look_ahead_ang;
+} // trackProximity
+
+float NPFG::trackErrorBound(const float ground_speed, const float time_const) const
+{
+	if (ground_speed > 1.0f) {
+		return ground_speed * time_const;
+
+	} else {
+		// limit bound to some minimum ground speed to avoid singularities in track
+		// error normalization. the following equation assumes ground speed minimum = 1.0
+		return 0.5f * time_const * (ground_speed * ground_speed + 1.0f);
+	}
+} // trackErrorBound
+
+float NPFG::pGain(const float period, const float damping) const
+{
+	return 4.0f * M_PI_F * damping / period;
+} // pGain
+
+float NPFG::timeConst(const float period, const float damping) const
+{
+	return period * damping;
+} // timeConst
+
+float NPFG::lookAheadAngle(const float normalized_track_error) const
+{
+	return M_PI_F * 0.5f * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f);
+} // lookAheadAngle
+
+Vector2f NPFG::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang,
+			  const float signed_track_error) const
+{
+	const float cos_look_ahead_ang = cosf(look_ahead_ang);
+	const float sin_look_ahead_ang = sinf(look_ahead_ang);
+
+	Vector2f unit_path_normal(-unit_path_tangent(1.0f), unit_path_tangent(0.0f)); // right handed 90 deg (clockwise) turn
+	Vector2f unit_track_error = -((signed_track_error < 0.0f) ? -1.0f : 1.0f) * unit_path_normal;
+
+	return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent;
+} // bearingVec
+
+float NPFG::minGroundSpeed(const float normalized_track_error, const float feas)
+{
+	// minimum ground speed demand from track keeping logic
+	min_gsp_track_keeping_ = 0.0f;
+
+	if (en_track_keeping_ && en_wind_excess_regulation_) {
+		// zero out track keeping speed increment when bearing is feasible
+		min_gsp_track_keeping_ = (1.0f - feas) * min_gsp_track_keeping_max_ * math::constrain(
+						 normalized_track_error * inv_nte_fraction_, 0.0f,
+						 1.0f);
+	}
+
+	// minimum ground speed demand from minimum forward ground speed user setting
+	float min_gsp_desired = 0.0f;
+
+	if (en_min_ground_speed_ && en_wind_excess_regulation_) {
+		min_gsp_desired = min_gsp_desired_;
+	}
+
+	return math::max(min_gsp_track_keeping_, min_gsp_desired);
+} // minGroundSpeed
+
+Vector2f NPFG::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec,
+			      const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
+			      const float min_ground_speed) const
+{
+	Vector2f air_vel_ref;
+
+	if (min_ground_speed > wind_dot_bearing && (en_min_ground_speed_ || en_track_keeping_) && en_wind_excess_regulation_) {
+		// minimum ground speed and/or track keeping
+
+		// airspeed required to achieve minimum ground speed along bearing vector
+		const float airspeed_min = sqrtf((min_ground_speed - wind_dot_bearing) * (min_ground_speed - wind_dot_bearing) +
+						 wind_cross_bearing * wind_cross_bearing);
+
+		if (airspeed_min > airspeed_max_) {
+			if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)) {
+				// we will not maintain the minimum ground speed, but can still achieve the bearing at maximum airspeed
+				const float airsp_dot_bearing = projectAirspOnBearing(airspeed_max_, wind_cross_bearing);
+				air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+
+			} else {
+				// bearing is maximally infeasible, employ mitigation law
+				air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_max_);
+			}
+
+		} else if (airspeed_min > airspeed_nom_) {
+			// the minimum ground speed is achievable within the nom - max airspeed range
+			// solve wind triangle with for air velocity reference with minimum airspeed
+			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_min, wind_cross_bearing);
+			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+
+		} else {
+			// the minimum required airspeed is less than nominal, so we can track the bearing and minimum
+			// ground speed with our nominal airspeed reference
+			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing);
+			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+		}
+
+	} else {
+		// wind excess regulation and/or mitigation
+
+		if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_nom_, wind_speed)) {
+			// bearing is nominally feasible, solve wind triangle for air velocity reference using nominal airspeed
+			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing);
+			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+
+		} else if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)
+			   && en_wind_excess_regulation_) {
+			// bearing is maximally feasible
+			if (wind_dot_bearing <= 0.0f) {
+				// we only increment the airspeed to regulate, but not overcome, excess wind
+				// NOTE: in the terminal condition, this will result in a zero ground velocity configuration
+				air_vel_ref = wind_vel;
+
+			} else {
+				// the bearing is achievable within the nom - max airspeed range
+				// solve wind triangle with for air velocity reference with minimum airspeed
+				const float airsp_dot_bearing = 0.0f; // right angle to the bearing line gives minimal airspeed usage
+				air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+			}
+
+		} else {
+			// bearing is maximally infeasible, employ mitigation law
+			const float airspeed_input = (en_wind_excess_regulation_) ? airspeed_max_ : airspeed_nom_;
+			air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_input);
+		}
+	}
+
+	return air_vel_ref;
+} // refAirVelocity
+
+float NPFG::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const
+{
+	// NOTE: wind_cross_bearing must be less than airspeed to use this function
+	// it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method
+	// otherwise the return will be erroneous
+	return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f));
+} // projectAirspOnBearing
+
+int NPFG::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+			    const float wind_speed) const
+{
+	return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed));
+} // bearingIsFeasible
+
+Vector2f NPFG::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
+				 const Vector2f &bearing_vec) const
+{
+	// essentially a 2D rotation with the speeds (magnitudes) baked in
+	return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1),
+			wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)};
+} // solveWindTriangle
+
+Vector2f NPFG::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec, const float wind_speed,
+				   const float airspeed) const
+{
+	// NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function
+	// it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method
+	// otherwise the normalization of the air velocity vector could have a division by zero
+	Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel;
+	return air_vel_ref.normalized() * airspeed;
+} // infeasibleAirVelRef
+
+float NPFG::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+			       const float wind_speed) const
+{
+	if (wind_dot_bearing < 0.0f) {
+		wind_cross_bearing = wind_speed;
+
+	} else {
+		wind_cross_bearing = fabsf(wind_cross_bearing);
+	}
+
+	float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / airspeed_buffer_, 0.0f, 1.0f));
+	return sin_arg * sin_arg;
+} // bearingFeasibility
+
+float NPFG::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel,
+			   const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
+			   const float wind_speed, const float signed_track_error, const float path_curvature) const
+{
+	// NOTE: all calculations within this function take place at the closet point
+	// on the path, as if the aircraft were already tracking the given path at
+	// this point with zero angular error. this allows us to evaluate curvature
+	// induced requirements for lateral acceleration incrementation and ramp them
+	// in with the track proximity and further consider the bearing feasibility
+	// in excess wind conditions (this is done external to this method).
+
+	// path frame curvature is the instantaneous curvature at our current distance
+	// from the actual path (considering e.g. concentric circles emanating outward/inward)
+	float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error,
+				     fabsf(path_curvature) * MIN_RADIUS);
+
+	// limit tangent ground speed to along track (forward moving) direction
+	const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f);
+
+	const float path_frame_rate = path_frame_curvature * tangent_ground_speed;
+
+	// speed ratio = projection of ground vel on track / projection of air velocity
+	// on track
+	const float speed_ratio = (1.0f + wind_dot_upt / math::max(projectAirspOnBearing(airspeed, wind_cross_upt), EPSILON));
+
+	// note the use of airspeed * speed_ratio as oppose to ground_speed^2 here --
+	// the prior considers that we command lateral acceleration in the air mass
+	// relative frame while the latter does not
+	return airspeed * speed_ratio * path_frame_rate;
+} // lateralAccelFF
+
+float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, const float airspeed) const
+{
+	// lateral acceleration demand only from the heading error
+
+	const float dot_air_vel_err = air_vel.dot(air_vel_ref);
+	const float cross_air_vel_err = cross2D(air_vel, air_vel_ref);
+
+	if (dot_air_vel_err < 0.0f) {
+		// hold max lateral acceleration command above 90 deg heading error
+		return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed);
+
+	} else {
+		// airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed
+		// for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed)
+		return p_gain_ * cross_air_vel_err / airspeed_ref_;
+	}
+} // lateralAccel
+
+/*******************************************************************************
+ * PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
+ */
+
+void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoint_B,
+			     const Vector2d &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	// similar to logic found in ECL_L1_Pos_Controller method of same name
+	// BUT no arbitrary max approach angle, approach entirely determined by generated
+	// bearing vectors
+
+	path_type_loiter_ = false;
+
+	Vector2f vector_A_to_B = getLocalPlanarVector(waypoint_A, waypoint_B);
+	Vector2f vector_A_to_vehicle = getLocalPlanarVector(waypoint_A, vehicle_pos);
+
+	if (vector_A_to_B.norm() < EPSILON) {
+		// the waypoints are on top of each other and should be considered as a
+		// single waypoint, fly directly to it
+		unit_path_tangent_ = -vector_A_to_vehicle.normalized();
+		signed_track_error_ = 0.0f;
+		evaluate(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
+
+	} else if (vector_A_to_B.dot(vector_A_to_vehicle) < 0.0f) {
+		// we are in front of waypoint A, fly directly to it until the bearing generated
+		// to the line segement between A and B is shallower than that from the
+		// bearing to the first waypoint (A).
+		unit_path_tangent_ = vector_A_to_B.normalized();
+		signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
+		evaluate(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f, true, -vector_A_to_vehicle.normalized());
+
+	} else {
+		// track the line segment between A and B
+		unit_path_tangent_ = vector_A_to_B.normalized();
+		signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
+		evaluate(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
+	}
+
+	updateRollSetpoint();
+} // navigateWaypoints
+
+void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle_pos,
+			  float radius, int8_t loiter_direction, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	path_type_loiter_ = true;
+
+	radius = math::max(radius, MIN_RADIUS);
+
+	Vector2f vector_center_to_vehicle = getLocalPlanarVector(loiter_center, vehicle_pos);
+	const float dist_to_center = vector_center_to_vehicle.norm();
+
+	// find the direction from the circle center to the closest point on its perimeter
+	// from the vehicle position
+	Vector2f unit_vec_center_to_closest_pt;
+
+	if (dist_to_center < 0.1f) {
+		// the logic breaks down at the circle center, employ some mitigation strategies
+		// until we exit this region
+		if (ground_vel.norm() < 0.1f) {
+			// arbitrarily set the point in the northern top of the circle
+			unit_vec_center_to_closest_pt = Vector2f{1.0f, 0.0f};
+
+		} else {
+			// set the point in the direction we are moving
+			unit_vec_center_to_closest_pt = ground_vel.normalized();
+		}
+
+	} else {
+		// set the point in the direction of the aircraft
+		unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
+	}
+
+	// 90 deg clockwise rotation * loiter direction
+	unit_path_tangent_ = float(loiter_direction) * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
+
+	// positive in direction of path normal
+	signed_track_error_ = -loiter_direction * (dist_to_center - radius);
+
+	float path_curvature = float(loiter_direction) / radius;
+
+	evaluate(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, path_curvature);
+
+	updateRollSetpoint();
+} // navigateLoiter
+
+void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	path_type_loiter_ = false;
+
+	Vector2f air_vel = ground_vel - wind_vel;
+	unit_path_tangent_ = Vector2f{cosf(heading_ref), sinf(heading_ref)};
+	signed_track_error_ = 0.0f;
+
+	// use the guidance law to regular heading error - ignoring wind or inertial position
+	evaluate(air_vel, Vector2f{0.0f, 0.0f}, unit_path_tangent_, signed_track_error_, 0.0f);
+
+	updateRollSetpoint();
+} // navigateHeading
+
+void NPFG::navigateBearing(float bearing, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	path_type_loiter_ = false;
+
+	unit_path_tangent_ = Vector2f{cosf(bearing), sinf(bearing)};
+
+	signed_track_error_ = 0.0f;
+
+	// no track error or path curvature to consider, just regulate ground velocity
+	// to bearing vector
+	evaluate(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
+
+	updateRollSetpoint();
+} // navigateBearing
+
+void NPFG::navigateLevelFlight(const float heading)
+{
+	path_type_loiter_ = false;
+
+	airspeed_ref_ = airspeed_nom_;
+	lateral_accel_ = 0.0f;
+	feas_ = 1.0f;
+	bearing_vec_ = Vector2f{cosf(heading), sinf(heading)};
+	unit_path_tangent_ = bearing_vec_;
+	signed_track_error_ = 0.0f;
+
+	updateRollSetpoint();
+} // navigateLevelFlight
+
+float NPFG::switchDistance(float wp_radius) const
+{
+	return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
+} // switchDistance
+
+Vector2f NPFG::getLocalPlanarVector(const Vector2d &origin, const Vector2d &target) const
+{
+	/* this is an approximation for small angles, proposed by [2] */
+	const double x_angle = math::radians(target(0) - origin(0));
+	const double y_angle = math::radians(target(1) - origin(1));
+	const double x_origin_cos = cos(math::radians(origin(0)));
+
+	return Vector2f{
+		static_cast<float>(x_angle * CONSTANTS_RADIUS_OF_EARTH),
+		static_cast<float>(y_angle *x_origin_cos * CONSTANTS_RADIUS_OF_EARTH),
+	};
+} // getLocalPlanarVector
+
+void NPFG::updateRollSetpoint()
+{
+	float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G);
+	roll_new = math::constrain(roll_new, -roll_lim_rad_, roll_lim_rad_);
+
+	if (dt_ > 0.0f && roll_slew_rate_ > 0.0f) {
+		// slew rate limiting active
+		roll_new = math::constrain(roll_new, roll_setpoint_ - roll_slew_rate_ * dt_, roll_setpoint_ + roll_slew_rate_ * dt_);
+	}
+
+	if (PX4_ISFINITE(roll_new)) {
+		roll_setpoint_ = roll_new;
+	}
+} // updateRollSetpoint
@@ -0,0 +1,729 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2021 Autonomous Systems Lab, ETH Zurich. 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 npfg.hpp
+ * Implementation of a lateral-directional nonlinear path following guidance
+ * law with excess wind handling.
+ *
+ * Acknowledgements and References:
+ *
+ * TODO
+ *
+ */
+
+#ifndef NPFG_H_
+#define NPFG_H_
+
+#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+
+/*
+ * NPFG
+ * Lateral-directional nonlinear path following guidance logic with excess wind handling
+ */
+class NPFG
+{
+
+public:
+
+	/*
+	 * Set the nominal controller period [s].
+	 */
+	void setPeriod(float period) { period_ = math::max(period, EPSILON); }
+
+	/*
+	 * Set the nominal controller damping ratio.
+	 */
+	void setDamping(float damping) { damping_ = math::constrain(damping, EPSILON, 1.0f); }
+
+	/*
+	 * Enable automatic lower bounding of the user set controller period.
+	 */
+	void enablePeriodLB(const bool en) { en_period_lb_ = en; }
+
+	/*
+	 * Enable automatic adaptation of the user set controller period for track keeping
+	 * performance.
+	 */
+	void enablePeriodUB(const bool en) { en_period_ub_ = en; }
+
+	/*
+	 * Ramp in any automatic period adaptations with the track proximity.
+	 */
+	void rampInAdaptedPeriod(const bool en) { ramp_in_adapted_period_ = en; }
+
+	/*
+	 * Enable minimum forward ground speed maintenance logic.
+	 */
+	void enableMinGroundSpeed(const bool en) { en_min_ground_speed_ = en; }
+
+	/*
+	 * Enable track keeping logic in excess wind conditions.
+	 */
+	void enableTrackKeeping(const bool en) { en_track_keeping_ = en; }
+
+	/*
+	 * Enable wind excess regulation. Disabling this param disables all airspeed
+	 * reference incrementaion (airspeed reference will always be nominal).
+	 */
+	void enableWindExcessRegulation(const bool en) { en_wind_excess_regulation_ = en; }
+
+	/*
+	 * Set the minimum allowed forward ground speed [m/s].
+	 */
+	void setMinGroundSpeed(float min_gsp) { min_gsp_desired_ = math::max(min_gsp, 0.0f); }
+
+	/*
+	 * Set the maximum value of the minimum forward ground speed command for track
+	 * keeping (occurs at the track error boundary) [m/s].
+	 */
+	void setMaxTrackKeepingMinGroundSpeed(float min_gsp) { min_gsp_track_keeping_max_ = math::max(min_gsp, 0.0f); }
+
+	/*
+	 * Set the normalized track error fraction.
+	 */
+	void setNormalizedTrackErrorFraction(float nte_fraction) { inv_nte_fraction_ = 1.0f / math::max(nte_fraction, 0.1f); }
+
+	/*
+	 * Set the nominal airspeed reference [m/s].
+	 */
+	void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); }
+
+	/*
+	 * Set the maximum airspeed reference [m/s].
+	 */
+	void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, 0.1f); }
+
+	/*
+	 * Set the autopilot roll response time constant [s].
+	 */
+	void setRollTimeConst(float tc) { roll_time_const_ = math::max(tc, 0.1f); }
+
+	/*
+	 * Set the airspeed buffer size.
+	 */
+	void setAirspeedBuffer(float buf) { airspeed_buffer_ = math::max(buf, 0.1f); }
+
+	/*
+	 * Set the switch distance multiplier.
+	 */
+	void setSwitchDistanceMultiplier(float mult) { switch_distance_multiplier_ = math::max(mult, 0.1f); }
+
+	/*
+	 * @return Controller proportional gain [rad/s]
+	 */
+	float getPGain() const { return p_gain_; }
+
+	/*
+	 * @return Controller time constant [s]
+	 */
+	float getTimeConst() const { return time_const_; }
+
+	/*
+	 * @return Adapted controller period [s]
+	 */
+	float getAdaptedPeriod() const { return adapted_period_; }
+
+	/*
+	 * @return Track error boundary [m]
+	 */
+	float getTrackErrorBound() const { return track_error_bound_; }
+
+	/*
+	 * @return Signed track error [m]
+	 */
+	float getTrackError() const { return signed_track_error_; }
+
+	/*
+	 * @return Airspeed reference [m/s]
+	 */
+	float getAirspeedRef() const { return airspeed_ref_; }
+
+	/*
+	 * @return Heading angle reference [rad]
+	 */
+	float getHeadingRef() const { return atan2f(air_vel_ref_(1), air_vel_ref_(0)); }
+
+	/*
+	 * @return Bearing angle [rad]
+	 */
+	float getBearing() const { return atan2f(bearing_vec_(1), bearing_vec_(0)); }
+
+	/*
+	 * @return Lateral acceleration command [m/s^2]
+	 */
+	float getLateralAccel() const { return lateral_accel_; }
+
+	/*
+	 * @return Feed-forward lateral acceleration command increment for tracking
+	 * path curvature [m/s^2]
+	 */
+	float getLateralAccelFF() const { return lateral_accel_ff_; }
+
+	/*
+	 * @return Bearing feasibility [0, 1]
+	 */
+	float getBearingFeas() const { return feas_; }
+
+	/*
+	 * @return On-track bearing feasibility [0, 1]
+	 */
+	float getOnTrackBearingFeas() const { return feas_on_track_; }
+
+	/*
+	 * @return Minimum forward ground speed reference [m/s]
+	 */
+	float getMinGroundSpeedRef() const { return min_ground_speed_ref_; }
+
+	/*******************************************************************************
+	 * PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
+	 */
+
+	/*
+	 * Waypoint handling logic following closely to the ECL_L1_Pos_Controller
+	 * method of the same name. Takes two waypoints and determines the relevant
+	 * parameters for evaluating the NPFG guidance law, then updates control setpoints.
+	 *
+	 * @param[in] waypoint_A Waypoint A (segment start) position in WGS84 coordinates
+	 *            (lat,lon) [deg]
+	 * @param[in] waypoint_B Waypoint B (segment end) position in WGS84 coordinates
+	 *            (lat,lon) [deg]
+	 * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 */
+	void navigateWaypoints(const matrix::Vector2d &waypoint_A, const matrix::Vector2d &waypoint_B,
+			       const matrix::Vector2d &vehicle_pos, const matrix::Vector2f &ground_vel,
+			       const matrix::Vector2f &wind_vel);
+
+	/*
+	 * Loitering (unlimited) logic. Takes loiter center, radius, and direction and
+	 * determines the relevant parameters for evaluating the NPFG guidance law,
+	 * then updates control setpoints.
+	 *
+	 * @param[in] loiter_center The position of the center of the loiter circle [m]
+	 * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] radius Loiter radius [m]
+	 * @param[in] loiter_direction Loiter direction: -1=counter-clockwise, 1=clockwise
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 */
+	void navigateLoiter(const matrix::Vector2d &loiter_center, const matrix::Vector2d &vehicle_pos,
+			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
+			    const matrix::Vector2f &wind_vel);
+
+	/*
+	 * Navigate on a fixed heading.
+	 *
+	 * This only holds a certain (air mass relative) direction and does not perform
+	 * cross track correction. Helpful for semi-autonomous modes. Introduced
+	 * by in ECL_L1_Pos_Controller, augmented for use with NPFG here.
+	 *
+	 * @param[in] heading_ref Reference heading angle [rad]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 */
+	void navigateHeading(float heading_ref, const matrix::Vector2f &ground_vel,
+			     const matrix::Vector2f &wind_vel);
+
+	/*
+	 * Navigate on a fixed bearing.
+	 *
+	 * This only holds a certain (ground relative) direction and does not perform
+	 * cross track correction. Helpful for semi-autonomous modes. Similar to navigateHeading.
+	 *
+	 * @param[in] bearing Bearing angle [rad]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 */
+	void navigateBearing(float bearing, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
+
+	/*
+	 * Keep the wings level.
+	 *
+	 * @param[in] heading Heading angle [rad]
+	 */
+	void navigateLevelFlight(const float heading);
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Set the maximum roll angle output in radians
+	 */
+	void setRollLimit(float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Set roll angle slew rate. Set to zero to deactivate.
+	 */
+	void setRollSlewRate(float roll_slew_rate) { roll_slew_rate_ = roll_slew_rate; }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
+	 */
+	void setDt(const float dt) { dt_ = dt; }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Get the switch distance
+	 *
+	 * This is the distance at which the system will switch to the next waypoint.
+	 * This depends on the period and damping
+	 *
+	 * @param[in] wp_radius The switching radius the waypoint has set.
+	 */
+	float switchDistance(float wp_radius) const;
+
+	/*
+	 * The path bearing
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame) [rad]
+	 */
+	float targetBearing() const { return atan2f(unit_path_tangent_(1), unit_path_tangent_(0)); }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Returns true if the loiter waypoint has been reached
+	 */
+	bool reachedLoiterTarget() { return circleMode(); }
+
+	/*
+	 * Returns true if following a circle (loiter)
+	 */
+	bool circleMode() { return path_type_loiter_ && track_proximity_ > EPSILON; }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Get roll angle setpoint for fixed wing.
+	 *
+	 * @return Roll angle (in NED frame)
+	 */
+	float getRollSetpoint() { return roll_setpoint_; }
+
+private:
+
+	static constexpr float EPSILON = 1.0e-4;
+	static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m]
+	static constexpr float PERIOD_SAFETY_FACTOR = 4.0f; // multiplier for period lower bound [s]
+
+	/*
+	 * tuning
+	 */
+
+	float period_{20.0f}; // nominal (desired) period -- user defined [s]
+	float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined
+	float p_gain_{0.4442}; // proportional gain (computed from period_ and damping_) [rad/s]
+	float time_const_{14.142f}; // time constant (computed from period_ and damping_) [s]
+	float adapted_period_{20.0f}; // auto-adapted period (if stability bounds enabled) [s]
+
+	/*
+	 * user defined guidance settings
+	 */
+
+	// guidance options
+	bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period
+	bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled)
+	bool ramp_in_adapted_period_{true}; // linearly ramps in upper bounded period adaptations from the nominal user setting according to track proximity
+	bool en_min_ground_speed_{true}; // the airspeed reference is incremented to sustain a user defined minimum forward ground speed
+	bool en_track_keeping_{false}; // the airspeed reference is incremented to return to the track and sustain zero ground velocity until excess wind subsides
+	bool en_wind_excess_regulation_{true}; // the airspeed reference is incremented to regulate the excess wind, but not overcome it ...
+	// ^disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference
+
+	// guidance settings
+	float airspeed_nom_{15.0f}; // nominal (desired) airspeed reference (generally equivalent to cruise optimized airspeed) [m/s]
+	float airspeed_max_{20.0f}; // maximum airspeed reference - the maximum achievable/allowed airspeed reference [m/s]
+	float roll_time_const_{0.5f}; // autopilot roll response time constant [s]
+	float min_gsp_desired_{0.0f}; // user defined miminum desired forward ground speed [m/s]
+	float min_gsp_track_keeping_max_{5.0f}; // maximum, minimum forward ground speed demand from track keeping logic [m/s]
+
+	// guidance parameters
+	float switch_distance_multiplier_{0.318f}; // a value multiplied by the track error boundary resulting in a lower switch distance
+	// ^as the bearing angle changes quadratically (instead of linearly as in L1), the time constant (automatically calculated for on track stability) proportional track error boundary typically over estimates the required switching distance
+	float airspeed_buffer_{1.5f}; // size of the region above the feasibility boundary (into feasible space) where a continuous transition from feasible to infeasible is imposed [m/s]
+	float inv_nte_fraction_{0.5f}; // inverse normalized track error fraction ...
+	// ^determines at what fraction of the normalized track error the maximum track keeping forward ground speed demand is reached
+
+	/*
+	 * internal guidance states
+	 */
+
+	// speeds
+	float min_gsp_track_keeping_{0.0f}; // minimum forward ground speed demand from track keeping logic [m/s]
+	float min_ground_speed_ref_{0.0f}; // resultant minimum forward ground speed reference considering all active guidance logic [m/s]
+
+	//bearing feasibility
+	float feas_{1.0f}; // continous representation of bearing feasibility in [0,1] (0=infeasible, 1=feasible)
+	float feas_on_track_{1.0f}; // continuous bearing feasibility "on track"
+
+	// track proximity
+	float track_error_bound_{212.13f}; // the current ground speed dependent track error bound [m]
+	float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track
+
+	// path following states
+	matrix::Vector2f unit_path_tangent_{matrix::Vector2f{1.0f, 0.0f}}; // unit path tangent vector
+	float signed_track_error_{0.0f}; // signed track error [m]
+	matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector
+
+	/*
+	 * guidance outputs
+	 */
+	float airspeed_ref_{15.0f}; // airspeed reference [m/s]
+	matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // air velocity reference vector [m/s]
+	float lateral_accel_{0.0f}; // lateral acceleration reference [m/s^2]
+	float lateral_accel_ff_{0.0f}; // lateral acceleration demand to maintain path curvature [m/s^2]
+
+	/*
+	 * ECL_L1_Pos_Controller functionality
+	 */
+
+	float dt_{0}; // control loop time [s]
+	float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad]
+	float roll_setpoint_{0.0f}; // current roll angle setpoint [rad]
+	float roll_slew_rate_{0.0f}; // roll angle setpoint slew rate limit [rad/s]
+	bool path_type_loiter_{false}; // true if the guidance law is tracking a loiter circle
+
+
+	/*
+	 * Computes the lateral acceleration and airspeed references necessary to track
+	 * a path in wind (including excess wind conditions).
+	 *
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] signed_track_error Signed error to track at closest point (sign
+	 *            determined by path normal direction) [m]
+	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
+	 * @param[in] in_front_of_wp True if we are in front of the starting point of
+	 *            a path segment
+	 * @param[in] bearing_vec_to_point Bearing vector to starting point of path
+	 *            segment, if relevant
+	 */
+	void evaluate(const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel,
+		      matrix::Vector2f &unit_path_tangent, float signed_track_error,
+		      const float path_curvature, bool in_front_of_wp = false,
+		      const matrix::Vector2f &bearing_vec_to_point = matrix::Vector2f{0.0f, 0.0f});
+
+	/*
+	 * Adapts the controller period considering user defined inputs, current flight
+	 * condition, path properties, and stability bounds.
+	 *
+	 * @param[in] ground_speed Vehicle ground speed [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] track_error Track error (magnitude) [m]
+	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
+	 * @param[in] wind_vel Wind velocity vector in inertial frame [m/s]
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] feas_on_track Bearing feasibility on track at the closest point
+	 * @return Adapted period [s]
+	 */
+	float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
+			  const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel,
+			  const matrix::Vector2f &unit_path_tangent, const float feas_on_track) const;
+
+	/*
+	 * Returns normalized (unitless) and constrained track error [0,1].
+	 *
+	 * @param[in] track_error Track error (magnitude) [m]
+	 * @param[in] track_error_bound Track error boundary [m]
+	 * @return Normalized track error
+	 */
+	float normalizedTrackError(const float track_error, const float track_error_bound) const;
+
+	/*
+	 * Cacluates an approximation of the wind factor (see [TODO: include citation]).
+	 *
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @return Non-dimensional wind factor approximation
+	 */
+	float windFactor(const float airspeed, const float wind_speed) const;
+
+	/*
+	 * Calculates a theoretical upper bound on the user defined period to maintain
+	 * track keeping stability.
+	 *
+	 * @param[in] air_turn_rate The turn rate required to track the current path
+	 *            curvature at the current airspeed, in a no-wind condition [rad/s]
+	 * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation])
+	 * @return Period upper bound [s]
+	 */
+	float periodUB(const float air_turn_rate, const float wind_factor, const float feas_on_track) const;
+
+	/*
+	 * Calculates a theoretical lower bound on the user defined period to avoid
+	 * limit cycle oscillations considering an acceleration actuation delay (e.g.
+	 * roll response delay). Note this lower bound defines *marginal stability,
+	 * and a safety factor should be applied in addition to the returned value.
+	 *
+	 * @param[in] air_turn_rate The turn rate required to track the current path
+	 *            curvature at the current airspeed, in a no-wind condition [rad/s]
+	 * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation])
+	 * @return Period lower bound [s]
+	 */
+	float periodLB(const float air_turn_rate, const float wind_factor, const float feas_on_track) const;
+
+	/*
+	 * Computes a continous non-dimensional track proximity [0,1] - 0 when the
+	 * vehicle is at the track error boundary, and 1 when on track.
+	 *
+	 * @param[in] look_ahead_ang The angle at which the bearing vector deviates
+	 *            from the unit track error vector [rad]
+	 * @return Track proximity
+	 */
+	float trackProximity(const float look_ahead_ang) const;
+
+	/*
+	 * Calculates a ground speed modulated track error bound under which the
+	 * look ahead angle is quadratically transitioned from alignment with the
+	 * track error vector to that of the path tangent vector.
+	 *
+	 * @param[in] ground_speed Vehicle ground speed [m/s]
+	 * @param[in] time_const Controller time constant [s]
+	 * @return Track error boundary [m]
+	 */
+	float trackErrorBound(const float ground_speed, const float time_const) const;
+
+	/*
+	 * Calculates the required controller proportional gain to achieve the desired
+	 * system period and damping ratio. NOTE: actual period and damping will vary
+	 * when following paths with curvature in wind.
+	 *
+	 * @param[in] period Desired system period [s]
+	 * @param[in] damping Desired system damping ratio
+	 * @return Proportional gain [rad/s]
+	 */
+	float pGain(const float period, const float damping) const;
+
+	/*
+	 * Calculates the required controller time constant to achieve the desired
+	 * system period and damping ratio. NOTE: actual period and damping will vary
+	 * when following paths with curvature in wind.
+	 *
+	 * @param[in] period Desired system period [s]
+	 * @param[in] damping Desired system damping ratio
+	 * @return Time constant [s]
+	 */
+	float timeConst(const float period, const float damping) const;
+
+	/*
+	 * Cacluates the look ahead angle as a quadratic function of the normalized
+	 * track error.
+	 *
+	 * @param[in] normalized_track_error Normalized track error (track error / track error boundary)
+	 * @return Look ahead angle [rad]
+	 */
+	float lookAheadAngle(const float normalized_track_error) const;
+
+	/*
+	 * Calculates the bearing vector and track proximity transitioning variable
+	 * from the look-ahead angle mapping.
+	 *
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] look_ahead_ang The bearing vector lies at this angle from
+	 *            the path normal vector [rad]
+	 * @param[in] signed_track_error Signed error to track at closest point (sign
+	 *            determined by path normal direction) [m]
+	 * @return Unit bearing vector
+	 */
+	matrix::Vector2f bearingVec(const matrix::Vector2f &unit_path_tangent, const float look_ahead_ang,
+				    const float signed_track_error) const;
+
+	/*
+	 * Calculates the minimum forward ground speed demand for minimum forward
+	 * ground speed maintanence as well as track keeping logic.
+	 *
+	 * @param[in] normalized_track_error Normalized track error (track error / track error boundary)
+	 * @param[in] feas Bearing feasibility
+	 * @return Minimum forward ground speed demand [m/s]
+	 */
+	float minGroundSpeed(const float normalized_track_error, const float feas);
+
+	/*
+	 * Determines a reference air velocity *without curvature compensation, but
+	 * including "optimal" airspeed reference compensation in excess wind conditions.
+	 * Nominal and maximum airspeed member variables must be set before using this method.
+	 *
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s]
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
+					const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
+					const float min_ground_speed) const;
+
+	/*
+	 * Projection of the air velocity vector onto the bearing line considering
+	 * a connected wind triangle.
+	 *
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @return Projection of air velocity vector on bearing vector [m/s]
+	 */
+	float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const;
+
+	/*
+	 * Check for binary bearing feasibility.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible
+	 */
+	int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+			      const float wind_speed) const;
+
+	/*
+	 * Air velocity solution for a given wind velocity and bearing vector assuming
+	 * a "high speed" (not backwards) solution in the excess wind case.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
+					   const matrix::Vector2f &bearing_vec) const;
+
+
+	/*
+	 * Air velocity solution for an infeasible bearing.
+	 *
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
+					     const float wind_speed, const float airspeed) const;
+
+
+	/*
+	 * Cacluates a continuous representation of the bearing feasibility from [0,1].
+	 * 0 = infeasible, 1 = fully feasible, partial feasibility in between.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @return bearing feasibility
+	 */
+	float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+				 const float wind_speed) const;
+
+	/*
+	 * Calculates an additional feed-forward lateral acceleration demand considering
+	 * the path curvature.
+	 *
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] signed_track_error Signed error to track at closest point (sign
+	 *             determined by path normal direction) [m]
+	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
+	 * @return Feed-forward lateral acceleration command [m/s^2]
+	 */
+	float lateralAccelFF(const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &ground_vel,
+			     const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
+			     const float wind_speed, const float signed_track_error, const float path_curvature) const;
+
+	/*
+	 * Calculates a lateral acceleration demand from the heading error.
+	 * (does not consider path curvature)
+	 *
+	 * @param[in] air_vel Vechile air velocity vector [m/s]
+	 * @param[in] air_vel_ref Reference air velocity vector [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @return Lateral acceleration demand [m/s^2]
+	 */
+	float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,
+			   const float airspeed) const;
+
+	/*
+	 * Calculates two-dimensional "cross product" of two vectors.
+	 * TODO: move to matrix lib (Vector2 operation)
+	 *
+	 * @param[in] vec_1 Vector 1
+	 * @param[in] vec_2 Vector 2
+	 * @return 2D cross product
+	 */
+	float cross2D(const matrix::Vector2f &vec_1, const matrix::Vector2f &vec_2) const { return vec_1(0) * vec_2(1) - vec_1(1) * vec_2(0); }
+
+	/*******************************************************************************
+	 * PX4 POSITION SETPOINT INTERFACE FUNCTIONS
+	 */
+
+	/**
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Convert a 2D vector from WGS84 to planar coordinates.
+	 *
+	 * This converts from latitude and longitude to planar
+	 * coordinates with (0,0) being at the position of ref and
+	 * returns a vector in meters towards wp.
+	 *
+	 * @param ref The reference position in WGS84 coordinates
+	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
+	 * @return The vector in meters pointing from the reference position to the coordinates
+	 */
+	matrix::Vector2f getLocalPlanarVector(const matrix::Vector2d &origin, const matrix::Vector2d &target) const;
+
+	/**
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Update roll angle setpoint. This will also apply slew rate limits if set.
+	 */
+	void updateRollSetpoint();
+
+}; // class NPFG
+
+#endif // NPFG_H_
@@ -174,51 +174,38 @@ void TECS::_update_speed_setpoint()

 }

-void TECS::_update_height_setpoint(float desired, float state)
+void TECS::updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s,
+				    float alt_amsl)
 {
-	// Detect first time through and initialize previous value to demand
-	if (PX4_ISFINITE(desired) && fabsf(_hgt_setpoint_in_prev) < 0.1f) {
-		_hgt_setpoint_in_prev = desired;
+	target_climbrate_m_s = math::min(target_climbrate_m_s, _max_climb_rate);
+	target_sinkrate_m_s = math::min(target_sinkrate_m_s, _max_sink_rate);
+
+	float feedforward_height_rate = 0.0f;
+
+	if (fabsf(alt_sp_amsl_m - _hgt_setpoint) < math::max(target_sinkrate_m_s, target_climbrate_m_s) * _dt) {
+		_hgt_setpoint = alt_sp_amsl_m;
+
+	} else if (alt_sp_amsl_m > _hgt_setpoint) {
+		_hgt_setpoint += target_climbrate_m_s * _dt;
+		feedforward_height_rate = target_climbrate_m_s;
+
+	} else if (alt_sp_amsl_m < _hgt_setpoint) {
+		_hgt_setpoint -= target_sinkrate_m_s * _dt;
+		feedforward_height_rate = -target_sinkrate_m_s;
 	}

-	// Apply a 2 point moving average to demanded height to reduce
-	// intersampling noise effects.
-	if (PX4_ISFINITE(desired)) {
-		_hgt_setpoint = 0.5f * (desired + _hgt_setpoint_in_prev);
-
-	} else {
-		_hgt_setpoint = _hgt_setpoint_in_prev;
-	}
-
-	_hgt_setpoint_in_prev = _hgt_setpoint;
-
-	// Apply a rate limit to respect vehicle performance limitations
-	if ((_hgt_setpoint - _hgt_setpoint_prev) > (_max_climb_rate * _dt)) {
-		_hgt_setpoint = _hgt_setpoint_prev + _max_climb_rate * _dt;
-
-	} else if ((_hgt_setpoint - _hgt_setpoint_prev) < (-_max_sink_rate * _dt)) {
-		_hgt_setpoint = _hgt_setpoint_prev - _max_sink_rate * _dt;
-	}
-
-	_hgt_setpoint_prev = _hgt_setpoint;
-
-	// Apply a first order noise filter
-	_hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev;
-
 	// Use a first order system to calculate a height rate setpoint from the current height error.
 	// Additionally, allow to add feedforward from heigh setpoint change
-	_hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff *
-			     (_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt;
-
-	_hgt_setpoint_adj_prev = _hgt_setpoint_adj;
+	_hgt_rate_setpoint = (_hgt_setpoint - alt_amsl) * _height_error_gain + _height_setpoint_gain_ff *
+			     feedforward_height_rate;
+	_hgt_rate_setpoint = math::constrain(_hgt_rate_setpoint, -_max_sink_rate, _max_climb_rate);
+}

+void TECS::_update_height_rate_setpoint(float hgt_rate_sp)
+{
 	// Limit the rate of change of height demand to respect vehicle performance limits
-	if (_hgt_rate_setpoint > _max_climb_rate) {
-		_hgt_rate_setpoint = _max_climb_rate;
-
-	} else if (_hgt_rate_setpoint < -_max_sink_rate) {
-		_hgt_rate_setpoint = -_max_sink_rate;
-	}
+	_hgt_rate_setpoint = math::constrain(hgt_rate_sp, -_max_sink_rate, _max_climb_rate);
+	_hgt_setpoint = _vert_pos_state;
 }

 void TECS::_detect_underspeed()
@@ -229,7 +216,7 @@ void TECS::_detect_underspeed()
 	}

 	if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
-	    || ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) {
+	    || ((_vert_pos_state < _hgt_setpoint) && _underspeed_detected)) {

 		_underspeed_detected = true;

@@ -241,7 +228,7 @@ void TECS::_detect_underspeed()
 void TECS::_update_energy_estimates()
 {
 	// Calculate specific energy demands in units of (m**2/sec**2)
-	_SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy
+	_SPE_setpoint = _hgt_setpoint * CONSTANTS_ONE_G; // potential energy
 	_SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy

 	// Calculate total energy error
@@ -477,10 +464,7 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 		_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
 		_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
 		_pitch_setpoint_unc = _last_pitch_setpoint;
-		_hgt_setpoint_adj_prev = baro_altitude;
-		_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
-		_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
-		_hgt_setpoint_in_prev = _hgt_setpoint_adj_prev;
+		_hgt_setpoint = baro_altitude;
 		_TAS_setpoint_last = _EAS * EAS2TAS;
 		_TAS_setpoint_adj = _TAS_setpoint_last;
 		_underspeed_detected = false;
@@ -499,15 +483,12 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 		// throttle lower limit is set to a value that prevents throttle reduction
 		_throttle_setpoint_min  = _throttle_setpoint_max - 0.01f;

-		// height demand and associated states are set to track the measured height
-		_hgt_setpoint_adj_prev  = baro_altitude;
-		_hgt_setpoint_adj       = _hgt_setpoint_adj_prev;
-		_hgt_setpoint_prev      = _hgt_setpoint_adj_prev;
-
 		// airspeed demand states are set to track the measured airspeed
 		_TAS_setpoint_last      = _EAS * EAS2TAS;
 		_TAS_setpoint_adj       = _EAS * EAS2TAS;

+		_hgt_setpoint = baro_altitude;
+
 		// disable speed and decent error condition checks
 		_underspeed_detected = false;
 		_uncommanded_descent_recovery = false;
@@ -535,7 +516,8 @@ void TECS::_update_STE_rate_lim()

 void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
 				 float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
-				 float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
+				 float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max,
+				 float target_climbrate, float target_sinkrate, float hgt_rate_sp)
 {
 	// Calculate the time since last update (seconds)
 	uint64_t now = hrt_absolute_time();
@@ -573,8 +555,14 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	// Calculate the demanded true airspeed
 	_update_speed_setpoint();

-	// Calculate the demanded height
-	_update_height_setpoint(hgt_setpoint, baro_altitude);
+	if (PX4_ISFINITE(hgt_rate_sp)) {
+		// use the provided height rate setpoint instead of the height setpoint
+		_update_height_rate_setpoint(hgt_rate_sp);
+
+	} else {
+		// calculate heigh rate setpoint based on altitude demand
+		updateHeightRateSetpoint(hgt_setpoint, target_climbrate, target_sinkrate, baro_altitude);
+	}

 	// Calculate the specific energy values required by the control loop
 	_update_energy_estimates();
@@ -84,7 +84,7 @@ public:
 	void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
 				   float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
 				   float throttle_min, float throttle_setpoint_max, float throttle_cruise,
-				   float pitch_limit_min, float pitch_limit_max);
+				   float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);

 	float get_throttle_setpoint() { return _last_throttle_setpoint; }
 	float get_pitch_setpoint() { return _last_pitch_setpoint; }
@@ -138,7 +138,7 @@ public:
 	uint64_t timestamp() { return _pitch_update_timestamp; }
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }

-	float hgt_setpoint_adj() { return _hgt_setpoint_adj; }
+	float hgt_setpoint() { return _hgt_setpoint; }
 	float vert_pos_state() { return _vert_pos_state; }

 	float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
@@ -185,11 +185,7 @@ public:
 	 */
 	void handle_alt_step(float delta_alt, float altitude)
 	{
-		// add height reset delta to all variables involved
-		// in filtering the demanded height
-		_hgt_setpoint_in_prev += delta_alt;
-		_hgt_setpoint_prev += delta_alt;
-		_hgt_setpoint_adj_prev += delta_alt;
+		_hgt_setpoint += delta_alt;

 		// reset height states
 		_vert_pos_state = altitude;
@@ -254,10 +250,6 @@ private:

 	// height demand calculations
 	float _hgt_setpoint{0.0f};					///< demanded height tracked by the TECS algorithm (m)
-	float _hgt_setpoint_in_prev{0.0f};				///< previous value of _hgt_setpoint after noise filtering (m)
-	float _hgt_setpoint_prev{0.0f};					///< previous value of _hgt_setpoint after noise filtering and rate limiting (m)
-	float _hgt_setpoint_adj{0.0f};					///< demanded height used by the control loops after all filtering has been applied (m)
-	float _hgt_setpoint_adj_prev{0.0f};				///< value of _hgt_setpoint_adj from previous frame (m)
 	float _hgt_rate_setpoint{0.0f};					///< demanded climb rate tracked by the TECS algorithm

 	// vehicle physical limits
@@ -313,9 +305,16 @@ private:
 	void _update_speed_setpoint();

 	/**
-	 * Update the desired height
+	 * Calculate desired height rate from altitude demand
 	 */
-	void _update_height_setpoint(float desired, float state);
+	void updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s,
+				      float alt_amsl);
+
+
+	/**
+	 * Update the desired height rate setpoint
+	 */
+	void _update_height_rate_setpoint(float hgt_rate_sp);

 	/**
 	 * Detect if the system is not capable of maintaining airspeed
@@ -3903,12 +3903,13 @@ Commander::offboard_control_update()
 			    old.velocity != ocm.velocity ||
 			    old.acceleration != ocm.acceleration ||
 			    old.attitude != ocm.attitude ||
-			    old.body_rate != ocm.body_rate) {
+			    old.body_rate != ocm.body_rate ||
+			    old.actuator != ocm.actuator) {

 				_status_changed = true;
 			}

-			if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate) {
+			if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate || ocm.actuator) {
 				offboard_available = true;
 			}
 		}
@@ -370,7 +370,7 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f);
 *
 * @group Commander
 */
-PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
+PARAM_DEFINE_INT32(COM_OBL_ACT, 2);

 /**
 * Set offboard loss failsafe mode when RC is available
@@ -389,7 +389,7 @@ PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
 * @value  7 Lockdown
 * @group Commander
 */
-PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
+PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 3);

 /**
 * Time-out to wait when onboard computer connection is lost before warning about loss connection.
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015-2017 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#add_subdirectory(launchdetection)
+
+px4_add_module(
+	MODULE modules__fw_dyn_soar_control
+	MAIN fw_dyn_soar_control
+	STACK_MAIN 4096
+	SRCS
+		FixedwingPositionINDIControl.cpp
+		FixedwingPositionINDIControl.hpp
+	DEPENDS
+	)
@@ -0,0 +1,451 @@
+/**
+ * Implementation of a generic incremental position controller
+ * using incremental nonlinear dynamic inversion and differential flatness
+ * for a fixed wing aircraft during dynamic soaring cycles.
+ * The controller directly outputs actuator deflections for ailerons, elevator and rudder.
+ *
+ * @author Marvin Harms <marv@teleport.ch>
+ */
+
+// use inclusion guards
+#ifndef FIXEDWINGPOSITIONINDICONTROL_HPP_
+#define FIXEDWINGPOSITIONINDICONTROL_HPP_
+
+#include <float.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+#include "fw_att_control/ecl_pitch_controller.h"
+#include "fw_att_control/ecl_roll_controller.h"
+#include "fw_att_control/ecl_wheel_controller.h"
+#include "fw_att_control/ecl_yaw_controller.h"
+#include <lib/ecl/geo/geo.h>
+#include <lib/l1/ECL_L1_Pos_Controller.hpp>
+#include <lib/landing_slope/Landingslope.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/airflow_aoa.h>
+#include <uORB/topics/airflow_slip.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/soaring_controller_heartbeat.h>
+#include <uORB/topics/soaring_controller_position_setpoint.h>
+#include <uORB/topics/soaring_controller_position.h>
+#include <uORB/topics/soaring_controller_status.h>
+#include <uORB/topics/soaring_controller_wind.h>
+#include <uORB/topics/soaring_estimator_shear.h>
+#include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/debug_value.h>
+#include <uORB/topics/wind.h>
+#include <uORB/uORB.h>
+
+
+
+using namespace time_literals;
+
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector;
+using matrix::Matrix3f;
+using matrix::Vector3f;
+
+
+class FixedwingPositionINDIControl final : public ModuleBase<FixedwingPositionINDIControl>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FixedwingPositionINDIControl();
+	~FixedwingPositionINDIControl() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	orb_advert_t	_mavlink_log_pub{nullptr};
+
+	// make the main task run, whenever a new body rate becomes available
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	// Subscriptions
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};						// vehicle status
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};             // airspeed
+	uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)};                           // angle of attack
+	uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)};                         // angle of sideslip
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};     	// linear acceleration
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};     // local NED position
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};						// home position
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};                 // vehicle attitude
+	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel
+	uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)};			// vehicle status flags
+	uORB::Subscription _soaring_estimator_shear_sub{ORB_ID(soaring_estimator_shear)};	// shear params for trajectory selection
+	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)};
+
+	// Publishers
+	uORB::Publication<actuator_controls_s>							_actuators_0_pub;
+	uORB::Publication<vehicle_attitude_setpoint_s>					_attitude_sp_pub;
+	uORB::Publication<vehicle_rates_setpoint_s>						_angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Publication<vehicle_angular_acceleration_setpoint_s>		_angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};
+	uORB::PublicationMulti<rate_ctrl_status_s>						_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
+	uORB::Publication<soaring_controller_heartbeat_s>				_soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)};
+	uORB::Publication<soaring_controller_position_setpoint_s>		_soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)};
+	uORB::Publication<soaring_controller_position_s>				_soaring_controller_position_pub{ORB_ID(soaring_controller_position)};
+	uORB::Publication<soaring_controller_wind_s>					_soaring_controller_wind_pub{ORB_ID(soaring_controller_wind)};
+	uORB::Publication<offboard_control_mode_s>						_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
+	uORB::Publication<debug_value_s>								_debug_value_pub{ORB_ID(debug_value)};
+
+	// Message structs
+	vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
+	actuator_controls_s			_actuators {};			// actuator commands
+	manual_control_setpoint_s	_manual_control_setpoint {};			///< r/c channel data
+	rc_channels_s				_rc_channels {};		///< rc channels
+	vehicle_local_position_s	_local_pos {};			///< vehicle local position
+	vehicle_acceleration_s		_acceleration {};		///< vehicle acceleration
+	vehicle_attitude_s			_attitude {};			///< vehicle attitude
+	vehicle_attitude_setpoint_s	_attitude_sp {};		///< vehicle attitude setpoint
+	vehicle_angular_velocity_s 	_angular_vel {};		///< vehicle angular velocity
+	vehicle_rates_setpoint_s 	_angular_vel_sp {};		///< vehicle angular velocity setpoint
+	vehicle_angular_acceleration_s	_angular_accel {};	///< vehicle angular acceleration
+	home_position_s				_home_pos {};			///< home position
+	map_projection_reference_s 	_global_local_proj_ref{};
+	vehicle_control_mode_s		_control_mode {};		///< control mode
+	offboard_control_mode_s		_offboard_control_mode {};	///< offboard control mode
+	vehicle_status_s		    _vehicle_status {};		///< vehicle status
+	soaring_controller_status_s	_soaring_controller_status {};	///< soaring controller status
+	soaring_controller_heartbeat_s	_soaring_controller_heartbeat{};	///< soaring controller hrt
+	soaring_controller_position_setpoint_s	_soaring_controller_position_setpoint{};	///< soaring controller pos setpoint
+	soaring_controller_position_s	_soaring_controller_position{};	///< soaring controller pos
+	soaring_controller_wind_s	_soaring_controller_wind{};	///< soaring controller wind
+	soaring_estimator_shear_s	_soaring_estimator_shear{}; ///< soaring estimator shear
+	debug_value_s				_debug_value{};			// slip angle
+
+	// parameter struct
+	DEFINE_PARAMETERS(
+		// aircraft params
+		(ParamFloat<px4::params::DS_INERTIA_ROLL>) _param_fw_inertia_roll,
+		(ParamFloat<px4::params::DS_INERTIA_PITCH>) _param_fw_inertia_pitch,
+		(ParamFloat<px4::params::DS_INERTIA_YAW>) _param_fw_inertia_yaw,
+		(ParamFloat<px4::params::DS_INERTIA_RP>) _param_fw_inertia_rp,
+		(ParamFloat<px4::params::DS_MASS>) _param_fw_mass,
+		(ParamFloat<px4::params::DS_WING_AREA>) _param_fw_wing_area,
+		(ParamFloat<px4::params::DS_RHO>) _param_rho,
+		// aerodynamic params (INDI)
+		(ParamFloat<px4::params::DS_C_L0>) _param_fw_c_l0,
+		(ParamFloat<px4::params::DS_C_L1>) _param_fw_c_l1,
+		(ParamFloat<px4::params::DS_C_D0>) _param_fw_c_d0,
+		(ParamFloat<px4::params::DS_C_D1>) _param_fw_c_d1,
+		(ParamFloat<px4::params::DS_C_D2>) _param_fw_c_d2,
+		(ParamFloat<px4::params::DS_C_B1>) _param_fw_c_b1,
+		(ParamFloat<px4::params::DS_AOA_OFFSET>) _param_aoa_offset,
+		(ParamFloat<px4::params::DS_STALL_SPEED>) _param_stall_speed,
+		// position PD control params
+		(ParamFloat<px4::params::DS_LIN_K_X>) _param_lin_k_x,
+		(ParamFloat<px4::params::DS_LIN_K_Y>) _param_lin_k_y,
+		(ParamFloat<px4::params::DS_LIN_K_Z>) _param_lin_k_z,
+		(ParamFloat<px4::params::DS_LIN_C_X>) _param_lin_c_x,
+		(ParamFloat<px4::params::DS_LIN_C_Y>) _param_lin_c_y,
+		(ParamFloat<px4::params::DS_LIN_C_Z>) _param_lin_c_z,
+		(ParamFloat<px4::params::DS_LIN_FF_X>) _param_lin_ff_x,
+		(ParamFloat<px4::params::DS_LIN_FF_Y>) _param_lin_ff_y,
+		(ParamFloat<px4::params::DS_LIN_FF_Z>) _param_lin_ff_z,
+		// attitude PD control params
+		(ParamFloat<px4::params::DS_ROT_K_ROLL>) _param_rot_k_roll,
+		(ParamFloat<px4::params::DS_ROT_K_PITCH>) _param_rot_k_pitch,
+		(ParamFloat<px4::params::DS_ROT_FF_YAW>) _param_rot_ff_yaw,
+		(ParamFloat<px4::params::DS_ROT_C_ROLL>) _param_rot_c_roll,
+		(ParamFloat<px4::params::DS_ROT_C_PITCH>) _param_rot_c_pitch,
+		(ParamFloat<px4::params::DS_ROT_P_YAW>) _param_rot_p_yaw,
+		// low-level controller params (INDI)
+		(ParamFloat<px4::params::DS_K_ACT_ROLL>) _param_k_act_roll,
+		(ParamFloat<px4::params::DS_K_ACT_PITCH>) _param_k_act_pitch,
+		(ParamFloat<px4::params::DS_K_DAMP_ROLL>) _param_k_damping_roll,
+		(ParamFloat<px4::params::DS_K_DAMP_PITCH>) _param_k_damping_pitch,
+		// location params
+		(ParamFloat<px4::params::DS_ORIGIN_LAT>) _param_origin_lat,
+		(ParamFloat<px4::params::DS_ORIGIN_LON>) _param_origin_lon,
+		(ParamFloat<px4::params::DS_ORIGIN_ALT>) _param_origin_alt,
+		// loiter params
+		(ParamInt<px4::params::DS_LOITER>) _param_loiter,
+		(ParamInt<px4::params::DS_W_HEADING>) _param_shear_heading,
+		(ParamFloat<px4::params::DS_W_HEIGHT>) _param_shear_height,
+		// thrust params
+		(ParamFloat<px4::params::DS_THRUST>) _param_thrust,
+		// force saturation
+		(ParamInt<px4::params::DS_SWITCH_SAT>) _param_switch_saturation,
+		// hardcoded trajectory center
+		(ParamInt<px4::params::DS_SWITCH_ORI_HC>) _param_switch_origin_hardcoded,
+		// manual switch if manual feedthrough is used, only active in STIL mode
+		(ParamInt<px4::params::DS_SWITCH_MANUAL>) _param_switch_manual,
+		// manual switch if manual feedthrough is used, REMOVE!!!
+		(ParamInt<px4::params::DS_SWITCH_CLOOP>) _param_switch_cloop,
+		// manual switch if we are in SITL mode
+		(ParamInt<px4::params::DS_SWITCH_SITL>) _param_switch_sitl,
+		(ParamFloat<px4::params::DS_WINDEST_VMAX>) _param_shear_estimated_v_max,
+		(ParamFloat<px4::params::DS_WINDEST_HGHT>) _param_shear_estimated_h_ref,
+		(ParamFloat<px4::params::DS_WINDEST_HDG>) _param_shear_estimated_heading,
+		(ParamFloat<px4::params::DS_WINDEST_ALPHA>) _param_shear_estimated_alpha
+
+	)
+
+
+	perf_counter_t	_loop_perf;				///< loop performance counter
+
+	// estimator reset counters
+	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
+	uint8_t _alt_reset_counter{0};				///< captures the number of times the estimator has reset the altitude state
+
+
+	// Update our local parameter cache.
+	int		parameters_update();
+
+	// Update subscriptions
+	void        wind_poll();
+	void		airspeed_poll();
+	void		airflow_aoa_poll();
+	void 		airflow_slip_poll();
+
+	void		vehicle_local_position_poll();
+	void		vehicle_acceleration_poll();
+	void		vehicle_attitude_poll();
+	void		vehicle_angular_velocity_poll();
+	void		vehicle_angular_acceleration_poll();
+	void		actuator_controls_poll();
+
+	void		control_update();
+	void 		manual_control_setpoint_poll();
+	void		rc_channels_poll();
+	void		vehicle_command_poll();
+	void		vehicle_control_mode_poll();
+	void		vehicle_status_poll();
+	void		soaring_controller_status_poll();
+	void		soaring_estimator_shear_poll();
+
+	//
+	void		status_publish();
+
+	const static size_t _num_basis_funs = 16;		// number of basis functions used for the trajectory approximation
+
+	// controller methods
+	void _compute_trajectory_transform();			// compute the transform between trajectory frame and ENU frame (soaring frame) based on shear params
+	void _select_loiter_trajectory();				// select the correct loiter trajectory based on available energy
+	void _select_soaring_trajectory();				// select the correct loiter trajectory based on available energy
+	void _read_trajectory_coeffs_csv(char *filename);				// read in the correct coefficients of the appropriate trajectory
+	float _get_closest_t(Vector3f
+			     pos);				// get the normalized time, at which the reference path is closest to the current position
+	Vector3f _compute_wind_estimate();				// compute a wind estimate to be used only inside the controller
+	Vector3f _compute_wind_estimate_EKF();			// compute a wind estimate to be used only as a measurement for the shear estimator
+	void _set_wind_estimate(Vector3f wind);
+	void _set_wind_estimate_EKF(Vector3f wind);
+	Vector<float, _num_basis_funs> _get_basis_funs(float t =
+				0);			// compute the vector of basis functions at normalized time t in [0,1]
+	Vector<float, _num_basis_funs> _get_d_dt_basis_funs(float t =
+				0);		// compute the vector of basis function gradients at normalized time t in [0,1]
+	Vector<float, _num_basis_funs> _get_d2_dt2_basis_funs(float t =
+				0);	// compute the vector of basis function curvatures at normalized time t in [0,1]
+	void _load_basis_coefficients();									// load the coefficients of the current path approximation
+	Vector3f _get_position_ref(float t =
+					   0);								// get the reference position on the current path, at normalized time t in [0,1]
+	Vector3f _get_velocity_ref(float t = 0,
+				   float T = 1);					// get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_acceleration_ref(float t = 0,
+				       float T = 1);				// get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Quatf _get_attitude_ref(float t = 0,
+				float T = 1);						// get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_angular_velocity_ref(float t = 0,
+					   float T = 1);			// get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_angular_acceleration_ref(float t = 0,
+					       float T = 1);		// get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Quatf _get_attitude(Vector3f vel, Vector3f
+			    f);						// get the attitude to produce force f while flying with velocity vel
+	Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref,
+				       Vector3f alpha_ref);
+	Vector3f _compute_INDI_stage_2(Vector3f ctrl);
+	Vector3f _compute_actuator_deflections(Vector3f ctrl);
+
+	// helper methods
+	void _reverse(char *str, int len);						// reverse a string of length 'len'
+	int _int_to_str(int x, char str[], int d);				// convert an integer x into a string of length d
+	void _float_to_str(float n, char *res, int afterpoint);	// convert float to string
+
+
+	// control variables
+	Vector<float, _num_basis_funs> _basis_coeffs_x = {};				// coefficients of the current path
+	Vector<float, _num_basis_funs> _basis_coeffs_y = {};				// coefficients of the current path
+	Vector<float, _num_basis_funs> _basis_coeffs_z = {};				// coefficients of the current path
+	Vector3f _alpha_sp;
+	Vector3f _wind_estimate;											// wind estimate used internally in the controller
+	Vector3f _wind_estimate_EKF;										// wind estimate only used in the wind estimator
+	Matrix3f _K_x;
+	Matrix3f _K_v;
+	Matrix3f _K_a;
+	Matrix3f _K_q;
+	Matrix3f _K_w;
+	Vector3f _pos;		// current position
+	Vector3f _vel;		// current velocity
+	Vector3f _acc;		// current acceleration
+	Quatf _att;			// attitude quaternion
+	Vector3f _omega;	// angular rate vector
+	Vector3f _alpha;	// angular acceleration vector
+	float _k_ail;
+	float _k_ele;
+	float _k_d_roll;
+	float _k_d_pitch;
+	hrt_abstime _last_run{0};
+
+	// controller frequency
+	const float _sample_frequency = 200.f;
+	// Low-Pass filters stage 1
+	const float _cutoff_frequency_1 = 20.f;
+	math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// linear acceleration
+	math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// force command
+	math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// body rates
+	// smoothing filter to reject HF noise in control output
+	const float _cutoff_frequency_smoothing =
+		20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
+	math::LowPassFilter2p _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}};	// force command stage 1
+	math::LowPassFilter2p _lp_filter_rud {_sample_frequency, 10};	// rudder command
+	// Low-Pass filters stage 2
+	const float _cutoff_frequency_2 = 20.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF"
+	math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}};	// filter to match acceleration processing delay
+	math::LowPassFilter2p _lp_filter_omega_2[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}};	// body rates
+	// Low-Pass filter for wind estimate
+	const float _cutoff_frequency_wind = 1.f;
+	math::LowPassFilter2p _lp_filter_wind[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}};	// wind_estimate inside controller
+	math::LowPassFilter2p _lp_filter_wind_EKF[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}};	// wind_estimate for EKF
+	uint _counter = 0;
+	hrt_abstime _last_time{0};
+
+	// parameter variables
+	Matrix3f _inertia {};
+	float _mass;
+	float _area;
+	float _rho;
+	float _C_L0;
+	float _C_L1;
+	float _C_D0;
+	float _C_D1;
+	float _C_D2;
+	float _C_B1;
+	float _aoa_offset;
+	float _stall_speed;
+	// trajectory origin in WGS84
+	float _origin_lat;
+	float _origin_lon;
+	float _origin_alt;
+	// trajectory origin in current NED local frame
+	float _origin_N;
+	float _origin_E;
+	float _origin_D;
+	// wind shear parameters
+	float _shear_v_max;
+	float _shear_alpha;
+	float _shear_energy;
+	float _shear_h_ref;
+	float _shear_heading;
+	float _shear_aspd;
+	// loiter circle
+	int _loiter;
+	// thrust
+	float _thrust;
+	float _thrust_pos;
+	// ==================
+	// controler switches
+	// ==================
+	// controller mode
+	bool _switch_manual;
+	// soaring mode
+	bool _switch_cl_soaring;
+	// force limit
+	bool _switch_saturation;
+	// use shear height from estimator
+	bool _switch_origin_hardcoded;
+	//
+	bool _switch_sitl;
+	//
+	bool _soaring_feasible;
+
+
+	bool _airspeed_valid{false};				///< flag if a valid airspeed estimate exists
+	hrt_abstime _airspeed_last_valid{0};			///< last time airspeed was received. Used to detect timeouts.
+	float _true_airspeed{0.0f};
+	float _cal_airspeed{0.0f};
+
+	bool _aoa_valid{false};				///< flag if a valid AoA estimate exists
+	hrt_abstime _aoa_last_valid{0};			///< last time Aoa was received. Used to detect timeouts.
+	float _aoa{0.0f};
+
+	bool _slip_valid{false};				///< flag if a valid AoA estimate exists
+	hrt_abstime _slip_last_valid{0};			///< last time Aoa was received. Used to detect timeouts.
+	float _slip{0.0f};
+
+	// vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength
+	const static size_t _gridsize = 11;
+
+
+	// helper variables
+	Dcmf _R_ned_to_enu;	// rotation matrix from NED to ENU frame
+	Dcmf _R_enu_to_ned;	// rotation matrix from ENU to NED frame
+	Dcmf _R_trajec_to_enu; // rotation matrix from trajectory frame to ENU frame
+	Dcmf _R_enu_to_trajec; // rotation matrix from ENU frame to trajectory frame
+	Vector3f _vec_enu_to_trajec; // 3D vector from ENU origin to trajectory frame origin (expressed in ENU)
+	Vector3f _zero_crossing_local_pos;	// vector denoting the zero crossing of the trajectories in NED frame
+	Vector3f _f_command {};
+	Vector3f _m_command {};
+	Vector3f _w_err {};
+	hrt_abstime _last_time_trajec{0};
+
+};
+
+
+
+#endif // FIXEDWINGPOSITIONINDICONTROL_HPP_
@@ -0,0 +1,705 @@
+/**
+ * @file fw_dyn_soar_control_params.c
+ *
+ * Parameters defined by the INDI position controller
+ *
+ * @author Marvin Harms <marv@teleport.ch>
+ */
+
+/*
+ * Controller parameters, accessible via MAVLink
+ */
+
+/**
+ * inertia around body x-axis
+ *
+ * This is the inertia of the aircraft, used for the INDI
+ *
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INERTIA_ROLL, 0.197563f);
+
+/**
+ * inertia around body y-axis
+ *
+ * This is the inertia of the aircraft, used for the INDI
+ *
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f);
+
+/**
+ * inertia around body z-axis
+ *
+ * This is the inertia of the aircraft, used for the INDI
+ *
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f);
+
+/**
+ * inertia tensor term in body xz-axis (roll-yaw coupling)
+ *
+ * This is the inertia of the aircraft, used for the INDI
+ *
+ * @unit kg
+ * @min -0.5
+ * @max 0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INERTIA_RP, -0.0f);
+
+/**
+ * total takeoff mass
+ *
+ * This is the mass of the aircraft, used for the INDI
+ *
+ * @unit kg
+ * @min 1.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_MASS, 1.4f);
+
+/**
+ * total wing area used for lift and drag computation
+ *
+ * @unit m^2
+ * @min 0.1
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_WING_AREA, 0.4f);
+
+
+/**
+ * air density used for lift and drag computation
+ *
+ * @unit
+ * @min 0.5
+ * @max 1.225
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_RHO, 1.223f);
+
+/**
+ * estimated lift coefficients used for lift and drag computation
+ *
+ * Used as L = C_l0 + C_l1*alpha,
+ * where alpha is the angle of attack.
+ *
+ * @unit
+ * @min -100
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_L0, 0.356f);
+
+/**
+ * estimated lift coefficients used for lift and drag computation
+ *
+ * Used as L = C_l0 + C_l1*alpha,
+ * where alpha is the angle of attack.
+ *
+ * @unit
+ * @min -100
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_L1, 2.354f);
+
+
+/**
+ * estimated drag coefficients used for lift and drag computation
+ *
+ * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
+ * where alpha is the angle of attack.
+ *
+ * @unit
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_D0, 0.0288f);
+
+/**
+ * estimated drag coefficients used for lift and drag computation
+ *
+ * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
+ * where alpha is the angle of attack.
+ *
+ * @unit
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_D1, 0.3783f);
+
+/**
+ * estimated drag coefficients used for lift and drag computation
+ *
+ * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
+ * where alpha is the angle of attack.
+ *
+ * @unit
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f);
+
+/**
+ * estimated sideslip sensitivity coefficients used for wind estimation
+ *
+ * Used as F_y = 0.5 * DS_RHO * ASPD^2 * DS_C_B1,
+ * where alpha is the angle of attack.
+ *
+ * @unit
+ * @min -100
+ * @max -0.01
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_B1, -3.32f);
+
+/**
+ * offset angle between body frame (Pixhawk) and the wing chord
+ *
+ * Used to compute the AoA
+ *
+ * @unit rad
+ * @min 0
+ * @max 0.1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.07f);
+
+/**
+ * stall speed of the aircraft
+ *
+ * @unit m/s
+ * @min 5
+ * @max 10
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 9.0f);
+
+
+// ========================================================
+// =================== CONTROL GAINS ======================
+// ========================================================
+/**
+ * control gain of position PD-controller (body x-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_K_X, 1.0f);
+
+/**
+ * control gain of position PD-controller (body y-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_K_Y, 1.0f);
+
+/**
+ * control gain of position PD-controller (body z-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_K_Z, 1.0f);
+
+/**
+ * normalized damping coefficient of position PD-controller (body x-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_C_X, 1.0f);
+
+/**
+ * normalized damping coefficient of position PD-controller (body y-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 1.0f);
+
+/**
+ * normalized damping coefficient of position PD-controller (body z-direction)
+ *
+* @unit
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_C_Z, 1.0f);
+
+/**
+ * acceleration feedback gain of position PD-controller (body x-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 10
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_FF_X, 0.5f);
+
+/**
+ * acceleration feedback gain of position PD-controller (body y-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 10
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_FF_Y, 0.5f);
+
+/**
+ * acceleration feedback gain of position PD-controller (body z-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 10
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_FF_Z, 0.5f);
+
+/**
+ * control gain of attitude PD-controller (body roll-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 30.0f);
+
+/**
+ * control gain of attitude PD-controller (body pitch-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 30.0f);
+
+/**
+ * rudder turn coordination FF-gain
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_FF_YAW, 1.0f);
+
+/**
+ * normalized damping coefficient of attitude PD-controller (body roll-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 1.0f);
+
+/**
+ * normalized damping coefficient of attitude PD-controller (body pitch-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 1.0f);
+
+/**
+ * rudder turn coordination P-gain
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_P_YAW, 1.0f);
+
+
+// =============================
+// low level INDI control params
+// =============================
+
+/**
+ * roll gain of K_ACT (actuator deflection gain)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.25f);
+
+/**
+ * pitch gain of K_ACT (actuator deflection gain)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.05f);
+
+
+/**
+ * roll gain of K_ACT_DAMPING (actuator damping gain)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.04f);
+
+/**
+ * pitch gain of K_ACT_DAMPING (actuator damping gain)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f);
+
+
+// ===================================================
+// ==============  trajectory center =================
+// ===================================================
+
+/**
+ * latitude of trajectory start point (WGS84)
+ *
+ * @unit
+ * @min -90
+ * @max 90
+ * @decimal 7
+ * @increment 0.0000001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.397754f);
+
+/**
+ * longitude of trajectory start point (WGS84)
+ *
+ * @unit
+ * @min -180
+ * @max 180
+ * @decimal 7
+ * @increment 0.0000001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.546372f);
+
+/**
+ * altitude of trajectory start point (WGS84)
+ *
+ * @unit
+ * @min 0
+ * @max 2000
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f);
+
+// ======================================================
+// ==============  loiter circle number =================
+// ======================================================
+
+/**
+ * integer in {0,1,2,3,4,5} defining the loiter trajectory
+ *
+ * @unit
+ * @min 0
+ * @max 7
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_LOITER, 0);
+
+// ======================================================
+// ============ wind shear heading ======================
+// ======================================================
+
+/**
+ * integer defining wind heading
+ *
+ * @unit deg
+ * @min -180
+ * @max 180
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_W_HEADING, 0);
+
+// ======================================================
+// ======= wind shear height in soaring frame ===========
+// ======================================================
+
+/**
+ * float defining wind shear vertical postition in soaring frame
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f);
+
+// ==============================================
+// ============== engine thrust =================
+// ==============================================
+/**
+ * float in [0,1] corresponding to the engine thrust
+ *
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_THRUST, 0);
+
+// ======================================================
+// ============= controller force saturation ============
+// ======================================================
+
+/**
+ * integer in {0,1} defining if the commanded force has an upper bound (saturates), 0=no saturation, 1=saturation
+ *
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1);
+
+
+// ======================================================
+// ============= hardcoded trajectory center ============
+// ======================================================
+
+/**
+ * integer in {0,1} defining if the trajectory origin is taken from hardcoded params or shear estimate, 1=params, 0=estimate
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_ORI_HC, 1);
+
+// =================================================
+// ============= loiter trajectory test ============
+// =================================================
+
+/**
+ * integer in {0,1} defining if the loiter circle defined by param DS_LOITER shall be used, 0=soaring, 1=loiter
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_LOITER, 1);
+
+// ====================================================
+// ============= manual feedthrough switch ============
+// ====================================================
+
+/**
+ * integer in {0,1} defining if we are using manual feedthrough, only used in SITL mode
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 1);
+
+// =====================================================
+// ============= open loop / closed loop DS ============
+// =====================================================
+
+/**
+ * integer in {0,1} defining if the shear parameters are changed by the estimator while soaring (closed loop). 0=fixed shear, 1=changing shear
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_CLOOP, 0);
+
+// =====================================================
+// ============= open loop / closed loop DS ============
+// =====================================================
+
+/**
+ * integer in {0,1} defining if we are running the controller in SITL. 0=hardware, 1=sitl
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_SITL, 0);
+
+/**
+ * float in [0,1] corresponding to the engine thrust
+ *
+ * @unit
+ * @min 0
+ * @max 2000
+ * @decimal 2
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_WINDEST_HGHT, 0);
+
+/**
+ * float in [0,1] corresponding to the engine thrust
+ *
+ * @unit
+ * @min -90
+ * @max 90
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_WINDEST_HDG, 0);
+
+/**
+ * float in [0,1] corresponding to the engine thrust
+ *
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_WINDEST_ALPHA, 0);
+
+/**
+ * float in [0,1] corresponding to the engine thrust
+ *
+ * @unit
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_WINDEST_VMAX, 0);
@@ -0,0 +1,3 @@
+20.249834,-1498.036199,8991.961311,-24958.956623,41340.383016,-41631.426333,17050.379042,19263.586599,-39432.861865,25757.956616,11245.661203,-42344.771826,46970.231532,-29988.424357,10855.560938,-1677.368349
+-7.218114,5279.878939,-27707.912587,70515.279933,-109233.570552,102935.722143,-36476.002615,-51554.017800,94917.464988,-58786.945547,-27934.800286,100170.063042,-114150.638209,78214.058653,-32445.366144,6339.659956
+-3.934978,2391.325102,-14411.271810,42203.244020,-76716.253156,88945.359914,-51569.236117,-25572.454067,87611.185180,-77472.095071,-5984.618456,99770.259330,-135165.631325,101511.221087,-44631.608657,9162.447954
@@ -0,0 +1,3 @@
+30.389860,912.982507,-2859.936980,3349.275775,-415.369310,-3481.089396,3363.872340,1179.921845,-4478.256881,1837.357593,3498.540940,-4492.878780,-393.125759,5151.515013,-4941.188034,1720.463927
+-2.598681,6191.674693,-33961.873408,91771.100021,-153409.649369,160707.118624,-75518.823722,-63886.003518,154241.187907,-115027.482925,-29953.909268,169169.609712,-207234.688426,146197.933872,-61050.853957,11854.416042
+-5.175201,6337.029321,-35104.159570,93181.991417,-150770.497726,150093.399535,-61550.310528,-68258.178502,141363.130375,-95038.720689,-36247.135711,151579.287214,-176414.846548,120921.610558,-49646.077608,9643.082070
@@ -0,0 +1,3 @@
+32.051371,1650.704144,-6598.531219,12390.119252,-13493.269501,7461.379871,1207.142418,-5970.998355,5109.589560,-2016.879398,-1051.211663,4872.887766,-9270.656434,10649.395680,-7172.952936,2187.636700
+-2.536588,5861.765277,-31409.143579,83398.251187,-137714.029102,143429.216873,-67935.630936,-55607.262617,137500.343143,-105281.108656,-23844.091823,152076.782007,-190543.222950,136636.797526,-57806.608731,11327.398362
+-5.904772,4632.654322,-26493.025191,72582.753211,-121329.073480,125497.496032,-56176.364936,-52937.280101,119529.487901,-84942.741316,-26808.719959,129359.716901,-154410.364036,107437.399311,-44588.163669,8729.336822
@@ -0,0 +1,3 @@
+31.550461,1802.021465,-7280.950015,13535.491205,-13709.001555,4905.090872,6059.191197,-9051.239430,2193.646055,5653.911056,-5994.939291,187.142042,3940.145313,-3036.799806,467.894600,264.261964
+-1.408562,1530.681950,-7030.394025,17659.485566,-29768.713956,34377.591980,-22054.294715,-6577.428854,34071.279368,-35600.224927,3176.621270,41037.278387,-63095.706736,50947.639967,-23488.896320,4861.320938
+-6.656401,3614.186108,-20979.720257,57857.341097,-96439.348021,98059.213352,-40613.389558,-45559.747550,93161.401353,-60017.407117,-27238.757931,98854.244272,-108993.602048,70675.872603,-27268.100397,4939.680351
@@ -0,0 +1,3 @@
+25.410335,12.369353,1959.341036,-8789.968366,18176.031111,-21228.034894,10779.468992,8250.834025,-20609.224850,14712.910998,5000.912523,-22268.932175,25004.319692,-15842.902093,5610.391090,-829.009529
+-3.716716,280.704489,-355.903224,-42.054028,411.441751,48.519658,-966.642939,1056.796821,306.389270,-1784.185307,1405.545188,941.197024,-3024.804943,2941.961264,-1386.139276,194.633758
+-12.998982,-54.523260,-107.431879,894.728680,-1811.407856,1537.414454,269.917037,-1799.743712,1168.710656,907.451000,-1775.945580,467.630285,1230.793798,-1536.221303,816.460944,-188.715147
@@ -0,0 +1,3 @@
+26.364149,177.069561,1575.881647,-9015.105328,20284.748852,-24877.180293,13157.069666,9693.742113,-24630.776658,17017.249779,6929.891099,-26402.315476,27658.672872,-15960.791210,4816.324261,-481.249223
+-2.697646,1243.905991,-6235.286985,17263.905468,-30739.116331,35075.762820,-19491.224818,-11225.334388,34615.605572,-28943.673202,-4147.157668,38977.054051,-50259.872605,36240.469382,-15200.785366,2862.042425
+-14.014894,-1092.010145,5356.556949,-12997.656894,19726.458704,-18688.134723,7112.080636,8917.358306,-17613.793090,11582.620986,4857.978470,-19032.575481,21707.141219,-14570.913691,5928.978876,-1176.673154
@@ -0,0 +1,3 @@
+27.488500,656.714564,-711.172967,-3607.344213,12064.577564,-16596.522652,8958.423854,7053.652016,-16703.379958,9887.716036,6672.954466,-17214.521473,14463.030066,-5503.534726,-56.863972,586.594485
+-1.853833,2825.199512,-14819.713309,39749.522052,-66654.957686,70216.103407,-33192.369894,-27929.172351,67590.964130,-50210.497342,-13479.819481,74116.096587,-90020.883283,62825.979648,-25821.438511,4852.075439
+-15.520495,202.519559,-1992.254633,6620.692936,-11609.000166,11322.598007,-3304.062495,-6929.925323,10421.235612,-4187.407718,-5464.971653,10171.718394,-7863.904914,3026.319376,-146.513897,-237.651350
@@ -0,0 +1,3 @@
+28.743402,2078.362604,-7793.565123,13384.615828,-12830.619300,5492.366699,2331.149832,-5092.810031,3520.435807,-1488.401128,-400.152676,3956.617004,-8314.481482,9383.643006,-5997.567239,1730.742485
+-1.213005,2555.978690,-13236.079583,35779.247030,-61178.493015,66475.594832,-33888.467431,-23956.790239,64629.202523,-51159.544433,-10075.409300,71917.010022,-90613.598094,64688.363705,-27025.918149,5135.121115
+-17.411436,702.346154,-4990.389270,14886.524601,-24997.074775,24043.706420,-7220.267390,-14427.214360,22387.881697,-9673.656090,-11279.697705,22289.916422,-17857.848174,7289.262350,-761.324352,-358.570553
@@ -0,0 +1,3 @@
+25.807032,-2925.872531,17626.900070,-48527.605690,78596.032935,-75357.034383,25236.004181,41036.456675,-70062.204933,37296.741902,27673.478531,-72007.179628,70351.596401,-40377.817337,13192.714073,-1823.721759
+-4.269878,3512.386346,-18619.333231,49177.358335,-80294.515135,81470.119172,-35154.890428,-35584.523231,77472.889766,-53740.991735,-18680.370211,83663.230433,-98251.161352,67389.126785,-27495.736176,5196.375807
+-8.223582,1274.911396,-6466.041128,15689.522036,-23322.244799,21493.532705,-8013.896644,-9664.522910,19462.409749,-14105.767315,-3182.501226,21122.648162,-28453.328443,22442.344490,-10549.264290,2316.605428
@@ -0,0 +1,3 @@
+28.265176,-979.386702,7419.686600,-22583.464563,37939.387710,-35793.651208,9323.655186,23112.091262,-32987.349099,11844.627678,18895.507035,-32027.986782,22354.058112,-6353.378068,-1374.824508,1159.236562
+-2.753809,5740.750259,-30974.191410,82232.589509,-134269.093565,135707.092980,-57790.468823,-59997.024412,128740.514055,-88400.008749,-31810.351026,138751.390069,-162076.229407,110706.908019,-44985.254781,8497.949168
+-10.288734,788.619135,-4015.272551,9464.238302,-13149.796146,10620.242441,-2203.465112,-6358.643341,9115.441480,-4803.765988,-3024.660611,9465.572049,-11175.374802,8141.654632,-3502.987926,673.165984
@@ -0,0 +1,3 @@
+30.325031,998.588079,-2506.097464,1112.679847,4050.362173,-7575.827035,3610.703835,4807.328728,-7912.810595,1423.062563,6908.855151,-7047.123610,-733.895950,6947.392420,-6118.106380,1990.418874
+-1.862138,5260.310326,-28362.138836,75966.525157,-125941.228823,130403.059750,-59433.171446,-53682.762909,124674.457655,-90703.860259,-26247.739993,136080.693972,-164317.109412,114677.998992,-47345.783502,9043.380773
+-12.328782,1149.268543,-6165.422014,15011.048121,-20807.425742,15269.068476,396.019698,-13600.991138,12819.784834,-209.559789,-10963.031285,11285.062493,-3827.872287,-2235.123927,2898.333874,-992.707506
@@ -0,0 +1,3 @@
+31.527635,1914.680208,-7066.786700,12001.498543,-11639.251141,5661.716637,833.889449,-3872.102019,4050.195339,-3333.621620,780.586620,4854.753625,-10852.776692,11992.859997,-7521.880502,2151.180312
+-1.186170,3479.031721,-18698.030760,51454.523473,-89011.366270,97857.359510,-51243.303570,-33721.655799,95275.494046,-77464.993449,-12838.707630,106615.132589,-136961.458686,99212.786606,-42070.122449,8177.263621
+-14.202558,-169.760541,386.222977,-846.701641,3119.384246,-7575.860335,9931.127792,-4154.570319,-8387.043291,16071.808162,-7926.249406,-12194.138559,27588.778783,-26471.170926,14053.983232,-3417.014125
@@ -0,0 +1,3 @@
+23.841122,-2194.779263,12904.935717,-34666.854475,54673.688646,-50635.353984,15102.364305,29120.803253,-46437.225110,22974.724498,19662.679126,-47233.232100,44960.041833,-25267.742184,7979.427992,-1008.602929
+-5.541790,2437.156126,-12346.835276,31462.020082,-49779.670442,48984.591505,-19933.267591,-22260.621101,46139.632315,-31280.871902,-11476.000089,49587.080568,-58457.462709,40644.366832,-16922.470009,3256.705963
+-6.078312,4452.065484,-24199.711547,63335.871292,-101626.656720,101030.434432,-42179.328607,-44663.085381,95065.016826,-66090.072687,-21936.472305,102617.427709,-123300.181672,86973.857880,-36758.005156,7353.148051
@@ -0,0 +1,3 @@
+28.618106,-288.792161,3141.199165,-10182.309050,16315.417146,-12565.923060,-1804.904848,13936.301857,-10686.677337,-4628.945671,14412.868226,-7786.295636,-7077.609479,14394.202010,-10151.845969,2932.493559
+-3.146467,6224.002286,-33674.042838,89395.509712,-145919.391650,147545.429134,-63071.762176,-64892.129879,139981.750282,-96642.262001,-34043.616251,151037.881495,-177225.657573,121541.053753,-49595.003797,9419.304769
+-8.139910,1042.248379,-5428.501831,13729.525415,-21836.979841,22646.227162,-11832.542811,-6780.352416,21416.920927,-19818.471600,130.693847,24841.636573,-37042.455599,29967.380524,-13973.035278,2985.845469
@@ -0,0 +1,3 @@
+30.810604,1307.872770,-4685.931426,8016.638132,-8868.736924,7291.247921,-4460.229391,-212.227482,6632.103103,-10276.374662,4770.665287,9048.021536,-21033.757954,21529.664708,-12359.641022,3262.133326
+-2.003713,5926.719259,-32038.394880,85604.852385,-141394.314399,145789.327084,-65936.879985,-60406.008704,139213.824099,-100916.225923,-29562.362503,151847.930645,-183232.805959,127952.503579,-52916.094043,10148.154329
+-9.920525,1375.816163,-7255.885038,17902.439980,-26219.206645,22434.826259,-5206.533249,-13942.467627,20123.256080,-9092.569591,-9086.668740,20409.257860,-19272.018210,10906.073918,-3480.786190,437.460015
@@ -0,0 +1,3 @@
+31.975666,1612.611465,-5710.058715,8983.152580,-7361.156073,1755.791235,2351.559612,-2104.864685,317.859148,-653.395208,1629.681386,644.939260,-5839.181279,8640.160027,-6266.314301,1950.947453
+-1.550916,5009.240516,-26964.424008,72614.247722,-121816.149633,128860.007481,-62472.103628,-49000.676378,124022.725423,-95624.567987,-21136.606762,137214.809484,-171792.694479,122820.585593,-51702.096671,10044.368612
+-11.512307,26.148037,-286.079982,260.064174,1800.601902,-6094.047095,8527.457143,-3801.084297,-6814.632040,13419.917602,-6774.440351,-9934.632608,22788.215433,-21918.884601,11642.657247,-2831.370660
@@ -0,0 +1,3 @@
+26.197525,-437.694497,3176.392508,-8582.550287,11493.328808,-5761.250067,-5973.267536,11639.945217,-3699.985563,-9689.670713,12511.953326,-272.462659,-14745.197405,18589.305875,-11322.773440,3035.082897
+-4.312181,5382.750782,-28956.350536,76477.012326,-124302.299392,125306.448999,-53475.187432,-54956.732272,118708.057825,-82386.303956,-28234.161379,128172.934300,-151638.507587,104956.992453,-43302.356871,8327.542935
+-6.237767,3407.862003,-18668.402438,49496.114618,-81134.142058,83758.382323,-39258.050462,-32407.212901,79742.732300,-61609.230412,-12768.578095,88282.327808,-112834.619373,82498.944583,-35641.096688,7206.281828
@@ -0,0 +1,3 @@
+30.257964,583.830210,-1208.136762,-2.526959,2307.048704,-1826.152951,-2722.543133,5796.497598,-1519.328038,-6933.762924,8772.887269,847.683336,-13639.685047,17503.530819,-11083.224620,3083.566374
+-2.528112,6221.602720,-33807.095242,90470.864195,-149454.640693,153985.111287,-69411.581584,-64092.936762,147001.161372,-106122.201244,-31627.909356,160149.763590,-192729.186294,134384.450096,-55553.298745,10670.421890
+-7.861770,2108.565894,-11258.246108,28746.526750,-44780.195243,42947.213456,-16524.082758,-20072.479055,39946.152469,-26746.123591,-9972.380017,42961.319178,-51085.084241,35820.355063,-14965.417848,2921.278175
@@ -0,0 +1,3 @@
+31.744656,1534.523358,-5660.106640,9645.556878,-9481.323315,4899.638607,124.608471,-2681.289259,3453.268987,-3692.985628,1667.287674,4202.151013,-10916.439306,12600.541302,-8088.121086,2347.044083
+-1.790821,5649.091225,-30477.524411,81634.880739,-135764.873445,141910.195977,-66938.534500,-55774.428590,136111.400298,-102746.145147,-25136.060368,149881.851485,-185513.231742,131780.189555,-55262.445023,10728.169679
+-9.274054,1126.681500,-6140.164082,15578.187652,-23326.557093,20201.799894,-4413.227212,-13265.477828,18310.881555,-6984.991725,-9777.115374,18115.716168,-14520.542549,6295.746276,-1029.613190,-144.927788
@@ -0,0 +1,3 @@
+32.313506,1584.092324,-5605.223993,8686.890411,-6523.413622,-2.202002,4551.203533,-2989.327460,-1631.950770,3005.277452,-68.230017,-2288.821021,379.995442,3006.308255,-3509.060531,1350.213936
+-1.506337,5345.528013,-28375.292418,75104.162089,-123727.629750,128389.113566,-60241.407731,-50241.425284,122781.994923,-93542.027541,-21546.018222,135630.097151,-169909.998876,121905.256717,-51560.297189,10067.545975
+-10.460958,-1316.085987,6799.771231,-17445.271239,28278.564054,-29478.079693,14794.815126,9955.022320,-27932.830735,23885.694804,1863.570759,-31465.729600,44324.464288,-35103.544734,16533.867367,-3693.047661
@@ -0,0 +1,3 @@
+27.951832,638.328802,-2351.931860,4975.986695,-8596.067151,12018.262162,-11023.162232,1571.306001,12384.143733,-18278.506780,6593.108603,16287.247678,-32348.383393,30172.504130,-16062.609657,3987.108885
+-3.365734,6018.273497,-32517.973622,86211.183101,-140724.620243,142658.110939,-61742.681870,-61802.247860,135455.457804,-94888.078792,-31579.184826,146601.962661,-173914.026220,120363.800930,-49560.596992,9502.418884
+-6.104348,3201.275314,-17754.562836,47695.895661,-79329.702458,83416.393064,-40758.508779,-30635.945148,79769.551898,-63605.472743,-10946.521484,88831.098618,-115702.271403,85680.661552,-37444.718056,7658.547740
@@ -0,0 +1,3 @@
+31.001824,930.719531,-2793.108593,3141.476068,-661.183171,-1864.663995,764.308655,2442.430751,-2442.139019,-2166.699457,5355.493755,-1429.245593,-6839.577772,10965.631673,-7804.379965,2355.559021
+-2.219340,5936.165707,-32321.742502,86962.840562,-144877.866358,151271.084273,-70695.329596,-60394.820305,145028.774887,-107983.603836,-28250.329929,159080.962287,-194920.224629,137529.914036,-57399.015738,11118.445993
+-7.431729,2898.169478,-15788.839688,41074.656624,-64932.563599,62736.861890,-23724.908095,-30398.636526,58608.021645,-37210.526407,-16967.686410,62273.922682,-70282.054004,47062.879008,-18838.177823,3539.121855
@@ -0,0 +1,3 @@
+32.040952,1616.150391,-6006.612591,10103.834150,-9091.665921,2665.776624,3395.883844,-4060.733768,762.016598,1191.746895,-390.783941,298.603416,-3136.929357,5769.256275,-4802.103048,1634.597926
+-1.731759,5375.646507,-28720.205073,76389.339010,-126524.838131,132252.829871,-63093.298986,-50836.460308,126848.228539,-97650.463985,-21480.671839,140429.500638,-176591.235969,126919.183230,-53763.216414,10527.978609
+-8.566198,1112.969579,-6329.117843,16823.318957,-26605.949524,24911.270891,-7640.792073,-14419.877024,23218.405056,-11214.921311,-10412.068216,23593.670297,-21189.234632,10816.082774,-2832.909204,195.963399
@@ -0,0 +1,3 @@
+29.031243,1024.647321,-4033.114979,8135.361831,-11329.985559,11613.567786,-7429.836583,-1456.944928,11074.724697,-13576.678971,3659.856096,13706.996434,-25848.280416,24274.757660,-13239.575621,3387.966126
+-2.869995,6307.197708,-34222.898443,91148.821264,-149598.974883,152769.155060,-67355.864507,-65020.079706,145410.184862,-103247.130664,-32732.918444,157819.533683,-188440.442327,130895.541062,-54031.350214,10383.858405
+-5.874280,3482.595948,-19408.919657,52253.155752,-86779.602070,90621.011001,-43171.320274,-34612.088869,86472.749651,-67050.484129,-13692.676843,95597.036137,-122197.691754,89465.504669,-38797.665513,7896.161794
@@ -0,0 +1,3 @@
+31.364619,1212.655810,-4140.134031,5989.654081,-3741.079063,-1040.246094,3086.315995,-540.557386,-2345.872467,1007.627429,2465.798756,-2341.000707,-2489.858931,6474.019714,-5471.225438,1825.025631
+-2.085064,5342.386691,-29058.805335,78508.767204,-131865.271205,139602.464184,-67736.448951,-53113.804084,134433.995467,-103508.997051,-23081.781604,148606.763393,-185796.096666,132816.415605,-56006.731815,10940.088391
+-6.979470,3531.315531,-19481.141875,51319.072579,-82101.614578,80259.965487,-30994.103031,-38573.364768,75284.207028,-47868.363905,-21961.493090,79934.241557,-89537.598855,59416.260125,-23569.998180,4397.467051
@@ -0,0 +1,3 @@
+32.118330,1644.395389,-6254.807567,10873.889952,-10278.101757,3446.898034,3808.518260,-5268.521377,1341.247037,2017.006948,-1677.936611,477.703238,-1874.368294,4159.726362,-3880.770230,1411.729386
+-1.799531,5298.851513,-28031.782953,73822.225340,-121146.249448,125568.067255,-59252.700865,-48553.198324,120116.072837,-92517.590338,-20070.157422,133100.603297,-168171.510185,121441.114109,-51685.750957,10163.900045
+-7.948920,1165.252117,-6907.991430,19221.711094,-32063.063965,32330.929643,-12724.446375,-15983.772200,30791.208957,-18345.762287,-10642.076057,32259.043449,-33020.319993,19673.032543,-6721.993394,998.677553
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
JaeyoungLim	1fbf216cb6	Works	2025-08-02 05:56:38 +02:00
JaeyoungLim	b118a2878a	Disable lockstep	2025-08-02 05:38:36 +02:00
JaeyoungLim	93c54e7a9c	Run indi control by default	2025-08-02 05:31:21 +02:00
Jaeyoung-Lim	54746a96c3	Commit stored parameters Update shear parameters based on estimator	2023-11-21 13:43:09 +01:00
Jaeyoung-Lim	99bcd08c5a	Fix parameter orders	2023-11-17 15:29:36 +01:00
JaeyoungLim	56feb2cf7d	Parameter name was too long Fix param type	2023-11-15 15:41:00 +01:00
JaeyoungLim	eb7167fd11	Save shear params	2023-11-15 10:26:31 +01:00
JaeyoungLim	1b39d5795a	Fix format	2023-11-15 10:24:54 +01:00
JaeyoungLim	39cc1f7893	Add wind shear related parameters	2023-11-15 10:10:49 +01:00
Jaeyoung-Lim	5638500737	Add dynamic soaring to nolockstep target	2023-10-18 09:15:11 +02:00
JaeyoungLim	115fd53072	Update submodule link (#1 )	2023-02-21 15:47:32 +01:00
Marvin Harms	c36024e222	added parameter mirrorinf for negative alpha	2022-10-23 08:25:08 +02:00
Marvin Harms	4e72c6313a	TESTING CODE	2022-10-22 20:01:33 +02:00
Marvin Harms	9210fa1c90	changed wind computations coeffs	2022-10-22 14:35:54 +02:00
MarvinHarms	b28dc12aea	cleaned up code	2022-10-17 17:52:36 +02:00
Marvin Harms	d6a26d94d1	TESTING CODE, hardcoded C_L0 wind_est	2022-09-09 08:12:55 +02:00
Marvin Harms	f328307c41	Testing Code	2022-09-08 15:57:10 +02:00
Marvin Harms	d85c51f684	removed segfault in shear estimator	2022-09-07 17:35:30 +02:00
Marvin Harms	6555f2cbe7	added separate wind estimate for shear estimator	2022-09-07 15:36:14 +02:00
Marvin Harms	de80462f62	introduced calibrated aspd for force computation	2022-09-07 14:47:09 +02:00
Marvin Harms	7d0418a6e7	added airflow topics to logger	2022-09-05 23:19:06 +02:00
Marvin Harms	6358cb5059	fixed hardfault from tiny stack, TESTING CODE	2022-09-05 21:56:12 +02:00
Marvin Harms	eaa0d8a4b8	TESTING CODE	2022-09-05 17:02:42 +02:00
Marvin Harms	aa83b09cf8	corrected error with param lock	2022-09-05 16:39:34 +02:00
Marvin Harms	ed61738691	cleaned up params	2022-09-05 14:45:06 +02:00
Marvin Harms	cd029f4362	added SITL switch	2022-09-05 14:05:55 +02:00
Marvin Harms	f11ab2bf34	cleaned up code, switched wind estimation model	2022-09-05 12:13:29 +02:00
Marvin Harms	d5418dbcfb	added trajectories & enabled closed loop soaring	2022-09-04 23:07:15 +02:00
Marvin Harms	fb0f81dea1	implemented shear param update in feedthrough mode	2022-08-31 16:52:24 +02:00
Marvin Harms	910b8560f0	removed param DS_SWITCH_MANUAL, added shear message	2022-08-30 17:08:36 +02:00
MarvinHarms	dacd0b64d4	transfer push (not tested)	2022-08-23 08:34:31 +02:00
Marvin Harms	cf53c31d45	TESTING CODE	2022-08-20 14:37:06 +02:00
Marvin Harms	48beb3ead0	added shear height param and new trajectories	2022-08-20 11:43:03 +02:00
Marvin Harms	8c65424b34	changed back to successful wind estimation	2022-08-20 10:42:03 +02:00
Marvin Harms	4ffbb97209	fixed vertical shear to constant assumption	2022-08-19 12:12:57 +02:00
Marvin Harms	99c04bb12b	added feasibility check	2022-08-19 11:53:52 +02:00
Marvin Harms	73eab1710e	tuned shear estimator covariances	2022-08-18 11:42:10 +02:00
Marvin Harms	f5a8b9ac2d	corrected errors in shear estimator module	2022-08-18 10:23:46 +02:00
Marvin Harms	a7ebfd2443	removed error in filtering of omega (was filtered twice)	2022-08-12 14:49:43 +02:00
Marvin Harms	61f523b750	removed compilation of uuv modules to reduce flash	2022-08-12 14:26:15 +02:00
Marvin Harms	08633ece6b	changed airframe config for sitl	2022-08-11 10:41:45 +02:00
Marvin Harms	2441d58480	added requirements for wind shear plugin gazebo	2022-08-10 16:48:18 +02:00
Marvin Harms	965fdc2bb0	working soaring estimator shear	2022-08-10 11:50:31 +02:00
Marvin Harms	a2262d7ec4	intermediate push	2022-08-08 18:10:20 +02:00
Marvin Harms	33e7ec3882	added prior update and posterior update	2022-08-08 16:11:36 +02:00
Marvin Harms	784e7728a6	added shear estimator module, not working yet	2022-08-07 09:30:41 +02:00
Marvin Harms	5bce20e06a	increased buffer size and added trajec loader	2022-08-05 09:29:06 +02:00
Marvin Harms	ea602f8eab	TESTING CODE	2022-07-27 23:41:18 +02:00
Marvin Harms	bb1acb0ce3	added trajectroy loader	2022-07-26 12:24:29 +02:00
Marvin Harms	49263efed6	set params	2022-07-21 11:01:24 +02:00
Marvin Harms	55f8366c5c	changed to faster accel msg, improved filtering	2022-07-21 10:46:53 +02:00
Marvin Harms	ca150803d0	changed accel message to a faster source	2022-07-20 10:12:29 +02:00
Marvin Harms	1bbdffb32f	improved controller smoothing & CONSISTENCY	2022-07-18 11:28:39 +02:00
Marvin Harms	66e21bad47	changed filtering in high-levle indi	2022-07-16 18:54:42 +02:00
Marvin Harms	4f81d45201	TESTING CODE	2022-07-13 17:54:08 +02:00
Marvin Harms	5a9e2bbd9a	added trajec_to_ENU transform and force saturation	2022-07-12 12:12:12 +02:00
Marvin Harms	794d3bf03e	changed turn coordination to FF-P formulation	2022-07-11 16:13:17 +02:00
Marvin Harms	b4555c6e74	fixed error in high-level INDI	2022-07-09 18:06:29 +02:00
Marvin Harms	6b7d42eef5	fixed errors in turn coordination	2022-07-08 15:12:04 +02:00
Marvin Harms	9b056cf990	Merge branch 'develop' of github.com:MarvinHarms/ethzasl_fw_px4 into develop	2022-07-08 11:12:13 +02:00
Marvin Harms	b01e864271	updated control gains	2022-07-08 11:11:52 +02:00
MarvinHarms	f8a4928a43	added aoa offset to wind computation	2022-07-05 17:30:29 +02:00
Marvin Harms	c287c56f8d	re-added turn coordination gains (TESTING CODE)	2022-06-29 17:42:47 +02:00
Marvin Harms	4db29df3b7	TESTING CODE	2022-06-29 16:44:07 +02:00
Marvin Harms	521e8e0bff	added INDI wind estiamte message to logging	2022-06-29 14:37:52 +02:00
Marvin Harms	95a5081e64	unsave changes from prev commit	2022-06-29 14:01:47 +02:00
Marvin Harms	717d61e094	changed PD params to 2nd order sys formulation	2022-06-29 14:01:23 +02:00
Marvin Harms	401ccc054d	tuned high-level gains, corr AoA wind computing	2022-06-28 17:25:51 +02:00
Marvin Harms	abb09e5bba	increased low-level INIDIfilter frequency	2022-06-28 11:44:52 +02:00
Marvin Harms	0d6d749a38	changed freequency, reduced gyro cutoff for noise	2022-06-24 19:42:11 +02:00
Marvin Harms	cced080e5b	increased controller and logging frequency	2022-06-24 16:56:08 +02:00
Marvin Harms	14d77221ab	corr wind frame err and scaling err, TESTING CODE	2022-06-24 16:45:04 +02:00
Marvin Harms	554572011d	TESTING CODE PUSH	2022-06-22 09:05:20 +02:00
Marvin Harms	76c05f94b4	added error catch on trajectory read	2022-06-21 17:27:32 +02:00
Marvin Harms	258da619e0	added FF term to turn coordination	2022-06-21 11:19:56 +02:00
Marvin Harms	470ed93972	added param stall speed	2022-06-21 10:25:51 +02:00
Marvin Harms	7e38274e2a	debugged SD error	2022-06-20 15:22:08 +02:00
Marvin Harms	1e73cdc4d2	added turn coordination for rudder deflection	2022-06-20 08:46:04 +02:00
Marvin Harms	02d62a7eba	added throttle feedthrought, FINAL TESTING CODE	2022-06-15 18:33:12 +02:00
Marvin Harms	7511913fb0	added manual attitude control switch	2022-06-15 15:20:36 +02:00
Marvin Harms	e5689bdd0b	added min saturation for airspeed scaling	2022-06-14 16:12:14 +02:00
Marvin Harms	7cc062f384	cleaned up code and added NaN protections	2022-06-14 10:24:14 +02:00
Marvin Harms	80f43de103	corrected local_frame projection error	2022-06-13 11:01:39 +02:00
Marvin Harms	df9291a73c	set offboard params, fixed angle axis wrap	2022-06-10 12:18:11 +02:00
Marvin Harms	38bb39ae9b	renamed params, set freqto 100, rm SITL lockstep	2022-06-10 10:32:50 +02:00
Marvin Harms	bcb0bdd254	backup push	2022-06-09 16:47:57 +02:00
Marvin Harms	f499d3b688	changed trajectory read-in	2022-06-09 12:13:43 +02:00
Marvin Harms	b60a8ab7de	add thrust param and mulitstage t_ref computation	2022-06-08 13:55:13 +02:00
Marvin Harms	dbc14bbdb4	added pos message and corrected rot vec errror	2022-06-07 10:50:05 +02:00
Marvin Harms	3cb0418e0b	added dyn soar position setpoint message	2022-06-06 18:29:55 +02:00
Marvin Harms	c44c387b6d	changed basis fun sigma back to 1.0	2022-06-06 11:47:23 +02:00
Marvin Harms	55992cd3c7	backup changes	2022-06-06 09:33:37 +02:00
Marvin Harms	93b2cd1cd8	changed incremental filter settings	2022-05-05 14:53:50 +02:00
Marvin Harms	2886705d06	added trajecotry origin location params	2022-05-01 11:51:31 +02:00
Marvin Harms	e6445db239	added trajectory loader	2022-04-29 16:03:35 +02:00
Marvin Harms	8f77790f24	tuned params, reduce num points to 100	2022-04-28 15:29:41 +02:00
Marvin Harms	6886498dfa	decreased filter cutoff frequenmcy to 5Hz, nocoscillations	2022-04-28 11:59:16 +02:00
Marvin Harms	81c62bef24	added incremental force term, WORKING CONTROL MATH	2022-04-28 11:46:49 +02:00
Marvin Harms	2a470fc290	tuned incremental low-level controller, no oscillations	2022-04-27 19:03:50 +02:00
Marvin Harms	31ac4634b7	working loiter circle tracking	2022-04-26 17:52:43 +02:00
Marvin Harms	92af71bc37	detected error in reference attitude computation	2022-04-26 11:23:38 +02:00
Marvin Harms	cc2014ab08	fixed error in ref attitude computation	2022-04-25 23:22:15 +02:00
Marvin Harms	e2d2aa971d	backup push	2022-04-25 12:59:33 +02:00
Marvin Harms	ac9ac55b61	corrected typecast error in basis funs	2022-04-23 09:54:12 +02:00
Marvin Harms	c4103d6479	corrected error in trajectory assignment	2022-04-21 12:15:15 +02:00
Marvin Harms	56edb80d0f	corrected attitude conversion error	2022-04-20 17:24:07 +02:00
Marvin Harms	4525aa18d9	added offboard mode to dyn_soar_controller	2022-04-19 17:50:33 +02:00
Marvin Harms	266d7bc6da	created new messages	2022-04-19 13:58:30 +02:00
Marvin Harms	2c9ff19e4f	backup push	2022-04-12 13:38:31 +02:00
Marvin Harms	90bcdcf9e1	fixed error in position poll fun	2022-04-12 09:05:37 +02:00
Marvin Harms	7eef436c2c	debugging in process...	2022-04-11 15:55:09 +02:00
Marvin Harms	32ed2fa255	added basic init function wrappers	2022-04-11 13:47:44 +02:00
Marvin Harms	02a65961d6	added NDI control function	2022-04-08 18:36:01 +02:00
Marvin Harms	571e2077f3	backup push	2022-04-08 14:25:17 +02:00
Marvin Harms	5d0763b2f2	added cmakelists and functions	2022-04-07 14:59:42 +02:00
Marvin Harms	e1b770c99c	initial commit	2022-04-07 10:02:53 +02:00
Marvin Harms	0b16d672a1	created controller files	2022-04-06 16:23:31 +02:00
Jaeyoung-Lim	0c2bf7428f	Disable unused targets	2021-12-30 11:02:12 +01:00
Jaeyoung-Lim	a9df7366a6	Disable cuav nora	2021-12-30 11:02:12 +01:00
Jaeyoung-Lim	04c41d0fee	Disable fmu-v2 stack check Remove fmu	2021-12-30 11:02:12 +01:00
Jaeyoung-Lim	1030e7317d	Add rtps message id for npfg_status	2021-12-30 11:02:12 +01:00
Jaeyoung-Lim	159a116286	Fix python CI checks	2021-12-30 11:02:12 +01:00
Thomas Stastny	b4328e7459	eas2tas conversions for npfg input/output.. also some missing get airspeed refs	2021-12-30 00:10:52 +01:00
Jaeyoung-Lim	5857461dc7	Update sitl_gazebo	2021-11-25 13:14:46 +01:00
Jaeyoung-Lim	495721b350	Add airframe configuration and make target for believer This commit adds an airframe configuration and make target for believer	2021-11-25 13:14:46 +01:00
RomanBapst	0588d5f88e	TECS: enable direct height-rate control Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-11-25 13:14:32 +01:00
RomanBapst	a81515f50b	FixedWingPositionControl: control only height rate when using pitch stick in manual altitude controlled modes Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-11-25 13:14:32 +01:00
Silvan Fuhrer	545bf1e977	rename FW_T_CLIMB_R_SP to FW_T_CLMB_R_SP Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-11-25 13:14:32 +01:00
RomanBapst	4f6c1d9c00	FixedWingPositionControl: use target climb/sink rate parameters as maximum rates in manual altitude controlled modes Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-11-25 13:14:32 +01:00
RomanBapst	ba13bb73c3	tecs: propagate altitude setpoint based on target climb/sink rate - avoids tecs always climbing and sinking and max rates and allows to fine tune these rates - avoid numerical calculation of feedforward velocity using derivative, this was prone to jitter in dt Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-11-25 13:14:32 +01:00
RomanBapst	f73cf4870d	FixedWingPosControlL1: added target climb and sink rate parameters Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-11-25 13:14:32 +01:00
JaeyoungLim	8d864f64e9	Enable offboard actuator setpoints	2021-11-24 09:40:19 +01:00
Jaeyoung-Lim	1469342125	Handle negative path curvature for feedforward acceleration calculations	2021-11-24 08:19:19 +01:00
Jaeyoung-Lim	3213be28a8	Log npfg status as default	2021-11-24 08:19:19 +01:00
Jaeyoung-Lim	788e9c92f8	Remove unused variable from npfg This commit fixes the clang tidy check on the build test CI	2021-11-10 13:11:12 +01:00
Thomas Stastny	78d7ddbec7	ramp in curvature dependent lower period bound	2021-10-01 10:43:50 +02:00
Thomas Stastny	b0dc3a4f51	add option to disable use of wind estimates with npfg	2021-10-01 10:43:50 +02:00
Thomas Stastny	493bae1e09	fix publish wind est valid	2021-10-01 10:43:50 +02:00
Thomas Stastny	dd89665cd6	multiply track error boundary by scale factor in calculation of waypoint switch distance	2021-10-01 10:43:50 +02:00
Thomas Stastny	0b40586803	organze npfg member variables in header, rename min ground speed command -> desired	2021-10-01 10:43:50 +02:00
Thomas Stastny	4dce637ab7	new bearing feasibility for handling zero airspeed - simpler bearing feasibility function - get rid of wind ratio - replace wind ratio buffer with airspeed buffer	2021-10-01 10:43:50 +02:00
Thomas Stastny	5398428683	update default gains for npfg (now that accel command bug was fixed)	2021-10-01 10:43:50 +02:00
Thomas Stastny	8641cabeec	npfg: fix incorrect extra multiplication of airspeed to acceleration reference	2021-10-01 10:43:50 +02:00
Thomas Stastny	c70df1e324	npfg: rearrange eval method, handle case in front of starting point of path segment	2021-10-01 10:43:50 +02:00
Thomas Stastny	4d11b4e06c	npfg: header formatting	2021-10-01 10:43:50 +02:00
Thomas Stastny	e9b0a3eb66	npfg: create const adaptPeriod method and update control params outside	2021-10-01 10:43:50 +02:00
Thomas Stastny	0b705c6c6d	mavlink: add npfg_status, regenerate headers	2021-10-01 10:43:50 +02:00
Thomas Stastny	d240ee9448	fw_pos_control_l1: integrate optional use of npfg uorb: npfg_status msg	2021-10-01 10:43:50 +02:00
Thomas Stastny	ef2cc9abb7	add npfg lib	2021-10-01 10:43:50 +02:00
Thomas Stastny	cd6e2b7a07	switch back to standard dialect	2021-10-01 10:43:50 +02:00
Thomas Stastny	8928cb6331	move sensor_airflow_angles to common xml, regenerate mavlink headers	2021-10-01 10:43:50 +02:00
Jaeyoung-Lim	53b182cc0a	Try setting the throttle channels properly	2021-09-29 19:42:03 +02:00
Jaeyoung-Lim	e001ccec24	Set disarmed pwm value to 1500	2021-09-29 19:42:03 +02:00
Jaeyoung-Lim	cf318a4b6c	Add dual motor mixer	2021-09-29 19:42:03 +02:00
Jaeyoung-Lim	fea99faf4a	Add ASL Believer airframe config	2021-09-29 19:42:03 +02:00
Florian Achermann	a0efc3eb00	Merge pull request #26 from ethz-asl/feature/firmware_update Feature/firmware update	2021-08-23 16:11:41 +02:00
Florian Achermann	9424d17164	Change boot order to make sure the adis mag is the priority one	2021-07-27 13:57:46 +02:00
Florian Achermann	117150979e	Switch order booting up the internal/external sensors	2021-07-27 10:46:41 +02:00
Florian Achermann	77bd43ead3	Fix compilation error	2021-07-27 10:46:12 +02:00
Florian Achermann	6b00008dba	Change SPI chipselect order - The ADIS driver by default only selects the first chipselect	2021-07-27 10:13:39 +02:00
Daniel Agar	a65a591638	adis16448: add additional delay after transfer in case of back to back transcations - add verified register read method	2021-07-27 10:07:23 +02:00
Florian Achermann	01307a43fe	Merge pull request #23 from ethz-asl/pr-fix-rtps-builds Add airflow_slip and airflow_aoa to rtps message definitions	2021-07-06 16:41:55 +02:00
Jaeyoung-Lim	83d8d8d710	Add airflow_slip and airflow_aoa to rtps message definitions	2021-07-06 16:36:23 +02:00
Jaeyoung-Lim	4a590c5fd6	Specify orientaiton of ADIS16448 IMU for EZG from autostart script Specify orientaiton of ADIS16448 IMU for EZG from autostart script	2021-07-06 16:31:39 +02:00
JaeyoungLim	0f4eaec6c1	Merge pull request #17 from ethz-asl/feature/add_hall_effect_sensor [WIP!!!] Feature/add hall effect sensor	2021-06-24 13:27:02 +02:00
JaeyoungLim	2b4540aa64	Merge pull request #18 from ethz-asl/pr-port-vanes Port si7210 driver with new driver framework	2021-06-24 13:25:56 +02:00
Jaeyoung-Lim	389a096ace	Add SENSOR_AIRFLOW_ANGLES mavlink stream to config mode	2021-06-24 13:15:21 +02:00
Jaeyoung-Lim	81b860c37e	Add SENSOR_AIRFLOW_ANGLES mavlink stream	2021-06-24 13:10:09 +02:00
Jaeyoung-Lim	7fcad68c9d	Remove probe override for si7210 driver This commit removes the probe override for the si7210 driver, which seems to help with improving reliability of driver startup	2021-06-24 10:22:10 +02:00
Jaeyoung-Lim	66ad2fd586	Address review comments Remove crc	2021-06-21 10:34:22 +02:00
Florian Achermann	4f87cb0b76	Fix format	2021-06-21 08:47:45 +02:00
Florian Achermann	042b48aadf	Add si7210 to fmuv3	2021-06-21 08:47:45 +02:00
Florian Achermann	69aaa9bad2	Fix getting the aoa and slip hall id	2021-06-21 08:47:45 +02:00
Florian Achermann	538b8d9913	Add booting the si7210 sensors to rc.sensors	2021-06-21 08:47:45 +02:00
Florian Achermann	111321082f	Add hall_poll to the main loop in the sensors module	2021-06-21 08:47:45 +02:00
Florian Achermann	736514dc98	Implement handling the hall measurements in the sensors module	2021-06-21 08:47:45 +02:00
Florian Achermann	e9772b10ac	Implement the SENSOR_AIRFLOW_ANGLES mavlink stream	2021-06-21 08:47:45 +02:00
Florian Achermann	514cb8e4fa	Switch to the ASLUAV dialect	2021-06-21 08:47:45 +02:00
Florian Achermann	4a1b9fce05	Add uorb messages for the airflow angles	2021-06-21 08:47:45 +02:00
Florian Achermann	ba72e93ffb	Fill in the instance of the hall sensor msg with the i2c address	2021-06-21 08:47:45 +02:00
Daniel Agar	54cc96f66a	drivers ADIS16448 misc fixes - increase SPI stall time slightly - tolerate mag self test failure (could be due to local magnetic field) - register configuration compatible with older ADIS16448AMLZ - don't publish duplicate accel/gyro - only allocate CRC perf counter if using CRC	2021-05-27 12:19:27 +02:00
Jaeyoung-Lim	e62a569f63	Port si7210 drivers to new driver framework This commit ports the si7210 hall sensor to the new driver framework	2021-05-07 11:17:08 +02:00
Jaeyoung-Lim	ceb8c0271c	Add rotorS mavlink build test for PX4 This commit adds rotorS as a submodule of PX4 and adds a build test	2021-04-30 08:23:33 +02:00
Amir Melzer	d20f353342	update uorb message make file	2021-04-14 16:19:29 +02:00
Amir Melzer	2ffca74e49	add hall sensor uORB message	2021-04-14 16:18:54 +02:00
Amir Melzer	080e04b59b	update px4_fmu-v5_default cmake config with the new driver	2021-04-14 16:16:39 +02:00
Amir Melzer	b3ce86f45f	update the sensors list with the hall sensor dev	2021-04-14 16:15:21 +02:00
Amir Melzer	2e73460ad0	add hall sensor drv	2021-04-14 16:14:03 +02:00
Amir Melzer	a5c61eeb4f	add si7210 driver files	2021-04-14 16:12:50 +02:00
Amir Melzer	f4551faa6b	add si7210 make file	2021-04-14 16:07:18 +02:00
Jaeyoung-Lim	fabf702264	Enable adis16448 with parameter This commit adds enabling adis16448 with a parameter	2021-04-09 16:43:17 +02:00
JaeyoungLim	911f852834	Merge pull request #15 from ethz-asl/pr-upstream-merge 03/04/2021 upstream merge	2021-04-09 16:42:42 +02:00
JaeyoungLim	32a93d6355	Merge pull request #11 from ethz-asl/pr-upstream-merge 23/02/2021 Upstream Merge	2021-02-24 10:48:56 +01:00
JaeyoungLim	472e191fef	Merge pull request #10 from ethz-asl/pr-upstream-merge 16/02/2021 Upstream Merge	2021-02-23 09:43:53 +01:00
JaeyoungLim	c25e7e2362	Merge pull request #8 from ethz-asl/pr-upstream-merge 14/02/2021 Upstream merge	2021-02-14 15:54:41 +01:00
JaeyoungLim	34f0e3f09a	Merge branch 'develop' into pr-upstream-merge	2021-02-14 09:49:53 +01:00
JaeyoungLim	b7b5f159d5	Port custom airframe configs / mixers to public fork (#7 ) * Port custom airframe configs / mixers to new custom repo This PR ports ethz asl fixedwing team's custom airframe configs and mixers to the PX4 fork * Exclude fmu-v2 for ASL vehicles This commit removes the fmu v2 for ASL vehicles in order to fix the flash overflowing problem	2021-02-11 11:09:24 +01:00
JaeyoungLim	8bac069e55	Merge pull request #6 from ethz-asl/pr-automate-upstream-sync Sync master branch with upstream using github actions	2021-02-10 02:59:44 +01:00
Jaeyoung-Lim	7859174d6b	Sync master branch with upstream using github actions This commit adds a github actions pipeline that syncs the master branch with upstream every 15 minutes	2021-02-09 21:30:16 +01:00