Add windshear plugin

This commit adds SITL targets for the dynamic soaring aircraft and additional plugins in the sitl_gazebo submodule
Disable unused targets
2026-05-23 22:07:35 +08:00 · 2022-02-20 12:44:26 +01:00 · 2021-12-30 11:02:12 +01:00 · 2021-12-30 11:02:12 +01:00 · 2021-12-30 11:02:12 +01:00 · 2021-12-30 11:02:12 +01:00
70 changed files with 4652 additions and 240 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
@@ -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
@@ -0,0 +1,58 @@
+#!/bin/sh
+#
+# @name Plane SITL
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+if [ $AUTOCNF = yes ]
+then
+	param set EKF2_ARSP_THR 8
+	param set EKF2_FUSE_BETA 1
+	param set EKF2_MAG_ACCLIM 0
+	param set EKF2_MAG_YAWLIM 0
+
+	param set FW_LND_AIRSPD_SC 1
+	param set FW_LND_ANG 8
+	param set FW_THR_LND_MAX 0
+
+	param set FW_L1_PERIOD 12
+
+	param set FW_MAN_P_MAX 30
+
+	param set FW_PR_I 0.4
+	param set FW_PR_P 0.9
+	param set FW_PR_FF 0.2
+	param set FW_PSP_OFF 2
+	param set FW_P_LIM_MAX 32
+	param set FW_P_LIM_MIN -15
+
+	param set FW_RR_FF 0.1
+	param set FW_RR_P 0.3
+
+	param set FW_THR_MAX 0.6
+	param set FW_THR_MIN 0.05
+	param set FW_THR_CRUISE 0.25
+
+	param set FW_T_ALT_TC 2
+	param set FW_T_CLMB_MAX 8
+	param set FW_T_HRATE_FF 0.5
+	param set FW_T_SINK_MAX 2.7
+	param set FW_T_SINK_MIN 2.2
+	param set FW_T_TAS_TC 2
+
+	param set FW_W_EN 1
+
+	param set MIS_LTRMIN_ALT 30
+	param set MIS_TAKEOFF_ALT 30
+
+	param set NAV_ACC_RAD 15
+	param set NAV_DLL_ACT 2
+	param set NAV_LOITER_RAD 50
+
+	param set RWTO_TKOFF 1
+
+fi
+
+set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix
+set MIXER custom
@@ -62,6 +62,8 @@ px4_add_romfs_files(
 	1034_rascal-electric
 	1035_techpod
 	1036_malolo
+	1037_believer
+	1038_plane_dynamicsoaring
 	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

@@ -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
@@ -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
@@ -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
@@ -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
@@ -121,6 +140,7 @@ set(models
 	plane_cam
 	plane_catapult
 	plane_lidar
+	plane_dynamicsoaring
 	r1_rover
 	rover
 	shell
@@ -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;
 			}
 		}
@@ -44,6 +44,7 @@ px4_add_module(
 		l1
 		launchdetection
 		landing_slope
+        npfg
 		runway_takeoff
 		tecs
 	)
@@ -105,6 +105,24 @@ FixedwingPositionControl::parameters_update()
 	_l1_control.set_l1_roll_limit(radians(_param_fw_r_lim.get()));
 	_l1_control.set_roll_slew_rate(radians(_param_fw_l1_r_slew_max.get()));

+	// NPFG parameters
+	_npfg.setPeriod(_param_npfg_period.get());
+	_npfg.setDamping(_param_npfg_damping.get());
+	_npfg.enablePeriodLB(_param_npfg_en_period_lb.get());
+	_npfg.enablePeriodUB(_param_npfg_en_period_ub.get());
+	_npfg.rampInAdaptedPeriod(_param_npfg_ramp_adapted_period.get());
+	_npfg.enableMinGroundSpeed(_param_npfg_en_min_gsp.get());
+	_npfg.enableTrackKeeping(_param_npfg_en_track_keeping.get());
+	_npfg.enableWindExcessRegulation(_param_npfg_en_wind_reg.get());
+	_npfg.setMinGroundSpeed(_param_fw_gnd_spd_min.get());
+	_npfg.setMaxTrackKeepingMinGroundSpeed(_param_npfg_track_keeping_gsp_max.get());
+	_npfg.setNormalizedTrackErrorFraction(_param_npfg_nte_fraction.get());
+	_npfg.setRollTimeConst(_param_npfg_roll_time_const.get());
+	_npfg.setAirspeedBuffer(_param_npfg_airspeed_buffer.get());
+	_npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get());
+	_npfg.setRollLimit(radians(_param_fw_r_lim.get()));
+	_npfg.setRollSlewRate(radians(_param_fw_l1_r_slew_max.get()));
+
 	// TECS parameters
 	_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
 	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
@@ -228,6 +246,37 @@ FixedwingPositionControl::airspeed_poll()
 	}
 }

+void
+FixedwingPositionControl::wind_poll()
+{
+	if (_wind_sub.updated()) {
+		wind_s wind;
+		_wind_sub.update(&wind);
+		_wind_valid = PX4_ISFINITE(wind.windspeed_north)
+			      && PX4_ISFINITE(wind.windspeed_east);
+		_time_wind_last_received = hrt_absolute_time();
+
+		_wind_vel(0) = wind.windspeed_north;
+		_wind_vel(1) = wind.windspeed_east;
+
+	} else {
+		/* no wind updates for 10 seconds */
+		if (_wind_valid && (hrt_absolute_time() - _time_wind_last_received) > 1e7) {
+			_wind_valid = false;
+
+			/* consideration of wind estimate in l1 disabled, reverting to ground speed only formulation */
+			_wind_vel(0) = 0.0f;
+			_wind_vel(1) = 0.0f;
+		}
+	}
+
+	if (!_param_npfg_en_wind_estimates.get()) {
+		_wind_valid = false;
+		_wind_vel(0) = 0.0f;
+		_wind_vel(1) = 0.0f;
+	}
+}
+
 void
 FixedwingPositionControl::manual_control_setpoint_poll()
 {
@@ -332,7 +381,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const
 	}

 	// groundspeed undershoot
