3d meshfiles

Add airframes
F

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

@@ -0,0 +1,30 @@
+#!/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/1041_gazebo-classic_tailsitter

@@ -44,6 +44,12 @@ 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,50 +5,6 @@
 # @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}
 
-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
+. ${R}etc/init.d-posix/airframes/4001_gz_x500

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

@@ -10,6 +10,8 @@ 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,6 +11,8 @@ 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,8 +5,6 @@
 # @type Quadrotor
 #
 
-. ${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}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500

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

@@ -0,0 +1,54 @@
+#!/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,11 +75,13 @@ 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
 
-	10016_gazebo-classic_iris
+	10015_gazebo-classic_iris
+	10016_none_iris
 	10017_jmavsim_iris
 	10018_gazebo-classic_iris_foggy_lidar
 	10019_gazebo-classic_omnicopter

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

@@ -33,3 +33,10 @@ 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,8 +58,6 @@ 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

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

@@ -0,0 +1,24 @@
+<?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

@@ -0,0 +1,290 @@
+<?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>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</uri>
+      <uri>x500</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:u:p:qsr:f:i:loat" opt; do
+while getopts "b:d:ui:p:qsr: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: determin_hw_version
+  * Name: determine_hw_info
  *
  * Description:
  *
@@ -188,7 +188,10 @@ __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 and -1 on not supported
+ *  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.
  *
  *  hw_type Initial state is {'V','2',0, 0}
  *   V 2    - FMUv2
@@ -198,19 +201,18 @@ __EXPORT void board_on_reset(int status)
  *
  ************************************************************************************/
 
-static int determin_hw_version(int *version, int *revision)
+static int determine_hw_info(int *revision, int *version)
 {
 	*revision = 0; /* default revision */
-	int rv = 0;
 	int pos = 0;
 	stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB4 & ~GPIO_PUPD_MASK));
 	up_udelay(10);
-	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
+	*version |= stm32_gpioread(HW_VER_PB4) << pos++;
 	stm32_configgpio(HW_VER_PB4);
 	up_udelay(10);
-	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
+	*version |= stm32_gpioread(HW_VER_PB4) << pos++;
 
-	int votes = 16;
+	int votes = 100;
 	int ones[2] = {0, 0};
 	int zeros[2] = {0, 0};
 
@@ -223,19 +225,31 @@ static int determin_hw_version(int *version, int *revision)
 		stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;
 	}
 
-	if (ones[0] > zeros[0]) {
-		rv |= 1 << pos;
-	}
+	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.
 
-	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");
 
-	if (ones[1] > zeros[1]) {
-		rv |= 1 << pos;
+	} else {
+
+		if (ones[0] > zeros[0]) {
+			*version |= 1 << pos;
+		}
+
+		pos++;
+
+		if (ones[1] > zeros[1] + margin) {
+			*version |= 1 << pos;
+		}
 	}
 
 	stm32_configgpio(HW_VER_PB4_INIT);
 	stm32_configgpio(HW_VER_PB12_INIT);
-	*version = rv;
+
 	return OK;
 }
 
@@ -366,7 +380,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 == determin_hw_version(&hw_version, & hw_revision)) {
+	if (OK == determine_hw_info(&hw_revision, &hw_version)) {
 		switch (hw_version) {
 		case HW_VER_FMUV2_STATE:
 			break;
@@ -514,7 +528,6 @@ __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,6 +23,7 @@ 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: determin_hw_version
+  * Name: determine_hw_info
  *
  * Description:
  *
@@ -188,7 +188,10 @@ __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 and -1 on not supported
+ *  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.
  *
  *  hw_type Initial state is {'V','2',0, 0}
  *   V 2    - FMUv2
@@ -198,19 +201,18 @@ __EXPORT void board_on_reset(int status)
  *
  ************************************************************************************/
 
-static int determin_hw_version(int *version, int *revision)
+static int determine_hw_info(int *revision, int *version)
 {
 	*revision = 0; /* default revision */
-	int rv = 0;
 	int pos = 0;
 	stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB4 & ~GPIO_PUPD_MASK));
 	up_udelay(10);
-	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
+	*version |= stm32_gpioread(HW_VER_PB4) << pos++;
 	stm32_configgpio(HW_VER_PB4);
 	up_udelay(10);
-	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
+	*version |= stm32_gpioread(HW_VER_PB4) << pos++;
 
-	int votes = 16;
+	int votes = 100;
 	int ones[2] = {0, 0};
 	int zeros[2] = {0, 0};
 
@@ -223,19 +225,31 @@ static int determin_hw_version(int *version, int *revision)
 		stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;
 	}
 
-	if (ones[0] > zeros[0]) {
-		rv |= 1 << pos;
-	}
+	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.
 
-	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");
 
-	if (ones[1] > zeros[1]) {
-		rv |= 1 << pos;
+	} else {
+
+		if (ones[0] > zeros[0]) {
+			*version |= 1 << pos;
+		}
+
+		pos++;
+
+		if (ones[1] > zeros[1] + margin) {
+			*version |= 1 << pos;
+		}
 	}
 
 	stm32_configgpio(HW_VER_PB4_INIT);
 	stm32_configgpio(HW_VER_PB12_INIT);
-	*version = rv;
+
 	return OK;
 }
 
