uORB: Add subscription with single field selection

CMakeLists.txt

@@ -256,7 +256,7 @@ endif()
 
 set(package-contact "px4users@googlegroups.com")
 
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_C_STANDARD_REQUIRED ON)

ROMFS/px4fmu_common/init.d-posix/airframes/10016_none_iris

@@ -1,30 +0,0 @@
-#!/bin/sh
-#
-# @name Generic Iris Quadrotor SITL
-#
-# @type Quadrotor Wide
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-
-param set-default CA_AIRFRAME 0
-
-param set-default CA_ROTOR_COUNT 4
-param set-default CA_ROTOR0_PX 0.1515
-param set-default CA_ROTOR0_PY 0.245
-param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR1_PX -0.1515
-param set-default CA_ROTOR1_PY -0.1875
-param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR2_PX 0.1515
-param set-default CA_ROTOR2_PY -0.245
-param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.1515
-param set-default CA_ROTOR3_PY 0.1875
-param set-default CA_ROTOR3_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104

ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal

@@ -25,6 +25,7 @@ param set-default FW_W_EN 1
 
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
+param set-default MIS_DIST_WPS 10000
 
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric

@@ -25,6 +25,7 @@ param set-default FW_W_EN 1
 
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
+param set-default MIS_DIST_WPS 10000
 
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo

@@ -25,6 +25,7 @@ param set-default FW_W_EN 1
 
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
+param set-default MIS_DIST_WPS 10000
 
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal

@@ -27,6 +27,7 @@ param set-default FW_W_EN 1
 param set-default MIS_LTRMIN_ALT 30
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
+param set-default MIS_DIST_WPS 10000
 
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter

@@ -44,12 +44,6 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_REV 96 # invert both elevons
 
-# Single-EKF (for replay)
-param set-default EKF2_MULTI_IMU 0
-param set-default SENS_IMU_MODE 1
-param set-default EKF2_MULTI_MAG 0
-param set-default SENS_MAG_MODE 1
-
 param set-default NPFG_PERIOD 12
 param set-default FW_PR_I 0.2
 param set-default FW_PR_P 0.2

ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth

@@ -5,6 +5,50 @@
 # @type Quadrotor
 #
 
+. ${R}etc/init.d/rc.mc_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_depth}
 
-. ${R}etc/init.d-posix/airframes/4001_gz_x500
+param set-default SIM_GZ_EN 1
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+
+param set-default CA_AIRFRAME 0
+param set-default CA_ROTOR_COUNT 4
+
+param set-default CA_ROTOR0_PX 0.13
+param set-default CA_ROTOR0_PY 0.22
+param set-default CA_ROTOR0_KM  0.05
+
+param set-default CA_ROTOR1_PX -0.13
+param set-default CA_ROTOR1_PY -0.20
+param set-default CA_ROTOR1_KM  0.05
+
+param set-default CA_ROTOR2_PX 0.13
+param set-default CA_ROTOR2_PY -0.22
+param set-default CA_ROTOR2_KM -0.05
+
+param set-default CA_ROTOR3_PX -0.13
+param set-default CA_ROTOR3_PY 0.20
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_FUNC4 104
+
+param set-default SIM_GZ_EC_MIN1 150
+param set-default SIM_GZ_EC_MIN2 150
+param set-default SIM_GZ_EC_MIN3 150
+param set-default SIM_GZ_EC_MIN4 150
+
+param set-default SIM_GZ_EC_MAX1 1000
+param set-default SIM_GZ_EC_MAX2 1000
+param set-default SIM_GZ_EC_MAX3 1000
+param set-default SIM_GZ_EC_MAX4 1000
+
+param set-default MPC_THR_HOVER 0.60

ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna

@@ -10,8 +10,6 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
 PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna}
 
-param set-default SIM_GZ_EN 1
-
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 0
 param set-default SENS_EN_MAGSIM 1

ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol

@@ -11,8 +11,6 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
 PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
 
-param set-default SIM_GZ_EN 1
-
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 0
 param set-default SENS_EN_MAGSIM 1

ROMFS/px4fmu_common/init.d-posix/airframes/4005_gz_x500_vision

@@ -5,6 +5,8 @@
 # @type Quadrotor
 #
 
-PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_vision}
-
 . ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_vision}

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

@@ -1,54 +0,0 @@
-#!/bin/sh
-#
-# @name Gazebo x500 vision
-#
-# @type Quadrotor
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
-PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
-PX4_SIM_MODEL=${PX4_SIM_MODEL:=crazyflie}
-
-param set-default SIM_GZ_EN 1
-
-param set-default SENS_EN_GPSSIM 1
-param set-default SENS_EN_BAROSIM 0
-param set-default SENS_EN_MAGSIM 1
-
-param set-default CA_AIRFRAME 0
-param set-default CA_ROTOR_COUNT 4
-
-param set-default CA_ROTOR0_PX 0.13
-param set-default CA_ROTOR0_PY 0.22
-param set-default CA_ROTOR0_KM  0.05
-
-param set-default CA_ROTOR1_PX -0.13
-param set-default CA_ROTOR1_PY -0.20
-param set-default CA_ROTOR1_KM  0.05
-
-param set-default CA_ROTOR2_PX 0.13
-param set-default CA_ROTOR2_PY -0.22
-param set-default CA_ROTOR2_KM -0.05
-
-param set-default CA_ROTOR3_PX -0.13
-param set-default CA_ROTOR3_PY 0.20
-param set-default CA_ROTOR3_KM -0.05
-
-param set-default SIM_GZ_EC_FUNC1 101
-param set-default SIM_GZ_EC_FUNC2 102
-param set-default SIM_GZ_EC_FUNC3 103
-param set-default SIM_GZ_EC_FUNC4 104
-
-param set-default SIM_GZ_EC_MIN1 150
-param set-default SIM_GZ_EC_MIN2 150
-param set-default SIM_GZ_EC_MIN3 150
-param set-default SIM_GZ_EC_MIN4 150
-
-param set-default SIM_GZ_EC_MAX1 1000
-param set-default SIM_GZ_EC_MAX2 1000
-param set-default SIM_GZ_EC_MAX3 1000
-param set-default SIM_GZ_EC_MAX4 1000
-
-param set-default MPC_THR_HOVER 0.60

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

@@ -75,13 +75,11 @@ px4_add_romfs_files(
 	4003_gz_rc_cessna
 	4004_gz_standard_vtol
 	4005_gz_x500_vision
-	4006_gz_crazyflie
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
 
-	10015_gazebo-classic_iris
-	10016_none_iris
+	10016_gazebo-classic_iris
 	10017_jmavsim_iris
 	10018_gazebo-classic_iris_foggy_lidar
 	10019_gazebo-classic_omnicopter

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -277,7 +277,7 @@ then
 	# Override namespace if environment variable is defined
 	microdds_ns="-n $PX4_MICRODDS_NS"
 fi
-microdds_client start -t udp -h 127.0.0.1 -p 8888 $microdds_ns
+microdds_client start -t udp -p 8888 $microdds_ns
 
 if param greater -s MNT_MODE_IN -1
 then

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -79,6 +79,7 @@ param set-default MC_YAWRATE_MAX 20
 param set-default MC_AIRMODE 1
 
 param set-default MIS_DIST_1WP 100
+param set-default MIS_DIST_WPS 100000
 param set-default MIS_TAKEOFF_ALT 15
 
 param set-default MPC_XY_P 0.8

ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2

@@ -33,10 +33,3 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.25
 param set-default CA_ROTOR3_PY 0.25
 param set-default CA_ROTOR3_KM -0.05
-
-param set-default PWM_AUX_FUNC1 101
-param set-default PWM_AUX_FUNC2 102
-param set-default PWM_AUX_FUNC3 103
-param set-default PWM_AUX_FUNC4 104
-param set-default PWM_AUX_TIM0 -4
-

ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250

@@ -58,6 +58,8 @@ param set-default CA_ROTOR3_PX -0.15
 param set-default CA_ROTOR3_PY 0.15
 param set-default CA_ROTOR3_KM -0.05
 
+param set-default PWM_MAIN_TIM0 0
+
 param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 102
 param set-default PWM_MAIN_FUNC3 103

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -46,6 +46,7 @@ param set-default RTL_LAND_DELAY -1
 #
 param set-default NAV_ACC_RAD 10
 
+param set-default MIS_DIST_WPS 5000
 param set-default MIS_TAKEOFF_ALT 25
 param set-default MIS_TKO_LAND_REQ 2
 

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -23,6 +23,7 @@ param set-default EKF2_FUSE_BETA 1
 
 param set-default HTE_VXY_THR 2.0
 
+param set-default MIS_DIST_WPS 5000
 param set-default MIS_TKO_LAND_REQ 2
 
 param set-default MPC_ACC_HOR_MAX 2

Tools/convert_ip.py

@@ -1,51 +0,0 @@
-#!/usr/bin/env python3
-
-"""
-Converts IP address given in decimal dot notation to int32 to be used in XRCE_DDS_0_CFG parameter
-and vice-versa
-
-@author: Beniamino Pozzan (beniamino.pozzan@phd.unipd.it)
-"""
-
-
-import argparse
-
-parser = argparse.ArgumentParser(
-    prog = 'convert_ip',
-    description = 'converts IP to int32 and viceversa'
-    )
-parser.add_argument('input',
-                    type=str,
-                    help='IP address to convert')
-parser.add_argument('-r','--reverse',
-                    action='store_true',
-                    help='converts from int32 to dot decimal')
-
-args = parser.parse_args()
-
-if( args.reverse == False ):
-
-    try:
-        ip_parts = [int(x) for x in args.input.split('.')]
-    except:
-        raise ValueError("Not a valid IP")
-    if( len(ip_parts)!=4 ):
-        raise ValueError("Not a valid IP")
-    if( not all(x>=0 and x<255 for x in ip_parts) ):
-        raise ValueError("Not a valid IP")
-
-    ip = (ip_parts[0]<<24) + (ip_parts[1]<<16) + (ip_parts[2]<<8) + ip_parts[3]
-
-    if(ip & 0x80000000):
-        ip = -0x100000000 + ip
-
-    print(ip)
-
-else:
-    try:
-        ip = int(args.input)
-    except:
-        raise ValueError("Not a valid IP")
-    if(ip < 0):
-        ip = ip + 0x100000000
-    print('{}.{}.{}.{}'.format(ip>>24, (ip>>16)&0xff, (ip>>8)&0xff, ip&0xff))

Tools/simulation/gz/models/crazyflie/model.config

@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-<!--
- ...........       ____  _ __
- |  ,-^-,  |      / __ )(_) /_______________ _____  ___
- | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
- | / ,..´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
-    +.......   /_____/_/\__/\___/_/   \__,_/ /___/\___/
-
-MIT License
-
-Copyright (c) 2022 Bitcraze
--->
-<model>
-  <name>crazyflie</name>
-  <version>1.0</version>
-  <sdf version="1.6">model.sdf</sdf>
-  <author>
-    <name>Kimberly McGuire (Bitcraze AB)</name>
-    <email>kimberly@bitcraze.io</email>
-  </author>
-  <description>
-    A model of the Crazyflie 2.X nano-quadcopter
-  </description>
-</model>

Tools/simulation/gz/models/crazyflie/model.sdf

@@ -1,290 +0,0 @@
-<?xml version="1.0" ?>
-<!--
- ...........       ____  _ __
- |  ,-^-,  |      / __ )(_) /_______________ _____  ___
- | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
- | / ,..´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
-    +.......   /_____/_/\__/\___/_/   \__,_/ /___/\___/
-
-MIT License
-
-Copyright (c) 2022 Bitcraze
--->
-
-<sdf version="1.8">
-    <model name="crazyflie">
-        <pose>0 0 0.0 0 0 3.14</pose>
-        <link name="crazyflie/body">
-        <inertial>
-            <pose>0 0 0 0 0 0</pose>
-            <mass>0.025</mass>
-            <inertia>
-                <ixx>0.000016572</ixx>
-                <ixy>0.00</ixy>
-                <ixz>0.00</ixz>
-                <iyy>0.000016656</iyy>
-                <iyz>0.00000</iyz>
-                <izz>0.000029262</izz>
-            </inertia>
-        </inertial>
-        <collision name="crazyflie/body_collision">
-            <pose>0 0 0 0 0 0</pose>
-            <geometry>
-                <box>
-                    <size>0.10 0.10 0.03</size>
-                </box>
-            </geometry>
-        </collision>
-        <visual name="crazyflie/body_visual">
-            <pose>0 0 0 0 -0 0</pose>
-            <geometry>
-                <mesh>
-                    <scale>1 1 1</scale>
-                    <uri>model://crazyflie/meshes/collada_files/cf2_assembly.dae</uri>
-                </mesh>
-            </geometry>
-        </visual>
-        </link>
-        <link name="crazyflie/m1_prop">
-            <pose>0.031 -0.031 0.021 0 0 0</pose>
-            <inertial>
-                <pose>0 0 0 0 -0 0</pose>
-                <mass>0.0008</mass>
-                <inertia>
-                    <ixx>0.000000002*70</ixx>
-                    <iyy>0.000000167*70</iyy>
-                    <izz>0.000000168*70</izz>
-                    <ixy>0</ixy>
-                    <ixz>0</ixz>
-                    <iyz>0</iyz>
-                </inertia>
-            </inertial>
-            <visual name="crazyflie/m1_visual">
-                <pose>0 0 0 0 -0 0</pose>
-                <geometry>
-                    <mesh>
-                        <scale>1 1 1</scale>
-                        <uri>model://crazyflie/meshes/collada_files/ccw_prop.dae</uri>
-                    </mesh>
-                </geometry>
-            </visual>
-        </link>
-        <link name="crazyflie/m2_prop">
-            <pose>-0.031 -0.031 0.021 0 0 0</pose>
-            <inertial>
-                <pose>0 0 0 0 -0 0</pose>
-                <mass>0.0008</mass>
-                <inertia>
-                    <ixx>0.000000002*50</ixx>
-                    <iyy>0.000000167*50</iyy>
-                    <izz>0.000000168*50</izz>
-                    <ixy>0</ixy>
-                    <ixz>0</ixz>
-                    <iyz>0</iyz>
-                </inertia>
-            </inertial>
-            <visual name="crazyflie/m2_visual">
-                <pose>0 0 0 0 -0 0</pose>
-                <geometry>
-                    <mesh>
-                        <scale>1 1 1</scale>
-                        <uri>model://crazyflie/meshes/collada_files/cw_prop.dae</uri>
-                    </mesh>
-                </geometry>
-            </visual>
-        </link>
-        <link name="crazyflie/m3_prop">
-            <pose >-0.031 0.031 0.021 0 0 0</pose>
-            <inertial>
-                <pose>0 0 0 0 -0 0</pose>
-                <mass>0.0008</mass>
-                <inertia>
-                    <ixx>0.000000002*50</ixx>
-                    <iyy>0.000000167*50</iyy>
-                    <izz>0.000000168*50</izz>
-                    <ixy>0</ixy>
-                    <ixz>0</ixz>
-                    <iyz>0</iyz>
-                </inertia>
-            </inertial>
-            <visual name="crazyflie/m3_visual">
-                <pose>0 0 0 0 -0 0</pose>
-                <geometry>
-                    <mesh>
-                        <scale>1 1 1</scale>
-                        <uri>model://crazyflie/meshes/collada_files/ccw_prop.dae</uri>
-                    </mesh>
-                </geometry>
-            </visual>
-        </link>
-        <link name="crazyflie/m4_prop">
-            <pose>0.031 0.031 0.021 0 0 0</pose>
-            <inertial>
-                <pose>0 0 0 0 -0 0</pose>
-                <mass>0.0008</mass>
-                <inertia>
-                    <ixx>0.000000002*50</ixx>
-                    <iyy>0.000000167*50</iyy>
-                    <izz>0.000000168*50</izz>
-                    <ixy>0</ixy>
-                    <ixz>0</ixz>
-                    <iyz>0</iyz>
-                </inertia>
-            </inertial>
-            <visual name="crazyflie/m4_visual">
-                <pose>0 0 0 0 -0 0</pose>
-                <geometry>
-                    <mesh>
-                        <scale>1 1 1</scale>
-                        <uri>model://crazyflie/meshes/collada_files/cw_prop.dae</uri>
-                    </mesh>
-                </geometry>
-            </visual>
-        </link>
-        <joint name="crazyflie/m1_joint" type="revolute">
-            <child>crazyflie/m1_prop</child>
-            <parent>crazyflie/body</parent>
-            <axis>
-                <xyz>0 0 1</xyz>
-                <limit>
-                    <lower>-1e+16</lower>
-                    <upper>1e+16</upper>
-                </limit>
-                <dynamics>
-                    <spring_reference>0</spring_reference>
-                    <spring_stiffness>0</spring_stiffness>
-                </dynamics>
-            </axis>
-        </joint>
-                <joint name="crazyflie/m2_joint" type="revolute">
-            <child>crazyflie/m2_prop</child>
-            <parent>crazyflie/body</parent>
-            <axis>
-                <xyz>0 0 1</xyz>
-                <limit>
-                    <lower>-1e+16</lower>
-                    <upper>1e+16</upper>
-                </limit>
-                <dynamics>
-                    <spring_reference>0</spring_reference>
-                    <spring_stiffness>0</spring_stiffness>
-                </dynamics>
-            </axis>
-        </joint>
-                <joint name="crazyflie/m3_joint" type="revolute">
-            <child>crazyflie/m3_prop</child>
-            <parent>crazyflie/body</parent>
-            <axis>
-                <xyz>0 0 1</xyz>
-                <limit>
-                    <lower>-1e+16</lower>
-                    <upper>1e+16</upper>
-                </limit>
-                <dynamics>
-                    <spring_reference>0</spring_reference>
-                    <spring_stiffness>0</spring_stiffness>
-                </dynamics>
-            </axis>
-        </joint>
-        <joint name="crazyflie/m4_joint" type="revolute">
-            <child>crazyflie/m4_prop</child>
-            <parent>crazyflie/body</parent>
-            <axis>
-                <xyz>0 0 1</xyz>
-                <limit>
-                    <lower>-1e+16</lower>
-                    <upper>1e+16</upper>
-                </limit>
-                <dynamics>
-                    <spring_reference>0</spring_reference>
-                    <spring_stiffness>0</spring_stiffness>
-                </dynamics>
-            </axis>
-        </joint>
-    <plugin
-        filename="ignition-gazebo-multicopter-motor-model-system"
-        name="gz::sim::systems::MulticopterMotorModel">
-        <robotNamespace>crazyflie</robotNamespace>
-        <jointName>crazyflie/m1_joint</jointName>
-        <linkName>crazyflie/m1_prop</linkName>
-        <turningDirection>ccw</turningDirection>
-        <timeConstantUp>0.0125</timeConstantUp>
-        <timeConstantDown>0.025</timeConstantDown>
-        <maxRotVelocity>2618</maxRotVelocity>
-        <motorConstant>1.28192e-08</motorConstant>
-        <momentConstant>0.005964552</momentConstant>
-        <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>0</motorNumber>
-        <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
-        <rollingMomentCoefficient>0.0000001</rollingMomentCoefficient>
-        <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
-        <rotorVelocitySlowdownSim>150</rotorVelocitySlowdownSim>
-        <motorType>velocity</motorType>
-      </plugin>
-      <plugin
-        filename="ignition-gazebo-multicopter-motor-model-system"
-        name="gz::sim::systems::MulticopterMotorModel">
-        <robotNamespace>crazyflie</robotNamespace>
-        <jointName>crazyflie/m2_joint</jointName>
-        <linkName>crazyflie/m2_prop</linkName>
-        <turningDirection>cw</turningDirection>
-        <timeConstantUp>0.0125</timeConstantUp>
-        <timeConstantDown>0.025</timeConstantDown>
-        <maxRotVelocity>2618</maxRotVelocity>
-        <motorConstant>1.28192e-08</motorConstant>
-        <momentConstant>0.005964552</momentConstant>
-        <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>1</motorNumber>
-        <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
-        <rollingMomentCoefficient>0.0000001</rollingMomentCoefficient>
-        <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
-        <rotorVelocitySlowdownSim>150</rotorVelocitySlowdownSim>
-        <motorType>velocity</motorType>
-      </plugin>
-      <plugin
-        filename="ignition-gazebo-multicopter-motor-model-system"
-        name="gz::sim::systems::MulticopterMotorModel">
-        <robotNamespace>crazyflie</robotNamespace>
-        <jointName>crazyflie/m3_joint</jointName>
-        <linkName>crazyflie/m3_prop</linkName>
-        <turningDirection>ccw</turningDirection>
-        <timeConstantUp>0.0125</timeConstantUp>
-        <timeConstantDown>0.025</timeConstantDown>
-        <maxRotVelocity>2618</maxRotVelocity>
-        <motorConstant>1.28192e-08</motorConstant>
-        <momentConstant>0.005964552</momentConstant>
-        <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>2</motorNumber>
-        <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
-        <rollingMomentCoefficient>0.0000001</rollingMomentCoefficient>
-        <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
-        <rotorVelocitySlowdownSim>150</rotorVelocitySlowdownSim>
-        <motorType>velocity</motorType>
-      </plugin>
-      <plugin
-        filename="ignition-gazebo-multicopter-motor-model-system"
-        name="gz::sim::systems::MulticopterMotorModel">
-        <robotNamespace>crazyflie</robotNamespace>
-        <jointName>crazyflie/m4_joint</jointName>
-        <linkName>crazyflie/m4_prop</linkName>
-        <turningDirection>cw</turningDirection>
-        <timeConstantUp>0.0125</timeConstantUp>
-        <timeConstantDown>0.025</timeConstantDown>
-        <maxRotVelocity>2618</maxRotVelocity>
-        <motorConstant>1.28192e-08</motorConstant>
-        <momentConstant>0.005964552</momentConstant>
-        <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>3</motorNumber>
-        <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
-        <rollingMomentCoefficient>0.000001</rollingMomentCoefficient>
-        <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
-        <rotorVelocitySlowdownSim>150</rotorVelocitySlowdownSim>
-        <motorType>velocity</motorType>
-      </plugin>
-      <plugin
-        filename="ignition-gazebo-odometry-publisher-system"
-        name="ignition::gazebo::systems::OdometryPublisher">
-        <dimensions>3</dimensions>
-      </plugin>
-    </model>
-</sdf>

Tools/simulation/gz/models/x500_depth/model.sdf

@@ -2,7 +2,7 @@
 <sdf version='1.9'>
   <model name='x500-Depth'>
     <include merge='true'>
-      <uri>x500</uri>
+      <uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</uri>
     </include>
     <include merge='true'>
       <uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/OakD-Lite</uri>

Tools/simulation/jmavsim/jmavsim_run.sh

@@ -12,7 +12,7 @@ baudrate=921600
 device=
 ip="127.0.0.1"
 protocol="tcp"
-while getopts "b:d:ui:p:qsr:loat" opt; do
+while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
 	case $opt in
 		b)
 			baudrate=$OPTARG

boards/px4/fmu-v2/src/init.c

@@ -166,7 +166,7 @@ __EXPORT void board_on_reset(int status)
 }
 
 /************************************************************************************
-  * Name: determine_hw_info
+  * Name: determin_hw_version
  *
  * Description:
  *
@@ -188,10 +188,7 @@ __EXPORT void board_on_reset(int status)
  *  10   10 - 0xA PixhawkMini
  *  10   11 - 0xB FMUv2 questionable hardware (should be treated like regular FMUv2)
  *
- *  This will return OK on success.
- *
- *  If PB12 is not returning a consistent result we assume that it's a Cube Black
- *  with something connected to CAN1 and talking to it.
+ *  This will return OK on success and -1 on not supported
  *
  *  hw_type Initial state is {'V','2',0, 0}
  *   V 2    - FMUv2
@@ -201,18 +198,19 @@ __EXPORT void board_on_reset(int status)
  *
  ************************************************************************************/
 
-static int determine_hw_info(int *revision, int *version)
+static int determin_hw_version(int *version, int *revision)
 {
 	*revision = 0; /* default revision */
+	int rv = 0;
 	int pos = 0;
 	stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB4 & ~GPIO_PUPD_MASK));
 	up_udelay(10);
-	*version |= stm32_gpioread(HW_VER_PB4) << pos++;
+	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
 	stm32_configgpio(HW_VER_PB4);
 	up_udelay(10);
-	*version |= stm32_gpioread(HW_VER_PB4) << pos++;
+	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
 
-	int votes = 100;
+	int votes = 16;
 	int ones[2] = {0, 0};
 	int zeros[2] = {0, 0};
 
@@ -225,31 +223,19 @@ static int determine_hw_info(int *revision, int *version)
 		stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;
 	}
 
-	const int margin = 50;
-	// On Cube the detection does not work as expected when something
-	// is connected to CAN1. In that case, there is no clear winner
-	// between ones and zeros.
+	if (ones[0] > zeros[0]) {
+		rv |= 1 << pos;
+	}
 