-	if (!_l1_control.circle_mode()) {
+	if (!_l1_control.circle_mode() && !_param_fw_use_npfg.get()) {
 		/*
 		 * This error value ensures that a plane (as long as its throttle capability is
 		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
@@ -373,7 +422,7 @@ FixedwingPositionControl::tecs_status_publish()
 		break;
 	}

-	t.altitude_sp = _tecs.hgt_setpoint_adj();
+	t.altitude_sp = _tecs.hgt_setpoint();
 	t.altitude_filtered = _tecs.vert_pos_state();

 	t.true_airspeed_sp = _tecs.TAS_setpoint_adj();
@@ -419,23 +468,64 @@ FixedwingPositionControl::status_publish()

 	pos_ctrl_status.nav_roll = _att_sp.roll_body;
 	pos_ctrl_status.nav_pitch = _att_sp.pitch_body;
-	pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
-
-	pos_ctrl_status.target_bearing = _l1_control.target_bearing();
-	pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
-
 	pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
 				  _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);

-	pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
-
 	pos_ctrl_status.yaw_acceptance = NAN;
-
 	pos_ctrl_status.timestamp = hrt_absolute_time();
-
 	pos_ctrl_status.type = _type;

+	npfg_status_s npfg_status = {};
+
+	npfg_status.wind_est_valid = _wind_valid;
+
+	if (_param_fw_use_npfg.get()) {
+		float bearing = _npfg.getBearing(); // dont repeat atan2 calc
+
+		pos_ctrl_status.nav_bearing = bearing;
+		pos_ctrl_status.target_bearing = _npfg.targetBearing();
+		pos_ctrl_status.xtrack_error = _npfg.getTrackError();
+		pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f);
+
+		npfg_status.lat_accel = _npfg.getLateralAccel();
+		npfg_status.lat_accel_ff = _npfg.getLateralAccelFF();
+		npfg_status.heading_ref = _npfg.getHeadingRef();
+		npfg_status.bearing = bearing;
+		npfg_status.bearing_feas = _npfg.getBearingFeas();
+		npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas();
+		npfg_status.signed_track_error = _npfg.getTrackError();
+		npfg_status.track_error_bound = _npfg.getTrackErrorBound();
+		npfg_status.airspeed_ref = _npfg.getAirspeedRef();
+		npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef();
+		npfg_status.adapted_period = _npfg.getAdaptedPeriod();
+		npfg_status.p_gain = _npfg.getPGain();
+		npfg_status.time_const = _npfg.getTimeConst();
+
+	} else {
+		pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
+		pos_ctrl_status.target_bearing = _l1_control.target_bearing();
+		pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
+		pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
+
+		npfg_status.lat_accel = 0.0f;
+		npfg_status.lat_accel_ff = 0.0f;
+		npfg_status.heading_ref = 0.0f;
+		npfg_status.bearing = 0.0f;
+		npfg_status.bearing_feas = 0.0f;
+		npfg_status.bearing_feas_on_track = 0.0f;
+		npfg_status.signed_track_error = 0.0f;
+		npfg_status.track_error_bound = 0.0f;
+		npfg_status.airspeed_ref = 0.0f;
+		npfg_status.min_ground_speed_ref = 0.0f;
+		npfg_status.adapted_period = 0.0f;
+		npfg_status.p_gain = 0.0f;
+		npfg_status.time_const = 0.0f;
+	}
+
+	npfg_status.timestamp = hrt_absolute_time();
+
 	_pos_ctrl_status_pub.publish(pos_ctrl_status);
+	_npfg_status_pub.publish(npfg_status);
 }

 void
@@ -551,14 +641,22 @@ FixedwingPositionControl::update_desired_altitude(float dt)
 	if (_manual_control_setpoint_altitude > deadBand) {
 		/* pitching down */
 		float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
-		_hold_alt += (_param_fw_t_sink_max.get() * dt) * pitch;
-		_was_in_deadband = false;
+
+		if (pitch * _param_sinkrate_target.get() < _tecs.hgt_rate_setpoint()) {
+			_hold_alt += (_param_sinkrate_target.get() * dt) * pitch;
+			_manual_height_rate_setpoint_m_s = pitch * _param_sinkrate_target.get();
+			_was_in_deadband = false;
+		}

 	} else if (_manual_control_setpoint_altitude < - deadBand) {
 		/* pitching up */
 		float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
-		_hold_alt += (_param_fw_t_clmb_max.get() * dt) * pitch;
-		_was_in_deadband = false;
+
+		if (pitch * _param_climbrate_target.get() > _tecs.hgt_rate_setpoint()) {
+			_hold_alt += (_param_climbrate_target.get() * dt) * pitch;
+			_manual_height_rate_setpoint_m_s = pitch * _param_climbrate_target.get();
+			_was_in_deadband = false;
+		}

 	} else if (!_was_in_deadband) {
 		/* store altitude at which manual.x was inside deadBand
@@ -567,6 +665,7 @@ FixedwingPositionControl::update_desired_altitude(float dt)
 		_hold_alt = _current_altitude;
 		_althold_epv = _local_pos.epv;
 		_was_in_deadband = true;
+		_manual_height_rate_setpoint_m_s = NAN;
 	}

 	if (_vehicle_status.is_vtol) {
@@ -608,7 +707,12 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 	const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
 	_control_position_last_called = now;

-	_l1_control.set_dt(dt);
+	if (_param_fw_use_npfg.get()) {
+		_npfg.setDt(dt);
+
+	} else {
+		_l1_control.set_dt(dt);
+	}

 	/* only run position controller in fixed-wing mode and during transitions for VTOL */
 	if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
@@ -623,7 +727,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2

 	Vector2f nav_speed_2d{ground_speed};

-	if (_airspeed_valid) {
+	if (_airspeed_valid && !_param_fw_use_npfg.get()) {
 		// l1 navigation logic breaks down when wind speed exceeds max airspeed
 		// compute 2D groundspeed from airspeed-heading projection
 		const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
@@ -755,7 +859,14 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 			tecs_fw_mission_throttle = mission_throttle;
 		}

-		const float acc_rad = _l1_control.switch_distance(500.0f);
+		float acc_rad;
+
+		if (_param_fw_use_npfg.get()) {
+			acc_rad = _npfg.switchDistance(500.0f);
+
+		} else {
+			acc_rad = _l1_control.switch_distance(500.0f);
+		}

 		uint8_t position_sp_type = pos_sp_curr.type;

@@ -843,12 +954,24 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 				}
 			}

-			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
-			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
+			float target_airspeed = calculate_target_airspeed(mission_airspeed, ground_speed);
+
+			if (_param_fw_use_npfg.get()) {
+				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+				_att_sp.roll_body = _npfg.getRollSetpoint();
+				_att_sp.yaw_body = _npfg.getBearing();
+				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+			} else {
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
+				_att_sp.roll_body = _l1_control.get_roll_setpoint();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+			}

 			tecs_update_pitch_throttle(now, position_sp_alt,
-						   calculate_target_airspeed(mission_airspeed, ground_speed),
+						   target_airspeed,
 						   radians(_param_fw_p_lim_min.get()),
 						   radians(_param_fw_p_lim_max.get()),
 						   tecs_fw_thr_min,
@@ -869,15 +992,12 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2

 			}

-			_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, nav_speed_2d);
-
-			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
-
 			float alt_sp = pos_sp_curr.alt;

+			bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
+
 			if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
-			    && _l1_control.circle_mode() && _param_fw_lnd_earlycfg.get()) {
+			    && in_circle_mode && _param_fw_lnd_earlycfg.get()) {
 				// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
 				// landing airspeed and potentially tighter altitude control) already such that we don't
 				// have to do this switch (which can cause significant altitude errors) close to the ground.
@@ -886,6 +1006,22 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 				_att_sp.apply_flaps = true;
 			}

+			float target_airspeed = calculate_target_airspeed(mission_airspeed, ground_speed);
+
+			if (_param_fw_use_npfg.get()) {
+				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+				_npfg.navigateLoiter(curr_wp, curr_pos, loiter_radius, loiter_direction, ground_speed, _wind_vel);
+				_att_sp.roll_body = _npfg.getRollSetpoint();
+				_att_sp.yaw_body = _npfg.getBearing();
+				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+			} else {
+				_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, nav_speed_2d);
+				_att_sp.roll_body = _l1_control.get_roll_setpoint();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+			}
+
 			if (in_takeoff_situation()) {
 				alt_sp = max(alt_sp, _takeoff_ground_alt + _param_fw_clmbout_diff.get());
 				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
@@ -905,7 +1041,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 			}

 			tecs_update_pitch_throttle(now, alt_sp,
-						   calculate_target_airspeed(mission_airspeed, ground_speed),
+						   target_airspeed,
 						   radians(_param_fw_p_lim_min.get()),
 						   radians(_param_fw_p_lim_max.get()),
 						   tecs_fw_thr_min,
@@ -931,7 +1067,10 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 			reset_takeoff_state();
 		}