@@ -366,7 +380,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 == determin_hw_version(&hw_version, & hw_revision)) {
+	if (OK == determine_hw_info(&hw_revision, &hw_version)) {
 		switch (hw_version) {
 		case HW_VER_FMUV2_STATE:
 			break;

boards/px4/fmu-v4/default.px4board

@@ -21,6 +21,7 @@ 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,6 +27,7 @@ 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,4 +1,5 @@
 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_DRIVERS_INS_VECTORNAV=y
+CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y

boards/px4/fmu-v6x/default.px4board

@@ -25,6 +25,7 @@ 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,7 +1,9 @@
 uint64 timestamp		# time since system start (microseconds)
 
 float32 altitude_sp			# Altitude setpoint AMSL [m]
-float32 altitude_sp_filtered		# Altitude setpoint filtered 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 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/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c

@@ -66,19 +66,6 @@ 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,19 +71,6 @@ 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(1760);
+static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(1990);
 
 
 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 5
+ * @max 6
  * @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 5
+ * @max 6
  * @value 0 Auto detect
  * @value 1 u-blox
  * @value 2 MTK

src/drivers/ins/vectornav/CMakeLists.txt

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

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

@@ -56,7 +56,6 @@ 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/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", ret);
+				PX4_ERR("UavcanServers init: %d", rv);
 			}
 		}
 	}

src/lib/tecs/TECS.cpp

@@ -133,8 +133,8 @@ TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const
 	return _airspeed_state;
 }
 
-void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude,
-				const Param &param)
+void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude,
+					float height_rate, const Param &param)
 {
 	// Input checks
 	if (!TIMESTAMP_VALID(dt)) {
@@ -143,48 +143,61 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
 		return;
 	}
 
-	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;
-	}
+	const float current_alt = PX4_ISFINITE(altitude) ? altitude : 0.f;
 
+	_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));
 
-	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);
+	_velocity_control_traj_generator.setVelSpFeedback(setpoint.alt_rate);
 
-		const float delta_trajectory_to_target_m = setpoint.alt - _alt_control_traj_generator.getCurrentPosition();
+	bool control_altitude = true;
+	float altitude_setpoint = setpoint.alt;
 
-		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);
-
-		altitude_rate_target = math::constrain(altitude_rate_target, -target_sinkrate, target_climbrate);
-
-		_alt_control_traj_generator.updateDurations(altitude_rate_target);
-		_alt_control_traj_generator.updateTraj(dt);
+	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
 
 	} else {
-		_alt_control_traj_generator.reset(0.0f, 0.0f, altitude);
+		_velocity_control_traj_generator.reset(0, height_rate, altitude_setpoint);
+	}
+
+	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.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();
 	}
 }
 
-TECSReferenceModel::AltitudeReferenceState TECSReferenceModel::getAltitudeReference() const
+TECSAltitudeReferenceModel::AltitudeReferenceState TECSAltitudeReferenceModel::getAltitudeReference() const
 {
-	TECSReferenceModel::AltitudeReferenceState ref{
+	TECSAltitudeReferenceModel::AltitudeReferenceState ref{
 		.alt = _alt_control_traj_generator.getCurrentPosition(),
 		.alt_rate = _alt_control_traj_generator.getCurrentVelocity(),
 	};
@@ -192,25 +205,12 @@ TECSReferenceModel::AltitudeReferenceState TECSReferenceModel::getAltitudeRefere
 	return ref;
 }
 
-float TECSReferenceModel::getAltitudeRateReference() const
+void TECSAltitudeReferenceModel::initialize(const AltitudeReferenceState &state)
 {
-	return _alt_rate_ref;
-}
+	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;
 
-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);
+	_alt_control_traj_generator.reset(0.0f, init_state_alt_rate, init_state_alt);
 }
 
 void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag)