-	if (*version == 0x2 && abs(ones[0] - zeros[0]) < margin && abs(ones[1] - zeros[1]) < margin) {
-		*version = HW_VER_FMUV3_STATE;
-		syslog(LOG_DEBUG, "Ambiguous board detection, assuming Pixhawk Cube\n");
+	pos++;
 
-	} else {
-
-		if (ones[0] > zeros[0]) {
-			*version |= 1 << pos;
-		}
-
-		pos++;
-
-		if (ones[1] > zeros[1] + margin) {
-			*version |= 1 << pos;
-		}
+	if (ones[1] > zeros[1]) {
+		rv |= 1 << pos;
 	}
 
 	stm32_configgpio(HW_VER_PB4_INIT);
 	stm32_configgpio(HW_VER_PB12_INIT);
-
+	*version = rv;
 	return OK;
 }
 
@@ -380,7 +366,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	/* Ensure the power is on 1 ms before we drive the GPIO pins */
 	usleep(1000);
 
-	if (OK == determine_hw_info(&hw_revision, &hw_version)) {
+	if (OK == determin_hw_version(&hw_version, & hw_revision)) {
 		switch (hw_version) {
 		case HW_VER_FMUV2_STATE:
 			break;
@@ -528,6 +514,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 #endif
 
 	/* Configure the HW based on the manifest */
+
 	px4_platform_configure();
 
 	return OK;

boards/px4/fmu-v3/default.px4board

@@ -23,7 +23,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
 CONFIG_DRIVERS_IMU_L3GD20=y
 CONFIG_DRIVERS_IMU_LSM303D=y
-CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/px4/fmu-v3/src/init.c

@@ -166,7 +166,7 @@ __EXPORT void board_on_reset(int status)
 }
 
 /************************************************************************************
-  * Name: determine_hw_info
+  * Name: determin_hw_version
  *
  * Description:
  *
@@ -188,10 +188,7 @@ __EXPORT void board_on_reset(int status)
  *  10   10 - 0xA PixhawkMini
  *  10   11 - 0xB FMUv2 questionable hardware (should be treated like regular FMUv2)
  *
- *  This will return OK on success.
- *
- *  If PB12 is not returning a consistent result we assume that it's a Cube Black
- *  with something connected to CAN1 and talking to it.
+ *  This will return OK on success and -1 on not supported
  *
  *  hw_type Initial state is {'V','2',0, 0}
  *   V 2    - FMUv2
@@ -201,18 +198,19 @@ __EXPORT void board_on_reset(int status)
  *
  ************************************************************************************/
 
-static int determine_hw_info(int *revision, int *version)
+static int determin_hw_version(int *version, int *revision)
 {
 	*revision = 0; /* default revision */
+	int rv = 0;
 	int pos = 0;
 	stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB4 & ~GPIO_PUPD_MASK));
 	up_udelay(10);
-	*version |= stm32_gpioread(HW_VER_PB4) << pos++;
+	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
 	stm32_configgpio(HW_VER_PB4);
 	up_udelay(10);
-	*version |= stm32_gpioread(HW_VER_PB4) << pos++;
+	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
 
-	int votes = 100;
+	int votes = 16;
 	int ones[2] = {0, 0};
 	int zeros[2] = {0, 0};
 
@@ -225,31 +223,19 @@ static int determine_hw_info(int *revision, int *version)
 		stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;
 	}
 
-	const int margin = 50;
-	// On Cube the detection does not work as expected when something
-	// is connected to CAN1. In that case, there is no clear winner
-	// between ones and zeros.
+	if (ones[0] > zeros[0]) {
+		rv |= 1 << pos;
+	}
 
-	if (*version == 0x2 && abs(ones[0] - zeros[0]) < margin && abs(ones[1] - zeros[1]) < margin) {
-		*version = HW_VER_FMUV3_STATE;
-		syslog(LOG_DEBUG, "Ambiguous board detection, assuming Pixhawk Cube\n");
+	pos++;
 
-	} else {
-
-		if (ones[0] > zeros[0]) {
-			*version |= 1 << pos;
-		}
-
-		pos++;
-
-		if (ones[1] > zeros[1] + margin) {
-			*version |= 1 << pos;
-		}
+	if (ones[1] > zeros[1]) {
+		rv |= 1 << pos;
 	}
 
 	stm32_configgpio(HW_VER_PB4_INIT);
 	stm32_configgpio(HW_VER_PB12_INIT);
-
+	*version = rv;
 	return OK;
 }
 
@@ -380,7 +366,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	/* Ensure the power is on 1 ms before we drive the GPIO pins */
 	usleep(1000);
 
-	if (OK == determine_hw_info(&hw_revision, &hw_version)) {
+	if (OK == determin_hw_version(&hw_version, & hw_revision)) {
 		switch (hw_version) {
 		case HW_VER_FMUV2_STATE:
 			break;

boards/px4/fmu-v4/default.px4board

@@ -21,7 +21,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
-CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/px4/fmu-v5x/default.px4board

@@ -27,7 +27,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
-CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/px4/fmu-v5x/test.px4board

@@ -1,5 +1,4 @@
 CONFIG_COMMON_DISTANCE_SENSOR=n
-CONFIG_COMMON_INS=n
 CONFIG_DRIVERS_ADC_ADS1115=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n

boards/px4/fmu-v6c/default.px4board

@@ -17,7 +17,7 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
-CONFIG_COMMON_INS=y
+CONFIG_DRIVERS_INS_VECTORNAV=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y

boards/px4/fmu-v6x/default.px4board

@@ -25,7 +25,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
-CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y

msg/TecsStatus.msg

@@ -1,9 +1,7 @@
 uint64 timestamp		# time since system start (microseconds)
 
 float32 altitude_sp			# Altitude setpoint AMSL [m]
-float32 altitude_reference		# Altitude setpoint reference AMSL [m]
-float32 height_rate_reference		# Height rate setpoint reference [m/s]
-float32 height_rate_direct		# Direct height rate setpoint from velocity reference generator [m/s]
+float32 altitude_sp_filtered		# Altitude setpoint filtered AMSL [m]
 float32 height_rate_setpoint		# Height rate setpoint [m/s]
 float32 height_rate			# Height rate [m/s]
 float32 equivalent_airspeed_sp		# Equivalent airspeed setpoint [m/s]

platforms/common/uORB/Subscription.hpp

@@ -245,4 +245,71 @@ private:
 	T _data{};
 };
 
+template<auto member>
+class SubscriptionSelection;
+
+template <class T, class R, R T::*member>
+class SubscriptionSelection<member>: public Subscription
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param id The uORB metadata ORB_ID enum for the topic.
+	 * @param instance The instance for multi sub.
+	 */
+	SubscriptionSelection(ORB_ID id, uint8_t instance = 0) :
+		Subscription(id, instance)
+	{
+		_copySelection();
+	}
+
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
+	 * @param instance The instance for multi sub.
+	 */
+	SubscriptionSelection(const orb_metadata *meta, uint8_t instance = 0) :
+		Subscription(meta, instance)
+	{
+		_copySelection();
+	}
+
+	~SubscriptionSelection() = default;
+
+	// no copy, assignment, move, move assignment
+	SubscriptionSelection(const SubscriptionSelection &) = delete;
+	SubscriptionSelection &operator=(const SubscriptionSelection &) = delete;
+	SubscriptionSelection(SubscriptionSelection &&) = delete;
+	SubscriptionSelection &operator=(SubscriptionSelection &&) = delete;
+
+	// update the embedded struct.
+	bool update()
+	{
+		bool updated = Subscription::updated();
+
+		if (updated) {
+			_copySelection();
+		}
+
+		return updated;
+	}
+
+	const R &get() const { return _data; }
+
+private:
+	void _copySelection()
+	{
+		T full_data;
+
+		if (copy(&full_data)) {
+			_data = full_data.*member;
+		}
+	}
+
+	R _data{};
+};
+
+
 } // namespace uORB

platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c