-		if (was_circle_mode && !_l1_control.circle_mode()) {
+		bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() :
+				      _l1_control.circle_mode(); // XXX: repeated code.
+
+		if (was_circle_mode && !in_circle_mode) {
 			/* just kicked out of loiter, reset roll integrals */
 			_att_sp.roll_reset_integral = true;
 		}
@@ -983,7 +1122,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 					   _param_fw_thr_cruise.get(),
 					   false,
 					   pitch_limit_min,
-					   tecs_status_s::TECS_MODE_NORMAL);
+					   tecs_status_s::TECS_MODE_NORMAL,
+					   _manual_height_rate_setpoint_m_s);

 		/* heading control */
 		if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
@@ -1025,11 +1165,23 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 				Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
 				Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};

-				/* populate l1 control setpoint */
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+				if (_param_fw_use_npfg.get()) {
+					_npfg.enableWindExcessRegulation(false); // disable as we are not letting npfg control airspeed anyway
+					_npfg.setAirspeedNom(altctrl_airspeed * _eas2tas);
+					_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+					_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+					_att_sp.roll_body = _npfg.getRollSetpoint();
+					_att_sp.yaw_body = _npfg.getBearing();
+					_npfg.enableWindExcessRegulation(_param_npfg_en_wind_reg.get()); // reset to user defined value in case we switch modes

-				_att_sp.roll_body = _l1_control.get_roll_setpoint();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+					altctrl_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+				} else {
+					/* populate l1 control setpoint */
+					_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+					_att_sp.roll_body = _l1_control.get_roll_setpoint();
+					_att_sp.yaw_body = _l1_control.nav_bearing();
+				}

 				if (in_takeoff_situation()) {
 					/* limit roll motion to ensure enough lift */
@@ -1085,7 +1237,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 					   _param_fw_thr_cruise.get(),
 					   false,
 					   pitch_limit_min,
-					   tecs_status_s::TECS_MODE_NORMAL);
+					   tecs_status_s::TECS_MODE_NORMAL,
+					   _manual_height_rate_setpoint_m_s);

 		_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
 		_att_sp.yaw_body = 0;
@@ -1220,17 +1373,33 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
 		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
 				       _current_latitude, _current_longitude, &_mavlink_log_pub);

-		/*
-		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
-		 * If we use the navigator heading or not is decided later.
-		 */
-		_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed);
+		float target_airspeed = calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(),
+					ground_speed);
+
+		if (_param_fw_use_npfg.get()) {
+			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+			_npfg.navigateWaypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed, _wind_vel);
+			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
+			_att_sp.yaw_body = _runway_takeoff.getYaw(_npfg.getBearing());
+
+			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+		} else {
+			/*
+			 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
+			 * If we use the navigator heading or not is decided later.
+			 */
+			_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed);
+			_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
+			_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
+		}

 		// update tecs
 		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());

 		tecs_update_pitch_throttle(now, pos_sp_curr.alt,
-					   calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed),
+					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(takeoff_pitch_max_deg),
 					   _param_fw_thr_min.get(),
@@ -1241,8 +1410,6 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
 					   tecs_status_s::TECS_MODE_TAKEOFF);

 		// assign values
-		_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
-		_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
 		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());

@@ -1280,10 +1447,6 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
 		if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
 			/* Launch has been detected, hence we have to control the plane. */

-			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
-			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
-
 			/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
 			 * full throttle, otherwise we use idle throttle */
 			float takeoff_throttle = _param_fw_thr_max.get();
@@ -1300,8 +1463,25 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
 			/* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */
 			if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
 				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
+
+				float target_airspeed = _param_fw_airspd_trim.get();
+
+				if (_param_fw_use_npfg.get()) {
+					_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+					_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+					_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+					_att_sp.roll_body = _npfg.getRollSetpoint();
+					_att_sp.yaw_body = _npfg.getBearing();
+					target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+				} else {
+					_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+					_att_sp.roll_body = _l1_control.get_roll_setpoint();
+					_att_sp.yaw_body = _l1_control.nav_bearing();
+				}
+
 				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
-							   _param_fw_airspd_trim.get(),
+							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(takeoff_pitch_max_deg),
 							   _param_fw_thr_min.get(),
@@ -1315,8 +1495,25 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
 				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));

 			} else {
+
+				float target_airspeed = calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed);
+
+				if (_param_fw_use_npfg.get()) {
+					_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+					_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+					_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+					_att_sp.roll_body = _npfg.getRollSetpoint();
+					_att_sp.yaw_body = _npfg.getBearing();
+					target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+				} else {
+					_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+					_att_sp.roll_body = _l1_control.get_roll_setpoint();
+					_att_sp.yaw_body = _l1_control.nav_bearing();
+				}
+
 				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
-							   calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed),
+							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(_param_fw_p_lim_max.get()),
 							   _param_fw_thr_min.get(),
@@ -1428,23 +1625,6 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
 		mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold");
 	}

-	if (_land_noreturn_horizontal) {
-		// heading hold
-		_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
-
-	} else {
-		// normal navigation
-		_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
-	}
-
-	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _l1_control.nav_bearing();
-
-	if (_land_noreturn_horizontal) {
-		/* limit roll motion to prevent wings from touching the ground first */
-		_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
-	}
-
 	/* Vertical landing control */
 	/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */

@@ -1495,6 +1675,46 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d

 		/* land with minimal speed */

+		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
+
+		float target_airspeed = calculate_target_airspeed(airspeed_land, ground_speed);
+
+		if (_param_fw_use_npfg.get()) {
+			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+
+			if (_land_noreturn_horizontal) {
+				// heading hold
+				_npfg.navigateHeading(_target_bearing, Vector2f{cosf(_yaw), sinf(_yaw)}, Vector2f{0.0f, 0.0f});
+
+			} else {
+				// normal navigation
+				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+			}
+
+			_att_sp.roll_body = _npfg.getRollSetpoint();
+			_att_sp.yaw_body = _npfg.getBearing();
+			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+		} else {
+			if (_land_noreturn_horizontal) {
+				// heading hold
+				_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
+
+			} else {
+				// normal navigation
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+			}
+
+			_att_sp.roll_body = _l1_control.get_roll_setpoint();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
+		}
+
+		if (_land_noreturn_horizontal) {
+			/* limit roll motion to prevent wings from touching the ground first */
+			_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
+		}
+
 		/* force TECS to only control speed with pitch, altitude is only implicitly controlled now */
 		// _tecs.set_speed_weight(2.0f);

@@ -1527,11 +1747,10 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
 			_land_stayonground = true;
 		}

-		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
 		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;

 		tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
-					   calculate_target_airspeed(airspeed_land, ground_speed),
+					   target_airspeed,
 					   radians(_param_fw_lnd_fl_pmin.get()),
 					   radians(_param_fw_lnd_fl_pmax.get()),
 					   0.0f,
@@ -1598,8 +1817,46 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d

 		const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();

+		float target_airspeed = calculate_target_airspeed(airspeed_approach, ground_speed);
+
+		if (_param_fw_use_npfg.get()) {
+			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+
+			if (_land_noreturn_horizontal) {
+				// heading hold
+				_npfg.navigateHeading(_target_bearing, Vector2f{cosf(_yaw), sinf(_yaw)}, Vector2f{0.0f, 0.0f});
+
+			} else {
+				// normal navigation
+				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+			}
+
+			_att_sp.roll_body = _npfg.getRollSetpoint();
+			_att_sp.yaw_body = _npfg.getBearing();
+			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+		} else {
+			if (_land_noreturn_horizontal) {
+				// heading hold
+				_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
+
+			} else {
+				// normal navigation
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+			}
+
+			_att_sp.roll_body = _l1_control.get_roll_setpoint();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
+		}
+
+		if (_land_noreturn_horizontal) {
+			/* limit roll motion to prevent wings from touching the ground first */
+			_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
+		}
+
 		tecs_update_pitch_throttle(now, altitude_desired,
-					   calculate_target_airspeed(airspeed_approach, ground_speed),
+					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
 					   _param_fw_thr_min.get(),
@@ -1732,6 +1989,7 @@ FixedwingPositionControl::Run()
 		}

 		airspeed_poll();