@@ -247,7 +247,6 @@ 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;
 }
@@ -265,7 +264,14 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &
 
 	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
 
-	control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
+	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);
+	}
 
 	SpecificEnergyRates specific_energy_rate{_calcSpecificEnergyRates(control_setpoint, input)};
 
@@ -315,8 +321,9 @@ 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 + setpoint.altitude_rate_setpoint;
+	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 = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
 
 	return altitude_rate_output;
@@ -570,17 +577,21 @@ 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 + ste_rate.setpoint / limit.STE_rate_max *
-				     (param.throttle_max - param.throttle_trim);
+		throttle_predicted = param.throttle_trim_adjusted + ste_rate.setpoint * throttle_above_trim_per_ste_rate;
 
 	} else {
 		// throttle is between trim and minimum
-		throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_min *
-				     (param.throttle_min - param.throttle_trim);
+		throttle_predicted = param.throttle_trim_adjusted - ste_rate.setpoint * throttle_below_trim_per_ste_rate;
 
 	}
 
@@ -635,14 +646,15 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 		      const float eas_to_tas)
 {
 	// Init subclasses
-	TECSReferenceModel::AltitudeReferenceState current_state{	.alt = altitude,
+	TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude,
 			.alt_rate = altitude_rate};
-	_reference_model.initialize(current_state);
+	_altitude_reference_model.initialize(current_state);
 	_airspeed_filter.initialize(equivalent_airspeed);
 
 	TECSControl::Setpoint control_setpoint;
-	control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
-	control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
+	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.tas_setpoint = equivalent_airspeed * eas_to_tas;
 
 	const TECSControl::Input control_input{ .altitude = altitude,
@@ -654,21 +666,19 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 
 	_debug_status.tecs_mode = _tecs_mode;
 	_debug_status.control = _control.getDebugOutput();
-	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();
+	_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();
 
 	_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 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 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)
 {
 
 	// Calculate the time since last update (seconds)
@@ -684,6 +694,7 @@ 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;
 
@@ -705,14 +716,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 TECSReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
+		const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
 				.alt_rate = hgt_rate_sp};
 
-		_reference_model.update(dt, setpoint, altitude, _reference_param);
+		_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
 
 		TECSControl::Setpoint control_setpoint;
-		control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
-		control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
+		control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
+		control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect();
 
 		// Calculate the demanded true airspeed
 		// TODO this function should not be in the module. Only give feedback that the airspeed can't be achieved.
@@ -743,9 +754,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_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;
+		_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();
 	}
 }
 

src/lib/tecs/TECS.hpp

@@ -113,7 +113,7 @@ private:
 	AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f};	///< Complimentary filter state
 };
 
-class TECSReferenceModel
+class TECSAltitudeReferenceModel
 {
 public:
 	/**
@@ -139,8 +139,8 @@ public:
 	};
 
 public:
-	TECSReferenceModel() = default;
-	~TECSReferenceModel() = default;
+	TECSAltitudeReferenceModel() = default;
+	~TECSAltitudeReferenceModel() = default;
 
 	/**
 	 * @brief Initialize reference models.
@@ -155,9 +155,10 @@ 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, const Param &param);
+	void update(float dt, const AltitudeReferenceState &setpoint, float altitude, float height_rate, const Param &param);
 
 	/**
 	 * @brief Get the current altitude reference of altitude reference model.
@@ -167,17 +168,20 @@ public:
 	AltitudeReferenceState getAltitudeReference() const;
 
 	/**
-	 * @brief Get the altitude rate reference of the altitude rate reference model.
+	 * @brief Get the Height Rate Setpoint directly from the velocity trajector generator
 	 *
-	 * @return Current altitude rate reference point.
+	 * @return float direct height rate setpoint [m/s]
 	 */
-	float getAltitudeRateReference() const;
+	float getHeightRateSetpointDirect() const {return _height_rate_setpoint_direct; }
+
 
 private:
 	// State
 	VelocitySmoothing
 	_alt_control_traj_generator;		///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded.
-	float _alt_rate_ref; 			///< Altitude rate reference in [m/s].
+	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
 };
 
 class TECSControl