@@ -66,6 +66,19 @@ static char hw_info[HW_INFO_SIZE] = {0};
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
+/****************************************************************************
+  * Name: determin_hw_version
+ *
+ * Description:
+ *
+ * This function fist determines if revision  and version resistors are in place.
+ * if they it will read the ADC channels and decode the DN to ordinal numbers
+ * that will be returned by board_get_hw_version and board_get_hw_revision API
+ *
+ *  This will return OK on success and -1 on not supported
+*
+ *
+ ****************************************************************************/
 
 static int dn_to_ordinal(uint16_t dn)
 {

platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c

@@ -71,6 +71,19 @@ static char hw_info[HW_INFO_SIZE] = {0};
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
+/****************************************************************************
+  * Name: determin_hw_version
+ *
+ * Description:
+ *
+ * This function fist determines if revision  and version resistors are in place.
+ * if they it will read the ADC channels and decode the DN to ordinal numbers
+ * that will be returned by board_get_hw_version and board_get_hw_revision API
+ *
+ *  This will return OK on success and -1 on not supported
+*
+ *
+ ****************************************************************************/
 
 static int dn_to_ordinal(uint16_t dn)
 {

src/drivers/gps/gps.cpp

@@ -105,7 +105,7 @@ struct GPS_Sat_Info {
 	satellite_info_s _data;
 };
 
-static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(1990);
+static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(1760);
 
 
 class GPS : public ModuleBase<GPS>, public device::Device

src/drivers/gps/params.c

@@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
  * Auto-detection will probe all protocols, and thus is a bit slower.
  *
  * @min 0
- * @max 6
+ * @max 5
  * @value 0 Auto detect
  * @value 1 u-blox
  * @value 2 MTK
@@ -198,7 +198,7 @@ PARAM_DEFINE_INT32(GPS_1_PROTOCOL, 1);
  * Auto-detection will probe all protocols, and thus is a bit slower.
  *
  * @min 0
- * @max 6
+ * @max 5
  * @value 0 Auto detect
  * @value 1 u-blox
  * @value 2 MTK

src/drivers/ins/vectornav/CMakeLists.txt

@@ -39,6 +39,7 @@ px4_add_module(
 	INCLUDES
 		libvnc/include
 	COMPILE_FLAGS
+		-Wno-error
 	SRCS
 		VectorNav.cpp
 		VectorNav.hpp

src/drivers/ins/vectornav/libvnc/CMakeLists.txt

@@ -56,6 +56,7 @@ add_library(libvnc ${SOURCES})
 add_dependencies(libvnc prebuild_targets)
 target_compile_options(libvnc
         PRIVATE
+                -Wno-error
                 -Wno-double-promotion
                 -Wno-pointer-sign
                 -Wno-cast-align

src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c

@@ -2,7 +2,7 @@
 #include <string.h>
 #include "vn/util.h"
 
-//#define UNUSED(x) (void)(sizeof(x))
+#define UNUSED(x) (void)(sizeof(x))
 
 VnError VnSpi_genGenericCommand(
 	char cmdId,
@@ -103,7 +103,7 @@ VnError VnSpi_genRead(
 		buffer[i] = 0x00;
 
 	*size = desiredLength > 3 ? desiredLength : 3;
-
+	
 	return E_NONE;
 }
 
@@ -324,9 +324,9 @@ VnError VnSpi_parseAttitudeQuaternion(
 
 VnError VnSpi_parseQuaternionMagneticAccelerationAndAngularRates(
 	const char* response,
-	vec4f* quat,
-	vec3f* mag,
-	vec3f* accel,
+	vec4f* quat, 
+	vec3f* mag, 
+	vec3f* accel, 
 	vec3f* gyro)
 {
 	const char* pos = response + 3;
@@ -401,8 +401,8 @@ VnError VnSpi_parseAngularRateMeasurements(
 
 VnError VnSpi_parseMagneticAccelerationAndAngularRates(
 	const char* response,
-	vec3f* mag,
-	vec3f* accel,
+	vec3f* mag, 
+	vec3f* accel, 
 	vec3f* gyro)
 {
 	const char* pos = response + 3;
@@ -424,7 +424,7 @@ VnError VnSpi_parseMagneticAccelerationAndAngularRates(
 
 VnError VnSpi_parseMagneticAndGravityReferenceVectors(
 	const char* response,
-	vec3f* magRef,
+	vec3f* magRef, 
 	vec3f* accRef)
 {
 	const char* pos = response + 3;
@@ -444,9 +444,9 @@ VnError VnSpi_parseMagneticAndGravityReferenceVectors(
 
 VnError VnSpi_parseFilterMeasurementsVarianceParameters(
 	const char* response,
-	float* angularWalkVariance,
-	vec3f* angularRateVariance,
-	vec3f* magneticVariance,
+	float* angularWalkVariance, 
+	vec3f* angularRateVariance, 
+	vec3f* magneticVariance, 
 	vec3f* accelerationVariance)
 {
 	const char* pos = response + 3;
@@ -470,7 +470,7 @@ VnError VnSpi_parseFilterMeasurementsVarianceParameters(
 
 VnError VnSpi_parseMagnetometerCompensation(
 	const char* response,
-	mat3f* c,
+	mat3f* c, 
 	vec3f* b)
 {
 	const char* pos = response + 3;
@@ -490,9 +490,9 @@ VnError VnSpi_parseMagnetometerCompensation(
 
 VnError VnSpi_parseFilterActiveTuningParameters(
 	const char* response,
-	float* magneticDisturbanceGain,
-	float* accelerationDisturbanceGain,
-	float* magneticDisturbanceMemory,
+	float* magneticDisturbanceGain, 
+	float* accelerationDisturbanceGain, 
+	float* magneticDisturbanceMemory, 
 	float* accelerationDisturbanceMemory)
 {
 	const char* pos = response + 3;
@@ -516,7 +516,7 @@ VnError VnSpi_parseFilterActiveTuningParameters(
 
 VnError VnSpi_parseAccelerationCompensation(
 	const char* response,
-	mat3f* c,
+	mat3f* c, 
 	vec3f* b)
 {
 	const char* pos = response + 3;
@@ -553,9 +553,9 @@ VnError VnSpi_parseReferenceFrameRotation(
 
 VnError VnSpi_parseYawPitchRollMagneticAccelerationAndAngularRates(
 	const char* response,
-	vec3f* yawPitchRoll,
-	vec3f* mag,
-	vec3f* accel,
+	vec3f* yawPitchRoll, 
+	vec3f* mag, 
+	vec3f* accel, 
 	vec3f* gyro)
 {
 	const char* pos = response + 3;
@@ -579,12 +579,12 @@ VnError VnSpi_parseYawPitchRollMagneticAccelerationAndAngularRates(
 
 VnError VnSpi_parseCommunicationProtocolControl(
 	const char* response,
-	uint8_t* serialCount,
-	uint8_t* serialStatus,
-	uint8_t* spiCount,
-	uint8_t* spiStatus,
-	uint8_t* serialChecksum,
-	uint8_t* spiChecksum,
+	uint8_t* serialCount, 
+	uint8_t* serialStatus, 
+	uint8_t* spiCount, 
+	uint8_t* spiStatus, 
+	uint8_t* serialChecksum, 
+	uint8_t* spiChecksum, 
 	uint8_t* errorMode)
 {
 	const char* pos = response + 3;
@@ -614,14 +614,14 @@ VnError VnSpi_parseCommunicationProtocolControl(
 
 VnError VnSpi_parseSynchronizationControl(
 	const char* response,
-	uint8_t* syncInMode,
-	uint8_t* syncInEdge,
-	uint16_t* syncInSkipFactor,
-	uint32_t* reserved1,
-	uint8_t* syncOutMode,
-	uint8_t* syncOutPolarity,
-	uint16_t* syncOutSkipFactor,
-	uint32_t* syncOutPulseWidth,
+	uint8_t* syncInMode, 
+	uint8_t* syncInEdge, 
+	uint16_t* syncInSkipFactor, 
+	uint32_t* reserved1, 
+	uint8_t* syncOutMode, 
+	uint8_t* syncOutPolarity, 
+	uint16_t* syncOutSkipFactor, 
+	uint32_t* syncOutPulseWidth, 
 	uint32_t* reserved2)
 {
 	const char* pos = response + 3;
@@ -655,8 +655,8 @@ VnError VnSpi_parseSynchronizationControl(
 
 VnError VnSpi_parseSynchronizationStatus(
 	const char* response,
-	uint32_t* syncInCount,
-	uint32_t* syncInTime,
+	uint32_t* syncInCount, 
+	uint32_t* syncInTime, 
 	uint32_t* syncOutCount)
 {
 	const char* pos = response + 3;
@@ -678,10 +678,10 @@ VnError VnSpi_parseSynchronizationStatus(
 
 VnError VnSpi_parseFilterBasicControl(
 	const char* response,
-	uint8_t* magMode,
-	uint8_t* extMagMode,
-	uint8_t* extAccMode,
-	uint8_t* extGyroMode,
+	uint8_t* magMode, 
+	uint8_t* extMagMode, 
+	uint8_t* extAccMode, 
+	uint8_t* extGyroMode, 
 	vec3f* gyroLimit)
 {
 	const char* pos = response + 3;
@@ -707,9 +707,9 @@ VnError VnSpi_parseFilterBasicControl(
 
 VnError VnSpi_parseVpeBasicControl(
 	const char* response,
-	uint8_t* enable,
-	uint8_t* headingMode,
-	uint8_t* filteringMode,
+	uint8_t* enable, 
+	uint8_t* headingMode, 
+	uint8_t* filteringMode, 
 	uint8_t* tuningMode)
 {
 	const char* pos = response + 3;
@@ -733,8 +733,8 @@ VnError VnSpi_parseVpeBasicControl(
 
 VnError VnSpi_parseVpeMagnetometerBasicTuning(
 	const char* response,
-	vec3f* baseTuning,
-	vec3f* adaptiveTuning,
+	vec3f* baseTuning, 
+	vec3f* adaptiveTuning, 
 	vec3f* adaptiveFiltering)
 {
 	const char* pos = response + 3;
@@ -756,10 +756,10 @@ VnError VnSpi_parseVpeMagnetometerBasicTuning(
 
 VnError VnSpi_parseVpeMagnetometerAdvancedTuning(
 	const char* response,
-	vec3f* minFiltering,
-	vec3f* maxFiltering,
-	float* maxAdaptRate,
-	float* disturbanceWindow,
+	vec3f* minFiltering, 
+	vec3f* maxFiltering, 
+	float* maxAdaptRate, 
+	float* disturbanceWindow, 
 	float* maxTuning)
 {
 	const char* pos = response + 3;
@@ -785,8 +785,8 @@ VnError VnSpi_parseVpeMagnetometerAdvancedTuning(
 
 VnError VnSpi_parseVpeAccelerometerBasicTuning(
 	const char* response,
-	vec3f* baseTuning,
-	vec3f* adaptiveTuning,
+	vec3f* baseTuning, 
+	vec3f* adaptiveTuning, 
 	vec3f* adaptiveFiltering)
 {
 	const char* pos = response + 3;
@@ -808,10 +808,10 @@ VnError VnSpi_parseVpeAccelerometerBasicTuning(
 
 VnError VnSpi_parseVpeAccelerometerAdvancedTuning(
 	const char* response,
-	vec3f* minFiltering,
-	vec3f* maxFiltering,
-	float* maxAdaptRate,
-	float* disturbanceWindow,
+	vec3f* minFiltering, 
+	vec3f* maxFiltering, 
+	float* maxAdaptRate, 
+	float* disturbanceWindow, 
 	float* maxTuning)
 {
 	const char* pos = response + 3;
@@ -837,8 +837,8 @@ VnError VnSpi_parseVpeAccelerometerAdvancedTuning(
 
 VnError VnSpi_parseVpeGyroBasicTuning(
 	const char* response,
-	vec3f* angularWalkVariance,
-	vec3f* baseTuning,
+	vec3f* angularWalkVariance, 
+	vec3f* baseTuning, 
 	vec3f* adaptiveTuning)
 {
 	const char* pos = response + 3;
@@ -877,8 +877,8 @@ VnError VnSpi_parseFilterStartupGyroBias(
 
 VnError VnSpi_parseMagnetometerCalibrationControl(
 	const char* response,
-	uint8_t* hsiMode,
-	uint8_t* hsiOutput,
+	uint8_t* hsiMode, 
+	uint8_t* hsiOutput, 
 	uint8_t* convergeRate)
 {
 	const char* pos = response + 3;
@@ -900,7 +900,7 @@ VnError VnSpi_parseMagnetometerCalibrationControl(
 
 VnError VnSpi_parseCalculatedMagnetometerCalibration(
 	const char* response,
-	mat3f* c,
+	mat3f* c, 
 	vec3f* b)
 {
 	const char* pos = response + 3;
@@ -920,7 +920,7 @@ VnError VnSpi_parseCalculatedMagnetometerCalibration(
 
 VnError VnSpi_parseIndoorHeadingModeControl(
 	const char* response,
-	float* maxRateError,
+	float* maxRateError, 
 	uint8_t* reserved1)
 {
 	const char* pos = response + 3;
@@ -957,8 +957,8 @@ VnError VnSpi_parseVelocityCompensationMeasurement(
 
 VnError VnSpi_parseVelocityCompensationControl(
 	const char* response,
-	uint8_t* mode,
-	float* velocityTuning,
+	uint8_t* mode, 
+	float* velocityTuning, 
 	float* rateTuning)
 {
 	const char* pos = response + 3;
@@ -980,9 +980,9 @@ VnError VnSpi_parseVelocityCompensationControl(
 
 VnError VnSpi_parseVelocityCompensationStatus(
 	const char* response,
-	float* x,
-	float* xDot,
-	vec3f* accelOffset,
+	float* x, 
+	float* xDot, 
+	vec3f* accelOffset, 
 	vec3f* omega)
 {
 	const char* pos = response + 3;
@@ -1006,10 +1006,10 @@ VnError VnSpi_parseVelocityCompensationStatus(
 
 VnError VnSpi_parseImuMeasurements(
 	const char* response,
-	vec3f* mag,
-	vec3f* accel,
-	vec3f* gyro,
-	float* temp,
+	vec3f* mag, 
+	vec3f* accel, 
+	vec3f* gyro, 
+	float* temp, 
 	float* pressure)
 {
 	const char* pos = response + 3;
@@ -1035,10 +1035,10 @@ VnError VnSpi_parseImuMeasurements(
 
 VnError VnSpi_parseGpsConfiguration(
 	const char* response,
-	uint8_t* mode,
-	uint8_t* ppsSource,
-	uint8_t* reserved1,
-	uint8_t* reserved2,
+	uint8_t* mode, 
+	uint8_t* ppsSource, 
+	uint8_t* reserved1, 
+	uint8_t* reserved2, 
 	uint8_t* reserved3)
 {
 	const char* pos = response + 3;
@@ -1081,14 +1081,14 @@ VnError VnSpi_parseGpsAntennaOffset(
 
 VnError VnSpi_parseGpsSolutionLla(
 	const char* response,
-	double* time,
-	uint16_t* week,
-	uint8_t* gpsFix,
-	uint8_t* numSats,
-	vec3d* lla,
-	vec3f* nedVel,
-	vec3f* nedAcc,
-	float* speedAcc,
+	double* time, 
+	uint16_t* week, 
+	uint8_t* gpsFix, 
+	uint8_t* numSats, 
+	vec3d* lla, 
+	vec3f* nedVel, 
+	vec3f* nedAcc, 
+	float* speedAcc, 
 	float* timeAcc)
 {
 	const char* pos = response + 3;
@@ -1123,14 +1123,14 @@ VnError VnSpi_parseGpsSolutionLla(
 
 VnError VnSpi_parseGpsSolutionEcef(
 	const char* response,
-	double* tow,
-	uint16_t* week,
-	uint8_t* gpsFix,
-	uint8_t* numSats,
-	vec3d* position,
-	vec3f* velocity,
-	vec3f* posAcc,
-	float* speedAcc,
+	double* tow, 
+	uint16_t* week, 
+	uint8_t* gpsFix, 
+	uint8_t* numSats, 
+	vec3d* position, 
+	vec3f* velocity, 
+	vec3f* posAcc, 
+	float* speedAcc, 
 	float* timeAcc)
 {
 	const char* pos = response + 3;
@@ -1165,14 +1165,14 @@ VnError VnSpi_parseGpsSolutionEcef(
 
 VnError VnSpi_parseInsSolutionLla(
 	const char* response,
-	double* time,
-	uint16_t* week,
-	uint16_t* status,
-	vec3f* yawPitchRoll,
-	vec3d* position,
-	vec3f* nedVel,
-	float* attUncertainty,
-	float* posUncertainty,
+	double* time, 
+	uint16_t* week, 
+	uint16_t* status, 
+	vec3f* yawPitchRoll, 
+	vec3d* position, 
+	vec3f* nedVel, 
+	float* attUncertainty, 
+	float* posUncertainty, 
 	float* velUncertainty)
 {
 	const char* pos = response + 3;
@@ -1206,14 +1206,14 @@ VnError VnSpi_parseInsSolutionLla(
 
 VnError VnSpi_parseInsSolutionEcef(
 	const char* response,
-	double* time,
-	uint16_t* week,
-	uint16_t* status,
-	vec3f* yawPitchRoll,
-	vec3d* position,
-	vec3f* velocity,
-	float* attUncertainty,
-	float* posUncertainty,
+	double* time, 
+	uint16_t* week, 
+	uint16_t* status, 
+	vec3f* yawPitchRoll, 
+	vec3d* position, 
+	vec3f* velocity, 
+	float* attUncertainty, 
+	float* posUncertainty, 
 	float* velUncertainty)
 {
 	const char* pos = response + 3;
@@ -1247,9 +1247,9 @@ VnError VnSpi_parseInsSolutionEcef(
 
 VnError VnSpi_parseInsBasicConfiguration(
 	const char* response,
-	uint8_t* scenario,
-	uint8_t* ahrsAiding,
-	uint8_t* estBaseline,
+	uint8_t* scenario, 
+	uint8_t* ahrsAiding, 
+	uint8_t* estBaseline, 
 	uint8_t* resv2)
 {
 	const char* pos = response + 3;
@@ -1273,20 +1273,20 @@ VnError VnSpi_parseInsBasicConfiguration(
 
 VnError VnSpi_parseInsAdvancedConfiguration(
 	const char* response,
-	uint8_t* useMag,
-	uint8_t* usePres,
-	uint8_t* posAtt,
-	uint8_t* velAtt,
-	uint8_t* velBias,
-	uint8_t* useFoam,
-	uint8_t* gpsCovType,
-	uint8_t* velCount,
-	float* velInit,
-	float* moveOrigin,
-	float* gpsTimeout,
-	float* deltaLimitPos,
-	float* deltaLimitVel,
-	float* minPosUncertainty,
+	uint8_t* useMag, 
+	uint8_t* usePres, 
+	uint8_t* posAtt, 
+	uint8_t* velAtt, 
+	uint8_t* velBias, 
+	uint8_t* useFoam, 
+	uint8_t* gpsCovType, 
+	uint8_t* velCount, 
+	float* velInit, 
+	float* moveOrigin, 
+	float* gpsTimeout, 
+	float* deltaLimitPos, 
+	float* deltaLimitVel, 
+	float* minPosUncertainty, 
 	float* minVelUncertainty)
 {
 	const char* pos = response + 3;
@@ -1332,10 +1332,10 @@ VnError VnSpi_parseInsAdvancedConfiguration(
 
 VnError VnSpi_parseInsStateLla(
 	const char* response,
-	vec3f* yawPitchRoll,
-	vec3d* position,
-	vec3f* velocity,
-	vec3f* accel,
+	vec3f* yawPitchRoll, 
+	vec3d* position, 
+	vec3f* velocity, 
+	vec3f* accel, 
 	vec3f* angularRate)
 {
 	const char* pos = response + 3;
@@ -1361,10 +1361,10 @@ VnError VnSpi_parseInsStateLla(
 
 VnError VnSpi_parseInsStateEcef(
 	const char* response,
-	vec3f* yawPitchRoll,
-	vec3d* position,
-	vec3f* velocity,
-	vec3f* accel,
+	vec3f* yawPitchRoll, 
+	vec3d* position, 
+	vec3f* velocity, 
+	vec3f* accel, 
 	vec3f* angularRate)
 {
 	const char* pos = response + 3;
@@ -1390,8 +1390,8 @@ VnError VnSpi_parseInsStateEcef(
 
 VnError VnSpi_parseStartupFilterBiasEstimate(
 	const char* response,
-	vec3f* gyroBias,
-	vec3f* accelBias,
+	vec3f* gyroBias, 
+	vec3f* accelBias, 
 	float* pressureBias)
 {
 	const char* pos = response + 3;
@@ -1413,8 +1413,8 @@ VnError VnSpi_parseStartupFilterBiasEstimate(
 
 VnError VnSpi_parseDeltaThetaAndDeltaVelocity(
 	const char* response,
-	float* deltaTime,
-	vec3f* deltaTheta,
+	float* deltaTime, 
+	vec3f* deltaTheta, 
 	vec3f* deltaVelocity)
 {
 	const char* pos = response + 3;
@@ -1436,10 +1436,10 @@ VnError VnSpi_parseDeltaThetaAndDeltaVelocity(
 
 VnError VnSpi_parseDeltaThetaAndDeltaVelocityConfiguration(
 	const char* response,
-	uint8_t* integrationFrame,
-	uint8_t* gyroCompensation,
-	uint8_t* accelCompensation,
-	uint8_t* reserved1,
+	uint8_t* integrationFrame, 
+	uint8_t* gyroCompensation, 
+	uint8_t* accelCompensation, 
+	uint8_t* reserved1, 
 	uint16_t* reserved2)
 {
 	const char* pos = response + 3;
@@ -1465,12 +1465,12 @@ VnError VnSpi_parseDeltaThetaAndDeltaVelocityConfiguration(
 
 VnError VnSpi_parseReferenceVectorConfiguration(
 	const char* response,
-	uint8_t* useMagModel,
-	uint8_t* useGravityModel,
-	uint8_t* resv1,
-	uint8_t* resv2,
-	uint32_t* recalcThreshold,
-	float* year,
+	uint8_t* useMagModel, 
+	uint8_t* useGravityModel, 
+	uint8_t* resv1, 
+	uint8_t* resv2, 
+	uint32_t* recalcThreshold, 
+	float* year, 
 	vec3d* position)
 {
 	const char* pos = response + 3;
@@ -1501,7 +1501,7 @@ VnError VnSpi_parseReferenceVectorConfiguration(
 
 VnError VnSpi_parseGyroCompensation(
 	const char* response,
-	mat3f* c,
+	mat3f* c, 
 	vec3f* b)
 {
 	const char* pos = response + 3;
@@ -1521,15 +1521,15 @@ VnError VnSpi_parseGyroCompensation(
 
 VnError VnSpi_parseImuFilteringConfiguration(
 	const char* response,
-	uint16_t* magWindowSize,
-	uint16_t* accelWindowSize,
-	uint16_t* gyroWindowSize,
-	uint16_t* tempWindowSize,
-	uint16_t* presWindowSize,
-	uint8_t* magFilterMode,
-	uint8_t* accelFilterMode,
-	uint8_t* gyroFilterMode,
-	uint8_t* tempFilterMode,
+	uint16_t* magWindowSize, 
+	uint16_t* accelWindowSize, 
+	uint16_t* gyroWindowSize, 
+	uint16_t* tempWindowSize, 
+	uint16_t* presWindowSize, 
+	uint8_t* magFilterMode, 
+	uint8_t* accelFilterMode, 
+	uint8_t* gyroFilterMode, 
+	uint8_t* tempFilterMode, 
 	uint8_t* presFilterMode)
 {
 	const char* pos = response + 3;
@@ -1565,7 +1565,7 @@ VnError VnSpi_parseImuFilteringConfiguration(
 
 VnError VnSpi_parseGpsCompassBaseline(
 	const char* response,
-	vec3f* position,
+	vec3f* position, 
 	vec3f* uncertainty)
 {
 	const char* pos = response + 3;
@@ -1585,10 +1585,10 @@ VnError VnSpi_parseGpsCompassBaseline(
 
 VnError VnSpi_parseGpsCompassEstimatedBaseline(
 	const char* response,
-	uint8_t* estBaselineUsed,
-	uint8_t* resv,
-	uint16_t* numMeas,
-	vec3f* position,
+	uint8_t* estBaselineUsed, 
+	uint8_t* resv, 
+	uint16_t* numMeas, 
+	vec3f* position, 
 	vec3f* uncertainty)
 {
 	const char* pos = response + 3;
@@ -1614,9 +1614,9 @@ VnError VnSpi_parseGpsCompassEstimatedBaseline(
 
 VnError VnSpi_parseImuRateConfiguration(
 	const char* response,
-	uint16_t* imuRate,
-	uint16_t* navDivisor,
-	float* filterTargetRate,
+	uint16_t* imuRate, 
+	uint16_t* navDivisor, 
+	float* filterTargetRate, 
 	float* filterMinRate)
 {
 	const char* pos = response + 3;
@@ -1640,8 +1640,8 @@ VnError VnSpi_parseImuRateConfiguration(
 
 VnError VnSpi_parseYawPitchRollTrueBodyAccelerationAndAngularRates(
 	const char* response,
-	vec3f* yawPitchRoll,
-	vec3f* bodyAccel,
+	vec3f* yawPitchRoll, 
+	vec3f* bodyAccel, 
 	vec3f* gyro)
 {
 	const char* pos = response + 3;
@@ -1663,8 +1663,8 @@ VnError VnSpi_parseYawPitchRollTrueBodyAccelerationAndAngularRates(
 
 VnError VnSpi_parseYawPitchRollTrueInertialAccelerationAndAngularRates(
 	const char* response,
-	vec3f* yawPitchRoll,
-	vec3f* inertialAccel,
+	vec3f* yawPitchRoll, 
+	vec3f* inertialAccel, 
 	vec3f* gyro)
 {
 	const char* pos = response + 3;

src/drivers/ins/vectornav/libvnc/src/vn/sensors.c

@@ -9,7 +9,7 @@
 
 #include "vn/xplat/time.h"
 
-//#define UNUSED(x) (void)(sizeof(x))
+#define UNUSED(x) (void)(sizeof(x))
 
 #define DEFAULT_RESPONSE_TIMEOUT_MS	500
 #define DEFAULT_RETRANSMIT_DELAY_MS	200

src/drivers/rc_input/RCInput.cpp

@@ -325,13 +325,7 @@ void RCInput::Run()
 			updateParams();
 		}
 
-		if (_vehicle_status_sub.updated()) {
-			vehicle_status_s vehicle_status;
-
-			if (_vehicle_status_sub.copy(&vehicle_status)) {
-				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			}
-		}
+		_vehicle_status_arming_state_sub.update();
 
 		const hrt_abstime cycle_timestamp = hrt_absolute_time();
 
@@ -346,7 +340,7 @@ void RCInput::Run()
 				uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 #if defined(SPEKTRUM_POWER)
 
-				if (!_rc_scan_locked && !_armed) {
+				if (!_rc_scan_locked && !(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 					if ((int)vcmd.param1 == 0) {
 						// DSM binding command
 						int dsm_bind_mode = (int)vcmd.param2;
@@ -758,7 +752,8 @@ void RCInput::Run()
 
 			_to_input_rc.publish(_rc_in);
 
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		} else if (!rc_updated && !(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)
+			   && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
 			_rc_scan_locked = false;
 		}
 
@@ -767,7 +762,7 @@ void RCInput::Run()
 		}
 
 		// set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
-		if (!_armed && rc_updated && _rc_scan_locked
+		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) && rc_updated && _rc_scan_locked
 		    && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
 		    && (_param_rc_input_proto.get() < 0)
 		   ) {

src/drivers/rc_input/RCInput.hpp

@@ -138,16 +138,13 @@ private:
 
 	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state>	_vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
 
 	input_rc_s	_rc_in{};
 
 	float		_analog_rc_rssi_volt{-1.0f};
 	bool		_analog_rc_rssi_stable{false};
 
-	bool _armed{false};
-
-
 	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
 
 	int		_rcs_fd{-1};

src/drivers/uavcan/uavcan_main.cpp

@@ -577,7 +577,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 			int rv = _servers->init();
 
 			if (rv < 0) {
-				PX4_ERR("UavcanServers init: %d", rv);
+				PX4_ERR("UavcanServers init: %d", ret);
 			}
 		}
 	}

src/lib/tecs/TECS.cpp

@@ -133,8 +133,8 @@ TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const
 	return _airspeed_state;
 }
 
-void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude,
-					float height_rate, const Param &param)
+void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude,
+				const Param &param)
 {
 	// Input checks
 	if (!TIMESTAMP_VALID(dt)) {
@@ -143,61 +143,48 @@ void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceS
 		return;
 	}
 
-	const float current_alt = PX4_ISFINITE(altitude) ? altitude : 0.f;
+	if (!PX4_ISFINITE(altitude)) {
+		altitude = 0.0f;
+	}
+
+	// Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference.
+	if (PX4_ISFINITE(setpoint.alt_rate)) {
+		_alt_rate_ref = setpoint.alt_rate;
+
+	} else {
+		_alt_rate_ref = 0.0f;
+	}
 
-	_velocity_control_traj_generator.setMaxJerk(param.jerk_max);
-	_velocity_control_traj_generator.setMaxAccelUp(param.vert_accel_limit);
-	_velocity_control_traj_generator.setMaxAccelDown(param.vert_accel_limit);
-	_velocity_control_traj_generator.setMaxVelUp(param.max_sink_rate); // different convention for FW than for MC
-	_velocity_control_traj_generator.setMaxVelDown(param.max_climb_rate); // different convention for FW than for MC
 
 	// Altitude setpoint reference
+	const bool altitude_control_enable{PX4_ISFINITE(setpoint.alt)};
 	_alt_control_traj_generator.setMaxJerk(param.jerk_max);
 	_alt_control_traj_generator.setMaxAccel(param.vert_accel_limit);
 	_alt_control_traj_generator.setMaxVel(fmax(param.max_climb_rate, param.max_sink_rate));
 
-	_velocity_control_traj_generator.setVelSpFeedback(setpoint.alt_rate);
+	if (altitude_control_enable) {
+		const float target_climbrate = math::min(param.target_climbrate, param.max_climb_rate);
+		const float target_sinkrate = math::min(param.target_sinkrate, param.max_sink_rate);
 
-	bool control_altitude = true;
-	float altitude_setpoint = setpoint.alt;
+		const float delta_trajectory_to_target_m = setpoint.alt - _alt_control_traj_generator.getCurrentPosition();
 
-	if (PX4_ISFINITE(setpoint.alt_rate)) {
-		// input is height rate (not altitude)
-		_velocity_control_traj_generator.setCurrentPositionEstimate(current_alt);
-		_velocity_control_traj_generator.update(dt, setpoint.alt_rate);
-		altitude_setpoint = _velocity_control_traj_generator.getCurrentPosition();
-		control_altitude = PX4_ISFINITE(altitude_setpoint); // returns true if altitude is locked
+		float altitude_rate_target = math::signNoZero<float>(delta_trajectory_to_target_m) *
+					     math::trajectory::computeMaxSpeedFromDistance(
+						     param.jerk_max, param.vert_accel_limit, fabsf(delta_trajectory_to_target_m), 0.0f);
 
-	} else {
-		_velocity_control_traj_generator.reset(0, height_rate, altitude_setpoint);
-	}
+		altitude_rate_target = math::constrain(altitude_rate_target, -target_sinkrate, target_climbrate);
 
-	if (control_altitude) {
-		const float target_climbrate_m_s = math::min(param.target_climbrate, param.max_climb_rate);
-		const float target_sinkrate_m_s = math::min(param.target_sinkrate, param.max_sink_rate);
-
-		const float delta_trajectory_to_target_m = altitude_setpoint - _alt_control_traj_generator.getCurrentPosition();
-
-		float height_rate_target = math::signNoZero<float>(delta_trajectory_to_target_m) *
-					   math::trajectory::computeMaxSpeedFromDistance(
-						   param.jerk_max, param.vert_accel_limit, fabsf(delta_trajectory_to_target_m), 0.f);
-
-		height_rate_target = math::constrain(height_rate_target, -target_sinkrate_m_s, target_climbrate_m_s);
-
-		_alt_control_traj_generator.updateDurations(height_rate_target);
+		_alt_control_traj_generator.updateDurations(altitude_rate_target);
 		_alt_control_traj_generator.updateTraj(dt);
-		_height_rate_setpoint_direct = NAN;
 
 	} else {
-		_alt_control_traj_generator.setCurrentVelocity(_velocity_control_traj_generator.getCurrentVelocity());
-		_alt_control_traj_generator.setCurrentPosition(current_alt);
-		_height_rate_setpoint_direct = _velocity_control_traj_generator.getCurrentVelocity();
+		_alt_control_traj_generator.reset(0.0f, 0.0f, altitude);
 	}
 }
 
-TECSAltitudeReferenceModel::AltitudeReferenceState TECSAltitudeReferenceModel::getAltitudeReference() const
+TECSReferenceModel::AltitudeReferenceState TECSReferenceModel::getAltitudeReference() const
 {
-	TECSAltitudeReferenceModel::AltitudeReferenceState ref{
+	TECSReferenceModel::AltitudeReferenceState ref{
 		.alt = _alt_control_traj_generator.getCurrentPosition(),
 		.alt_rate = _alt_control_traj_generator.getCurrentVelocity(),
 	};
@@ -205,12 +192,25 @@ TECSAltitudeReferenceModel::AltitudeReferenceState TECSAltitudeReferenceModel::g
 	return ref;
 }
 
-void TECSAltitudeReferenceModel::initialize(const AltitudeReferenceState &state)
+float TECSReferenceModel::getAltitudeRateReference() const
 {
-	const float init_state_alt = PX4_ISFINITE(state.alt) ? state.alt : 0.f;
-	const float init_state_alt_rate = PX4_ISFINITE(state.alt_rate) ? state.alt_rate : 0.f;
+	return _alt_rate_ref;
+}
 
-	_alt_control_traj_generator.reset(0.0f, init_state_alt_rate, init_state_alt);
+void TECSReferenceModel::initialize(const AltitudeReferenceState &state)
+{
+	float init_state_alt{state.alt};
+	_alt_rate_ref = state.alt_rate;
+
+	if (!PX4_ISFINITE(state.alt)) {
+		init_state_alt = 0.0f;
+	}
+
+	if (!PX4_ISFINITE(state.alt_rate)) {
+		_alt_rate_ref = 0.0f;
+	}
+
+	_alt_control_traj_generator.reset(0.0f, _alt_rate_ref, init_state_alt);
 }
 
 void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag)
@@ -247,6 +247,7 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param
 	_debug_output.energy_balance_rate_estimate = seb_rate.estimate;
 	_debug_output.energy_balance_rate_sp = seb_rate.setpoint;
 	_debug_output.pitch_integrator = _pitch_integ_state;
+
 	_debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint;
 	_debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint;
 }
@@ -264,14 +265,7 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &
 
 	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
 
-	if (PX4_ISFINITE(setpoint.altitude_rate_setpoint_direct)) {
-		// direct height rate control
-		control_setpoint.altitude_rate_setpoint = setpoint.altitude_rate_setpoint_direct;
-
-	} else {
-		// altitude is locked, go through altitude outer loop
-		control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
-	}
+	control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
 
 	SpecificEnergyRates specific_energy_rate{_calcSpecificEnergyRates(control_setpoint, input)};
 
@@ -321,9 +315,8 @@ float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const In
 float TECSControl::_calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param &param) const
 {
 	float altitude_rate_output;
-	altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain
-			       + param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate;
-
+	altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
+			       param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate + setpoint.altitude_rate_setpoint;
 	altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
 
 	return altitude_rate_output;
@@ -577,21 +570,17 @@ float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const C
 	// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
 	// Specific total energy rate = 0 at cruise throttle
 	// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
-
-	// assume airspeed and density-independent delta_throttle to sink/climb rate mapping
-	// TODO: include air density for thrust mappings
-	const float throttle_above_trim_per_ste_rate = (param.throttle_max - param.throttle_trim) / limit.STE_rate_max;
-	const float throttle_below_trim_per_ste_rate = (param.throttle_trim - param.throttle_min) / limit.STE_rate_min;
-
 	float throttle_predicted = 0.0f;
 
 	if (ste_rate.setpoint >= FLT_EPSILON) {
 		// throttle is between trim and maximum
-		throttle_predicted = param.throttle_trim_adjusted + ste_rate.setpoint * throttle_above_trim_per_ste_rate;
+		throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_max *
+				     (param.throttle_max - param.throttle_trim);
 
 	} else {
 		// throttle is between trim and minimum
-		throttle_predicted = param.throttle_trim_adjusted - ste_rate.setpoint * throttle_below_trim_per_ste_rate;
+		throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_min *
+				     (param.throttle_min - param.throttle_trim);
 
 	}
 
@@ -646,15 +635,14 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 		      const float eas_to_tas)
 {
 	// Init subclasses
-	TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude,
+	TECSReferenceModel::AltitudeReferenceState current_state{	.alt = altitude,
 			.alt_rate = altitude_rate};
-	_altitude_reference_model.initialize(current_state);
+	_reference_model.initialize(current_state);
 	_airspeed_filter.initialize(equivalent_airspeed);
 
 	TECSControl::Setpoint control_setpoint;
-	control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
-	control_setpoint.altitude_rate_setpoint_direct =
-		_altitude_reference_model.getAltitudeReference().alt_rate; // init to reference altitude rate
+	control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
+	control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
 	control_setpoint.tas_setpoint = equivalent_airspeed * eas_to_tas;
 
 	const TECSControl::Input control_input{ .altitude = altitude,
@@ -666,19 +654,21 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 
 	_debug_status.tecs_mode = _tecs_mode;
 	_debug_status.control = _control.getDebugOutput();
-	_debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed;
-	_debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate;
-	_debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt;
-	_debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate;
-	_debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect();
+	const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
+	_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
+	_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
+	const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()};
+	_debug_status.altitude_sp_ref = ref_alt.alt;
+	_debug_status.altitude_rate_alt_ref = ref_alt.alt_rate;
+	_debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference();
 
 	_update_timestamp = hrt_absolute_time();
 }
 
 void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
 		  float eas_to_tas, float throttle_min, float throttle_setpoint_max,
-		  float throttle_trim, float throttle_trim_adjusted, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
-		  float target_sinkrate, const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
+		  float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
+		  const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
 {
 
 	// Calculate the time since last update (seconds)
@@ -694,7 +684,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	_control_param.pitch_max = pitch_limit_max;
 	_control_param.pitch_min = pitch_limit_min;
 	_control_param.throttle_trim = throttle_trim;
-	_control_param.throttle_trim_adjusted = throttle_trim_adjusted;
 	_control_param.throttle_max = throttle_setpoint_max;
 	_control_param.throttle_min = throttle_min;
 
@@ -716,14 +705,14 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
 
 		// Update Reference model submodule
-		const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
+		const TECSReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
 				.alt_rate = hgt_rate_sp};
 
-		_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
+		_reference_model.update(dt, setpoint, altitude, _reference_param);
 
 		TECSControl::Setpoint control_setpoint;
-		control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
-		control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect();
+		control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
+		control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
 
 		// Calculate the demanded true airspeed
 		// TODO this function should not be in the module. Only give feedback that the airspeed can't be achieved.
@@ -754,9 +743,9 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		_debug_status.control = _control.getDebugOutput();
 		_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
 		_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
-		_debug_status.altitude_reference = control_setpoint.altitude_reference.alt;
-		_debug_status.height_rate_reference = control_setpoint.altitude_reference.alt_rate;
-		_debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect();
+		_debug_status.altitude_sp_ref = control_setpoint.altitude_reference.alt;
+		_debug_status.altitude_rate_alt_ref = control_setpoint.altitude_reference.alt_rate;
+		_debug_status.altitude_rate_feedforward = control_setpoint.altitude_rate_setpoint;
 	}
 }
 

src/lib/tecs/TECS.hpp

@@ -113,7 +113,7 @@ private:
 	AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f};	///< Complimentary filter state
 };
 
-class TECSAltitudeReferenceModel
+class TECSReferenceModel
 {
 public:
 	/**
@@ -139,8 +139,8 @@ public:
 	};
 
 public:
-	TECSAltitudeReferenceModel() = default;
-	~TECSAltitudeReferenceModel() = default;
+	TECSReferenceModel() = default;
+	~TECSReferenceModel() = default;
 
 	/**
 	 * @brief Initialize reference models.
@@ -155,10 +155,9 @@ public:
 	 * @param[in] dt is the update interval in [s].
 	 * @param[in] setpoint are the desired setpoints.
 	 * @param[in] altitude is the altitude amsl in [m].
-	 * @param[in] height_rate is the height rate setpoint in [m/s].
 	 * @param[in] param are the reference model parameters.
 	 */
-	void update(float dt, const AltitudeReferenceState &setpoint, float altitude, float height_rate, const Param &param);
+	void update(float dt, const AltitudeReferenceState &setpoint, float altitude, const Param &param);
 
 	/**
 	 * @brief Get the current altitude reference of altitude reference model.
@@ -168,20 +167,17 @@ public:
 	AltitudeReferenceState getAltitudeReference() const;
 
 	/**
-	 * @brief Get the Height Rate Setpoint directly from the velocity trajector generator
+	 * @brief Get the altitude rate reference of the altitude rate reference model.
 	 *
-	 * @return float direct height rate setpoint [m/s]
+	 * @return Current altitude rate reference point.
 	 */
-	float getHeightRateSetpointDirect() const {return _height_rate_setpoint_direct; }
-
+	float getAltitudeRateReference() const;
 
 private:
 	// State
 	VelocitySmoothing
 	_alt_control_traj_generator;		///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded.
-	ManualVelocitySmoothingZ
-	_velocity_control_traj_generator;	///< generates height rate trajectory when height rate is commanded
-	float _height_rate_setpoint_direct{NAN}; ///< generated direct height rate setpoint
+	float _alt_rate_ref; 			///< Altitude rate reference in [m/s].
 };
 
 class TECSControl
@@ -201,8 +197,7 @@ public:
 		float tas_min;				///< True airpeed demand lower limit [m/s].
 		float pitch_max;			///< Maximum pitch angle allowed in [rad].
 		float pitch_min;			///< Minimal pitch angle allowed in [rad].
-		float throttle_trim;			///< Normalized throttle required to fly level at trim airspeed and sea level
-		float throttle_trim_adjusted;		///< Trim throttle adjusted for airspeed, load factor and air density
+		float throttle_trim;			///< Normalized throttle required to fly level at given eas.
 		float throttle_max;			///< Normalized throttle upper limit.
 		float throttle_min;			///< Normalized throttle lower limit.
 
@@ -253,8 +248,8 @@ public:
 	 *
 	 */
 	struct Setpoint {
-		TECSAltitudeReferenceModel::AltitudeReferenceState altitude_reference;	///< Altitude/height rate reference.
-		float altitude_rate_setpoint_direct;					///< Direct height rate setpoint.
+		TECSReferenceModel::AltitudeReferenceState altitude_reference;	///< Altitude reference from reference model.
+		float altitude_rate_setpoint;					///< Altitude rate setpoint.
 		float tas_setpoint;						///< True airspeed setpoint.
 	};
 
@@ -548,9 +543,9 @@ public:
 		TECSControl::DebugOutput control;
 		float true_airspeed_filtered;
 		float true_airspeed_derivative;
-		float altitude_reference;
-		float height_rate_reference;
-		float height_rate_direct;
+		float altitude_sp_ref;
+		float altitude_rate_alt_ref;
+		float altitude_rate_feedforward;
 		enum ECL_TECS_MODE tecs_mode;
 	};
 public:
@@ -583,8 +578,8 @@ public:
 	 */
 	void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
 		    float eas_to_tas, float throttle_min, float throttle_setpoint_max,
-		    float throttle_trim, float throttle_trim_adjusted, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
-		    float target_sinkrate, float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
+		    float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
+		    float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
 
 	/**
 	 * @brief Initialize the control loop
@@ -642,11 +637,11 @@ public:
 	 */
 	void handle_alt_step(float altitude, float altitude_rate)
 	{
-		TECSAltitudeReferenceModel::AltitudeReferenceState init_state{ .alt = altitude,
+		TECSReferenceModel::AltitudeReferenceState init_state{ .alt = altitude,
 				.alt_rate = altitude_rate};
 
 		// reset altitude reference model.
-		_altitude_reference_model.initialize(init_state);
+		_reference_model.initialize(init_state);
 	}
 
 	float get_pitch_setpoint() {return _control.getPitchSetpoint();}
@@ -656,9 +651,9 @@ public:
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
 
 private:
-	TECSControl 			_control;			///< Control submodule.
-	TECSAirspeedFilter 		_airspeed_filter;		///< Airspeed filter submodule.
-	TECSAltitudeReferenceModel 	_altitude_reference_model;	///< Setpoint reference model submodule.
+	TECSControl 		_control;				///< Control submodule.
+	TECSAirspeedFilter 	_airspeed_filter;			///< Airspeed filter submodule.
+	TECSReferenceModel 	_reference_model;			///< Setpoint reference model submodule.
 
 	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};		///< Current activated mode.
 
@@ -681,7 +676,7 @@ private:
 		.airspeed_rate_noise_std_dev = 0.02f
 	};
 	/// Reference model parameters.
-	TECSAltitudeReferenceModel::Param _reference_param{
+	TECSReferenceModel::Param _reference_param{
 		.target_climbrate = 2.0f,
 		.target_sinkrate = 2.0f,
 		.jerk_max = 1000.0f,
@@ -700,7 +695,6 @@ private:
 		.pitch_max = 5.0f,
 		.pitch_min = -5.0f,
 		.throttle_trim = 0.0f,
-		.throttle_trim_adjusted = 0.f,
 		.throttle_max = 1.0f,
 		.throttle_min = 0.1f,
 		.altitude_error_gain = 0.2f,

src/modules/commander/failsafe/framework.cpp

@@ -646,7 +646,6 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
 {
 	uint32_t mode_mask = 1u << mode;
 	// mode_req_wind_and_flight_time_compliance: does not need to be handled here (these are separate failsafe triggers)
-	// mode_req_manual_control: is handled separately
 	return
 		(!status_flags.angular_velocity_invalid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) &&
 		(!status_flags.attitude_invalid || ((status_flags.mode_req_attitude & mode_mask) == 0)) &&
@@ -657,6 +656,7 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
 		(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
 		(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
 		(!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
+		(!status_flags.manual_control_signal_lost || ((status_flags.mode_req_manual_control & mode_mask) == 0)) &&
 		((status_flags.mode_req_other & mode_mask) == 0);
 }
 

src/modules/ekf2/EKF/ekf.cpp

@@ -243,8 +243,12 @@ bool Ekf::initialiseTilt()
 		return false;
 	}
 
-	// get initial tilt estimate from delta velocity vector, assuming vehicle is static
-	_state.quat_nominal = Quatf(_accel_lpf.getState(), Vector3f(0.f, 0.f, -1.f));
+	// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
+	const Vector3f gravity_in_body = _accel_lpf.getState().normalized();
+	const float pitch = asinf(gravity_in_body(0));
+	const float roll = atan2f(-gravity_in_body(1), -gravity_in_body(2));
+
+	_state.quat_nominal = Quatf{Eulerf{roll, pitch, 0.0f}};
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	return true;

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -459,6 +459,8 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 		}
 	}
 
+	_system_flag_buffer->push(system_flags);
+
 	const int64_t time_us = system_flags.time_us
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 

src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv

@@ -1,8 +1,8 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-90000,1,-0.0096,-0.01,-0.02,-0.0004,0.0026,-0.026,-1.4e-05,0.00011,-0.0023,0,0,0,0,0,0,0.19,-4.7e-10,0.4,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-190000,1,-0.0096,-0.011,-0.02,0.0002,0.0039,-0.041,6e-06,0.00043,-0.0044,-3e-13,-2.3e-14,5.8e-15,0,0,-2e-11,0.19,-4.7e-10,0.4,0,0,0,0,0,5.8e-07,0.0027,0.0027,0.0008,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-290000,1,-0.0097,-0.011,-0.02,0.0012,0.0063,-0.053,5e-05,0.00036,-0.007,9.1e-12,9.1e-13,-1.7e-13,0,0,-4.8e-09,0.19,-4.7e-10,0.4,0,0,0,0,0,6.8e-07,0.0029,0.0029,0.00067,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+10000,1,-0.0094,-0.01,-9.8e-05,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+90000,1,-0.0096,-0.01,-0.02,-0.0004,0.0026,-0.026,-1.4e-05,0.00011,-0.0023,0,0,0,0,0,0,0.19,0,0.4,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+190000,1,-0.0096,-0.011,-0.02,0.0002,0.0039,-0.041,6e-06,0.00043,-0.0044,-3e-13,-2.3e-14,5.8e-15,0,0,-2e-11,0.19,0,0.4,0,0,0,0,0,5.8e-07,0.0027,0.0027,0.0008,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+290000,1,-0.0097,-0.011,-0.02,0.0012,0.0063,-0.053,5e-05,0.00036,-0.007,9.1e-12,9.1e-13,-1.7e-13,0,0,-4.8e-09,0.19,0,0.4,0,0,0,0,0,6.8e-07,0.0029,0.0029,0.00067,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 390000,1,-0.0089,-0.012,0.046,0.0026,0.0083,-0.059,0.00024,0.0011,-0.0094,-1.1e-10,2.8e-11,2.8e-12,0,0,-4.5e-08,0.17,0.0017,0.41,0,0,0,0,0,7e-07,0.0031,0.0031,0.00062,25,25,8.1,0.97,0.97,0.32,1e-06,1e-06,8.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 490000,0.87,-0.0023,-0.015,0.5,0.0023,0.0057,-0.06,0.0002,0.00051,-0.011,1.6e-08,-3.8e-09,-3.3e-10,0,0,-1.6e-07,0.14,0.0014,0.42,0,0,0,0,0,2.6e-06,0.0033,0.0033,0.00061,7.8,7.8,5.9,0.34,0.34,0.31,1e-06,1e-06,8.3e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 590000,0.76,0.00067,-0.015,0.65,0.00071,0.0084,-0.059,0.00037,0.0012,-0.012,1.6e-08,-3.5e-09,-3.2e-10,0,0,-3.4e-07,0.18,0.0018,0.43,0,0,0,0,0,1.5e-05,0.0036,0.0036,0.00061,7.9,7.9,4.2,0.67,0.67,0.32,1e-06,1e-06,7.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
@@ -103,7 +103,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 10090000,0.7,0.0017,-0.014,0.71,-0.037,0.026,-0.096,-0.058,0.037,-3.7e+02,-1.3e-05,-5.7e-05,9.1e-07,0,0,-0.00091,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,0.00041,0.00041,9.5e-05,1.6,1.6,0.08,6.9,6.9,0.085,8e-09,8e-09,3e-09,4e-06,4e-06,1.6e-06,0,0,0,0,0,0,0,0
 10190000,0.7,0.0017,-0.014,0.71,-0.038,0.029,-0.096,-0.061,0.039,-3.7e+02,-1.3e-05,-5.7e-05,3.4e-07,0,0,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,0.00042,0.00042,9.5e-05,1.7,1.7,0.078,7.6,7.6,0.084,8e-09,8e-09,3e-09,4e-06,4e-06,1.4e-06,0,0,0,0,0,0,0,0
 10290000,0.7,0.0017,-0.014,0.71,-0.038,0.028,-0.084,-0.066,0.042,-3.7e+02,-1.3e-05,-5.7e-05,-1.8e-08,0,0,-0.00098,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,0.00043,0.00043,9.5e-05,1.8,1.8,0.076,8.3,8.3,0.085,8e-09,8e-09,3e-09,4e-06,4e-06,1.4e-06,0,0,0,0,0,0,0,0
-10390000,0.7,0.0016,-0.014,0.71,0.01,-0.02,0.0096,0.0009,-0.0018,-3.7e+02,-1.3e-05,-5.7e-05,1.2e-09,-6.4e-10,5.1e-10,-0.001,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00044,0.00044,9.4e-05,0.25,0.25,0.56,0.25,0.25,0.078,8e-09,8e-09,3e-09,4e-06,4e-06,1.3e-06,0,0,0,0,0,0,0,0
+10390000,0.7,0.0016,-0.014,0.71,0.01,-0.02,0.0096,0.0009,-0.0018,-3.7e+02,-1.3e-05,-5.7e-05,1.3e-09,-6.4e-10,5.1e-10,-0.001,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00044,0.00044,9.4e-05,0.25,0.25,0.56,0.25,0.25,0.078,8e-09,8e-09,3e-09,4e-06,4e-06,1.3e-06,0,0,0,0,0,0,0,0
 10490000,0.7,0.0017,-0.014,0.71,0.0097,-0.018,0.023,0.0019,-0.0036,-3.7e+02,-1.3e-05,-5.7e-05,-3.5e-07,-1.7e-08,1.3e-08,-0.001,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00046,0.00046,9.4e-05,0.26,0.26,0.55,0.26,0.26,0.08,8e-09,8e-09,2.9e-09,4e-06,4e-06,1.2e-06,0,0,0,0,0,0,0,0
 10590000,0.7,0.0017,-0.014,0.71,0.0092,-0.0076,0.026,0.0018,-0.00084,-3.7e+02,-1.3e-05,-5.7e-05,-3.3e-07,-2.8e-06,7.2e-08,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,0.00047,0.00047,9.4e-05,0.13,0.13,0.27,0.13,0.13,0.073,7.9e-09,7.9e-09,2.9e-09,4e-06,4e-06,1.2e-06,0,0,0,0,0,0,0,0
 10690000,0.7,0.0018,-0.014,0.71,0.0079,-0.0075,0.03,0.0027,-0.0016,-3.7e+02,-1.3e-05,-5.7e-05,-4.5e-07,-2.8e-06,8.4e-08,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,0.00048,0.00048,9.4e-05,0.14,0.14,0.26,0.14,0.14,0.078,7.9e-09,7.9e-09,2.9e-09,4e-06,4e-06,1.1e-06,0,0,0,0,0,0,0,0
@@ -119,7 +119,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 11690000,0.7,0.0018,-0.014,0.71,-0.002,0.011,-0.0079,0.0046,-0.0006,-3.7e+02,-1.3e-05,-5.7e-05,-2.1e-06,-5.7e-06,2.5e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.4e-05,0.00037,0.00037,9.3e-05,0.11,0.11,0.046,0.062,0.062,0.066,6e-09,6e-09,2.5e-09,3.7e-06,3.7e-06,1e-06,0,0,0,0,0,0,0,0
 11790000,0.7,0.0019,-0.014,0.71,-0.008,0.011,-0.0098,0.0021,0.00045,-3.7e+02,-1.3e-05,-5.7e-05,-2.2e-06,-5.2e-06,-1.2e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.4e-05,0.00032,0.00032,9.3e-05,0.087,0.087,0.039,0.052,0.052,0.063,5.5e-09,5.5e-09,2.5e-09,3.6e-06,3.6e-06,1e-06,0,0,0,0,0,0,0,0
 11890000,0.7,0.0019,-0.014,0.71,-0.0092,0.012,-0.011,0.0013,0.0016,-3.7e+02,-1.3e-05,-5.7e-05,-2.6e-06,-5.2e-06,-1.2e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00033,0.00033,9.3e-05,0.1,0.1,0.037,0.06,0.06,0.063,5.5e-09,5.5e-09,2.4e-09,3.6e-06,3.6e-06,1e-06,0,0,0,0,0,0,0,0
-11990000,0.7,0.002,-0.014,0.71,-0.011,0.013,-0.016,-5.7e-05,0.0022,-3.7e+02,-1.3e-05,-5.7e-05,-2.5e-06,-3.9e-06,-1.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00029,0.00029,9.3e-05,0.083,0.083,0.033,0.051,0.051,0.061,5.1e-09,5.1e-09,2.4e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0
+11990000,0.7,0.002,-0.014,0.71,-0.011,0.013,-0.016,-5.7e-05,0.0022,-3.7e+02,-1.3e-05,-5.7e-05,-2.5e-06,-3.9e-06,-1.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.4e-05,0.00029,0.00029,9.3e-05,0.083,0.083,0.033,0.051,0.051,0.061,5.1e-09,5.1e-09,2.4e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0
 12090000,0.7,0.002,-0.014,0.71,-0.013,0.015,-0.022,-0.0013,0.0036,-3.7e+02,-1.3e-05,-5.7e-05,-2.2e-06,-3.8e-06,-1.8e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00029,0.00029,9.3e-05,0.099,0.099,0.031,0.059,0.059,0.061,5.1e-09,5.1e-09,2.4e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0
 12190000,0.7,0.0017,-0.014,0.71,-0.007,0.012,-0.017,0.0015,0.002,-3.7e+02,-1.3e-05,-5.7e-05,-2.1e-06,1.6e-07,5.5e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00026,0.00026,9.3e-05,0.079,0.079,0.028,0.05,0.05,0.059,4.8e-09,4.8e-09,2.3e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0
 12290000,0.7,0.0016,-0.014,0.71,-0.0094,0.014,-0.016,0.00069,0.0033,-3.7e+02,-1.3e-05,-5.7e-05,-2e-06,-1.1e-07,5.7e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00027,0.00027,9.3e-05,0.093,0.093,0.028,0.058,0.058,0.059,4.8e-09,4.8e-09,2.3e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0
@@ -136,7 +136,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 13390000,0.7,0.00081,-0.014,0.71,0.001,0.0062,-0.02,0.0032,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-2e-06,2.6e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.055,0.055,0.026,0.048,0.048,0.05,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,8.8e-07,0,0,0,0,0,0,0,0
 13490000,0.7,0.00084,-0.014,0.71,0.0006,0.0062,-0.019,0.0033,0.0018,-3.7e+02,-1.1e-05,-5.9e-05,-1.7e-06,1.8e-06,1.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.063,0.063,0.028,0.056,0.056,0.05,3.5e-09,3.5e-09,1.8e-09,3.5e-06,3.5e-06,8.7e-07,0,0,0,0,0,0,0,0
 13590000,0.7,0.00078,-0.014,0.71,0.0008,0.0064,-0.021,0.0023,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-1.9e-06,1.7e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.052,0.052,0.028,0.048,0.048,0.05,3.3e-09,3.4e-09,1.8e-09,3.5e-06,3.5e-06,8.4e-07,0,0,0,0,0,0,0,0
-13690000,0.7,0.00076,-0.014,0.71,0.0016,0.0082,-0.025,0.0024,0.0019,-3.7e+02,-1.1e-05,-5.9e-05,-1.4e-06,2e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.059,0.059,0.029,0.056,0.056,0.05,3.3e-09,3.4e-09,1.8e-09,3.5e-06,3.5e-06,8.3e-07,0,0,0,0,0,0,0,0
+13690000,0.7,0.00076,-0.014,0.71,0.0015,0.0082,-0.025,0.0024,0.0019,-3.7e+02,-1.1e-05,-5.9e-05,-1.4e-06,2e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.059,0.059,0.029,0.056,0.056,0.05,3.3e-09,3.4e-09,1.8e-09,3.5e-06,3.5e-06,8.3e-07,0,0,0,0,0,0,0,0
 13790000,0.7,0.00065,-0.014,0.71,0.0021,0.0041,-0.027,0.0034,-0.00048,-3.7e+02,-1.1e-05,-5.9e-05,-1.5e-06,4.8e-07,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.05,0.05,0.029,0.048,0.048,0.049,3.2e-09,3.2e-09,1.7e-09,3.5e-06,3.5e-06,7.9e-07,0,0,0,0,0,0,0,0
 13890000,0.7,0.00062,-0.014,0.71,0.0026,0.004,-0.031,0.0037,-9.5e-05,-3.7e+02,-1.1e-05,-5.9e-05,-1.2e-06,7.8e-07,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.056,0.056,0.03,0.056,0.056,0.05,3.2e-09,3.2e-09,1.7e-09,3.5e-06,3.5e-06,7.8e-07,0,0,0,0,0,0,0,0
 13990000,0.7,0.00056,-0.014,0.71,0.0029,0.0016,-0.03,0.0044,-0.0019,-3.7e+02,-1.1e-05,-6e-05,-1.1e-06,-1.5e-06,1.3e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,0.00019,0.00019,9.3e-05,0.048,0.048,0.03,0.048,0.048,0.05,3e-09,3.1e-09,1.7e-09,3.5e-06,3.5e-06,7.4e-07,0,0,0,0,0,0,0,0
@@ -147,7 +147,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 14490000,0.7,0.00035,-0.014,0.71,0.0089,0.0042,-0.037,0.0096,-0.00085,-3.7e+02,-1e-05,-6e-05,6.8e-07,-2.2e-06,8.9e-06,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,0.00019,0.00019,9.2e-05,0.05,0.05,0.032,0.055,0.055,0.051,2.8e-09,2.8e-09,1.5e-09,3.4e-06,3.4e-06,6.2e-07,0,0,0,0,0,0,0,0
 14590000,0.7,0.00034,-0.013,0.71,0.0055,0.0026,-0.037,0.006,-0.0023,-3.7e+02,-1.1e-05,-6e-05,7.1e-07,-5.1e-06,1.3e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,0.00018,0.00018,9.3e-05,0.043,0.043,0.031,0.047,0.047,0.051,2.6e-09,2.6e-09,1.5e-09,3.4e-06,3.4e-06,5.8e-07,0,0,0,0,0,0,0,0
 14690000,0.7,0.00029,-0.013,0.71,0.0069,-0.00031,-0.034,0.0067,-0.0022,-3.7e+02,-1.1e-05,-6e-05,1.1e-06,-6.1e-06,1.4e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,0.00019,0.00019,9.2e-05,0.048,0.048,0.032,0.054,0.054,0.051,2.6e-09,2.6e-09,1.4e-09,3.4e-06,3.4e-06,5.6e-07,0,0,0,0,0,0,0,0
-14790000,0.7,0.00032,-0.013,0.71,0.0037,-0.0019,-0.03,0.0038,-0.0032,-3.7e+02,-1.1e-05,-6e-05,1.2e-06,-9.4e-06,1.8e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.1e-05,0.00018,0.00018,9.2e-05,0.042,0.042,0.031,0.047,0.047,0.051,2.5e-09,2.5e-09,1.4e-09,3.4e-06,3.4e-06,5.3e-07,0,0,0,0,0,0,0,0
+14790000,0.7,0.00032,-0.013,0.71,0.0037,-0.0019,-0.03,0.0038,-0.0032,-3.7e+02,-1.1e-05,-6e-05,1.2e-06,-9.4e-06,1.8e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,0.00018,0.00018,9.2e-05,0.042,0.042,0.031,0.047,0.047,0.051,2.5e-09,2.5e-09,1.4e-09,3.4e-06,3.4e-06,5.3e-07,0,0,0,0,0,0,0,0
 14890000,0.7,0.00031,-0.013,0.71,0.0053,-0.00088,-0.033,0.0042,-0.0034,-3.7e+02,-1.1e-05,-6e-05,1.5e-06,-9.2e-06,1.8e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,0.00019,0.00019,9.2e-05,0.047,0.047,0.031,0.054,0.054,0.052,2.5e-09,2.5e-09,1.4e-09,3.4e-06,3.4e-06,5.1e-07,0,0,0,0,0,0,0,0
 14990000,0.7,0.00031,-0.013,0.71,0.004,-0.0011,-0.029,0.0032,-0.0027,-3.7e+02,-1.1e-05,-6e-05,1.4e-06,-9.9e-06,2.1e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.1e-05,0.00018,0.00018,9.2e-05,0.041,0.041,0.03,0.047,0.047,0.051,2.3e-09,2.3e-09,1.4e-09,3.4e-06,3.4e-06,4.8e-07,0,0,0,0,0,0,0,0
 15090000,0.7,0.00023,-0.013,0.71,0.0045,-0.0012,-0.032,0.0036,-0.0028,-3.7e+02,-1.1e-05,-6e-05,1.4e-06,-9.4e-06,2e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.1e-05,0.00019,0.00019,9.2e-05,0.046,0.046,0.031,0.054,0.054,0.052,2.3e-09,2.3e-09,1.3e-09,3.4e-06,3.4e-06,4.6e-07,0,0,0,0,0,0,0,0
@@ -171,7 +171,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 16890000,0.71,0.00056,-0.013,0.71,-0.00095,0.0031,-0.011,-0.0045,0.003,-3.7e+02,-1.3e-05,-6e-05,3.1e-06,-2.6e-06,5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,0.00016,0.00016,8.9e-05,0.04,0.04,0.021,0.052,0.052,0.051,1.1e-09,1.1e-09,9.2e-10,3.2e-06,3.2e-06,1.7e-07,0,0,0,0,0,0,0,0
 16990000,0.71,0.0005,-0.013,0.71,-0.00098,0.00099,-0.01,-0.005,0.0011,-3.7e+02,-1.3e-05,-6e-05,2.8e-06,-6.6e-06,5.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,0.00015,0.00015,8.9e-05,0.035,0.035,0.02,0.046,0.046,0.05,1e-09,1e-09,9e-10,3.2e-06,3.2e-06,1.6e-07,0,0,0,0,0,0,0,0
 17090000,0.71,0.00047,-0.013,0.71,-0.00014,0.002,-0.01,-0.0051,0.0012,-3.7e+02,-1.3e-05,-6e-05,2.9e-06,-6.5e-06,5.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,0.00015,0.00015,8.9e-05,0.039,0.039,0.02,0.052,0.052,0.05,1e-09,1e-09,8.8e-10,3.2e-06,3.2e-06,1.6e-07,0,0,0,0,0,0,0,0
-17190000,0.71,0.00046,-0.013,0.71,0.00024,0.0019,-0.011,-0.0054,-0.00032,-3.7e+02,-1.3e-05,-6e-05,3e-06,-1e-05,5.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,0.00015,0.00015,8.8e-05,0.035,0.035,0.019,0.046,0.046,0.049,9.4e-10,9.4e-10,8.6e-10,3.1e-06,3.1e-06,1.5e-07,0,0,0,0,0,0,0,0
+17190000,0.71,0.00046,-0.013,0.71,0.00024,0.0019,-0.011,-0.0054,-0.00032,-3.7e+02,-1.3e-05,-6e-05,3e-06,-1e-05,5.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,0.00015,0.00015,8.8e-05,0.035,0.035,0.019,0.046,0.046,0.049,9.4e-10,9.4e-10,8.6e-10,3.1e-06,3.1e-06,1.5e-07,0,0,0,0,0,0,0,0
 17290000,0.71,0.00043,-0.013,0.71,0.0024,0.003,-0.0066,-0.0053,-9.5e-05,-3.7e+02,-1.3e-05,-6e-05,2.8e-06,-1e-05,5.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,0.00015,0.00015,8.8e-05,0.039,0.039,0.019,0.052,0.052,0.049,9.4e-10,9.4e-10,8.4e-10,3.1e-06,3.1e-06,1.4e-07,0,0,0,0,0,0,0,0
 17390000,0.71,0.0004,-0.013,0.71,0.003,0.0021,-0.0047,-0.0045,-0.0014,-3.7e+02,-1.3e-05,-6e-05,3.1e-06,-1.4e-05,5.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,0.00014,0.00014,8.8e-05,0.034,0.034,0.018,0.046,0.046,0.048,8.5e-10,8.5e-10,8.3e-10,3.1e-06,3.1e-06,1.3e-07,0,0,0,0,0,0,0,0
 17490000,0.71,0.00039,-0.013,0.71,0.0035,0.0017,-0.003,-0.0042,-0.0012,-3.7e+02,-1.3e-05,-6e-05,3.1e-06,-1.4e-05,5.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,0.00014,0.00014,8.8e-05,0.038,0.038,0.018,0.052,0.052,0.049,8.5e-10,8.5e-10,8.1e-10,3.1e-06,3.1e-06,1.3e-07,0,0,0,0,0,0,0,0
@@ -209,7 +209,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 20690000,0.71,0.00037,-0.012,0.71,-0.002,-0.012,0.015,0.0018,-0.003,-3.7e+02,-1.4e-05,-6e-05,6.9e-06,-1.1e-05,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.1e-05,0.0001,9.9e-05,8.2e-05,0.026,0.026,0.0093,0.049,0.049,0.04,1.9e-10,1.9e-10,4.4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 20790000,0.71,0.0004,-0.012,0.71,-0.0031,-0.011,0.015,0.0015,-0.0024,-3.7e+02,-1.4e-05,-6e-05,7e-06,-9.6e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,9.7e-05,9.7e-05,8.1e-05,0.023,0.023,0.0091,0.043,0.043,0.04,1.8e-10,1.8e-10,4.3e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 20890000,0.71,0.00039,-0.012,0.71,-0.0036,-0.013,0.014,0.0012,-0.0036,-3.7e+02,-1.4e-05,-6e-05,7.2e-06,-9.6e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,9.8e-05,9.8e-05,8.1e-05,0.025,0.025,0.0091,0.049,0.049,0.04,1.8e-10,1.8e-10,4.3e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
-20990000,0.71,0.00039,-0.012,0.71,-0.0038,-0.014,0.015,0.0028,-0.003,-3.7e+02,-1.4e-05,-6e-05,7.2e-06,-7.6e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,9.6e-05,9.6e-05,8.1e-05,0.023,0.023,0.0089,0.043,0.043,0.039,1.7e-10,1.7e-10,4.2e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
+20990000,0.71,0.00039,-0.012,0.71,-0.0038,-0.014,0.015,0.0028,-0.003,-3.7e+02,-1.4e-05,-6e-05,7.3e-06,-7.6e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,9.6e-05,9.6e-05,8.1e-05,0.023,0.023,0.0089,0.043,0.043,0.039,1.7e-10,1.7e-10,4.2e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21090000,0.71,0.00039,-0.012,0.71,-0.004,-0.017,0.015,0.0024,-0.0045,-3.7e+02,-1.4e-05,-6e-05,7.4e-06,-7.6e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,9.7e-05,9.6e-05,8.1e-05,0.025,0.025,0.0089,0.048,0.048,0.039,1.7e-10,1.7e-10,4.1e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21190000,0.71,0.00043,-0.012,0.71,-0.0032,-0.015,0.014,0.0038,-0.0037,-3.7e+02,-1.4e-05,-6e-05,7.3e-06,-5.3e-06,7.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,9.5e-05,9.4e-05,8.1e-05,0.022,0.022,0.0087,0.043,0.043,0.039,1.5e-10,1.5e-10,4.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 21290000,0.71,0.00047,-0.012,0.71,-0.0038,-0.018,0.016,0.0035,-0.0053,-3.7e+02,-1.4e-05,-6e-05,7.6e-06,-5.3e-06,7.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,9.5e-05,9.5e-05,8e-05,0.024,0.024,0.0086,0.048,0.048,0.039,1.5e-10,1.5e-10,4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
@@ -235,7 +235,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 23290000,0.71,0.00059,-0.012,0.71,-0.015,-0.0077,0.024,-0.013,-0.0019,-3.7e+02,-1.4e-05,-5.9e-05,6.5e-06,1.6e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.6e-05,8.7e-05,8.6e-05,7.7e-05,0.019,0.019,0.0078,0.046,0.046,0.036,8.2e-11,8.2e-11,2.9e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 23390000,0.71,0.00068,-0.012,0.71,-0.016,-0.0079,0.022,-0.016,-0.0017,-3.7e+02,-1.4e-05,-5.9e-05,6.4e-06,1.7e-05,7.2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.6e-05,8.6e-05,8.5e-05,7.7e-05,0.018,0.018,0.0077,0.041,0.041,0.036,7.8e-11,7.8e-11,2.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 23490000,0.71,0.0031,-0.0096,0.71,-0.023,-0.0088,-0.012,-0.018,-0.0025,-3.7e+02,-1.4e-05,-5.9e-05,6.5e-06,1.6e-05,7.2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.6e-05,8.6e-05,8.6e-05,7.7e-05,0.019,0.019,0.0078,0.045,0.045,0.036,7.8e-11,7.8e-11,2.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
-23590000,0.71,0.0083,-0.0018,0.71,-0.033,-0.0075,-0.044,-0.017,-0.0012,-3.7e+02,-1.4e-05,-5.9e-05,6.4e-06,1.8e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.6e-05,8.5e-05,8.5e-05,7.6e-05,0.017,0.017,0.0077,0.041,0.041,0.035,7.4e-11,7.4e-11,2.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
+23590000,0.71,0.0083,-0.0018,0.71,-0.033,-0.0075,-0.044,-0.017,-0.0012,-3.7e+02,-1.4e-05,-5.9e-05,6.4e-06,1.8e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.6e-05,8.5e-05,8.5e-05,7.7e-05,0.017,0.017,0.0077,0.041,0.041,0.035,7.4e-11,7.4e-11,2.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 23690000,0.71,0.0079,0.004,0.71,-0.065,-0.016,-0.094,-0.021,-0.0023,-3.7e+02,-1.4e-05,-5.9e-05,6.3e-06,1.9e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.6e-05,8.5e-05,8.5e-05,7.6e-05,0.019,0.019,0.0078,0.045,0.045,0.036,7.4e-11,7.4e-11,2.7e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 23790000,0.71,0.005,0.00065,0.71,-0.089,-0.027,-0.15,-0.021,-0.0017,-3.7e+02,-1.4e-05,-5.9e-05,6.3e-06,2.1e-05,6.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.5e-05,8.5e-05,8.4e-05,7.6e-05,0.017,0.017,0.0077,0.041,0.041,0.035,7.1e-11,7.1e-11,2.7e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 23890000,0.71,0.0024,-0.0054,0.71,-0.11,-0.036,-0.2,-0.03,-0.0049,-3.7e+02,-1.4e-05,-5.9e-05,6.3e-06,2.1e-05,6.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.5e-05,8.5e-05,8.5e-05,7.6e-05,0.019,0.019,0.0078,0.045,0.045,0.035,7.1e-11,7.1e-11,2.6e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
@@ -254,7 +254,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 25190000,0.71,0.0082,0.0027,0.71,-0.29,-0.14,-0.91,-0.17,-0.12,-3.7e+02,-1.3e-05,-5.9e-05,5.8e-06,2.8e-05,1.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.3e-05,8e-05,8e-05,7.4e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.2e-11,5.2e-11,2.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25290000,0.71,0.01,0.0095,0.71,-0.32,-0.15,-0.96,-0.2,-0.13,-3.7e+02,-1.3e-05,-5.9e-05,5.7e-06,2.8e-05,1.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.3e-05,8e-05,8e-05,7.4e-05,0.017,0.017,0.0079,0.044,0.044,0.035,5.3e-11,5.3e-11,2.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25390000,0.71,0.011,0.016,0.71,-0.35,-0.17,-1,-0.22,-0.15,-3.7e+02,-1.2e-05,-5.9e-05,5.8e-06,3e-05,-1.1e-07,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.3e-05,7.9e-05,7.9e-05,7.4e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.1e-11,5.1e-11,2.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
-25490000,0.71,0.012,0.017,0.71,-0.4,-0.19,-1.1,-0.25,-0.17,-3.7e+02,-1.2e-05,-5.9e-05,5.8e-06,3e-05,2.4e-08,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.3e-05,8e-05,7.9e-05,7.4e-05,0.017,0.017,0.0079,0.044,0.044,0.035,5.1e-11,5.1e-11,2.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
+25490000,0.71,0.012,0.017,0.71,-0.4,-0.19,-1.1,-0.25,-0.17,-3.7e+02,-1.2e-05,-5.9e-05,5.8e-06,3e-05,2.3e-08,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.3e-05,8e-05,7.9e-05,7.4e-05,0.017,0.017,0.0079,0.044,0.044,0.035,5.1e-11,5.1e-11,2.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25590000,0.71,0.011,0.015,0.71,-0.44,-0.22,-1.1,-0.28,-0.21,-3.7e+02,-1.2e-05,-5.9e-05,5.8e-06,2.6e-05,-9.2e-06,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.3e-05,7.9e-05,7.9e-05,7.4e-05,0.016,0.016,0.0079,0.04,0.04,0.035,4.9e-11,4.9e-11,2.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25690000,0.71,0.015,0.022,0.71,-0.49,-0.24,-1.2,-0.33,-0.23,-3.7e+02,-1.2e-05,-5.9e-05,5.8e-06,2.6e-05,-8.9e-06,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.3e-05,7.9e-05,7.9e-05,7.4e-05,0.017,0.017,0.0079,0.044,0.044,0.035,4.9e-11,4.9e-11,2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25790000,0.71,0.017,0.028,0.71,-0.53,-0.27,-1.2,-0.34,-0.26,-3.7e+02,-1.2e-05,-5.9e-05,5.9e-06,3.4e-05,-3.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.3e-05,7.8e-05,7.8e-05,7.4e-05,0.016,0.016,0.0079,0.04,0.04,0.035,4.7e-11,4.7e-11,2e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
@@ -302,7 +302,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 29990000,0.71,0.0031,0.0076,0.71,-2.1,-1.1,0.9,-9,-4.3,-3.7e+02,-1.2e-05,-5.7e-05,3.5e-06,-0.00014,-0.00025,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.7e-05,7.5e-05,6.7e-05,0.025,0.025,0.009,0.19,0.18,0.036,3.4e-11,3.4e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 30090000,0.71,0.0031,0.0075,0.71,-2.1,-1,0.89,-9.2,-4.4,-3.7e+02,-1.2e-05,-5.7e-05,3.4e-06,-0.00015,-0.00024,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.7e-05,7.5e-05,6.7e-05,0.026,0.027,0.0091,0.2,0.2,0.036,3.4e-11,3.4e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 30190000,0.71,0.0031,0.0074,0.71,-2,-1,0.88,-9.4,-4.5,-3.7e+02,-1.2e-05,-5.7e-05,3.4e-06,-0.00016,-0.00025,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.7e-05,7.5e-05,6.7e-05,0.025,0.025,0.009,0.2,0.19,0.037,3.4e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
-30290000,0.71,0.003,0.0072,0.71,-2,-1,0.86,-9.6,-4.6,-3.7e+02,-1.2e-05,-5.7e-05,3.3e-06,-0.00017,-0.00024,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.8e-05,7.5e-05,6.6e-05,0.026,0.027,0.0091,0.21,0.21,0.037,3.4e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
+30290000,0.71,0.003,0.0072,0.71,-2,-1,0.86,-9.6,-4.6,-3.7e+02,-1.2e-05,-5.7e-05,3.3e-06,-0.00017,-0.00024,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.8e-05,7.5e-05,6.7e-05,0.026,0.027,0.0091,0.21,0.21,0.037,3.4e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 30390000,0.71,0.0031,0.007,0.71,-2,-1,0.85,-9.9,-4.7,-3.7e+02,-1.3e-05,-5.7e-05,3.2e-06,-0.00018,-0.00023,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.5e-05,7.8e-05,7.5e-05,6.6e-05,0.025,0.025,0.009,0.21,0.2,0.036,3.3e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 30490000,0.71,0.0029,0.0068,0.71,-2,-1,0.83,-10,-4.8,-3.7e+02,-1.3e-05,-5.7e-05,3.2e-06,-0.00018,-0.00022,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.5e-05,7.8e-05,7.5e-05,6.6e-05,0.026,0.027,0.0091,0.22,0.22,0.037,3.3e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 30590000,0.71,0.0029,0.0065,0.71,-1.9,-1,0.79,-10,-4.9,-3.7e+02,-1.3e-05,-5.7e-05,3.3e-06,-0.00019,-0.00021,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.5e-05,7.8e-05,7.5e-05,6.6e-05,0.025,0.025,0.009,0.21,0.21,0.037,3.3e-11,3.2e-11,1.2e-10,2.6e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
@@ -323,7 +323,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 32090000,0.71,0.00049,-0.00086,0.71,-1.6,-0.86,0.72,-13,-6.3,-3.7e+02,-1.4e-05,-5.7e-05,2.5e-06,-0.00029,-7.7e-06,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,6.4e-05,7.8e-05,7.3e-05,6.4e-05,0.026,0.026,0.0087,0.29,0.29,0.037,3e-11,3e-11,1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32190000,0.71,0.00028,-0.0016,0.71,-1.5,-0.85,0.72,-13,-6.4,-3.7e+02,-1.4e-05,-5.7e-05,2.3e-06,-0.0003,1.2e-05,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,6.4e-05,7.8e-05,7.3e-05,6.3e-05,0.025,0.025,0.0086,0.29,0.29,0.036,2.9e-11,2.9e-11,9.9e-11,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32290000,0.71,4.7e-05,-0.0024,0.71,-1.5,-0.84,0.71,-13,-6.4,-3.7e+02,-1.4e-05,-5.7e-05,2.3e-06,-0.00031,2.8e-05,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,6.4e-05,7.8e-05,7.3e-05,6.3e-05,0.026,0.026,0.0086,0.3,0.3,0.036,3e-11,2.9e-11,9.8e-11,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
-32390000,0.71,-0.00013,-0.003,0.71,-1.5,-0.83,0.71,-14,-6.5,-3.7e+02,-1.4e-05,-5.7e-05,2.3e-06,-0.00031,3.6e-05,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,7.8e-05,7.3e-05,6.3e-05,0.025,0.025,0.0085,0.3,0.3,0.037,2.9e-11,2.9e-11,9.7e-11,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
+32390000,0.71,-0.00013,-0.003,0.71,-1.5,-0.83,0.71,-14,-6.5,-3.7e+02,-1.4e-05,-5.7e-05,2.3e-06,-0.00031,3.6e-05,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,6.4e-05,7.8e-05,7.3e-05,6.3e-05,0.025,0.025,0.0085,0.3,0.3,0.037,2.9e-11,2.9e-11,9.7e-11,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32490000,0.71,-0.00026,-0.0033,0.71,-1.4,-0.81,0.72,-14,-6.6,-3.7e+02,-1.4e-05,-5.7e-05,2.3e-06,-0.00031,4.7e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,7.8e-05,7.3e-05,6.3e-05,0.026,0.026,0.0085,0.31,0.31,0.037,2.9e-11,2.9e-11,9.6e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0
 32590000,0.71,-0.00025,-0.0035,0.71,-1.4,-0.8,0.71,-14,-6.7,-3.7e+02,-1.5e-05,-5.7e-05,2.2e-06,-0.00032,5.5e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,7.8e-05,7.2e-05,6.3e-05,0.025,0.025,0.0084,0.31,0.31,0.036,2.9e-11,2.9e-11,9.5e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0
 32690000,0.71,-0.0003,-0.0036,0.71,-1.4,-0.79,0.71,-14,-6.8,-3.7e+02,-1.5e-05,-5.7e-05,2.2e-06,-0.00032,6e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,7.8e-05,7.3e-05,6.3e-05,0.026,0.026,0.0085,0.32,0.32,0.036,2.9e-11,2.9e-11,9.4e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0
@@ -372,7 +372,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 36990000,-0.68,0.0022,-0.0036,0.74,0.15,0.19,-0.16,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.7e-06,-0.0015,0.001,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,5.9e-05,1.9e-05,2e-05,6.1e-05,0.051,0.052,0.0056,0.54,0.54,0.031,2.8e-11,2.8e-11,6.8e-11,2.1e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
 37090000,-0.68,0.0021,-0.0035,0.74,0.15,0.2,-0.15,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.9e-06,-0.0015,0.001,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,5.9e-05,1.9e-05,2e-05,6.1e-05,0.056,0.057,0.0057,0.55,0.55,0.031,2.8e-11,2.9e-11,6.7e-11,2.1e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
 37190000,-0.68,0.0024,-0.0034,0.74,0.13,0.17,-0.14,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.1e-06,-0.0016,0.0011,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,5.9e-05,1.8e-05,1.9e-05,6.1e-05,0.05,0.051,0.0057,0.55,0.55,0.031,2.8e-11,2.9e-11,6.7e-11,2e-06,1.9e-06,5e-08,0,0,0,0,0,0,0,0
-37290000,-0.68,0.0024,-0.0034,0.74,0.13,0.18,-0.14,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.3e-06,-0.0016,0.0011,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,5.8e-05,1.8e-05,1.9e-05,6.1e-05,0.055,0.056,0.0059,0.56,0.56,0.031,2.8e-11,2.9e-11,6.6e-11,2e-06,1.9e-06,5e-08,0,0,0,0,0,0,0,0
+37290000,-0.68,0.0024,-0.0034,0.74,0.13,0.18,-0.14,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.3e-06,-0.0016,0.0011,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,5.9e-05,1.8e-05,1.9e-05,6.1e-05,0.055,0.056,0.0059,0.56,0.56,0.031,2.8e-11,2.9e-11,6.6e-11,2e-06,1.9e-06,5e-08,0,0,0,0,0,0,0,0
 37390000,-0.68,0.0026,-0.0033,0.74,0.1,0.15,-0.13,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.4e-06,-0.0017,0.0012,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,5.8e-05,1.8e-05,1.9e-05,6.1e-05,0.049,0.05,0.0059,0.56,0.56,0.031,2.9e-11,2.9e-11,6.6e-11,1.9e-06,1.8e-06,5e-08,0,0,0,0,0,0,0,0
 37490000,-0.68,0.0026,-0.0032,0.74,0.1,0.16,-0.12,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.5e-06,-0.0017,0.0012,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,5.8e-05,1.8e-05,1.9e-05,6.1e-05,0.054,0.055,0.006,0.57,0.57,0.031,2.9e-11,2.9e-11,6.6e-11,1.9e-06,1.8e-06,5e-08,0,0,0,0,0,0,0,0
 37590000,-0.68,0.0027,-0.0031,0.74,0.084,0.13,-0.12,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.7e-06,-0.0018,0.0012,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,5.8e-05,1.7e-05,1.8e-05,6.1e-05,0.048,0.049,0.0061,0.57,0.57,0.031,2.9e-11,2.9e-11,6.5e-11,1.8e-06,1.7e-06,5e-08,0,0,0,0,0,0,0,0

src/modules/ekf2/test/change_indication/iris_gps.csv

@@ -1,6 +1,6 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-90000,0.98,-0.0095,-0.012,0.18,-5.5e-05,-0.0032,-0.024,-3.6e-06,-0.00014,-0.0021,0,0,0,0,0,0,0.2,-3.3e-09,0.43,0,0,0,0,0,8.9e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+10000,1,-0.011,-0.01,0.00012,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+90000,0.98,-0.0095,-0.012,0.18,-5.5e-05,-0.0032,-0.024,-3.6e-06,-0.00014,-0.0021,0,0,0,0,0,0,0.2,9.3e-09,0.43,0,0,0,0,0,8.9e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 190000,0.98,-0.0092,-0.013,0.2,-0.0013,-0.0036,-0.037,-4.4e-05,-0.00046,-0.017,5.2e-12,-4.3e-12,-1.5e-13,0,0,-6.8e-10,0.2,0.011,0.43,0,0,0,0,0,2.8e-06,0.0027,0.0027,0.00081,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 290000,0.98,-0.0092,-0.013,0.2,-0.0016,-0.0053,-0.046,-0.00015,-0.00032,-0.018,4.4e-11,-5.4e-11,-2.6e-12,0,0,-2.9e-08,0.2,0.011,0.43,0,0,0,0,0,6.6e-06,0.0029,0.0029,0.00068,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 390000,0.98,-0.0095,-0.013,0.19,-0.00033,-0.0065,-0.063,-0.00029,-0.00087,-0.013,-6.7e-11,-6.3e-11,3e-12,0,0,8.8e-08,0.2,0.002,0.44,0,0,0,0,0,1.1e-05,0.0031,0.0031,0.00062,25,25,8.1,0.97,0.97,0.32,1e-06,1e-06,8.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
@@ -87,7 +87,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 8490000,0.98,-0.0055,-0.013,0.19,0.0018,0.084,-0.017,0.0012,0.096,-0.041,-1.8e-05,-5.6e-05,-1.7e-08,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1e-05,0.00042,0.00042,0.00014,0.94,0.94,0.019,2.4,2.4,0.056,1.1e-08,1.1e-08,4.3e-09,4e-06,4e-06,1.1e-07,0,0,0,0,0,0,0,0
 8590000,0.98,-0.0054,-0.013,0.19,0.0028,0.088,-0.012,0.0014,0.1,-0.036,-1.8e-05,-5.6e-05,-1.4e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.9e-06,0.00043,0.00043,0.00013,1,1,0.018,2.7,2.7,0.055,1.1e-08,1.1e-08,4.3e-09,4e-06,4e-06,1.1e-07,0,0,0,0,0,0,0,0
 8690000,0.98,-0.0055,-0.013,0.19,0.0028,0.089,-0.014,0.0016,0.11,-0.037,-1.8e-05,-5.6e-05,-1.2e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.8e-06,0.00043,0.00043,0.00013,1.1,1.1,0.018,2.9,2.9,0.055,1.1e-08,1.1e-08,4.3e-09,4e-06,4e-06,1e-07,0,0,0,0,0,0,0,0
-8790000,0.98,-0.0054,-0.013,0.19,0.0042,0.093,-0.013,0.0018,0.12,-0.035,-1.8e-05,-5.6e-05,-3.5e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.6e-06,0.00045,0.00045,0.00013,1.2,1.2,0.018,3.3,3.3,0.055,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,9.6e-08,0,0,0,0,0,0,0,0
+8790000,0.98,-0.0054,-0.013,0.19,0.0042,0.093,-0.013,0.0018,0.12,-0.035,-1.8e-05,-5.6e-05,-3.5e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.7e-06,0.00045,0.00045,0.00013,1.2,1.2,0.018,3.3,3.3,0.055,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,9.6e-08,0,0,0,0,0,0,0,0
 8890000,0.98,-0.0055,-0.013,0.19,0.0041,0.096,-0.0091,0.0022,0.13,-0.029,-1.8e-05,-5.6e-05,-1.8e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.5e-06,0.00045,0.00045,0.00012,1.2,1.2,0.017,3.5,3.6,0.054,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,9.1e-08,0,0,0,0,0,0,0,0
 8990000,0.98,-0.0054,-0.013,0.19,0.0033,0.1,-0.0083,0.0026,0.14,-0.032,-1.8e-05,-5.6e-05,8.4e-08,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.5e-06,0.00046,0.00046,0.00012,1.3,1.3,0.017,4,4,0.054,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,8.8e-08,0,0,0,0,0,0,0,0
 9090000,0.98,-0.0055,-0.013,0.19,0.0036,0.1,-0.0093,0.0029,0.14,-0.032,-1.7e-05,-5.6e-05,3.8e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.4e-06,0.00046,0.00046,0.00012,1.3,1.3,0.016,4.2,4.2,0.053,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,8.4e-08,0,0,0,0,0,0,0,0
@@ -95,7 +95,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 9290000,0.98,-0.0053,-0.013,0.19,0.0094,0.11,-0.0072,0.0042,0.15,-0.03,-1.7e-05,-5.6e-05,7.2e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.2e-06,0.00047,0.00047,0.00011,1.5,1.5,0.015,5,5,0.052,1.1e-08,1.1e-08,4.1e-09,4e-06,4e-06,7.6e-08,0,0,0,0,0,0,0,0
 9390000,0.98,-0.0052,-0.013,0.19,0.0095,0.11,-0.0061,0.0051,0.16,-0.03,-1.7e-05,-5.6e-05,4e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.2e-06,0.00048,0.00048,0.00011,1.6,1.6,0.015,5.5,5.5,0.052,1.1e-08,1.1e-08,4.1e-09,4e-06,4e-06,7.3e-08,0,0,0,0,0,0,0,0
 9490000,0.98,-0.0053,-0.013,0.19,0.0092,0.11,-0.0044,0.0058,0.17,-0.027,-1.7e-05,-5.7e-05,5.1e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.1e-06,0.00047,0.00047,0.00011,1.6,1.6,0.015,5.8,5.8,0.051,1e-08,1e-08,4e-09,4e-06,4e-06,7e-08,0,0,0,0,0,0,0,0
-9590000,0.98,-0.0054,-0.013,0.19,0.0091,0.11,-0.0043,0.0064,0.18,-0.029,-1.7e-05,-5.6e-05,-1.1e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9e-06,0.00049,0.00049,0.00011,1.7,1.7,0.014,6.4,6.4,0.05,1e-08,1e-08,4e-09,4e-06,4e-06,6.7e-08,0,0,0,0,0,0,0,0
+9590000,0.98,-0.0054,-0.013,0.19,0.0091,0.11,-0.0043,0.0064,0.18,-0.029,-1.7e-05,-5.6e-05,-1.1e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.1e-06,0.00049,0.00049,0.00011,1.7,1.7,0.014,6.4,6.4,0.05,1e-08,1e-08,4e-09,4e-06,4e-06,6.7e-08,0,0,0,0,0,0,0,0
 9690000,0.98,-0.0054,-0.013,0.19,0.0088,0.11,-0.0014,0.0069,0.18,-0.027,-1.7e-05,-5.7e-05,-2.9e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9e-06,0.00047,0.00047,0.00011,1.7,1.7,0.014,6.6,6.6,0.05,9.9e-09,9.9e-09,3.9e-09,4e-06,4e-06,6.5e-08,0,0,0,0,0,0,0,0
 9790000,0.98,-0.0054,-0.013,0.19,0.01,0.11,-0.0027,0.0077,0.19,-0.028,-1.7e-05,-5.7e-05,-9.1e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9e-06,0.00049,0.00049,0.00011,1.8,1.8,0.014,7.3,7.3,0.05,9.9e-09,9.9e-09,3.9e-09,4e-06,4e-06,6.2e-08,0,0,0,0,0,0,0,0
 9890000,0.98,-0.0055,-0.013,0.19,0.012,0.11,-0.0014,0.0082,0.18,-0.029,-1.6e-05,-5.7e-05,-8e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00047,0.00047,0.00011,1.8,1.8,0.013,7.4,7.4,0.049,9.6e-09,9.6e-09,3.9e-09,4e-06,4e-06,6e-08,0,0,0,0,0,0,0,0
@@ -116,7 +116,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 11390000,0.98,-0.0059,-0.013,0.19,0.0022,0.016,0.016,0.00085,-0.00079,-0.0087,-1.4e-05,-5.8e-05,-3.2e-06,1.1e-05,2.3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00044,0.00044,0.00011,0.093,0.093,0.062,0.057,0.057,0.066,7.4e-09,7.4e-09,3.1e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0
 11490000,0.98,-0.0059,-0.013,0.19,0.0012,0.017,0.02,0.00095,0.00085,-0.0026,-1.4e-05,-5.8e-05,-3.1e-06,1.1e-05,2.3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00046,0.00046,0.00011,0.11,0.11,0.057,0.065,0.065,0.067,7.4e-09,7.4e-09,3e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0
 11590000,0.98,-0.0062,-0.012,0.19,0.0031,0.013,0.018,0.00081,-0.00023,-0.0037,-1.3e-05,-5.8e-05,-3.1e-06,1.5e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00039,0.00039,0.00011,0.092,0.092,0.048,0.054,0.054,0.065,6.7e-09,6.7e-09,3e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0
-11690000,0.98,-0.0061,-0.012,0.19,0.0035,0.017,0.018,0.0012,0.0013,-0.0051,-1.3e-05,-5.8e-05,-2.8e-06,1.5e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.0004,0.0004,0.00011,0.11,0.11,0.044,0.062,0.062,0.066,6.7e-09,6.7e-09,2.9e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0
+11690000,0.98,-0.0061,-0.012,0.19,0.0035,0.017,0.018,0.0012,0.0013,-0.0051,-1.3e-05,-5.8e-05,-2.8e-06,1.5e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.0004,0.0004,0.00011,0.11,0.11,0.044,0.062,0.062,0.066,6.7e-09,6.7e-09,2.9e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0
 11790000,0.98,-0.0065,-0.012,0.19,0.0023,0.011,0.019,0.00068,-0.0016,-0.0021,-1.2e-05,-5.9e-05,-2.2e-06,2e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00034,0.00034,0.00011,0.089,0.09,0.037,0.052,0.052,0.063,6.2e-09,6.2e-09,2.9e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0
 11890000,0.98,-0.0066,-0.012,0.19,0.0049,0.013,0.017,0.00098,-0.00042,-0.0014,-1.2e-05,-5.9e-05,-2.2e-06,2e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00035,0.00035,0.0001,0.11,0.11,0.034,0.06,0.06,0.063,6.2e-09,6.2e-09,2.8e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0
 11990000,0.98,-0.0068,-0.012,0.19,0.0079,0.013,0.015,0.0021,-0.0016,-0.0051,-1.2e-05,-5.9e-05,-2e-06,2e-05,4.1e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.0003,0.0003,0.00011,0.086,0.086,0.03,0.051,0.051,0.061,5.7e-09,5.7e-09,2.8e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0
@@ -129,7 +129,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 12690000,0.98,-0.007,-0.012,0.19,0.0083,9.1e-05,0.019,0.004,-0.0011,0.0032,-1.1e-05,-5.9e-05,-2.1e-06,2.6e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00024,0.00024,0.0001,0.083,0.083,0.016,0.058,0.058,0.053,4.7e-09,4.7e-09,2.4e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 12790000,0.98,-0.0072,-0.012,0.19,0.0099,-0.0034,0.021,0.0041,-0.0043,0.0053,-1.1e-05,-5.9e-05,-1.2e-06,2.7e-05,4.7e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00022,0.00022,0.0001,0.067,0.067,0.014,0.049,0.049,0.052,4.4e-09,4.4e-09,2.3e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 12890000,0.98,-0.0072,-0.012,0.19,0.01,-0.0042,0.022,0.0051,-0.0046,0.0083,-1.1e-05,-5.9e-05,-6.7e-07,2.7e-05,4.7e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00023,0.00023,0.0001,0.077,0.077,0.013,0.058,0.058,0.051,4.4e-09,4.4e-09,2.3e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
-12990000,0.98,-0.0072,-0.012,0.19,0.008,-0.0023,0.022,0.0036,-0.0034,0.0095,-1.1e-05,-6e-05,-1.2e-07,2.7e-05,4.6e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.00021,0.00021,0.0001,0.063,0.063,0.012,0.049,0.049,0.05,4.2e-09,4.2e-09,2.3e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
+12990000,0.98,-0.0072,-0.012,0.19,0.008,-0.0023,0.022,0.0036,-0.0034,0.0095,-1.1e-05,-6e-05,-1.2e-07,2.7e-05,4.6e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00021,0.00021,0.0001,0.063,0.063,0.012,0.049,0.049,0.05,4.2e-09,4.2e-09,2.3e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 13090000,0.98,-0.0072,-0.012,0.19,0.0089,-0.0022,0.02,0.0044,-0.0036,0.0084,-1.1e-05,-6e-05,5.7e-07,2.8e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.00022,0.00022,0.0001,0.072,0.072,0.012,0.057,0.057,0.049,4.2e-09,4.2e-09,2.2e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 13190000,0.98,-0.0072,-0.012,0.19,0.0039,-0.0039,0.019,0.00096,-0.0044,0.009,-1.1e-05,-6e-05,1e-06,2.9e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.00021,0.00021,0.0001,0.059,0.059,0.011,0.049,0.049,0.047,4e-09,4e-09,2.2e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 13290000,0.98,-0.0073,-0.012,0.19,0.0036,-0.0048,0.016,0.0013,-0.0048,0.0083,-1.1e-05,-6e-05,1.1e-06,2.9e-05,4.4e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.00021,0.00021,0.0001,0.067,0.067,0.01,0.057,0.057,0.047,4e-09,4e-09,2.1e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
@@ -139,7 +139,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 13690000,0.98,-0.0071,-0.012,0.19,0.0076,-0.0029,0.017,0.0048,-0.0033,0.0063,-1.1e-05,-6e-05,1.5e-06,3.2e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.0002,0.0002,0.0001,0.06,0.06,0.0085,0.056,0.056,0.044,3.6e-09,3.6e-09,1.9e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 13790000,0.98,-0.0071,-0.012,0.19,0.015,0.0011,0.017,0.0083,-0.00086,0.0058,-1.1e-05,-5.9e-05,1.4e-06,3.6e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.0002,0.0002,0.0001,0.05,0.05,0.0082,0.048,0.048,0.043,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 13890000,0.98,-0.007,-0.012,0.19,0.016,0.0019,0.018,0.0098,-0.0007,0.008,-1.1e-05,-5.9e-05,1.9e-06,3.6e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.0002,0.0002,0.0001,0.057,0.057,0.008,0.056,0.056,0.042,3.5e-09,3.5e-09,1.8e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
-13990000,0.98,-0.007,-0.012,0.19,0.015,0.002,0.017,0.0074,-0.0022,0.0069,-1.1e-05,-5.9e-05,2.4e-06,3.4e-05,4e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.00019,0.00019,0.0001,0.048,0.048,0.0077,0.048,0.048,0.041,3.3e-09,3.3e-09,1.8e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
+13990000,0.98,-0.007,-0.012,0.19,0.015,0.002,0.017,0.0074,-0.0022,0.0069,-1.1e-05,-5.9e-05,2.4e-06,3.4e-05,4e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.00019,0.00019,0.0001,0.048,0.048,0.0077,0.048,0.048,0.041,3.3e-09,3.3e-09,1.8e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 14090000,0.98,-0.0071,-0.012,0.19,0.013,-0.0024,0.018,0.0088,-0.0023,0.0033,-1.1e-05,-5.9e-05,1.5e-06,3.5e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.0002,0.0002,0.0001,0.055,0.055,0.0076,0.055,0.055,0.041,3.3e-09,3.3e-09,1.8e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 14190000,0.98,-0.007,-0.012,0.19,0.01,-0.0012,0.018,0.008,-0.0017,0.0035,-1.1e-05,-5.9e-05,1e-06,3.6e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.00019,0.00019,0.0001,0.046,0.046,0.0074,0.048,0.048,0.04,3.1e-09,3.1e-09,1.7e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
 14290000,0.98,-0.007,-0.011,0.19,0.012,-0.0012,0.016,0.009,-0.0018,0.0078,-1.1e-05,-6e-05,1.1e-06,3.6e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.0002,0.0002,0.0001,0.052,0.052,0.0073,0.055,0.055,0.04,3.1e-09,3.1e-09,1.7e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0
@@ -164,12 +164,12 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 16190000,0.98,-0.0074,-0.011,0.19,-0.001,-0.0046,0.023,0.0028,-0.0033,0.016,-1.2e-05,-6.1e-05,3.1e-06,4.5e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00017,0.00017,9.4e-05,0.038,0.038,0.0076,0.046,0.046,0.034,1.5e-09,1.5e-09,1.1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0
 16290000,0.98,-0.0075,-0.011,0.19,-0.00067,-0.0061,0.023,0.0027,-0.0039,0.017,-1.2e-05,-6.1e-05,3.3e-06,4.5e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00017,0.00017,9.3e-05,0.043,0.043,0.0077,0.053,0.053,0.034,1.5e-09,1.5e-09,1.1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0
 16390000,0.98,-0.0074,-0.011,0.19,0.0018,-0.0055,0.023,0.0037,-0.0029,0.017,-1.2e-05,-6.1e-05,3.6e-06,5.1e-05,2.8e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00016,0.00016,9.2e-05,0.037,0.038,0.0077,0.046,0.046,0.034,1.4e-09,1.4e-09,1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0
-16490000,0.98,-0.0075,-0.011,0.19,0.0037,-0.007,0.026,0.0039,-0.0036,0.021,-1.2e-05,-6.1e-05,3.5e-06,5e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00017,0.00017,9.2e-05,0.042,0.042,0.0079,0.052,0.053,0.034,1.4e-09,1.4e-09,1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0
+16490000,0.98,-0.0075,-0.011,0.19,0.0037,-0.007,0.026,0.0039,-0.0036,0.021,-1.2e-05,-6.1e-05,3.5e-06,5e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00017,0.00017,9.2e-05,0.042,0.042,0.0079,0.052,0.053,0.034,1.4e-09,1.4e-09,1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0
 16590000,0.98,-0.0075,-0.011,0.19,0.0078,-0.0071,0.029,0.0034,-0.0028,0.021,-1.2e-05,-6.1e-05,3.5e-06,5.1e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00016,0.00016,9.2e-05,0.037,0.037,0.0079,0.046,0.046,0.034,1.3e-09,1.3e-09,1e-09,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
 16690000,0.98,-0.0076,-0.011,0.19,0.0093,-0.012,0.029,0.0043,-0.0037,0.022,-1.2e-05,-6.1e-05,3.7e-06,5.2e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00016,0.00016,9.1e-05,0.041,0.041,0.008,0.052,0.052,0.034,1.3e-09,1.3e-09,9.8e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
 16790000,0.98,-0.0074,-0.011,0.19,0.01,-0.011,0.028,0.0033,-0.0027,0.022,-1.2e-05,-6.1e-05,3.8e-06,5.3e-05,3.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00016,0.00015,9.1e-05,0.036,0.036,0.008,0.046,0.046,0.034,1.2e-09,1.2e-09,9.6e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
 16890000,0.98,-0.0074,-0.011,0.19,0.0092,-0.011,0.029,0.0043,-0.0037,0.02,-1.2e-05,-6.1e-05,4.2e-06,5.6e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00016,0.00016,9e-05,0.041,0.041,0.0082,0.052,0.052,0.034,1.2e-09,1.2e-09,9.3e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
-16990000,0.98,-0.0074,-0.011,0.19,0.0088,-0.01,0.029,0.0041,-0.0028,0.019,-1.3e-05,-6.1e-05,4.3e-06,6.2e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8e-06,0.00015,0.00015,9e-05,0.036,0.036,0.0082,0.046,0.046,0.034,1.1e-09,1.1e-09,9.1e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
+16990000,0.98,-0.0074,-0.011,0.19,0.0088,-0.01,0.029,0.0041,-0.0028,0.019,-1.3e-05,-6.1e-05,4.3e-06,6.2e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00015,0.00015,9e-05,0.036,0.036,0.0082,0.046,0.046,0.034,1.1e-09,1.1e-09,9.1e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
 17090000,0.98,-0.0075,-0.011,0.19,0.01,-0.013,0.028,0.0051,-0.004,0.018,-1.2e-05,-6.1e-05,4.2e-06,6.4e-05,2.8e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8e-06,0.00015,0.00015,8.9e-05,0.04,0.04,0.0083,0.052,0.052,0.034,1.1e-09,1.1e-09,8.9e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
 17190000,0.98,-0.0076,-0.011,0.19,0.0092,-0.018,0.03,0.0034,-0.0075,0.021,-1.2e-05,-6.1e-05,3.8e-06,6.2e-05,2.6e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8e-06,0.00015,0.00015,8.9e-05,0.035,0.035,0.0083,0.046,0.046,0.034,9.7e-10,9.7e-10,8.8e-10,3.1e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
 17290000,0.98,-0.0076,-0.011,0.19,0.01,-0.019,0.03,0.0044,-0.0093,0.021,-1.2e-05,-6.1e-05,3.5e-06,6.5e-05,2.4e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8e-06,0.00015,0.00015,8.8e-05,0.039,0.039,0.0084,0.052,0.052,0.034,9.7e-10,9.7e-10,8.6e-10,3.1e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0
@@ -212,7 +212,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 20990000,0.98,0.0061,-0.0038,0.19,-0.032,0.03,-0.25,0.0023,0.00052,-0.011,-1.5e-05,-5.9e-05,2.5e-06,0.00015,2.3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.6e-05,9.5e-05,7.2e-05,0.023,0.023,0.0083,0.043,0.043,0.036,1.7e-10,1.7e-10,4.2e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21090000,0.98,0.0045,-0.0042,0.19,-0.046,0.046,-0.37,-0.0016,0.0044,-0.042,-1.5e-05,-5.9e-05,2.5e-06,0.00014,2.3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.7e-05,9.5e-05,7.2e-05,0.026,0.026,0.0084,0.048,0.048,0.036,1.7e-10,1.7e-10,4.1e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21190000,0.98,0.0017,-0.0058,0.19,-0.049,0.05,-0.5,-0.0012,0.0035,-0.078,-1.4e-05,-5.9e-05,2.5e-06,0.00014,1.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.5e-05,9.3e-05,7.2e-05,0.023,0.023,0.0083,0.043,0.043,0.036,1.5e-10,1.5e-10,4e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
-21290000,0.98,-0.00049,-0.0072,0.19,-0.049,0.054,-0.63,-0.0061,0.0087,-0.14,-1.4e-05,-5.9e-05,2.2e-06,0.00015,1.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,9.5e-05,9.4e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.5e-10,1.5e-10,3.9e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
+21290000,0.98,-0.00049,-0.0072,0.19,-0.049,0.054,-0.63,-0.0061,0.0087,-0.14,-1.4e-05,-5.9e-05,2.2e-06,0.00015,1.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.5e-05,9.4e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.5e-10,1.5e-10,3.9e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21390000,0.98,-0.002,-0.0079,0.19,-0.044,0.05,-0.75,-0.005,0.011,-0.2,-1.4e-05,-5.9e-05,2.2e-06,0.00015,1.2e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,9.3e-05,9.1e-05,7.1e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.4e-10,1.4e-10,3.9e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0092,0.016,-0.29,-1.4e-05,-5.9e-05,2.3e-06,0.00015,1.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,9.3e-05,9.2e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-10,1.4e-10,3.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 21590000,0.98,-0.0033,-0.0083,0.19,-0.031,0.043,-1,-0.0079,0.016,-0.38,-1.4e-05,-5.9e-05,2.4e-06,0.00015,1.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,9.1e-05,9e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-10,1.3e-10,3.7e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
@@ -235,7 +235,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 23290000,0.98,-0.0079,-0.013,0.19,0.052,-0.051,-1.4,0.071,-0.05,-2.8,-1.4e-05,-5.8e-05,2.7e-06,0.00017,-6.8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.9e-05,7.8e-05,6.5e-05,0.019,0.02,0.0081,0.046,0.046,0.036,7.9e-11,7.9e-11,2.9e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 23390000,0.98,-0.0078,-0.014,0.19,0.057,-0.053,-1.4,0.082,-0.055,-2.9,-1.4e-05,-5.8e-05,2.5e-06,0.00017,-4.1e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.8e-05,7.7e-05,6.4e-05,0.018,0.018,0.008,0.041,0.041,0.036,7.5e-11,7.5e-11,2.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 23490000,0.98,-0.0079,-0.014,0.19,0.061,-0.055,-1.4,0.088,-0.061,-3,-1.4e-05,-5.8e-05,2.7e-06,0.00017,-4.5e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.9e-05,7.7e-05,6.4e-05,0.019,0.019,0.0081,0.046,0.046,0.036,7.5e-11,7.5e-11,2.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-23590000,0.98,-0.0082,-0.014,0.18,0.064,-0.058,-1.4,0.095,-0.07,-3.2,-1.4e-05,-5.8e-05,2.7e-06,0.00017,-5.7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.8e-05,7.7e-05,6.4e-05,0.017,0.017,0.008,0.041,0.041,0.035,7.2e-11,7.2e-11,2.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+23590000,0.98,-0.0082,-0.014,0.18,0.064,-0.058,-1.4,0.095,-0.07,-3.2,-1.4e-05,-5.8e-05,2.7e-06,0.00017,-5.7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.8e-05,7.7e-05,6.4e-05,0.017,0.017,0.008,0.041,0.041,0.035,7.2e-11,7.2e-11,2.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 23690000,0.98,-0.0088,-0.014,0.18,0.062,-0.061,-1.3,0.1,-0.076,-3.3,-1.4e-05,-5.8e-05,2.8e-06,0.00017,-5.9e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.8e-05,7.7e-05,6.3e-05,0.018,0.018,0.0081,0.046,0.046,0.036,7.2e-11,7.2e-11,2.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 23790000,0.98,-0.011,-0.017,0.18,0.057,-0.058,-0.96,0.11,-0.081,-3.4,-1.4e-05,-5.8e-05,3e-06,0.00017,-5.9e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.3e-05,0.016,0.016,0.008,0.041,0.041,0.035,6.8e-11,6.8e-11,2.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 23890000,0.98,-0.014,-0.021,0.18,0.053,-0.059,-0.52,0.12,-0.087,-3.5,-1.4e-05,-5.8e-05,3e-06,0.00017,-6e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.3e-05,0.017,0.017,0.008,0.045,0.045,0.035,6.9e-11,6.9e-11,2.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
@@ -244,8 +244,8 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 24190000,0.98,-0.013,-0.019,0.18,0.072,-0.071,0.077,0.14,-0.1,-3.6,-1.4e-05,-5.8e-05,2.9e-06,0.00019,-1.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.2e-05,0.015,0.015,0.008,0.04,0.04,0.035,6.3e-11,6.3e-11,2.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 24290000,0.98,-0.01,-0.016,0.18,0.075,-0.075,0.055,0.15,-0.11,-3.6,-1.4e-05,-5.8e-05,2.9e-06,0.00019,-1.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.2e-05,0.016,0.016,0.0081,0.044,0.044,0.036,6.3e-11,6.3e-11,2.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 24390000,0.98,-0.0096,-0.015,0.18,0.069,-0.069,0.071,0.15,-0.11,-3.6,-1.4e-05,-5.8e-05,3.1e-06,0.00019,-2.7e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.1e-05,0.015,0.015,0.008,0.04,0.04,0.035,6.1e-11,6.1e-11,2.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-24490000,0.98,-0.0098,-0.015,0.18,0.064,-0.066,0.069,0.16,-0.11,-3.6,-1.4e-05,-5.8e-05,3.3e-06,0.00019,-2.7e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.7e-05,7.6e-05,6.1e-05,0.016,0.016,0.0081,0.044,0.044,0.035,6.1e-11,6.1e-11,2.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-24590000,0.98,-0.01,-0.015,0.18,0.061,-0.062,0.065,0.16,-0.11,-3.6,-1.4e-05,-5.8e-05,3.3e-06,0.00019,-3.4e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.7e-05,7.6e-05,6.1e-05,0.015,0.015,0.008,0.04,0.04,0.036,5.9e-11,5.9e-11,2.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+24490000,0.98,-0.0098,-0.015,0.18,0.064,-0.066,0.069,0.16,-0.11,-3.6,-1.4e-05,-5.8e-05,3.3e-06,0.00019,-2.7e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.1e-05,0.016,0.016,0.0081,0.044,0.044,0.035,6.1e-11,6.1e-11,2.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+24590000,0.98,-0.01,-0.015,0.18,0.061,-0.062,0.065,0.16,-0.11,-3.6,-1.4e-05,-5.8e-05,3.3e-06,0.00019,-3.4e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.1e-05,0.015,0.015,0.008,0.04,0.04,0.036,5.9e-11,5.9e-11,2.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 24690000,0.98,-0.011,-0.014,0.18,0.059,-0.061,0.064,0.17,-0.12,-3.6,-1.4e-05,-5.8e-05,3.3e-06,0.00019,-3.4e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.7e-05,7.6e-05,6.1e-05,0.016,0.016,0.0081,0.044,0.044,0.036,5.9e-11,5.9e-11,2.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 24790000,0.98,-0.011,-0.014,0.18,0.056,-0.059,0.056,0.17,-0.11,-3.6,-1.5e-05,-5.8e-05,3.3e-06,0.00019,-3.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.7e-05,7.6e-05,6e-05,0.015,0.015,0.008,0.039,0.039,0.035,5.7e-11,5.7e-11,2.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 24890000,0.98,-0.011,-0.013,0.18,0.054,-0.062,0.045,0.18,-0.12,-3.6,-1.5e-05,-5.8e-05,3.4e-06,0.00019,-3.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.7e-05,7.6e-05,6e-05,0.016,0.016,0.008,0.043,0.043,0.035,5.7e-11,5.7e-11,2.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
@@ -277,8 +277,8 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 27490000,0.98,-0.01,-0.017,0.18,-0.081,0.053,0.76,0.047,-0.03,-3.5,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-8.8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.8e-05,5.4e-05,0.014,0.014,0.0081,0.042,0.042,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 27590000,0.98,-0.01,-0.016,0.18,-0.075,0.055,0.85,0.038,-0.025,-3.4,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.8e-05,5.4e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 27690000,0.98,-0.0089,-0.013,0.18,-0.072,0.052,0.76,0.031,-0.02,-3.3,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-9.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.3e-05,0.014,0.014,0.0081,0.042,0.042,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-27790000,0.98,-0.0076,-0.011,0.18,-0.071,0.05,0.75,0.025,-0.018,-3.3,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-8.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8e-05,7.9e-05,5.3e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-27890000,0.98,-0.0072,-0.012,0.19,-0.078,0.057,0.79,0.018,-0.012,-3.2,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-8.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8e-05,7.9e-05,5.3e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+27790000,0.98,-0.0076,-0.011,0.18,-0.071,0.05,0.75,0.025,-0.018,-3.3,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-8.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.3e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+27890000,0.98,-0.0072,-0.012,0.19,-0.078,0.057,0.79,0.018,-0.012,-3.2,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-8.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.3e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 27990000,0.98,-0.0077,-0.012,0.18,-0.078,0.058,0.78,0.012,-0.011,-3.1,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-8.2e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8e-05,7.9e-05,5.3e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.7e-11,3.7e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 28090000,0.98,-0.008,-0.012,0.18,-0.082,0.059,0.78,0.0043,-0.0048,-3,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-8.3e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8e-05,7.9e-05,5.2e-05,0.014,0.014,0.0081,0.041,0.041,0.035,3.7e-11,3.7e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 28190000,0.98,-0.0074,-0.012,0.18,-0.082,0.055,0.79,-0.0022,-0.0043,-3,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-7.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8e-05,7.9e-05,5.2e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-11,3.6e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
@@ -293,7 +293,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 29090000,0.98,-0.0058,-0.013,0.18,-0.082,0.063,0.78,-0.054,0.028,-2.3,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8.2e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.4e-11,3.4e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 29190000,0.98,-0.0058,-0.013,0.18,-0.078,0.061,0.78,-0.051,0.027,-2.2,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8.4e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-11,3.4e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 29290000,0.98,-0.006,-0.013,0.18,-0.08,0.067,0.78,-0.059,0.033,-2.2,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8.4e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.4e-11,3.4e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-29390000,0.98,-0.0065,-0.012,0.18,-0.075,0.065,0.78,-0.057,0.034,-2.1,-1.5e-05,-5.8e-05,1.5e-06,0.0002,-8.6e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,5e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+29390000,0.98,-0.0065,-0.012,0.18,-0.075,0.065,0.78,-0.057,0.034,-2.1,-1.5e-05,-5.8e-05,1.5e-06,0.0002,-8.6e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 29490000,0.98,-0.0065,-0.012,0.18,-0.078,0.066,0.78,-0.065,0.041,-2,-1.5e-05,-5.8e-05,1.6e-06,0.0002,-8.6e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,5e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 29590000,0.98,-0.0064,-0.012,0.18,-0.074,0.064,0.78,-0.062,0.04,-1.9,-1.5e-05,-5.8e-05,1.7e-06,0.0002,-8.8e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,5e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 29690000,0.98,-0.0064,-0.012,0.18,-0.078,0.063,0.78,-0.07,0.046,-1.9,-1.5e-05,-5.8e-05,1.7e-06,0.0002,-9e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.9e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
@@ -311,7 +311,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 30890000,0.98,-0.0058,-0.013,0.18,-0.044,0.032,0.76,-0.067,0.056,-1,-1.4e-05,-5.7e-05,1.9e-06,0.00022,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.04,0.04,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 30990000,0.98,-0.006,-0.013,0.18,-0.037,0.026,0.76,-0.057,0.049,-0.94,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 31090000,0.98,-0.0061,-0.013,0.19,-0.035,0.025,0.76,-0.061,0.051,-0.86,-1.4e-05,-5.7e-05,1.8e-06,0.00023,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.04,0.04,0.036,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-31190000,0.98,-0.0063,-0.013,0.19,-0.031,0.021,0.76,-0.052,0.046,-0.79,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+31190000,0.98,-0.0063,-0.013,0.19,-0.031,0.021,0.76,-0.052,0.046,-0.79,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 31290000,0.98,-0.0066,-0.014,0.18,-0.028,0.018,0.76,-0.055,0.048,-0.72,-1.4e-05,-5.7e-05,2e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 31390000,0.98,-0.0063,-0.013,0.18,-0.022,0.012,0.76,-0.046,0.042,-0.65,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8.1e-05,7.9e-05,4.6e-05,0.013,0.013,0.0079,0.037,0.037,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 31490000,0.98,-0.0061,-0.014,0.19,-0.022,0.009,0.76,-0.049,0.043,-0.58,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8.1e-05,8e-05,4.6e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-11,2.9e-11,1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
@@ -329,8 +329,8 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 32690000,0.98,-0.0095,-0.013,0.18,0.035,-0.08,-0.12,0.025,0.012,0.02,-1.4e-05,-5.7e-05,2.1e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.9e-05,7.8e-05,4.5e-05,0.019,0.019,0.0074,0.041,0.041,0.035,2.7e-11,2.7e-11,9.4e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 32790000,0.98,-0.0092,-0.013,0.18,0.034,-0.078,-0.12,0.034,0.01,0.0042,-1.4e-05,-5.7e-05,2.2e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.6e-05,7.5e-05,4.4e-05,0.019,0.019,0.0071,0.037,0.037,0.035,2.7e-11,2.7e-11,9.3e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 32890000,0.98,-0.0091,-0.013,0.18,0.033,-0.084,-0.13,0.038,0.0021,-0.011,-1.4e-05,-5.7e-05,2.2e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.6e-05,7.5e-05,4.4e-05,0.023,0.023,0.007,0.041,0.041,0.035,2.7e-11,2.7e-11,9.2e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-32990000,0.98,-0.0089,-0.013,0.18,0.03,-0.08,-0.13,0.045,-0.0011,-0.024,-1.4e-05,-5.6e-05,2.3e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,7.2e-05,7.1e-05,4.4e-05,0.024,0.024,0.0067,0.038,0.038,0.035,2.7e-11,2.7e-11,9.1e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-33090000,0.98,-0.0088,-0.013,0.18,0.026,-0.083,-0.12,0.048,-0.0092,-0.031,-1.4e-05,-5.6e-05,2.3e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,7.2e-05,7.1e-05,4.4e-05,0.028,0.029,0.0066,0.042,0.042,0.035,2.7e-11,2.7e-11,9.1e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+32990000,0.98,-0.0089,-0.013,0.18,0.03,-0.08,-0.13,0.045,-0.0011,-0.024,-1.4e-05,-5.6e-05,2.3e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.2e-05,7.1e-05,4.4e-05,0.024,0.024,0.0067,0.038,0.038,0.035,2.7e-11,2.7e-11,9.1e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+33090000,0.98,-0.0088,-0.013,0.18,0.026,-0.083,-0.12,0.048,-0.0092,-0.031,-1.4e-05,-5.6e-05,2.3e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.2e-05,7.1e-05,4.4e-05,0.028,0.029,0.0066,0.042,0.042,0.035,2.7e-11,2.7e-11,9.1e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 33190000,0.98,-0.0085,-0.013,0.18,0.022,-0.079,-0.12,0.054,-0.011,-0.037,-1.4e-05,-5.6e-05,2.2e-06,0.00026,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,6.6e-05,6.6e-05,4.4e-05,0.029,0.029,0.0064,0.038,0.038,0.035,2.6e-11,2.6e-11,9e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 33290000,0.98,-0.0086,-0.013,0.18,0.019,-0.08,-0.12,0.056,-0.019,-0.045,-1.4e-05,-5.6e-05,2.3e-06,0.00026,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,6.7e-05,6.6e-05,4.4e-05,0.035,0.035,0.0063,0.043,0.043,0.034,2.6e-11,2.6e-11,8.9e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 33390000,0.98,-0.0081,-0.013,0.18,0.014,-0.066,-0.12,0.059,-0.014,-0.053,-1.4e-05,-5.6e-05,2.3e-06,0.00025,-0.00016,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,6e-05,5.9e-05,4.4e-05,0.035,0.035,0.0062,0.039,0.039,0.034,2.6e-11,2.6e-11,8.8e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -141,13 +141,9 @@ bool FlightTaskAuto::update()
 		_velocity_setpoint(2) = NAN;
 		break;
 
-	case WaypointType::loiter:
-		if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
-			rcHelpModifyYaw(_yaw_setpoint);
-		}
-
 	// FALLTHROUGH
 	case WaypointType::takeoff:
+	case WaypointType::loiter:
 	case WaypointType::position:
 	default:
 		// Simple waypoint navigation: go to xyz target, with standard limitations
@@ -217,18 +213,6 @@ void FlightTaskAuto::overrideCruiseSpeed(const float cruise_speed_m_s)
 	_time_last_cruise_speed_override = hrt_absolute_time();
 }
 
-void FlightTaskAuto::rcHelpModifyYaw(float &yaw_sp)
-{
-	// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
-	if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
-		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
-					       _deltatime);
-
-		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
-		_yaw_sp_aligned = true;
-	}
-}
-
 void FlightTaskAuto::_prepareLandSetpoints()
 {
 	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
@@ -259,7 +243,11 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		// Stick full up -1 -> stop, stick full down 1 -> double the speed
 		vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo());
 
-		rcHelpModifyYaw(_land_heading);
+		// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
+		if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
+			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
+						       _deltatime);
+		}
 
 		Vector2f sticks_xy = _sticks.getPitchRollExpo();
 		Vector2f sticks_ne = sticks_xy;
@@ -287,6 +275,11 @@ void FlightTaskAuto::_prepareLandSetpoints()
 				_velocity_setpoint_feedback.xy(), _deltatime);
 		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
 
+		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
+		if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
+			_yaw_sp_aligned = true;
+		}
+
 	} else {
 		// Make sure we have a valid land position even in the case we loose RC while amending it
 		if (!PX4_ISFINITE(_land_position(0))) {

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -109,8 +109,6 @@ protected:
 	bool isTargetModified() const;
 	void _updateTrajConstraints();
 
-	void rcHelpModifyYaw(float &yaw_sp);
-
 	/** determines when to trigger a takeoff (ignored in flight) */
 	bool _checkTakeoff() override { return _want_takeoff; };
 

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -106,14 +106,8 @@ void FwAutotuneAttitudeControl::Run()
 		return;
 	}
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			_nav_state = vehicle_status.nav_state;
-		}
-	}
+	_vehicle_status_armed_state_sub.update();
+	_vehicle_status_nav_state_sub.update();
 
 	if (_actuator_controls_status_sub.updated()) {
 		actuator_controls_status_s controls_status;
@@ -296,7 +290,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 			mavlink_log_info(&_mavlink_log_pub, "Autotune started");
 			_state = state::init;
 			_state_start_time = now;
-			_start_flight_mode = _nav_state;
+			_start_flight_mode = _vehicle_status_nav_state_sub.get();
 		}
 
 		break;
@@ -429,7 +423,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::wait_for_disarm:
-		if (!_armed) {
+		if (!(_vehicle_status_armed_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 			saveGainsToParams();
 			_state = state::complete;
 			_state_start_time = now;
@@ -481,7 +475,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 	if (_state != state::wait_for_disarm && _state != state::idle && _state != state::fail && _state != state::complete) {
 		if (now - _state_start_time > 20_s
 		    || (_param_fw_at_man_aux.get() && !_aux_switch_en)
-		    || _start_flight_mode != _nav_state) {
+		    || _start_flight_mode != _vehicle_status_nav_state_sub.get()) {
 			orb_advert_t mavlink_log_pub = nullptr;
 			mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing");
 			_state = state::fail;

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp

@@ -114,7 +114,8 @@ private:
 	uORB::Subscription _actuator_controls_status_sub;
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_armed_state_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::nav_state> _vehicle_status_nav_state_sub{ORB_ID(vehicle_status)};
 
 	uORB::PublicationData<autotune_attitude_control_status_s> _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)};
 
@@ -142,8 +143,6 @@ private:
 	uint8_t _max_steps{5};
 	int8_t _signal_sign{0};
 
-	bool _armed{false};
-	uint8_t _nav_state{0};
 	uint8_t _start_flight_mode{0};
 	bool _aux_switch_en{false};
 

src/modules/fw_path_navigation/FixedwingPositionControl.cpp

@@ -468,45 +468,43 @@ void
 FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
 		float true_airspeed_derivative_raw, float throttle_trim)
 {
-	tecs_status_s tecs_status{};
+	tecs_status_s t{};
 
 	const TECS::DebugOutput &debug_output{_tecs.getStatus()};
 
 	switch (_tecs.tecs_mode()) {
 	case TECS::ECL_TECS_MODE_NORMAL:
-		tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
+		t.mode = tecs_status_s::TECS_MODE_NORMAL;
 		break;
 
 	case TECS::ECL_TECS_MODE_UNDERSPEED:
-		tecs_status.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+		t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
 		break;
 	}
 
-	tecs_status.altitude_sp = alt_sp;
-	tecs_status.altitude_reference = debug_output.altitude_reference;
-	tecs_status.height_rate_reference = debug_output.height_rate_reference;
-	tecs_status.height_rate_direct = debug_output.height_rate_direct;
-	tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
-	tecs_status.height_rate = -_local_pos.vz;
-	tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp;
-	tecs_status.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp;
-	tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered;
-	tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
-	tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative;
-	tecs_status.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
-	tecs_status.total_energy_rate = debug_output.control.total_energy_rate_estimate;
-	tecs_status.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
-	tecs_status.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
-	tecs_status.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
-	tecs_status.throttle_integ = debug_output.control.throttle_integrator;
-	tecs_status.pitch_integ = debug_output.control.pitch_integrator;
-	tecs_status.throttle_sp = _tecs.get_throttle_setpoint();
-	tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint();
-	tecs_status.throttle_trim = throttle_trim;
+	t.altitude_sp = alt_sp;
+	t.altitude_sp_filtered = debug_output.altitude_sp_ref;
+	t.height_rate_setpoint = debug_output.control.altitude_rate_control;
+	t.height_rate = -_local_pos.vz;
+	t.equivalent_airspeed_sp = equivalent_airspeed_sp;
+	t.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp;
+	t.true_airspeed_filtered = debug_output.true_airspeed_filtered;
+	t.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
+	t.true_airspeed_derivative = debug_output.true_airspeed_derivative;
+	t.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
+	t.total_energy_rate = debug_output.control.total_energy_rate_estimate;
+	t.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
+	t.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
+	t.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
+	t.throttle_integ = debug_output.control.throttle_integrator;
+	t.pitch_integ = debug_output.control.pitch_integrator;
+	t.throttle_sp = _tecs.get_throttle_setpoint();
+	t.pitch_sp_rad = _tecs.get_pitch_setpoint();
+	t.throttle_trim = throttle_trim;
 
-	tecs_status.timestamp = hrt_absolute_time();
+	t.timestamp = hrt_absolute_time();
 
-	_tecs_status_pub.publish(tecs_status);
+	_tecs_status_pub.publish(t);
 }
 
 void
@@ -2460,25 +2458,9 @@ FixedwingPositionControl::reset_landing_state()
 	}
 }
 
-float FixedwingPositionControl::calculateTrimThrottle(float throttle_min,
-		float throttle_max, float airspeed_sp)
+float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min,
+		float throttle_max)
 {
-	float throttle_trim =
-		_param_fw_thr_trim.get(); // throttle required for level flight at trim airspeed, at sea level (standard atmosphere)
-
-	// Drag modelling (parasite drag): calculate mapping airspeed-->throttle, assuming a linear relation with different gradient
-	// above and below trim. This is tunable thorugh FW_THR_ASPD_MIN and FW_THR_ASPD_MAX.
-	if (PX4_ISFINITE(airspeed_sp) && _param_fw_thr_aspd_min.get() > FLT_EPSILON
-	    && airspeed_sp < _param_fw_airspd_trim.get()) {
-		throttle_trim = _param_fw_thr_trim.get() - (_param_fw_thr_trim.get() - _param_fw_thr_aspd_min.get()) /
-				(_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * (_param_fw_airspd_trim.get() - airspeed_sp);
-
-	} else if (PX4_ISFINITE(airspeed_sp) && _param_fw_thr_aspd_max.get() > FLT_EPSILON
-		   && airspeed_sp > _param_fw_airspd_trim.get()) {
-		throttle_trim = _param_fw_thr_trim.get() + (_param_fw_thr_aspd_max.get() - _param_fw_thr_trim.get()) /
-				(_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) * (airspeed_sp - _param_fw_airspd_trim.get());
-	}
-
 	float weight_ratio = 1.0f;
 
 	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
@@ -2568,8 +2550,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	/* update TECS vehicle state estimates */
-	const float throttle_trim_adjusted = calculateTrimThrottle(throttle_min,
-					     throttle_max, airspeed_sp);
+	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
+					 throttle_max);
 
 	// HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases
 	// when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0.
@@ -2583,8 +2565,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     _eas2tas,
 		     throttle_min,
 		     throttle_max,
-		     _param_fw_thr_trim.get(),
-		     throttle_trim_adjusted,
+		     throttle_trim_comp,
 		     pitch_min_rad - radians(_param_fw_psp_off.get()),
 		     pitch_max_rad - radians(_param_fw_psp_off.get()),
 		     desired_max_climbrate,
@@ -2593,7 +2574,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     -_local_pos.vz,
 		     hgt_rate_sp);
 
-	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_adjusted);
+	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_comp);
 }
 
 float

src/modules/fw_path_navigation/FixedwingPositionControl.hpp

@@ -689,15 +689,14 @@ private:
 	void set_control_mode_current(const hrt_abstime &now);
 
 	/**
-	 * @brief Estimate trim throttle for air density, vehicle weight and current airspeed
+	 * @brief Compensate trim throttle for air density and vehicle weight.
 	 *
+	 * @param trim throttle required at sea level during standard conditions.
 	 * @param throttle_min Minimum allowed trim throttle.
 	 * @param throttle_max Maximum allowed trim throttle.
-	 * @param airspeed_sp Current airspeed setpoint (CAS) [m/s]
-	 * @return Estimated trim throttle
+	 * @return Trim throttle compensated for air density and vehicle weight.
 	 */
-	float calculateTrimThrottle(float throttle_min, float throttle_max,
-				    float airspeed_sp);
+	float compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, float throttle_max);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
 
@@ -907,9 +906,6 @@ private:
 		(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
 		(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
 
-		(ParamFloat<px4::params::FW_THR_ASPD_MIN>) _param_fw_thr_aspd_min,
-		(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _param_fw_thr_aspd_max,
-
 		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
 		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,

src/modules/fw_path_navigation/fw_path_navigation_params.c

@@ -1063,33 +1063,3 @@ PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
-
-/**
- * Throttle at min airspeed
- *
- * Required throttle for level flight at minimum airspeed FW_AIRSPD_MIN (sea level, standard atmosphere)
- *
- * Set to 0 to disable mapping of airspeed to trim throttle below FW_AIRSPD_TRIM.
- *
- * @min 0
- * @max 1
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_THR_ASPD_MIN, 0.f);
-
-/**
- * Throttle at max airspeed
- *
- * Required throttle for level flight at maximum airspeed FW_AIRSPD_MAX (sea level, standard atmosphere)
- *
- * Set to 0 to disable mapping of airspeed to trim throttle.
- *
- * @min 0
- * @max 1
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_THR_ASPD_MAX, 0.f);

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp

@@ -96,13 +96,7 @@ void McAutotuneAttitudeControl::Run()
 		return;
 	}
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-		}
-	}
+	_vehicle_status_arming_state_sub.update();
 
 	if (_actuator_controls_status_sub.updated()) {
 		actuator_controls_status_s controls_status;
@@ -399,7 +393,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::wait_for_disarm:
-		if (!_armed) {
+		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 			saveGainsToParams();
 			_state = state::complete;
 			_state_start_time = now;

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp

@@ -108,7 +108,7 @@ private:
 	uORB::Subscription _actuator_controls_status_sub{ORB_ID(actuator_controls_status_0)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
 
 	uORB::PublicationData<autotune_attitude_control_status_s> _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)};
 
@@ -136,8 +136,6 @@ private:
 	uint8_t _max_steps{5};
 	int8_t _signal_sign{0};
 
-	bool _armed{false};
-
 	matrix::Vector3f _kid{};
 	matrix::Vector3f _rate_k{};
 	matrix::Vector3f _rate_i{};

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -151,18 +151,13 @@ void MulticopterHoverThrustEstimator::Run()
 
 	perf_begin(_cycle_perf);
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-		}
-	}
+	_vehicle_status_arming_state_sub.update();
 
 	const float dt = (local_pos.timestamp - _timestamp_last) * 1e-6f;
 	_timestamp_last = local_pos.timestamp;
 
-	if (_armed && _in_air && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
+	if ((_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) && _in_air && (dt > 0.001f)
+	    && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
 
 		_hover_thrust_ekf.predict(dt);
 
@@ -198,7 +193,7 @@ void MulticopterHoverThrustEstimator::Run()
 	} else {
 		_valid_hysteresis.set_state_and_update(false, hrt_absolute_time());
 
-		if (!_armed) {
+		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 			reset();
 		}
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -102,12 +102,11 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 
 	hrt_abstime _timestamp_last{0};
 
-	bool _armed{false};
 	bool _landed{false};
 	bool _in_air{false};
 

src/modules/microdds_client/microdds_client.cpp

@@ -79,7 +79,7 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta
 	}
 }
 
-MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip,
+MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host,
 			       const char *port, bool localhost_only, bool custom_participant, const char *client_namespace) :
 	ModuleParams(nullptr),
 	_localhost_only(localhost_only), _custom_participant(custom_participant),
@@ -122,11 +122,9 @@ MicroddsClient::MicroddsClient(Transport transport, const char *device, int baud
 
 #if defined(MICRODDS_CLIENT_UDP)
 		_transport_udp = new uxrUDPTransport();
-		strncpy(_port, port, PORT_MAX_LENGTH - 1);
-		strncpy(_agent_ip, agent_ip, AGENT_IP_MAX_LENGTH - 1);
 
 		if (_transport_udp) {
-			if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, _agent_ip, _port)) {
+			if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, host, port)) {
 				_comm = &_transport_udp->comm;
 				_fd = _transport_udp->platform.poll_fd.fd;
 
@@ -267,7 +265,7 @@ void MicroddsClient::run()
 				   "</dds>"
 				  );
 
-		if (ret < 0 || ret >= PARTICIPANT_XML_SIZE) {
+		if (ret < 0 || ret >= TOPIC_NAME_SIZE) {
 			PX4_ERR("create entities failed: namespace too long");
 			return;
 		}
@@ -548,26 +546,8 @@ int MicroddsClient::task_spawn(int argc, char *argv[])
 int MicroddsClient::print_status()
 {
 	PX4_INFO("Running, %s", _connected ? "connected" : "disconnected");
-#if defined(MICRODDS_CLIENT_UDP)
-
-	if (_transport_udp != nullptr) {
-		PX4_INFO("Using transport: udp");
-		PX4_INFO("Agent IP: %s", _agent_ip);
-		PX4_INFO("Agent port: %s", _port);
-
-	}
-
-#endif
-
-	if (_transport_serial != nullptr) {
-		PX4_INFO("Using transport: serial");
-	}
-
-	if (_connected) {
-		PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
-		PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
-	}
-
+	PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
+	PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
 	return 0;
 }
 
@@ -578,19 +558,19 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 	int ch;
 	const char *myoptarg = nullptr;
 
-	char port[PORT_MAX_LENGTH] = {0};
-	char agent_ip[AGENT_IP_MAX_LENGTH] = {0};
-
 #if defined(MICRODDS_CLIENT_UDP)
 	Transport transport = Transport::Udp;
+
 #else
 	Transport transport = Transport::Serial;
 #endif
 	const char *device = nullptr;
 	int baudrate = 921600;
 
+	const char *port = "8888";
 	bool localhost_only = false;
 	bool custom_participant = false;
+	const char *ip = "127.0.0.1";
 
 	const char *client_namespace = nullptr;//"px4";
 
@@ -625,11 +605,11 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 #if defined(MICRODDS_CLIENT_UDP)
 
 		case 'h':
-			snprintf(agent_ip, AGENT_IP_MAX_LENGTH, "%s", myoptarg);
+			ip = myoptarg;
 			break;
 
 		case 'p':
-			snprintf(port, PORT_MAX_LENGTH, "%s", myoptarg);
+			port = myoptarg;
 			break;
 
 		case 'l':
@@ -656,33 +636,6 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 		}
 	}
 
-#if defined(MICRODDS_CLIENT_UDP)
-
-	if (port[0] == '\0') {
-		// no port specified, use XRCE_DDS_PRT
-		int32_t port_i = 0;
-		param_get(param_find("XRCE_DDS_PRT"), &port_i);
-
-		if (port_i < 0 || port_i > 65535) {
-			PX4_ERR("port must be between 0 and 65535");
-			return nullptr;
-		}
-
-		snprintf(port, PORT_MAX_LENGTH, "%u", (uint16_t)port_i);
-	}
-
-	if (agent_ip[0] == '\0') {
-		// no agent ip specified, use XRCE_DDS_AG_IP
-		int32_t ip_i = 0;
-		param_get(param_find("XRCE_DDS_AG_IP"), &ip_i);
-		snprintf(agent_ip, AGENT_IP_MAX_LENGTH, "%u.%u.%u.%u", static_cast<uint8_t>(((ip_i) >> 24) & 0xff),
-			 static_cast<uint8_t>(((ip_i) >> 16) & 0xff),
-			 static_cast<uint8_t>(((ip_i) >> 8) & 0xff),
-			 static_cast<uint8_t>(ip_i & 0xff));
-	}
-
-#endif // MICRODDS_CLIENT_UDP
-
 	if (error_flag) {
 		return nullptr;
 	}
@@ -694,8 +647,7 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 		}
 	}
 
-	return new MicroddsClient(transport, device, baudrate, agent_ip, port, localhost_only, custom_participant,
-				  client_namespace);
+	return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only, custom_participant, client_namespace);
 }
 
 int MicroddsClient::print_usage(const char *reason)
@@ -719,8 +671,8 @@ $ microdds_client start -t udp -h 127.0.0.1 -p 15555
 	PRINT_MODULE_USAGE_PARAM_STRING('t', "udp", "serial|udp", "Transport protocol", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "serial device", true);
 	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:<param_name>)", true);
-	PRINT_MODULE_USAGE_PARAM_STRING('h', nullptr, "<IP>", "Agent IP. If not provided, defaults to XRCE_DDS_AG_IP", true);
-	PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 65535, "Agent listening port. If not provided, defaults to XRCE_DDS_PRT", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "<IP>", "Host IP", true);
+	PRINT_MODULE_USAGE_PARAM_INT('p', 8888, 0, 65535, "Remote Port", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('c', "Use custom participant config (profile_name=\"px4_participant\")", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true);

src/modules/microdds_client/microdds_client.h

@@ -78,17 +78,6 @@ private:
 	const bool _custom_participant;
 	const char *_client_namespace;
 
-
-	// max port characters (5+'\0')
-	static const uint8_t PORT_MAX_LENGTH = 6;
-	// max agent ip characters (15+'\0')
-	static const uint8_t AGENT_IP_MAX_LENGTH = 16;
-
-#if defined(CONFIG_NET) || defined(__PX4_POSIX)
-	char _port[PORT_MAX_LENGTH];
-	char _agent_ip[AGENT_IP_MAX_LENGTH];
-#endif
-
 	SendTopicsSubs *_subs{nullptr};
 	RcvTopicsPubs *_pubs{nullptr};
 

src/modules/microdds_client/module.yaml

@@ -1,3 +1,16 @@
+
+
+# parameters to auto start
+#  mode (normal, minimal)
+#  UDP port
+#  max rate
+#  DDS DOMAIN ID
+#
+
+
+# multiple instances?
+
+
 module_name: Micro XRCE-DDS
 serial_config:
     - command: |
@@ -9,8 +22,13 @@ serial_config:
         microdds_client start ${XRCE_DDS_ARGS}
 
       port_config_param:
-        name: XRCE_DDS_CFG
+        name: XRCE_DDS_${i}_CFG
         group: Micro XRCE-DDS
+        # MAVLink instances:
+        # 0: Telem1 Port (Telemetry Link)
+        # 1: Telem2 Port (Companion Link). Disabled by default to reduce RAM usage
+        # 2: Board-specific / no fixed function or port
+        #default: [TEL1, "", ""]
       supports_networking: true
 
 parameters:
@@ -28,7 +46,7 @@ parameters:
 
         XRCE_DDS_KEY:
             description:
-                short: XRCE DDS Session key
+                short: XRCE DDS key
                 long: |
                     XRCE DDS key, must be different from zero.
                     In a single agent - multi client configuration, each client
@@ -38,29 +56,13 @@ parameters:
             reboot_required: true
             default: 1
 
-        XRCE_DDS_PRT:
+        XRCE_DDS_UDP_PRT:
             description:
                 short: Micro DDS UDP Port
                 long: |
                     If ethernet enabled and selected as configuration for micro DDS,
                     selected udp port will be set and used.
             type: int32
-            min: 0
-            max: 65535
             reboot_required: true
             default: 8888
             requires_ethernet: true
-
-        XRCE_DDS_AG_IP:
-            description:
-                short: Micro DDS Agent IP address
-                long: |
-                    If ethernet enabled and selected as configuration for micro DDS,
-                    selected Agent IP address will be set and used.
-                    Decimal dot notation is not supported. IP address must be provided
-                    in int32 format. For example, 192.168.1.2 is mapped to -1062731518;
-                    127.0.0.1 is mapped to 2130706433.
-            type: int32
-            reboot_required: true
-            default: 2130706433
-            requires_ethernet: true

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -50,7 +50,6 @@ void FeasibilityChecker::reset()
 	_is_landed = false;
 	_home_alt_msl = NAN;
 	_home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
-	_current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	_vehicle_type = VehicleType::RotaryWing;
 
 	_mission_validity_failed = false;
@@ -120,12 +119,6 @@ void FeasibilityChecker::updateData()
 		_is_landed = land_detected.landed;
 	}
 
-	if (_vehicle_global_position_sub.updated()) {
-		vehicle_global_position_s vehicle_global_position = {};
-		_vehicle_global_position_sub.copy(&vehicle_global_position);
-		_current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
-	}
-
 	param_t handle = param_find("FW_LND_ANG");
 
 	if (handle != PARAM_INVALID) {
@@ -138,6 +131,12 @@ void FeasibilityChecker::updateData()
 		param_get(handle, &_param_mis_dist_1wp);
 	}
 
+	handle = param_find("MIS_DIST_WPS");
+
+	if (handle != PARAM_INVALID) {
+		param_get(handle, &_param_mis_dist_wps);
+	}
+
 	handle = param_find("NAV_ACC_RAD");
 
 	if (handle != PARAM_INVALID) {
@@ -196,7 +195,7 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int
 	}
 
 	if (!_distance_first_waypoint_failed) {
-		_distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item);
+		_distance_first_waypoint_failed = !checkDistanceToFirstWaypoint(mission_item);
 	}
 
 	if (!_below_home_alt_failed) {
@@ -602,44 +601,50 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
 }
 
 
-bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
+bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item)
 {
-	if (_param_mis_dist_1wp > FLT_EPSILON &&
-	    (_current_position_lat_lon.isAllFinite()) && !_first_waypoint_found &&
-	    MissionBlock::item_contains_position(mission_item)) {
+	if (_param_mis_dist_1wp <= 0.0f || !_home_lat_lon.isAllFinite()) {
+		/* param not set, check is ok */
+		return true;
+	}
 
+	if (!_first_waypoint_found && MissionBlock::item_contains_position(mission_item)) {
 		_first_waypoint_found = true;
 
-		float dist_to_1wp_from_current_pos = 1e6f;
 
-		if (_current_position_lat_lon.isAllFinite()) {
-			dist_to_1wp_from_current_pos = get_distance_to_next_waypoint(
-							       mission_item.lat, mission_item.lon,
-							       _current_position_lat_lon(0), _current_position_lat_lon(1));
-		}
+		/* check distance from current position to item */
+		float dist_to_1wp = get_distance_to_next_waypoint(
+					    mission_item.lat, mission_item.lon,
+					    _home_lat_lon(0), _home_lat_lon(1));
 
-		if (dist_to_1wp_from_current_pos < _param_mis_dist_1wp) {
+		if (dist_to_1wp < _param_mis_dist_1wp) {
 
 			return true;
 
 		} else {
-			/* item is too far from current position */
+			/* item is too far from home */
 			mavlink_log_critical(_mavlink_log_pub,
 					     "First waypoint too far away: %dm, %d max\t",
-					     (int)dist_to_1wp_from_current_pos, (int)_param_mis_dist_1wp);
+					     (int)dist_to_1wp, (int)_param_mis_dist_1wp);
 			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info},
-							 "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp_from_current_pos,
-							 (uint32_t)_param_mis_dist_1wp);
+							 "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)_param_mis_dist_1wp);
 
 			return false;
 		}
 	}
 
+	/* no waypoints found in mission, then we will not fly far away */
 	return true;
 }
 
 bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mission_item)
 {
+	if (_param_mis_dist_wps <= 0.0f) {
+		/* param not set, check is ok */
+		return true;
+	}
+
+
 	/* check only items with valid lat/lon */
 	if (!MissionBlock::item_contains_position(mission_item)) {
 		return true;
@@ -653,8 +658,24 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi
 				_last_lat, _last_lon);
 
 
-		if (dist_between_waypoints < 0.05f &&
-		    (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || _last_cmd == NAV_CMD_CONDITION_GATE)) {
+		if (dist_between_waypoints > _param_mis_dist_wps) {
+			/* distance between waypoints is too high */
+			mavlink_log_critical(_mavlink_log_pub,
+					     "Distance between waypoints too far: %d meters, %d max.\t",
+					     (int)dist_between_waypoints, (int)_param_mis_dist_wps);
+			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info},
+							 "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints,
+							 (uint32_t)_param_mis_dist_wps);
+
+
+			return false;
+
+			/* do not allow waypoints that are literally on top of each other */
+
+			/* and do not allow condition gates that are at the same position as a navigation waypoint */
+
+		} else if (dist_between_waypoints < 0.05f &&
+			   (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || _last_cmd == NAV_CMD_CONDITION_GATE)) {
 
 			/* Waypoints and gate are at the exact same position, which indicates an
 			 * invalid mission and makes calculating the direction from one waypoint

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,6 @@
 #include <mathlib/mathlib.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/Subscription.hpp>
 #include <px4_platform_common/module_params.h>
@@ -97,18 +96,17 @@ private:
 	uORB::Subscription _home_pos_sub{ORB_ID(home_position)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 
 	// parameters
 	float _param_fw_lnd_ang{0.f};
 	float _param_mis_dist_1wp{0.f};
+	float _param_mis_dist_wps{0.f};
 	float _param_nav_acc_rad{0.f};
 	int32_t _param_mis_takeoff_land_req{0};
 
 	bool _is_landed{false};
 	float _home_alt_msl{NAN};
 	matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
-	matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	VehicleType _vehicle_type{VehicleType::RotaryWing};
 
 	// internal flags to keep track of which checks failed
@@ -132,7 +130,7 @@ private:
 	int _landing_approach_index{-1};
 	mission_item_s _mission_item_previous = {};
 
-	// internal checkHorizontalDistanceToFirstWaypoint variables
+	// internal checkDistanceToFirstWaypoint variables
 	bool _first_waypoint_found{false};
 
 	// internal checkDistancesBetweenWaypoints variables
@@ -174,12 +172,12 @@ private:
 	bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index);
 
 	/**
-	 * @brief Check distance to first waypoint from current vehicle position (if available).
+	 * @brief Check distance to first waypoint.
 	 *
 	 * @param mission_item The current mission item
 	 * @return False if the check failed.
 	*/
-	bool checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item);
+	bool checkDistanceToFirstWaypoint(mission_item_s &mission_item);
 
 	/**
 	 * @brief Check distances between waypoints

src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,6 +33,8 @@
 
 #include <gtest/gtest.h>
 #include "FeasibilityChecker.hpp"
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <lib/geo/geo.h>
 
 
@@ -65,15 +67,6 @@ public:
 		orb_publish(ORB_ID(home_position), home_pub, &home);
 	}
 
-	void publishCurrentPosition(double lat, double lon)
-	{
-		vehicle_global_position_s gpos = {};
-		gpos.lat = lat;
-		gpos.lon = lon;
-		orb_advert_t gpos_pub = orb_advertise(ORB_ID(vehicle_global_position), &gpos);
-		orb_publish(ORB_ID(vehicle_global_position), gpos_pub, &gpos);
-	}
-
 	void publishLanded(bool landed)
 	{
 		vehicle_land_detected_s land_detected = {};
@@ -130,47 +123,73 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity)
 	ASSERT_EQ(ret, false);
 }
 
-TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
+TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints)
 {
-	// GIVEN: MIS_DIST_1WP set to 500m
 	TestFeasibilityChecker checker;
 	checker.publishLanded(true);
+
+	param_t param = param_handle(px4::params::MIS_DIST_WPS);
+
+	float max_dist = 1000.0f;
+	param_set(param, &max_dist);
+	checker.paramsChanged();
+
+	mission_item_s mission_item = {};
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+	checker.processNextItem(mission_item, 0, 3);
+
+	// waypoints are within limits, check should pass
+	double lat_new, lon_new;
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 999, &lat_new, &lon_new);
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	checker.processNextItem(mission_item, 1, 3);
+
+	ASSERT_EQ(checker.someCheckFailed(), false);
+
+	// waypoints are above limits, check should fail
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1001, &lat_new, &lon_new);
+
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	checker.processNextItem(mission_item, 2, 3);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+}
+
+
+TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
+{
+	TestFeasibilityChecker checker;
+	checker.publishLanded(true);
+	checker.publishHomePosition(0, 0, 0);
+
 	param_t param = param_handle(px4::params::MIS_DIST_1WP);
+
 	float max_dist = 500.0f;
 	param_set(param, &max_dist);
 	checker.paramsChanged();
+
+
 	mission_item_s mission_item = {};
 	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 	double lat_new, lon_new;
-
-	// GIVEN: no valid Current position
-
-	// THEN: always pass
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), false);
-
-	// BUT WHEN: valid current position, first WP 501m away from current pos
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishCurrentPosition(0, 0);
-	waypoint_from_heading_and_distance(0, 0, 0, 501, &lat_new, &lon_new);
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 501, &lat_new, &lon_new);
 	mission_item.lat = lat_new;
 	mission_item.lon = lon_new;
-
-	// THEN: fail
 	checker.processNextItem(mission_item, 0, 1);
+
 	ASSERT_EQ(checker.someCheckFailed(), true);
 
-	// BUT WHEN: valid current position fist WP 499m away from current
 	checker.reset();
 	checker.publishLanded(true);
-	checker.publishCurrentPosition(0, 0);
+	checker.publishHomePosition(0, 0, 0);
 	waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new);
 	mission_item.lat = lat_new;
 	mission_item.lon = lon_new;
-
-	// THEN: pass
 	checker.processNextItem(mission_item, 0, 1);
+
 	ASSERT_EQ(checker.someCheckFailed(), false);
 }
 

src/modules/navigator/mission.cpp

@@ -96,6 +96,12 @@ void Mission::mission_init()
 void
 Mission::on_inactive()
 {
+	// if we were executing an landing but have been inactive for 2 seconds, then make the landing invalid
+	// this prevents RTL to just continue at the current mission index
+	if (_navigator->getMissionLandingInProgress() && (hrt_absolute_time() - _time_mission_deactivated) > 2_s) {
+		_navigator->setMissionLandingInProgress(false);
+	}
+
 	/* Without home a mission can't be valid yet anyway, let's wait. */
 	if (!_navigator->home_global_position_valid()) {
 		return;
@@ -174,6 +180,8 @@ Mission::on_inactivation()
 		_navigator->get_precland()->on_inactivation();
 	}
 
+	_time_mission_deactivated = hrt_absolute_time();
+
 	/* reset so current mission item gets restarted if mission was paused */
 	_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 }
@@ -468,12 +476,15 @@ Mission::land_start()
 {
 	// if not currently landing, jump to do_land_start
 	if (_land_start_available) {
-		if (landing()) {
+		if (_navigator->getMissionLandingInProgress()) {
 			return true;
 
 		} else {
 			set_current_mission_index(get_land_start_index());
-			return landing();
+
+			const bool can_land_now = landing();
+			_navigator->setMissionLandingInProgress(can_land_now);
+			return can_land_now;
 		}
 	}
 
@@ -484,24 +495,10 @@ bool
 Mission::landing()
 {
 	// vehicle is currently landing if
-	//  mission valid, still flying, and in the landing portion of mission (past land start marker)
+	//  mission valid, still flying, and in the landing portion of mission
 
 	const bool mission_valid = _navigator->get_mission_result()->valid;
-	bool on_landing_stage = _land_start_available && _current_mission_index > get_land_start_index();
-
-	// special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the
-	// distance to the WP is below the loiter radius + acceptance.
-	if (_current_mission_index == get_land_start_index() && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
-		const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-					_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-
-		// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
-		const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
-				&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
-				_navigator->get_loiter_radius();
-
-		on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs);
-	}
+	const bool on_landing_stage = _land_start_available && (_current_mission_index >= get_land_start_index());
 
 	return mission_valid && on_landing_stage;
 }
@@ -1721,6 +1718,12 @@ Mission::set_mission_item_reached()
 {
 	_navigator->get_mission_result()->seq_reached = _current_mission_index;
 	_navigator->set_mission_result_updated();
+
+	// let the navigator know that we are currently executing the mission landing.
+	// Using the method landing() itself is not accurate as it only give information about the mission index
+	// but the vehicle could still be very far from the actual landing items
+	_navigator->setMissionLandingInProgress(landing());
+
 	reset_mission_item_reached();
 }
 

src/modules/navigator/mission.h

@@ -242,6 +242,7 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
+		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
 		(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
 	)
 
@@ -267,6 +268,8 @@ private:
 
 	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 
+	hrt_abstime _time_mission_deactivated{0};
+
 	enum {
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_MISSION

src/modules/navigator/mission_params.c

@@ -73,14 +73,14 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
 PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
 
 /**
- * Maximal horizontal distance from current position to first waypoint
+ * Maximal horizontal distance from home to first waypoint
  *
  * Failsafe check to prevent running mission stored from previous flight at a new takeoff location.
  * Set a value of zero or less to disable. The mission will not be started if the current
- * waypoint is more distant than MIS_DIST_1WP from the current position.
+ * waypoint is more distant than MIS_DIST_1WP from the home position.
  *
  * @unit m
- * @min -1
+ * @min 0
  * @max 10000
  * @decimal 1
  * @increment 100
@@ -88,6 +88,22 @@ PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
  */
 PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
 
+/**
+ * Maximal horizontal distance between waypoint
+ *
+ * Failsafe check to prevent running missions which are way too big.
+ * Set a value of zero or less to disable. The mission will not be started if any distance between
+ * two subsequent waypoints is greater than MIS_DIST_WPS.
+ *
+ * @unit m
+ * @min 0
+ * @max 10000
+ * @decimal 1
+ * @increment 100
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_DIST_WPS, 900);
+
 /**
 * Enable yaw control of the mount. (Only affects multicopters and ROI mission items)
 *

src/modules/navigator/navigator.h

@@ -259,6 +259,10 @@ public:
 
 	void set_mission_failure_heading_timeout();
 
+	void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; }
+
+	bool getMissionLandingInProgress() { return _mission_landing_in_progress; }
+
 	bool is_planned_mission() const { return _navigation_mode == &_mission; }
 
 	bool on_mission_landing() { return _mission.landing(); }
@@ -344,7 +348,7 @@ private:
 	vehicle_local_position_s			_local_pos{};		/**< local vehicle position */
 	vehicle_status_s				_vstatus{};		/**< vehicle status */
 
-	bool						_rtl_activated{false};
+	uint8_t						_previous_nav_state{}; /**< nav_state of the previous iteration*/
 
 	// Publications
 	geofence_result_s				_geofence_result{};
@@ -389,6 +393,9 @@ private:
 	float _mission_cruising_speed_fw{-1.0f};
 	float _mission_throttle{NAN};
 
+	bool _mission_landing_in_progress{false};	/**< this flag gets set if the mission is currently executing on a landing pattern
+							 * if mission mode is inactive, this flag will be cleared after 2 seconds */
+
 	traffic_buffer_s _traffic_buffer{};
 
 	bool _is_capturing_images{false}; // keep track if we need to stop capturing images

src/modules/navigator/navigator_main.cpp

@@ -664,22 +664,24 @@ void Navigator::run()
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
 				_pos_sp_triplet_published_invalid_once = false;
-				const bool rtl_activated_now = !_rtl_activated;
+
+				const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
 				switch (_rtl.get_rtl_type()) {
 				case RTL::RTL_TYPE_MISSION_LANDING:
 				case RTL::RTL_TYPE_CLOSEST:
-					if (on_mission_landing() && _rtl.getShouldEngageMissionForLanding()) {
+
+					if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_LOITER
+					    && _rtl.getShouldEngageMissionForLanding()) {
 						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
 
-						navigation_mode_new = &_mission;
-
-						if (rtl_activated_now) {
-							mavlink_log_info(get_mavlink_log_pub(), "RTL to Mission landing, continue landing\t");
-							events::send(events::ID("rtl_land_at_mission_continue_landing"), events::Log::Info,
-								     "RTL to Mission landing, continue landing");
+						if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED
+						    && !get_land_detected()->landed) {
+							start_mission_landing();
 						}
 
+						navigation_mode_new = &_mission;
+
 					} else {
 						navigation_mode_new = &_rtl;
 					}
@@ -704,7 +706,7 @@ void Navigator::run()
 							}
 						}
 
-						if (rtl_activated_now) {
+						if (rtl_activated) {
 							mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t");
 							events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info,
 								     "RTL Mission activated, continue mission");
@@ -724,11 +726,11 @@ void Navigator::run()
 							// The seconds condition is required so that when no mission was uploaded and one is available the closest
 							// mission item is determined and also that if the user changes the active mission index while rtl is active
 							// always that waypoint is tracked first.
-							if ((_navigation_mode != &_mission) && (rtl_activated_now || _mission.get_mission_waypoints_changed())) {
+							if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) {
 								_mission.set_closest_item_as_current();
 							}
 
-							if (rtl_activated_now) {
+							if (rtl_activated) {
 								mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t");
 								events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info,
 									     "RTL Mission activated, fly mission in reverse");
@@ -737,7 +739,7 @@ void Navigator::run()
 							navigation_mode_new = &_mission;
 
 						} else {
-							if (rtl_activated_now) {
+							if (rtl_activated) {
 								mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t");
 								events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info,
 									     "RTL Mission activated, fly to home");
@@ -750,7 +752,7 @@ void Navigator::run()
 					break;
 
 				default:
-					if (rtl_activated_now) {
+					if (rtl_activated) {
 						mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t");
 						events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated");
 					}
@@ -760,7 +762,6 @@ void Navigator::run()
 
 				}
 
-				_rtl_activated = true;
 				break;
 			}
 
@@ -799,15 +800,14 @@ void Navigator::run()
 			break;
 		}
 
-		if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
-			_rtl_activated = false;
-		}
-
 		// Do not execute any state machine while we are disarmed
 		if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 			navigation_mode_new = nullptr;
 		}
 
+		// update the vehicle status
+		_previous_nav_state = _vstatus.nav_state;
+
 		/* we have a new navigation mode: reset triplet */
 		if (_navigation_mode != navigation_mode_new) {
 			// We don't reset the triplet in the following two cases:

src/modules/navigator/rtl.cpp

@@ -152,7 +152,7 @@ void RTL::find_RTL_destination()
 		double dist_squared = coord_dist_sq(dlat, dlon);
 
 		// always find closest destination if in hover and VTOL
-		if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->on_mission_landing())) {
+		if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->getMissionLandingInProgress())) {
 
 			// compare home position to landing position to decide which is closer
 			if (dist_squared < min_dist_squared) {
@@ -282,6 +282,12 @@ void RTL::on_activation()
 		// For safety reasons don't go into RTL if landed.
 		_rtl_state = RTL_STATE_LANDED;
 
+	} else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) {
+		// we were just on a mission landing, set _rtl_state past RTL_STATE_LOITER such that navigator will engage mission mode,
+		// which will continue executing the landing
+		_rtl_state = RTL_STATE_LAND;
+
+
 	} else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
 
 		// If lower than return altitude, climb up first.

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -181,13 +181,7 @@ void VehicleIMU::Run()
 	ScheduleDelayed(_backup_schedule_timeout_us);
 
 	// check vehicle status for changes to armed state
-	if (_vehicle_control_mode_sub.updated()) {
-		vehicle_control_mode_s vehicle_control_mode;
-
-		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
-			_armed = vehicle_control_mode.flag_armed;
-		}
-	}
+	_vehicle_control_mode_armed_sub.update();
 
 	// reset data gap monitor
 	_data_gap = false;
@@ -254,7 +248,7 @@ void VehicleIMU::Run()
 	}
 
 	if (_param_sens_imu_autocal.get() && !parameters_updated) {
-		if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
+		if ((_vehicle_control_mode_armed_sub.get() || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
 		    && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) {
 
 			SensorCalibrationUpdate();

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -109,7 +109,8 @@ private:
 	uORB::Subscription _sensor_accel_sub;
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
 
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::SubscriptionSelection<&vehicle_control_mode_s::flag_armed>
+	_vehicle_control_mode_armed_sub{ORB_ID(vehicle_control_mode)};
 
 	calibration::Accelerometer _accel_calibration{};
 	calibration::Gyroscope _gyro_calibration{};