+		wind_poll();
 		manual_control_setpoint_poll();
 		vehicle_attitude_poll();
 		vehicle_command_poll();
@@ -1831,7 +2089,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
 		bool climbout_mode, float climbout_pitch_min_rad,
-		uint8_t mode)
+		uint8_t mode, float hgt_rate_sp)
 {
 	const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f);
 	_last_tecs_update = now;
@@ -1931,7 +2189,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 				    climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad - radians(_param_fw_psp_off.get()),
-				    pitch_max_rad - radians(_param_fw_psp_off.get()));
+				    pitch_max_rad - radians(_param_fw_psp_off.get()),
+				    _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp);

 	tecs_status_publish();
 }
@@ -56,6 +56,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/geo/geo.h>
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
+#include <lib/npfg/npfg.hpp>
 #include <lib/tecs/TECS.hpp>
 #include <lib/landing_slope/Landingslope.hpp>
 #include <lib/mathlib/mathlib.h>
@@ -71,6 +72,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/npfg_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_landing_status.h>
 #include <uORB/topics/position_controller_status.h>
@@ -87,6 +89,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/wind.h>
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>

@@ -151,8 +154,10 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _wind_sub{ORB_ID(wind)};

 	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
+	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication
 	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
 	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
@@ -174,6 +179,7 @@ private:
 	float	_global_local_alt0{NAN};

 	float	_hold_alt{0.0f};				///< hold altitude for altitude mode
+	float 	_manual_height_rate_setpoint_m_s{NAN};
 	float	_takeoff_ground_alt{0.0f};			///< ground altitude at which plane was launched
 	float	_hdg_hold_yaw{0.0f};				///< hold heading for velocity mode
 	bool	_hdg_hold_enabled{false};			///< heading hold enabled
@@ -228,6 +234,11 @@ private:
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};

+	/* wind estimates */
+	Vector2f _wind_vel{0.0f, 0.0f}; ///< wind velocity vector [m/s]
+	bool _wind_valid{false}; ///< flag if a valid wind estimate exists
+	hrt_abstime _time_wind_last_received{0}; ///< last time wind estimate was received in microseconds. Used to detect timeouts.
+
 	float _pitch{0.0f};
 	float _yaw{0.0f};
 	float _yawrate{0.0f};
@@ -254,6 +265,7 @@ private:
 	float _manual_control_setpoint_airspeed{0.0f};

 	ECL_L1_Pos_Controller	_l1_control;
+	NPFG _npfg;
 	TECS			_tecs;

 	uint8_t _type{0};
@@ -278,6 +290,7 @@ private:
 	void		vehicle_command_poll();
 	void		vehicle_control_mode_poll();
 	void		vehicle_status_poll();
+	void        wind_poll();

 	void		status_publish();
 	void		landing_status_publish();
@@ -346,7 +359,7 @@ private:
 					float pitch_min_rad, float pitch_max_rad,
 					float throttle_min, float throttle_max, float throttle_cruise,
 					bool climbout_mode, float climbout_pitch_min_rad,