@@ -197,7 +201,8 @@ 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 given eas.
+		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_max;			///< Normalized throttle upper limit.
 		float throttle_min;			///< Normalized throttle lower limit.
 
@@ -248,8 +253,8 @@ public:
 	 *
 	 */
 	struct Setpoint {
-		TECSReferenceModel::AltitudeReferenceState altitude_reference;	///< Altitude reference from reference model.
-		float altitude_rate_setpoint;					///< Altitude rate setpoint.
+		TECSAltitudeReferenceModel::AltitudeReferenceState altitude_reference;	///< Altitude/height rate reference.
+		float altitude_rate_setpoint_direct;					///< Direct height rate setpoint.
 		float tas_setpoint;						///< True airspeed setpoint.
 	};
 
@@ -543,9 +548,9 @@ public:
 		TECSControl::DebugOutput control;
 		float true_airspeed_filtered;
 		float true_airspeed_derivative;
-		float altitude_sp_ref;
-		float altitude_rate_alt_ref;
-		float altitude_rate_feedforward;
+		float altitude_reference;
+		float height_rate_reference;
+		float height_rate_direct;
 		enum ECL_TECS_MODE tecs_mode;
 	};
 public:
@@ -578,8 +583,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 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 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);
 
 	/**
 	 * @brief Initialize the control loop
@@ -637,11 +642,11 @@ public:
 	 */
 	void handle_alt_step(float altitude, float altitude_rate)
 	{
-		TECSReferenceModel::AltitudeReferenceState init_state{ .alt = altitude,
+		TECSAltitudeReferenceModel::AltitudeReferenceState init_state{ .alt = altitude,
 				.alt_rate = altitude_rate};
 
 		// reset altitude reference model.
-		_reference_model.initialize(init_state);
+		_altitude_reference_model.initialize(init_state);
 	}
 
 	float get_pitch_setpoint() {return _control.getPitchSetpoint();}
@@ -651,9 +656,9 @@ public:
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
 
 private:
-	TECSControl 		_control;				///< Control submodule.
-	TECSAirspeedFilter 	_airspeed_filter;			///< Airspeed filter submodule.
-	TECSReferenceModel 	_reference_model;			///< Setpoint reference model submodule.
+	TECSControl 			_control;			///< Control submodule.
+	TECSAirspeedFilter 		_airspeed_filter;		///< Airspeed filter submodule.
+	TECSAltitudeReferenceModel 	_altitude_reference_model;	///< Setpoint reference model submodule.
 
 	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};		///< Current activated mode.
 
@@ -676,7 +681,7 @@ private:
 		.airspeed_rate_noise_std_dev = 0.02f
 	};
 	/// Reference model parameters.
-	TECSReferenceModel::Param _reference_param{
+	TECSAltitudeReferenceModel::Param _reference_param{
 		.target_climbrate = 2.0f,
 		.target_sinkrate = 2.0f,
 		.jerk_max = 1000.0f,
@@ -695,6 +700,7 @@ 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/camera_feedback/CameraFeedback.cpp

@@ -70,28 +70,27 @@ CameraFeedback::Run()
 
 	camera_trigger_s trig{};
 
-	vehicle_attitude_s att{};
+	while (_trigger_sub.update(&trig)) {
 
-	while (_att_sub.update(&att)) {
 		// update geotagging subscriptions
 		vehicle_global_position_s gpos{};
 		_gpos_sub.copy(&gpos);
 
+		vehicle_attitude_s att{};
 		_att_sub.copy(&att);
-		_trigger_sub.copy(&trig);
 
-		// if (trig.timestamp == 0 ||
-		//     gpos.timestamp == 0 ||
-		//     att.timestamp == 0) {
+		if (trig.timestamp == 0 ||
+		    gpos.timestamp == 0 ||
+		    att.timestamp == 0) {
 
-		// 	// reject until we have valid data
-		// 	continue;
-		// }
+			// reject until we have valid data
+			continue;
+		}
 
-		// if ((_cam_cap_fback >= 1) && !trig.feedback) {
-		// 	// Ignore triggers that are not feedback when camera capture feedback is enabled
-		// 	continue;
-		// }
+		if ((_cam_cap_fback >= 1) && !trig.feedback) {
+			// Ignore triggers that are not feedback when camera capture feedback is enabled
+			continue;
+		}
 
 		camera_capture_s capture{};
 

src/modules/commander/failsafe/framework.cpp

@@ -646,6 +646,7 @@ 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)) &&
@@ -656,7 +657,6 @@ 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,12 +243,8 @@ bool Ekf::initialiseTilt()
 		return false;
 	}
 
-	// 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}};
+	// 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));
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	return true;

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -459,8 +459,6 @@ 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,-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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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,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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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,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
+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
 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.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
+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
 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,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
+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
 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,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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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,9 +141,13 @@ 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
@@ -213,6 +217,18 @@ 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
@@ -243,11 +259,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		// Stick full up -1 -> stop, stick full down 1 -> double the speed
 		vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo());
 
-		// 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);
-		}
+		rcHelpModifyYaw(_land_heading);
 
 		Vector2f sticks_xy = _sticks.getPitchRollExpo();
 		Vector2f sticks_ne = sticks_xy;
@@ -275,11 +287,6 @@ 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,6 +109,8 @@ 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_path_navigation/FixedwingPositionControl.cpp

@@ -468,43 +468,45 @@ void
 FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
 		float true_airspeed_derivative_raw, float throttle_trim)
 {
-	tecs_status_s t{};
+	tecs_status_s tecs_status{};
 
 	const TECS::DebugOutput &debug_output{_tecs.getStatus()};
 
 	switch (_tecs.tecs_mode()) {
 	case TECS::ECL_TECS_MODE_NORMAL:
-		t.mode = tecs_status_s::TECS_MODE_NORMAL;
+		tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
 		break;
 
 	case TECS::ECL_TECS_MODE_UNDERSPEED:
-		t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+		tecs_status.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
 		break;
 	}
 
-	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.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.timestamp = hrt_absolute_time();
+	tecs_status.timestamp = hrt_absolute_time();
 
-	_tecs_status_pub.publish(t);
+	_tecs_status_pub.publish(tecs_status);
 }
 
 void
@@ -709,24 +711,8 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 	_skipping_takeoff_detection = false;
 
-	if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid) {
-		if (_control_mode.flag_control_position_enabled) {
-			if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)
-			    && PX4_ISFINITE(_pos_sp_triplet.current.vz)) {
-				// Offboard position with velocity setpoints
-				_control_mode_current = FW_POSCTRL_MODE_AUTO_PATH;
-				return;
-
-			} else {
-				// Offboard position setpoint only
-				_control_mode_current = FW_POSCTRL_MODE_AUTO;
-				return;
-			}
-		}
-	}
-
-	if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
-	    && _position_setpoint_current_valid) {
+	if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
+	     _control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) {
 
 		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
@@ -1113,7 +1099,17 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 
-	navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+	if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
+		// Navigate directly on position setpoint and path tangent
+		matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
+		float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
+				  0.0f;
+		navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed,
+				    _wind_vel, curvature);
+
+	} else {
+		navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+	}
 
 	_att_sp.roll_body = _npfg.getRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -1288,57 +1284,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 				   _param_climbrate_target.get());
 }
 
-void
-FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
-{
-
-	float tecs_fw_thr_min;
-	float tecs_fw_thr_max;
-
-	if (pos_sp_curr.gliding_enabled) {
-		/* enable gliding with this waypoint */
-		_tecs.set_speed_weight(2.0f);
-		tecs_fw_thr_min = 0.0;
-		tecs_fw_thr_max = 0.0;
-
-	} else {
-		tecs_fw_thr_min = _param_fw_thr_min.get();
-		tecs_fw_thr_max = _param_fw_thr_max.get();
-	}
-
-	// waypoint is a plain navigation waypoint
-	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
-				_param_fw_airspd_min.get(), ground_speed);
-
-	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-	Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
-
-	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-
-	// Navigate directly on position setpoint and path tangent
-	matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
-	float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
-			  0.0f;
-	navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature);
-
-	_att_sp.roll_body = _npfg.getRollSetpoint();
-	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
-
-	tecs_update_pitch_throttle(control_interval,
-				   pos_sp_curr.alt,
-				   target_airspeed,
-				   radians(_param_fw_p_lim_min.get()),
-				   radians(_param_fw_p_lim_max.get()),
-				   tecs_fw_thr_min,
-				   tecs_fw_thr_max,
-				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
-}
-
 void
 FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
 		const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