-					uint8_t mode = tecs_status_s::TECS_MODE_NORMAL);
+					uint8_t mode = tecs_status_s::TECS_MODE_NORMAL, float hgt_rate_sp = NAN);

 	DEFINE_PARAMETERS(

@@ -363,6 +376,22 @@ private:
 		(ParamFloat<px4::params::FW_L1_R_SLEW_MAX>) _param_fw_l1_r_slew_max,
 		(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,

+		(ParamBool<px4::params::FW_USE_NPFG>) _param_fw_use_npfg,
+		(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
+		(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
+		(ParamBool<px4::params::NPFG_LB_PERIOD>) _param_npfg_en_period_lb,
+		(ParamBool<px4::params::NPFG_UB_PERIOD>) _param_npfg_en_period_ub,
+		(ParamBool<px4::params::NPFG_RAMP_PERIOD>) _param_npfg_ramp_adapted_period,
+		(ParamBool<px4::params::NPFG_TRACK_KEEP>) _param_npfg_en_track_keeping,
+		(ParamBool<px4::params::NPFG_EN_MIN_GSP>) _param_npfg_en_min_gsp,
+		(ParamBool<px4::params::NPFG_WIND_REG>) _param_npfg_en_wind_reg,
+		(ParamFloat<px4::params::NPFG_GSP_MAX_TK>) _param_npfg_track_keeping_gsp_max,
+		(ParamFloat<px4::params::NPFG_NTE_FRAC>) _param_npfg_nte_fraction,
+		(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
+		(ParamFloat<px4::params::NPFG_ASPD_BUF>) _param_npfg_airspeed_buffer,
+		(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
+		(ParamBool<px4::params::NPFG_EN_WIND_EST>) _param_npfg_en_wind_estimates,
+
 		(ParamFloat<px4::params::FW_LND_AIRSPD_SC>) _param_fw_lnd_airspd_sc,
 		(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
 		(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
@@ -395,6 +424,8 @@ private:
 		(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
 		(ParamFloat<px4::params::FW_T_TAS_R_TC>) _param_tas_rate_time_const,
 		(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
+		(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
+		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,

 		(ParamFloat<px4::params::FW_THR_ALT_SCL>) _param_fw_thr_alt_scl,
 		(ParamFloat<px4::params::FW_THR_CRUISE>) _param_fw_thr_cruise,
@@ -85,6 +85,181 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
 */
 PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f);

+/**
+ * Use NPFG as lateral-directional guidance law for fixed-wing vehicles
+ *
+ * Replaces L1.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(FW_USE_NPFG, 0);
+
+/**
+ * NPFG period
+ *
+ * Period of the NPFG control law.
+ *
+ * @unit s
+ * @min 5.0
+ * @max 40.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_PERIOD, 20.0f);
+
+/**
+ * NPFG damping ratio
+ *
+ * Damping ratio of the NPFG control law.
+ *
+ * @min 0.10
+ * @max 1.00
+ * @decimal 2
+ * @increment 0.01
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_DAMPING, 0.7f);
+
+/**
+ * Enable automatic lower bound on the NPFG period
+ *
+ * Avoids limit cycling from a too aggressively tuned period/damping combination.
+ * If set to false, also disables the upper bound NPFG_PERIOD_UB.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_LB_PERIOD, 1);
+
+/**
+ * Enable automatic upper bound on the NPFG period
+ *
+ * Adapts period to maintain track keeping in variable winds and path curvature.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_UB_PERIOD, 1);
+
+/**
+ * Ramp in automatic period adaptations with track proximity
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_RAMP_PERIOD, 1);
+
+/**
+ * Enable track keeping excess wind handling logic.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_TRACK_KEEP, 1);
+
+/**
+ * Enable minimum ground speed maintaining excess wind handling logic
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_EN_MIN_GSP, 1);
+
+/**
+ * Enable wind excess regulation.
+ *
+ * Disabling this parameter further disables all other airspeed incrementation options.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_WIND_REG, 1);
+
+/**
+ * Maximum, minimum forward ground speed for track keeping in excess wind
+ *
+ * The maximum value of the minimum forward ground speed that may be commanded
+ * by the track keeping excess wind handling logic. Occurs at the track error
+ * boundary * normalized track error fraction and is reduced to zero on track.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_GSP_MAX_TK, 5.0f);
+
+/**
+ * Normalized track error fraction
+ *
+ * Determines at what fraction of the normalized track error the maximum track keeping
+ * forward ground speed demand is reached.
+ *
+ * @min 0.1
+ * @max 1.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_NTE_FRAC, 0.5f);
+
+/**
+ * Roll time constant
+ *
+ * Time constant of roll controller command / response, modeled as first order delay.
+ *
+ * @unit s
+ * @min 0.05
+ * @max 2.00
+ * @decimal 2
+ * @increment 0.05
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f);
+
+/**
+ * NPFG airspeed buffer
+ *
+ * The size of the feasibility transition region at cross wind angle >= 90 deg.
+ * This must be non-zero to avoid jumpy airspeed incrementation while using wind
+ * excess handling logic. Similarly used as buffer region around zero airspeed.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @max 5.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_ASPD_BUF, 1.5f);
+
+/**
+ * NPFG switch distance multiplier
+ *
+ * Multiplied by the track error boundary to determine when the aircraft switches
+ * to the next waypoint and/or path segment. Should be less than 1. 1/pi (0.32)
+ * sets the switch distance equivalent to that of the L1 controller.
+ *
+ * @min 0.1
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f);
+
+/**
+ * Enable use of wind estimates for NPFG. Disabling, controller assumes zero wind.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_EN_WIND_EST, 1);
+
 /**
 * Cruise throttle
 *
@@ -775,3 +950,36 @@ PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.2f);
 * @group FW TECS
 */
 PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f);
+
+/**
+ * Default target climbrate.
+ *
+ *
+ * The default rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints.
+ * In manual modes this defines the maximum rate at which the altitude setpoint can be increased.
+ *
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 15
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f);
+
+/**
+ * Default target sinkrate.
+ *
+ *
+ * The default rate at which the vehicle will sink in autonomous modes to achieve altitude setpoints.
+ * In manual modes this defines the maximum rate at which the altitude setpoint can be decreased.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 15
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f);
@@ -72,6 +72,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("mission");
 	add_topic("mission_result");
 	add_topic("navigator_mission_item");
+	add_topic("npfg_status", 100);
 	add_topic("offboard_control_mode", 100);
 	add_topic("onboard_computer_status", 10);
 	add_topic("parameter_update");
@@ -1528,6 +1528,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("LOCAL_POSITION_NED", 1.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f);
+		configure_stream_local("NPFG_STATUS", 1.0f);
 		configure_stream_local("OBSTACLE_DISTANCE", 1.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f);
 		configure_stream_local("PING", 0.1f);
@@ -1536,6 +1537,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("RAW_RPM", 2.0f);
 		configure_stream_local("RC_CHANNELS", 5.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
+		configure_stream_local("SENSOR_AIRFLOW_ANGLES", 10.0f);
 		configure_stream_local("SYS_STATUS", 1.0f);
 		configure_stream_local("UTM_GLOBAL_POSITION", 0.5f);
 		configure_stream_local("VFR_HUD", 4.0f);
@@ -1586,6 +1588,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_STATUS", 1.0f);
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream_local("NPFG_STATUS", 10.0f);
 		configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
 		configure_stream_local("PING", 1.0f);
@@ -1728,6 +1731,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("MANUAL_CONTROL", 5.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream_local("NPFG_STATUS", 10.0f);
 		configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
 		configure_stream_local("PING", 1.0f);
@@ -1739,6 +1743,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("SCALED_IMU3", 25.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f);
+		configure_stream_local("SENSOR_AIRFLOW_ANGLES", 20.0f);
 		configure_stream_local("SYS_STATUS", 1.0f);
 		configure_stream_local("SYSTEM_TIME", 1.0f);
 		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
@@ -88,6 +88,7 @@
 #include "streams/MANUAL_CONTROL.hpp"
 #include "streams/MOUNT_ORIENTATION.hpp"
 #include "streams/NAV_CONTROLLER_OUTPUT.hpp"
+#include "streams/NPFG_STATUS.hpp"
 #include "streams/OBSTACLE_DISTANCE.hpp"
 #include "streams/OPTICAL_FLOW_RAD.hpp"
 #include "streams/ORBIT_EXECUTION_STATUS.hpp"
@@ -99,6 +100,7 @@
 #include "streams/RC_CHANNELS.hpp"
 #include "streams/SCALED_IMU.hpp"
 #include "streams/SCALED_PRESSURE.hpp"
+#include "streams/SENSOR_AIRFLOW_ANGLES.hpp"
 #include "streams/SERVO_OUTPUT_RAW.hpp"
 #include "streams/STATUSTEXT.hpp"
 #include "streams/STORAGE_INFORMATION.hpp"
@@ -346,6 +348,9 @@ static const StreamListItem streams_list[] = {
 #if defined(SCALED_PRESSURE3)
 	create_stream_list_item<MavlinkStreamScaledPressure3>(),
 #endif // SCALED_PRESSURE3
+#if defined(SENSOR_AIRFLOW_ANGLES_HPP)
+	create_stream_list_item<MavlinkStreamSensorAirflowAngles>(),
+#endif // SENSOR_AIRFLOW_ANGLES
 #if defined(ACTUATOR_OUTPUT_STATUS_HPP)
 	create_stream_list_item<MavlinkStreamActuatorOutputStatus>(),
 #endif // ACTUATOR_OUTPUT_STATUS_HPP
@@ -529,8 +534,11 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamComponentInformation>(),
 #endif // COMPONENT_INFORMATION_HPP
 #if defined(RAW_RPM_HPP)
-	create_stream_list_item<MavlinkStreamRawRpm>()
+	create_stream_list_item<MavlinkStreamRawRpm>(),
 #endif // RAW_RPM_HPP
+#if defined(NPFG_STATUS_HPP)
+	create_stream_list_item<MavlinkStreamNPFGStatus>()
+#endif // NPFG_STATUS
 };

 const char *get_stream_name(const uint16_t msg_id)
@@ -1144,6 +1144,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
 		//bool ignore_setpoints = bool(actuator_target.group_mlx != 2);

 		offboard_control_mode_s offboard_control_mode{};
+		offboard_control_mode.actuator = true;
 		offboard_control_mode.timestamp = hrt_absolute_time();
 		_offboard_control_mode_pub.publish(offboard_control_mode);

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef NPFG_STATUS_HPP
+#define NPFG_STATUS_HPP
+
+#include <uORB/topics/npfg_status.h>
+
+class MavlinkStreamNPFGStatus : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamNPFGStatus(mavlink); }
+
+	static constexpr const char *get_name_static() { return "NPFG_STATUS"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_NPFG_STATUS; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		if (_npfg_status_sub.advertised()) {
+			return MAVLINK_MSG_ID_NPFG_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		}
+
+		return 0;
+	}
+
+private:
+	explicit MavlinkStreamNPFGStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _npfg_status_sub{ORB_ID(npfg_status)};
+
+	bool send() override
+	{
+		struct npfg_status_s npfg_status;
+
+		if (_npfg_status_sub.update(&npfg_status)) {
+			mavlink_npfg_status_t msg{};
+
+			msg.timestamp = npfg_status.timestamp;
+			msg.wind_est_valid = npfg_status.wind_est_valid;
+			msg.lat_accel = npfg_status.lat_accel;
+			msg.lat_accel_ff = npfg_status.lat_accel_ff;
+			msg.bearing_feas = npfg_status.bearing_feas;
+			msg.bearing_feas_on_track = npfg_status.bearing_feas_on_track;
+			msg.signed_track_error = npfg_status.signed_track_error;
+			msg.track_error_bound = npfg_status.track_error_bound;
+			msg.airspeed_ref = npfg_status.airspeed_ref;
+			msg.bearing = math::degrees(npfg_status.bearing);
+			msg.heading_ref = math::degrees(npfg_status.heading_ref);
+			msg.min_ground_speed_ref = npfg_status.min_ground_speed_ref;
+			msg.adapted_period = npfg_status.adapted_period;
+			msg.p_gain = npfg_status.p_gain;
+			msg.time_const = npfg_status.time_const;
+
+			mavlink_msg_npfg_status_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // NPFG_STATUS
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+#ifndef SENSOR_AIRFLOW_ANGLES_HPP
+#define SENSOR_AIRFLOW_ANGLES_HPP
+
+#include <uORB/topics/airflow_aoa.h>
+#include <uORB/topics/airflow_slip.h>
+
+class MavlinkStreamSensorAirflowAngles : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSensorAirflowAngles(mavlink); }
+
+	static constexpr const char *get_name_static() { return "SENSOR_AIRFLOW_ANGLES"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		if (_airflow_aoa_sub.advertised() || _airflow_slip_sub.advertised()) {
+			return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		}
+
+		return 0;
+	}
+
+private:
+	explicit MavlinkStreamSensorAirflowAngles(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)};
+	uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)};
+
+	bool send() override
+	{
+		if (_airflow_aoa_sub.updated() || _airflow_slip_sub.updated()) {
+			mavlink_sensor_airflow_angles_t msg{};
+
+			struct airflow_aoa_s airflow_aoa;
+
+			if (_airflow_aoa_sub.copy(&airflow_aoa)) {
+				msg.timestamp = airflow_aoa.timestamp / 1000;
+				msg.angleofattack = math::degrees(airflow_aoa.aoa_rad);
+				msg.angleofattack_valid = airflow_aoa.valid;
+
+			} else {
+				msg.timestamp = 0;
+				msg.angleofattack = 0.0;
+				msg.angleofattack_valid = false;
+			}
+
+			struct airflow_slip_s airflow_slip;
+
+			if (_airflow_slip_sub.copy(&airflow_slip)) {
+				const uint64_t timestamp = airflow_slip.timestamp / 1000;
+
+				if (timestamp > msg.timestamp) {
+					msg.timestamp = timestamp;
+				}
+
+				msg.sideslip = math::degrees(airflow_slip.slip_rad);
+				msg.sideslip_valid = airflow_slip.valid;
+
+			} else {
+				msg.timestamp = 0;
+				msg.sideslip = 0.0;
+				msg.sideslip_valid = false;
+			}
+
+			mavlink_msg_sensor_airflow_angles_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // SENSOR_AIRFLOW_ANGLES
@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Driver ID of the angle of attack vane (corresponds to sensor I2C address)
+ *
+ * setting -1 disables the vane, 0-3 correspond to hall driver IDs
+ *
+ * @min -1
+ * @max 3
+ * @reboot_required true
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_AOA_ID, -1);
+
+/**
+ * Angle of attack vane angular mounting offset in degrees
+ *
+ * This value is subtracted (as a bias) from the measured angle of attack
+ *
+ * @unit deg
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AV_AOA_OFF, 0.0);
+
+/**
+ * Minimum calibrated angle of attack vane angle in degrees
+ *
+ * @unit deg
+ * @min -90.0
+ * @max -1.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AV_AOA_MIN, -30.0);
+
+/**
+ * Maximum calibrated angle of attack vane angle in degrees
+ *
+ * @unit deg
+ * @min 1.0
+ * @max 90.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AV_AOA_MAX, 30.0);
+
+/**
+ * Reverse vane direction (mounting dependent)
+ *
+ * @min -1.0
+ * @max 1.0
+ * @value -1.0 Reverse
+ * @value 1.0 Normal
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AV_AOA_REV, 1.0);
+
+/**
+ * Calibrated polynomial coefficient 0 (*1e7)
+ *
+ * y = p0 + p1 * x + p2 * x^2 + ...
+ * y = vane angle in degrees
+ * x = magnetic field measurement in Tesla (assumes use of hall effect sensor)
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_AOA_P0, 0);
+
+/**
+ * Calibrated polynomial coefficient 1 (*1e7)
+ *
+ * y = p0 + p1 * x + p2 * x^2 + ...
+ * y = angle in degrees
+ * x = magnetic field measurement in Tesla (assumes use of hall effect sensor)
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_AOA_P1, 0);
+
+/**
+ * Calibrated polynomial coefficient 2 (*1e7)
+ *
+ * y = p0 + p1 * x + p2 * x^2 + ...
+ * y = angle in degrees
+ * x = magnetic field measurement in Tesla (assumes use of hall effect sensor)
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_AOA_P2, 0);
+
+/**
+ * Calibrated polynomial coefficient 3 (*1e7)
+ *
+ * y = p0 + p1 * x + p2 * x^2 + ...
+ * y = angle in degrees
+ * x = magnetic field measurement in Tesla (assumes use of hall effect sensor)
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_AOA_P3, 0);
+
+/**
+ * Driver ID of the sideslip vane (corresponds to sensor I2C address)
+ *
+ * setting -1 disables the vane, 0-3 correspond to hall driver IDs
+ *
+ * @min -1
+ * @max 3
+ * @reboot_required true
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_SLIP_ID, -1);
+
+/**
+ * Sideslip vane angular mounting offset in degrees
+ *
+ * This value is subtracted (as a bias) from the measured sideslip
+ *
+ * @unit deg
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AV_SLIP_OFF, 0.0);
+
+/**
+ * Minimum calibrated sideslip vane angle in degrees
+ *
+ * @unit deg
+ * @min -90.0
+ * @max -1.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AV_SLIP_MIN, -30.0);
+
+/**
+ * Maximum calibrated sideslip vane angle in degrees
+ *
+ * @unit deg
+ * @min 1.0
+ * @max 90.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AV_SLIP_MAX, 30.0);
+
+/**
+ * Reverse vane direction (mounting dependent)
+ *
+ * @min -1.0
+ * @max 1.0
+ * @value -1.0 Reverse
+ * @value 1.0 Normal
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AV_SLIP_REV, 1.0);
+
+/**
+ * Calibrated polynomial coefficient 0 (*1e7)
+ *
+ * y = p0 + p1 * x + p2 * x^2 + ...
+ * y = vane angle in degrees
+ * x = magnetic field measurement in Tesla (assumes use of hall effect sensor)
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_SLIP_P0, 0);
+
+/**
+ * Calibrated polynomial coefficient 1 (*1e7)
+ *
+ * y = p0 + p1 * x + p2 * x^2 + ...
+ * y = angle in degrees
+ * x = magnetic field measurement in Tesla (assumes use of hall effect sensor)
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_SLIP_P1, 0);
+
+/**
+ * Calibrated polynomial coefficient 2 (*1e7)
+ *
+ * y = p0 + p1 * x + p2 * x^2 + ...
+ * y = angle in degrees
+ * x = magnetic field measurement in Tesla (assumes use of hall effect sensor)
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_SLIP_P2, 0);
+
+/**
+ * Calibrated polynomial coefficient 3 (*1e7)
+ *
+ * y = p0 + p1 * x + p2 * x^2 + ...
+ * y = angle in degrees
+ * x = magnetic field measurement in Tesla (assumes use of hall effect sensor)
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AV_SLIP_P3, 0);
@@ -61,10 +61,13 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/airflow_aoa.h>
+#include <uORB/topics/airflow_slip.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensors_status_imu.h>
+#include <uORB/topics/sensor_hall.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu.h>
@@ -129,9 +132,20 @@ private:
 	uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
 	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _sensor_hall_subs[MAX_SENSOR_COUNT] { // could be set to ORB_MULTI_MAX_INSTANCES if needed
+		{ORB_ID(sensor_hall), 0},
+		{ORB_ID(sensor_hall), 1},
+		{ORB_ID(sensor_hall), 2},
+		{ORB_ID(sensor_hall), 3},
+	};
+
+	int _av_aoa_hall_sub_index = -1; // AoA vane index for hall effect subscription
+	int _av_slip_hall_sub_index = -1; // Slip vane index for hall effect subscription

 	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<sensor_combined_s>      _sensor_pub{ORB_ID(sensor_combined)};
+	uORB::Publication<airflow_aoa_s>          _airflow_aoa_pub{ORB_ID(airflow_aoa)};
+	uORB::Publication<airflow_slip_s>         _airflow_slip_pub{ORB_ID(airflow_slip)};

 	perf_counter_t	_loop_perf;			/**< loop performance counter */

@@ -156,6 +170,26 @@ private:
 		int32_t air_cmodel;
 		float air_tube_length;
 		float air_tube_diameter_mm;
+
+		int32_t CAL_AV_AOA_ID;
+		float CAL_AV_AOA_OFF;
+		float CAL_AV_AOA_MIN;
+		float CAL_AV_AOA_MAX;
+		float CAL_AV_AOA_REV;
+		float CAL_AV_AOA_P0;
+		float CAL_AV_AOA_P1;
+		float CAL_AV_AOA_P2;
+		float CAL_AV_AOA_P3;
+
+		int32_t CAL_AV_SLIP_ID;
+		float CAL_AV_SLIP_OFF;
+		float CAL_AV_SLIP_MIN;
+		float CAL_AV_SLIP_MAX;
+		float CAL_AV_SLIP_REV;
+		float CAL_AV_SLIP_P0;
+		float CAL_AV_SLIP_P1;
+		float CAL_AV_SLIP_P2;
+		float CAL_AV_SLIP_P3;
 	} _parameters{}; /**< local copies of interesting parameters */

 	struct ParameterHandles {
@@ -167,6 +201,26 @@ private:
 		param_t air_cmodel;
 		param_t air_tube_length;
 		param_t air_tube_diameter_mm;
+
+		param_t CAL_AV_AOA_ID;
+		param_t CAL_AV_AOA_OFF;
+		param_t CAL_AV_AOA_MIN;
+		param_t CAL_AV_AOA_MAX;
+		param_t CAL_AV_AOA_REV;
+		param_t CAL_AV_AOA_P0;
+		param_t CAL_AV_AOA_P1;
+		param_t CAL_AV_AOA_P2;
+		param_t CAL_AV_AOA_P3;
+
+		param_t CAL_AV_SLIP_ID;
+		param_t CAL_AV_SLIP_OFF;
+		param_t CAL_AV_SLIP_MIN;
+		param_t CAL_AV_SLIP_MAX;
+		param_t CAL_AV_SLIP_REV;
+		param_t CAL_AV_SLIP_P0;
+		param_t CAL_AV_SLIP_P1;
+		param_t CAL_AV_SLIP_P2;
+		param_t CAL_AV_SLIP_P3;
 	} _parameter_handles{};		/**< handles for interesting parameters */

 	VotedSensorsUpdate _voted_sensors_update;
@@ -205,6 +259,16 @@ private:
 	 */
 	void		adc_poll();

+	/**
+	 * Poll the hall effect sensor for updated data.
+	 */
+	void    hall_poll();
+
+	/**
+	 * Get the hall sensor subscription index for a given driver id.
+	 */
+	int   getHallSubIndex(const int av_driver_id);
+
 	void		InitializeVehicleAirData();
 	void		InitializeVehicleGPSPosition();
 	void		InitializeVehicleIMU();
@@ -233,6 +297,25 @@ Sensors::Sensors(bool hil_enabled) :
 	_parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
 	_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
 	_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
+	_parameter_handles.CAL_AV_AOA_ID = param_find("CAL_AV_AOA_ID");
+	_parameter_handles.CAL_AV_AOA_OFF = param_find("CAL_AV_AOA_OFF");
+	_parameter_handles.CAL_AV_AOA_MIN = param_find("CAL_AV_AOA_MIN");
+	_parameter_handles.CAL_AV_AOA_MAX = param_find("CAL_AV_AOA_MAX");
+	_parameter_handles.CAL_AV_AOA_REV = param_find("CAL_AV_AOA_REV");
+	_parameter_handles.CAL_AV_AOA_P0 = param_find("CAL_AV_AOA_P0");
+	_parameter_handles.CAL_AV_AOA_P1 = param_find("CAL_AV_AOA_P1");
+	_parameter_handles.CAL_AV_AOA_P2 = param_find("CAL_AV_AOA_P2");
+	_parameter_handles.CAL_AV_AOA_P3 = param_find("CAL_AV_AOA_P3");
+
+	_parameter_handles.CAL_AV_SLIP_ID = param_find("CAL_AV_SLIP_ID");
+	_parameter_handles.CAL_AV_SLIP_OFF = param_find("CAL_AV_SLIP_OFF");
+	_parameter_handles.CAL_AV_SLIP_MIN = param_find("CAL_AV_SLIP_MIN");
+	_parameter_handles.CAL_AV_SLIP_MAX = param_find("CAL_AV_SLIP_MAX");
+	_parameter_handles.CAL_AV_SLIP_REV = param_find("CAL_AV_SLIP_REV");
+	_parameter_handles.CAL_AV_SLIP_P0 = param_find("CAL_AV_SLIP_P0");
+	_parameter_handles.CAL_AV_SLIP_P1 = param_find("CAL_AV_SLIP_P1");
+	_parameter_handles.CAL_AV_SLIP_P2 = param_find("CAL_AV_SLIP_P2");
+	_parameter_handles.CAL_AV_SLIP_P3 = param_find("CAL_AV_SLIP_P3");

 	param_find("SYS_FAC_CAL_MODE");

@@ -306,6 +389,35 @@ int Sensors::parameters_update()
 	param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length);
 	param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm);

+	param_get(_parameter_handles.CAL_AV_AOA_ID, &_parameters.CAL_AV_AOA_ID);
+	param_get(_parameter_handles.CAL_AV_AOA_OFF, &_parameters.CAL_AV_AOA_OFF);
+	param_get(_parameter_handles.CAL_AV_AOA_MIN, &_parameters.CAL_AV_AOA_MIN);
+	param_get(_parameter_handles.CAL_AV_AOA_MAX, &_parameters.CAL_AV_AOA_MAX);
+	param_get(_parameter_handles.CAL_AV_AOA_REV, &_parameters.CAL_AV_AOA_REV);
+	int32_t temp_param{0};
+	param_get(_parameter_handles.CAL_AV_AOA_P0, &temp_param);
+	_parameters.CAL_AV_AOA_P0 = float(temp_param) * 1e-7f;
+	param_get(_parameter_handles.CAL_AV_AOA_P1, &temp_param);
+	_parameters.CAL_AV_AOA_P1 = float(temp_param) * 1e-7f;
+	param_get(_parameter_handles.CAL_AV_AOA_P2, &temp_param);
+	_parameters.CAL_AV_AOA_P2 = float(temp_param) * 1e-7f;
+	param_get(_parameter_handles.CAL_AV_AOA_P3, &temp_param);
+	_parameters.CAL_AV_AOA_P3 = float(temp_param) * 1e-7f;
+
+	param_get(_parameter_handles.CAL_AV_SLIP_ID, &_parameters.CAL_AV_SLIP_ID);
+	param_get(_parameter_handles.CAL_AV_SLIP_OFF, &_parameters.CAL_AV_SLIP_OFF);
+	param_get(_parameter_handles.CAL_AV_SLIP_MIN, &_parameters.CAL_AV_SLIP_MIN);
+	param_get(_parameter_handles.CAL_AV_SLIP_MAX, &_parameters.CAL_AV_SLIP_MAX);
+	param_get(_parameter_handles.CAL_AV_SLIP_REV, &_parameters.CAL_AV_SLIP_REV);
+	param_get(_parameter_handles.CAL_AV_SLIP_P0, &temp_param);
+	_parameters.CAL_AV_SLIP_P0 = float(temp_param) * 1e-7f;
+	param_get(_parameter_handles.CAL_AV_SLIP_P1, &temp_param);
+	_parameters.CAL_AV_SLIP_P1 = float(temp_param) * 1e-7f;
+	param_get(_parameter_handles.CAL_AV_SLIP_P2, &temp_param);
+	_parameters.CAL_AV_SLIP_P2 = float(temp_param) * 1e-7f;
+	param_get(_parameter_handles.CAL_AV_SLIP_P3, &temp_param);
+	_parameters.CAL_AV_SLIP_P3 = float(temp_param) * 1e-7f;
+
 	_voted_sensors_update.parametersUpdate();

 	return PX4_OK;
@@ -475,6 +587,108 @@ void Sensors::adc_poll()
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 }

+void Sensors::hall_poll()
+{
+	// process aoa measurements if enabled and valid
+	if ((_av_aoa_hall_sub_index >= 0) && (_av_aoa_hall_sub_index < MAX_SENSOR_COUNT)) {
+		// A hall sensor with selected driver ID for the AoA vane has been found/set
+		struct sensor_hall_s sensor_hall;
+
+		if (_sensor_hall_subs[_av_aoa_hall_sub_index].update(&sensor_hall)) {
+			airflow_aoa_s airflow_aoa;
+			airflow_aoa.timestamp = hrt_absolute_time();
+			airflow_aoa.valid = false;
+
+			float aoa_deg = _parameters.CAL_AV_AOA_REV * (_parameters.CAL_AV_AOA_P0
+					+ _parameters.CAL_AV_AOA_P1 * sensor_hall.mag_t
+					+ _parameters.CAL_AV_AOA_P2 * sensor_hall.mag_t * sensor_hall.mag_t
+					+ _parameters.CAL_AV_AOA_P3 * sensor_hall.mag_t * sensor_hall.mag_t * sensor_hall.mag_t);
+
+			// check if aoa measurement is in sensor/calibration range
+			if (aoa_deg > _parameters.CAL_AV_AOA_MAX) {
+				airflow_aoa.aoa_rad = math::radians(_parameters.CAL_AV_AOA_MAX);
+
+			} else if (aoa_deg >= _parameters.CAL_AV_AOA_MIN) {
+				airflow_aoa.aoa_rad = math::radians(aoa_deg - _parameters.CAL_AV_AOA_OFF);
+				airflow_aoa.valid = true;
+
+			} else {
+				airflow_aoa.aoa_rad = math::radians(_parameters.CAL_AV_AOA_MIN);
+			}
+
+			_airflow_aoa_pub.publish(airflow_aoa);
+		}
+	}
+
+
+	// process sideslip measurements if enabled and valid
+	if ((_av_slip_hall_sub_index >= 0) && (_av_slip_hall_sub_index < MAX_SENSOR_COUNT)) {
+		// A hall sensor with selected driver ID for the slip vane has been found/set
+		struct sensor_hall_s sensor_hall;
+
+		if (_sensor_hall_subs[_av_slip_hall_sub_index].update(&sensor_hall)) {
+			airflow_slip_s airflow_slip;
+			airflow_slip.timestamp = hrt_absolute_time();
+			airflow_slip.valid = false;
+
+			float slip_deg = _parameters.CAL_AV_SLIP_REV * (_parameters.CAL_AV_SLIP_P0
+					 + _parameters.CAL_AV_SLIP_P1 * sensor_hall.mag_t
+					 + _parameters.CAL_AV_SLIP_P2 * sensor_hall.mag_t * sensor_hall.mag_t
+					 + _parameters.CAL_AV_SLIP_P3 * sensor_hall.mag_t * sensor_hall.mag_t * sensor_hall.mag_t);
+
+			/* check if slip measurement is in sensor/calibration range */
+			if (slip_deg > _parameters.CAL_AV_SLIP_MAX) {
+				airflow_slip.slip_rad = math::radians(_parameters.CAL_AV_SLIP_MAX);
+
+			} else if (slip_deg >= _parameters.CAL_AV_SLIP_MIN) {
+				airflow_slip.slip_rad = math::radians(slip_deg - _parameters.CAL_AV_SLIP_OFF);
+				airflow_slip.valid = true;
+
+			} else {
+				airflow_slip.slip_rad = math::radians(_parameters.CAL_AV_SLIP_MIN);
+			}
+
+			_airflow_slip_pub.publish(airflow_slip);
+		}
+	}
+
+	// finding the indices needs to be done after processing the measurements since we are copying the
+	// messages and that would mess with the updated flags of the subscribers
+
+	// find the AoA hall sensor index if not set yet
+	if (_parameters.CAL_AV_AOA_ID >= 0 && _av_aoa_hall_sub_index < 0) {
+		_av_aoa_hall_sub_index = getHallSubIndex(_parameters.CAL_AV_AOA_ID);
+
+		if (_av_aoa_hall_sub_index >= 0) {
+			PX4_INFO("AoA vane using hall sensor with driver ID %d", _parameters.CAL_AV_AOA_ID);
+		}
+	}
+
+	// find the Slip hall sensor index if not set yet
+	if (_parameters.CAL_AV_SLIP_ID >= 0 && _av_slip_hall_sub_index < 0) {
+		_av_slip_hall_sub_index = getHallSubIndex(_parameters.CAL_AV_SLIP_ID);
+
+		if (_av_slip_hall_sub_index >= 0) {
+			PX4_INFO("Slip vane using hall sensor with driver ID %d", _parameters.CAL_AV_SLIP_ID);
+		}
+	}
+}
+
+int Sensors::getHallSubIndex(const int av_driver_id)
+{
+	for (unsigned i = 0; i < MAX_SENSOR_COUNT; i++) {
+		sensor_hall_s sensor_hall;
+
+		if (_sensor_hall_subs[i].copy(&sensor_hall)) {
+			if (sensor_hall.instance == av_driver_id) {
+				return i;
+			}
+		}
+	}
+
+	return -1;
+}
+
 void Sensors::InitializeVehicleAirData()
 {
 	if (_param_sys_has_baro.get()) {
@@ -600,6 +814,7 @@ void Sensors::Run()
 	adc_poll();

 	diff_pres_poll();
+	hall_poll();

 	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
Author	SHA1	Message	Date
Jaeyoung-Lim	f5a73e0e3c	Add windshear plugin This commit adds SITL targets for the dynamic soaring aircraft and additional plugins in the sitl_gazebo submodule	2022-02-20 12:44:26 +01: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