@@ -2402,11 +2347,6 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
-		case FW_POSCTRL_MODE_AUTO_PATH: {
-				control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
-				break;
-			}
-
 		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
 				control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
 				break;
@@ -2520,9 +2460,25 @@ FixedwingPositionControl::reset_landing_state()
 	}
 }
 
-float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min,
-		float throttle_max)
+float FixedwingPositionControl::calculateTrimThrottle(float throttle_min,
+		float throttle_max, float airspeed_sp)
 {
+	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) {
@@ -2612,8 +2568,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	/* update TECS vehicle state estimates */
-	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
-					 throttle_max);
+	const float throttle_trim_adjusted = calculateTrimThrottle(throttle_min,
+					     throttle_max, airspeed_sp);
 
 	// 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.
@@ -2627,7 +2583,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     _eas2tas,
 		     throttle_min,
 		     throttle_max,
-		     throttle_trim_comp,
+		     _param_fw_thr_trim.get(),
+		     throttle_trim_adjusted,
 		     pitch_min_rad - radians(_param_fw_psp_off.get()),
 		     pitch_max_rad - radians(_param_fw_psp_off.get()),
 		     desired_max_climbrate,
@@ -2636,7 +2593,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_comp);
+	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_adjusted);
 }
 
 float

src/modules/fw_path_navigation/FixedwingPositionControl.hpp

@@ -243,7 +243,6 @@ private:
 		FW_POSCTRL_MODE_AUTO_TAKEOFF,
 		FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
 		FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
-		FW_POSCTRL_MODE_AUTO_PATH,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
 		FW_POSCTRL_MODE_OTHER
@@ -586,18 +585,6 @@ private:
 	void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
 				 const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
 
-	/**
-	 * @brief Vehicle control for following a path.
-	 *
-	 * @param control_interval Time since last position control call [s]
-	 * @param curr_pos Current 2D local position vector of vehicle [m]
-	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
-	 * @param pos_sp_prev previous position setpoint
-	 * @param pos_sp_curr current position setpoint
-	 */
-	void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
-			       const position_setpoint_s &pos_sp_curr);
-
 	/**
 	 * @brief Controls a desired airspeed, bearing, and height rate.
 	 *
@@ -702,14 +689,15 @@ private:
 	void set_control_mode_current(const hrt_abstime &now);
 
 	/**
-	 * @brief Compensate trim throttle for air density and vehicle weight.
+	 * @brief Estimate trim throttle for air density, vehicle weight and current airspeed
 	 *
-	 * @param trim throttle required at sea level during standard conditions.
 	 * @param throttle_min Minimum allowed trim throttle.
 	 * @param throttle_max Maximum allowed trim throttle.
-	 * @return Trim throttle compensated for air density and vehicle weight.
+	 * @param airspeed_sp Current airspeed setpoint (CAS) [m/s]
+	 * @return Estimated trim throttle
 	 */
-	float compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, float throttle_max);
+	float calculateTrimThrottle(float throttle_min, float throttle_max,
+				    float airspeed_sp);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
 
@@ -919,6 +907,9 @@ 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,3 +1063,33 @@ 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/logger/logged_topics.cpp

@@ -51,7 +51,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
 	add_optional_topic("autotune_attitude_control_status", 100);
-	add_topic("camera_capture");
+	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");

src/modules/navigator/mission.cpp

@@ -96,12 +96,6 @@ 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;
@@ -180,8 +174,6 @@ 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;
 }
@@ -476,15 +468,12 @@ Mission::land_start()
 {
 	// if not currently landing, jump to do_land_start
 	if (_land_start_available) {
-		if (_navigator->getMissionLandingInProgress()) {
+		if (landing()) {
 			return true;
 
 		} else {
 			set_current_mission_index(get_land_start_index());
-
-			const bool can_land_now = landing();
-			_navigator->setMissionLandingInProgress(can_land_now);
-			return can_land_now;
+			return landing();
 		}
 	}
 
@@ -495,10 +484,24 @@ bool
 Mission::landing()
 {
 	// vehicle is currently landing if
-	//  mission valid, still flying, and in the landing portion of mission
+	//  mission valid, still flying, and in the landing portion of mission (past land start marker)
 
 	const bool mission_valid = _navigator->get_mission_result()->valid;
-	const bool on_landing_stage = _land_start_available && (_current_mission_index >= get_land_start_index());
+	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);
+	}
 
 	return mission_valid && on_landing_stage;
 }
@@ -1718,12 +1721,6 @@ 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

@@ -267,8 +267,6 @@ 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/navigator.h

@@ -259,10 +259,6 @@ 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(); }
@@ -348,7 +344,7 @@ private:
 	vehicle_local_position_s			_local_pos{};		/**< local vehicle position */
 	vehicle_status_s				_vstatus{};		/**< vehicle status */
 
-	uint8_t						_previous_nav_state{}; /**< nav_state of the previous iteration*/
+	bool						_rtl_activated{false};
 
 	// Publications
 	geofence_result_s				_geofence_result{};
@@ -393,9 +389,6 @@ 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,24 +664,22 @@ void Navigator::run()
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
 				_pos_sp_triplet_published_invalid_once = false;
-
-				const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+				const bool rtl_activated_now = !_rtl_activated;
 
 				switch (_rtl.get_rtl_type()) {
 				case RTL::RTL_TYPE_MISSION_LANDING:
 				case RTL::RTL_TYPE_CLOSEST:
-
-					if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_LOITER
-					    && _rtl.getShouldEngageMissionForLanding()) {
+					if (on_mission_landing() && _rtl.getShouldEngageMissionForLanding()) {
 						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
 
-						if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED
-						    && !get_land_detected()->landed) {
-							start_mission_landing();
-						}
-
 						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");
+						}
+
 					} else {
 						navigation_mode_new = &_rtl;
 					}
@@ -706,7 +704,7 @@ void Navigator::run()
 							}
 						}
 
-						if (rtl_activated) {
+						if (rtl_activated_now) {
 							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");
@@ -726,11 +724,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 || _mission.get_mission_waypoints_changed())) {
+							if ((_navigation_mode != &_mission) && (rtl_activated_now || _mission.get_mission_waypoints_changed())) {
 								_mission.set_closest_item_as_current();
 							}
 
-							if (rtl_activated) {
+							if (rtl_activated_now) {
 								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");
@@ -739,7 +737,7 @@ void Navigator::run()
 							navigation_mode_new = &_mission;
 
 						} else {
-							if (rtl_activated) {
+							if (rtl_activated_now) {
 								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");
@@ -752,7 +750,7 @@ void Navigator::run()
 					break;
 
 				default:
-					if (rtl_activated) {
+					if (rtl_activated_now) {
 						mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t");
 						events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated");
 					}
@@ -762,6 +760,7 @@ void Navigator::run()
 
 				}
 
+				_rtl_activated = true;
 				break;
 			}
 
@@ -800,14 +799,15 @@ 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->getMissionLandingInProgress())) {
+		if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->on_mission_landing())) {
 
 			// compare home position to landing position to decide which is closer
 			if (dist_squared < min_dist_squared) {
@@ -282,12 +282,6 @@ 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/simulation/simulator_mavlink/CMakeLists.txt

@@ -59,3 +59,15 @@ include(sitl_targets_flightgear.cmake)
 include(sitl_targets_gazebo-classic.cmake)
 include(sitl_targets_jmavsim.cmake)
 include(sitl_targets_jsbsim.cmake)
+
+
+# none_iris (legacy compatibility launch helper)
+add_custom_target(none_iris
+	COMMAND ${CMAKE_COMMAND} -E env PX4_SYS_AUTOSTART=10016 $<TARGET_FILE:px4>
+	WORKING_DIRECTORY ${SITL_WORKING_DIR}
+	USES_TERMINAL
+	DEPENDS
+		px4
+		${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes/10016_none_iris
+	COMMENT "launching px4 none_iris (SYS_AUTOSTART=10016)"
+)