make demo work with pca.

* match vehicle_status

* match home_position

* match vehicle_command_ack

* match actuator_armed

* match vehicle_control_mode

* match commander_state

ROMFS/cannode/CMakeLists.txt

@@ -32,3 +32,4 @@
 ############################################################################
 
 add_subdirectory(init.d)
+add_subdirectory(mixers)

ROMFS/cannode/init.d/CMakeLists.txt

@@ -33,4 +33,5 @@
 
 px4_add_romfs_files(
 	rcS
+	rc.output_defaults
 )

ROMFS/cannode/init.d/rc.output_defaults

@@ -0,0 +1,32 @@
+#!/bin/sh
+#
+# UGV default parameters.
+#
+# NOTE: Script variables are declared/initialized/unset in the rcS script.
+#
+
+#
+# Enable servo output on pins 3 and 4 (steering and thrust)
+# but also include 1+2 as they form together one output group
+# and need to be set together.
+#
+set PWM_OUT 12
+
+#
+# PWM Hz - 50 Hz is the normal rate in RC cars, higher rates
+# may damage analog servos.
+#
+set PWM_MAIN_RATE 50
+
+#
+# This is the gimbal pass mixer.
+#
+set MIXER_AUX pass
+set PWM_AUX_OUT 12
+
+param set-default PWM_MAIN_DISARM 1500
+param set-default PWM_MAIN_MAX 2000
+param set-default PWM_MAIN_MIN 1000
+
+# Set mixer
+set MIXER IO_pass_ucan

ROMFS/cannode/init.d/rcS

@@ -65,8 +65,8 @@ unset BOARD_RC_DEFAULTS
 #
 # Start system state indicator.
 #
-rgbled start -X -q
-rgbled_ncp5623c start -X -q
+#rgbled start -X -q
+#rgbled_ncp5623c start -X -q
 
 #
 # board sensors: rc.sensors
@@ -86,10 +86,10 @@ unset BOARD_RC_SENSORS
 . ${R}etc/init.d/rc.serial
 
 # Check for flow sensor
-if param compare SENS_EN_PX4FLOW 1
-then
-	px4flow start -X &
-fi
+#if param compare SENS_EN_PX4FLOW 1
+#then
+#	px4flow start -X &
+#fi
 
-uavcannode start
+#uavcannode start
 unset R

ROMFS/cannode/mixers/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_romfs_files(
+	IO_pass_ucan.main.mix
+)

ROMFS/cannode/mixers/IO_pass_ucan.main.mix

@@ -0,0 +1,12 @@
+Passthrough mixer for PX4IO
+============================
+
+This file defines passthrough mixers suitable for testing.
+
+Channel group 0, channels 0-7 are passed directly through to the outputs.
+
+M: 1
+S: 0 0  10000  10000      0 -10000  10000
+
+M: 1
+S: 0 1  10000  10000      0 -10000  10000

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -55,4 +55,5 @@ px4_add_romfs_files(
 	rc.vehicle_setup
 	rc.vtol_apps
 	rc.vtol_defaults
+	rc.output_defaults
 )

ROMFS/px4fmu_common/init.d/airframes/22222_generic_output

@@ -0,0 +1,21 @@
+#!/bin/sh
+#
+# @name Generic Output
+#
+# @maintainer
+#
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.output_defaults
+
+
+# Provide ESC a constant 1500 us pulse
+param set-default PWM_MAIN_DISARM 1500
+param set-default PWM_MAIN_MAX 2000
+param set-default PWM_MAIN_MIN 1000
+
+# Set mixer
+set MIXER IO_pass
+
+set PWM_OUT 12

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

@@ -147,7 +147,7 @@ px4_add_romfs_files(
 	18001_TF-B1
 
 	# [22000, 22999] Reserve for custom models
-
+	22222_generic_output
 	24001_dodeca_cox
 
 	50000_generic_ground_vehicle

ROMFS/px4fmu_common/init.d/rc.output_defaults

@@ -0,0 +1,25 @@
+#!/bin/sh
+#
+# UGV default parameters.
+#
+# NOTE: Script variables are declared/initialized/unset in the rcS script.
+#
+
+#
+# Enable servo output on pins 3 and 4 (steering and thrust)
+# but also include 1+2 as they form together one output group
+# and need to be set together.
+#
+set PWM_OUT 12
+
+#
+# PWM Hz - 50 Hz is the normal rate in RC cars, higher rates
+# may damage analog servos.
+#
+set PWM_MAIN_RATE 50
+
+#
+# This is the gimbal pass mixer.
+#
+set MIXER_AUX pass
+set PWM_AUX_OUT 12

Tools/ecl_ekf/analyse_logdata_ekf.py

@@ -11,7 +11,7 @@ from pyulog import ULog
 from analysis.detectors import InAirDetector, PreconditionError
 from analysis.metrics import calculate_ecl_ekf_metrics
 from analysis.checks import perform_ecl_ekf_checks
-from analysis.post_processing import get_estimator_check_flags
+from analysis.post_processing import get_gps_check_fail_flags
 
 def analyse_ekf(
         ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0,
@@ -40,6 +40,11 @@ def analyse_ekf(
     except:
         raise PreconditionError('could not find estimator_status instance', multi_instance)
 
+    try:
+        estimator_status_flags = ulog.get_dataset('estimator_status_flags', multi_instance).data
+    except:
+        raise PreconditionError('could not find estimator_status_flags instance', multi_instance)
+
     try:
         _ = ulog.get_dataset('estimator_innovations', multi_instance).data
     except:
@@ -61,14 +66,14 @@ def analyse_ekf(
         'in_air_transition_time': round(in_air.take_off + in_air.log_start, 2),
         'on_ground_transition_time': round(in_air.landing + in_air.log_start, 2)}
 
-    control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
+    gps_fail_flags = get_gps_check_fail_flags(estimator_status)
 
     sensor_checks, innov_fail_checks = find_checks_that_apply(
-        control_mode, estimator_status,
+        estimator_status_flags, estimator_status,
         pos_checks_when_sensors_not_fused=pos_checks_when_sensors_not_fused)
 
     metrics = calculate_ecl_ekf_metrics(
-        ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects,
+        ulog, estimator_status_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects,
         multi_instance, red_thresh=red_thresh, amb_thresh=amb_thresh)
 
     check_status, master_status = perform_ecl_ekf_checks(
@@ -78,12 +83,12 @@ def analyse_ekf(
 
 
 def find_checks_that_apply(
-    control_mode: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\
+    estimator_status_flags: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\
         Tuple[List[str], List[str]]:
     """
     finds the checks that apply and stores them in lists for the std checks and the innovation
     fail checks.
-    :param control_mode:
+    :param estimator_status_flags:
     :param estimator_status:
     :param b_pos_only_when_sensors_fused:
     :return: a tuple of two lists that contain strings for the std checks and for the innovation
@@ -97,7 +102,7 @@ def find_checks_that_apply(
     innov_fail_checks.append('posv')
 
     # Magnetometer Sensor Checks
-    if (np.amax(control_mode['yaw_aligned']) > 0.5):
+    if (np.amax(estimator_status_flags['cs_yaw_align']) > 0.5):
         sensor_checks.append('mag')
 
         innov_fail_checks.append('magx')
@@ -106,13 +111,14 @@ def find_checks_that_apply(
         innov_fail_checks.append('yaw')
 
     # Velocity Sensor Checks
-    if (np.amax(control_mode['using_gps']) > 0.5):
+    if (np.amax(estimator_status_flags['cs_gps']) > 0.5):
         sensor_checks.append('vel')
-        innov_fail_checks.append('vel')
+        innov_fail_checks.append('velh')
+        innov_fail_checks.append('velv')
 
     # Position Sensor Checks
-    if (pos_checks_when_sensors_not_fused or (np.amax(control_mode['using_gps']) > 0.5)
-        or (np.amax(control_mode['using_evpos']) > 0.5)):
+    if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gps']) > 0.5)
+        or (np.amax(estimator_status_flags['cs_ev_pos']) > 0.5)):
         sensor_checks.append('pos')
         innov_fail_checks.append('posh')
 
@@ -128,7 +134,7 @@ def find_checks_that_apply(
         innov_fail_checks.append('hagl')
 
     # optical flow sensor checks
-    if (np.amax(control_mode['using_optflow']) > 0.5):
+    if (np.amax(estimator_status_flags['cs_opt_flow']) > 0.5):
         innov_fail_checks.append('ofx')
         innov_fail_checks.append('ofy')
 

Tools/ecl_ekf/analysis/checks.py

@@ -123,7 +123,8 @@ def perform_sensor_innov_checks(
                                               ('magy', 'magy_fail_percentage', 'mag'),
                                               ('magz', 'magz_fail_percentage', 'mag'),
                                               ('yaw', 'yaw_fail_percentage', 'yaw'),
-                                              ('vel', 'vel_fail_percentage', 'vel'),
+                                              ('velh', 'vel_fail_percentage', 'vel'),
+                                              ('velv', 'vel_fail_percentage', 'vel'),
                                               ('posh', 'pos_fail_percentage', 'pos'),
                                               ('tas', 'tas_fail_percentage', 'tas'),
                                               ('hagl', 'hagl_fail_percentage', 'hagl'),

Tools/ecl_ekf/analysis/metrics.py

@@ -11,7 +11,7 @@ import numpy as np
 from analysis.detectors import InAirDetector
 
 def calculate_ecl_ekf_metrics(
-        ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str],
+        ulog: ULog, estimator_status_flags: Dict[str, float], innov_fail_checks: List[str],
         sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector,
         multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
 
@@ -20,7 +20,7 @@ def calculate_ecl_ekf_metrics(
         red_thresh=red_thresh, amb_thresh=amb_thresh)
 
     innov_fail_metrics = calculate_innov_fail_metrics(
-        innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
+        estimator_status_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
 
     imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects)
 
@@ -90,10 +90,10 @@ def calculate_sensor_metrics(
 
 
 def calculate_innov_fail_metrics(
-        innov_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
+        estimator_status_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
         in_air_no_ground_effects: InAirDetector) -> dict:
     """
-    :param innov_flags:
+    :param estimator_status_flags:
     :param innov_fail_checks:
     :param in_air:
     :param in_air_no_ground_effects:
@@ -103,17 +103,18 @@ def calculate_innov_fail_metrics(
     innov_fail_metrics = dict()
 
     # calculate innovation check fail metrics
-    for signal_id, signal, result in [('posv', 'posv_innov_fail', 'hgt_fail_percentage'),
-                                      ('magx', 'magx_innov_fail', 'magx_fail_percentage'),
-                                      ('magy', 'magy_innov_fail', 'magy_fail_percentage'),
-                                      ('magz', 'magz_innov_fail', 'magz_fail_percentage'),
-                                      ('yaw', 'yaw_innov_fail', 'yaw_fail_percentage'),
-                                      ('vel', 'vel_innov_fail', 'vel_fail_percentage'),
-                                      ('posh', 'posh_innov_fail', 'pos_fail_percentage'),
-                                      ('tas', 'tas_innov_fail', 'tas_fail_percentage'),
-                                      ('hagl', 'hagl_innov_fail', 'hagl_fail_percentage'),
-                                      ('ofx', 'ofx_innov_fail', 'ofx_fail_percentage'),
-                                      ('ofy', 'ofy_innov_fail', 'ofy_fail_percentage')]:
+    for signal_id, signal, result in [('posv', 'reject_ver_pos', 'hgt_fail_percentage'),
+                                      ('magx', 'reject_mag_x', 'magx_fail_percentage'),
+                                      ('magy', 'reject_mag_y', 'magy_fail_percentage'),
+                                      ('magz', 'reject_mag_z', 'magz_fail_percentage'),
+                                      ('yaw', 'reject_yaw', 'yaw_fail_percentage'),
+                                      ('velh', 'reject_hor_vel', 'vel_fail_percentage'),
+                                      ('velv', 'reject_ver_vel', 'vel_fail_percentage'),
+                                      ('posh', 'reject_hor_pos', 'pos_fail_percentage'),
+                                      ('tas', 'reject_airspeed', 'tas_fail_percentage'),
+                                      ('hagl', 'reject_hagl', 'hagl_fail_percentage'),
+                                      ('ofx', 'reject_optflow_x', 'ofx_fail_percentage'),
+                                      ('ofy', 'reject_optflow_y', 'ofy_fail_percentage')]:
 
         # only run innov fail checks, if they apply.
         if signal_id in innov_fail_checks:
@@ -125,7 +126,7 @@ def calculate_innov_fail_metrics(
                 in_air_detector = in_air
 
             innov_fail_metrics[result] = calculate_stat_from_signal(
-                innov_flags, 'estimator_status', signal, in_air_detector,
+                estimator_status_flags, 'estimator_status_flags', signal, in_air_detector,
                 lambda x: 100.0 * np.mean(x > 0.5))
 
     return innov_fail_metrics
@@ -152,7 +153,7 @@ def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects:
 
             if vehicle_imu_status_data['accel_device_id'][0] == estimator_status_data['accel_device_id'][0]:
 
-                for signal, result in [('delta_angle_coning_metric', 'imu_coning'),
+                for signal, result in [('gyro_coning_vibration', 'imu_coning'),
                                        ('gyro_vibration_metric', 'imu_hfgyro'),
                                        ('accel_vibration_metric', 'imu_hfaccel')]:
 

Tools/ecl_ekf/analysis/post_processing.py

@@ -7,115 +7,6 @@ from typing import Tuple
 
 import numpy as np
 
-
-def get_estimator_check_flags(estimator_status: dict) -> Tuple[dict, dict, dict]:
-    """
-    :param estimator_status:
-    :return:
-    """
-    control_mode = get_control_mode_flags(estimator_status)
-    innov_flags = get_innovation_check_flags(estimator_status)
-    gps_fail_flags = get_gps_check_fail_flags(estimator_status)
-    return control_mode, innov_flags, gps_fail_flags
-
-
-def get_control_mode_flags(estimator_status: dict) -> dict:
-    """
-    :param estimator_status:
-    :return:
-    """
-
-    control_mode = dict()
-    # extract control mode metadata from estimator_status.control_mode_flags
-    # 0 - true if the filter tilt alignment is complete
-    # 1 - true if the filter yaw alignment is complete
-    # 2 - true if GPS measurements are being fused
-    # 3 - true if optical flow measurements are being fused
-    # 4 - true if a simple magnetic yaw heading is being fused
-    # 5 - true if 3-axis magnetometer measurement are being fused
-    # 6 - true if synthetic magnetic declination measurements are being fused
-    # 7 - true when the vehicle is airborne
-    # 8 - true when wind velocity is being estimated
-    # 9 - true when baro height is being fused as a primary height reference
-    # 10 - true when range finder height is being fused as a primary height reference
-    # 11 - true when range finder height is being fused as a primary height reference
-    # 12 - true when local position data from external vision is being fused
-    # 13 - true when yaw data from external vision measurements is being fused
-    # 14 - true when height data from external vision measurements is being fused
-    # 15 - true when synthetic sideslip measurements are being fused
-    # 16 - true true when the mag field does not match the expected strength
-    # 17 - true true when the vehicle is operating as a fixed wing vehicle
-    # 18 - true when the magnetometer has been declared faulty and is no longer being used
-    # 19 - true true when airspeed measurements are being fused
-    # 20 - true true when protection from ground effect induced static pressure rise is active
-    # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
-    # 22 - true when yaw (not ground course) data from a GPS receiver is being fused
-    # 23 - true when the in-flight mag field alignment has been completed
-    # 24 - true when local earth frame velocity data from external vision measurements are being fused
-    # 25 - true when we are using a synthesized measurement for the magnetometer Z component
-    control_mode['tilt_aligned'] = ((2 ** 0 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['yaw_aligned'] = ((2 ** 1 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_gps'] = ((2 ** 2 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_optflow'] = ((2 ** 3 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_magyaw'] = ((2 ** 4 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_mag3d'] = ((2 ** 5 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_magdecl'] = ((2 ** 6 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['airborne'] = ((2 ** 7 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['estimating_wind'] = ((2 ** 8 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_barohgt'] = ((2 ** 9 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_rnghgt'] = ((2 ** 10 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_gpshgt'] = ((2 ** 11 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_evpos'] = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_evyaw'] = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_evhgt'] = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['fuse_beta'] = ((2 ** 15 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['mag_field_disturbed'] = ((2 ** 16 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['fixed_wing'] = ((2 ** 17 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['mag_fault'] = ((2 ** 18 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['fuse_aspd'] = ((2 ** 19 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['gnd_effect'] = ((2 ** 20 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['rng_stuck'] = ((2 ** 21 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['gps_yaw'] = ((2 ** 22 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['mag_aligned_in_flight'] = ((2 ** 23 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['ev_vel'] = ((2 ** 24 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['synthetic_mag_z'] = ((2 ** 25 & estimator_status['control_mode_flags']) > 0) * 1
-    return control_mode
-
-def get_innovation_check_flags(estimator_status: dict) -> dict:
-    """
-    :param estimator_status:
-    :return:
-    """
-
-    innov_flags = dict()
-    # innovation_check_flags summary
-    # 0 - true if velocity observations have been rejected
-    # 1 - true if horizontal position observations have been rejected
-    # 2 - true if true if vertical position observations have been rejected
-    # 3 - true if the X magnetometer observation has been rejected
-    # 4 - true if the Y magnetometer observation has been rejected
-    # 5 - true if the Z magnetometer observation has been rejected
-    # 6 - true if the yaw observation has been rejected
-    # 7 - true if the airspeed observation has been rejected
-    # 8 - true if synthetic sideslip observation has been rejected
-    # 9 - true if the height above ground observation has been rejected
-    # 10 - true if the X optical flow observation has been rejected
-    # 11 - true if the Y optical flow observation has been rejected
-    innov_flags['vel_innov_fail'] = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['posh_innov_fail'] = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['posv_innov_fail'] = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['magx_innov_fail'] = ((2 ** 3 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['magy_innov_fail'] = ((2 ** 4 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['magz_innov_fail'] = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['yaw_innov_fail'] = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['tas_innov_fail'] = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['sli_innov_fail'] = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['hagl_innov_fail'] = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['ofx_innov_fail'] = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['ofy_innov_fail'] = ((2 ** 11 & estimator_status['innovation_check_flags']) > 0) * 1
-    return innov_flags
-
-
 def get_gps_check_fail_flags(estimator_status: dict) -> dict:
     """
     :param estimator_status:

Tools/ecl_ekf/plotting/pdf_report.py

@@ -11,7 +11,7 @@ import numpy as np
 from matplotlib.backends.backend_pdf import PdfPages
 from pyulog import ULog
 
-from analysis.post_processing import magnetic_field_estimates_from_states, get_estimator_check_flags
+from analysis.post_processing import magnetic_field_estimates_from_states, get_gps_check_fail_flags
 from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
     CheckFlagsPlot
 from analysis.detectors import PreconditionError
@@ -33,6 +33,11 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
     except:
         raise PreconditionError('could not find estimator_status instance', multi_instance)
 
+    try:
+        estimator_status_flags = ulog.get_dataset('estimator_status_flags', multi_instance).data
+    except:
+        raise PreconditionError('could not find estimator_status_flags instance', multi_instance)
+
     try:
         estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
     except:
@@ -68,12 +73,13 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
     except:
         raise PreconditionError('could not find innovation data')
 
-    control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
+    gps_fail_flags = get_gps_check_fail_flags(estimator_status)
 
     status_time = 1e-6 * estimator_status['timestamp']
+    status_flags_time = 1e-6 * estimator_status_flags['timestamp']
 
     b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, \
-    on_ground_transition_time = detect_airtime(control_mode, status_time)
+    on_ground_transition_time = detect_airtime(estimator_status_flags, status_flags_time)
 
     with PdfPages(output_plot_filename) as pdf_pages:
 
@@ -173,9 +179,9 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
 
         # plot control mode summary A
         data_plot = ControlModeSummaryPlot(
-            status_time, control_mode, [['tilt_aligned', 'yaw_aligned'],
-            ['using_gps', 'using_optflow', 'using_evpos'], ['using_barohgt', 'using_gpshgt',
-             'using_rnghgt', 'using_evhgt'], ['using_magyaw', 'using_mag3d', 'using_magdecl']],
+            status_flags_time, estimator_status_flags, [['cs_tilt_align', 'cs_yaw_align'],
+            ['cs_gps', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt',
+             'cs_rng_hgt', 'cs_ev_hgt'], ['cs_mag_hdg', 'cs_mag_3d', 'cs_mag_dec']],
             x_label='time (sec)', y_labels=['aligned', 'pos aiding', 'hgt aiding', 'mag aiding'],
             annotation_text=[['tilt alignment', 'yaw alignment'], ['GPS aiding', 'optical flow aiding',
              'external vision aiding'], ['Baro aiding', 'GPS aiding', 'rangefinder aiding',
@@ -188,7 +194,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
         # plot control mode summary B
         # construct additional annotations for the airborne plot
         airborne_annotations = list()
-        if np.amin(np.diff(control_mode['airborne'])) > -0.5:
+        if np.amin(np.diff(estimator_status_flags['cs_in_air'])) > -0.5:
             airborne_annotations.append(
                 (on_ground_transition_time, 'air to ground transition not detected'))
         else:
@@ -197,7 +203,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
         if in_air_duration > 0.0:
             airborne_annotations.append(((in_air_transition_time + on_ground_transition_time) / 2,
                                          'duration = {:.1f} sec'.format(in_air_duration)))
-        if np.amax(np.diff(control_mode['airborne'])) < 0.5:
+        if np.amax(np.diff(estimator_status_flags['cs_in_air'])) < 0.5:
             airborne_annotations.append(
                 (in_air_transition_time, 'ground to air transition not detected'))
         else:
@@ -205,7 +211,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
                 (in_air_transition_time, 'in-air at {:.1f} sec'.format(in_air_transition_time)))
 
         data_plot = ControlModeSummaryPlot(
-            status_time, control_mode, [['airborne'], ['estimating_wind']],
+            status_flags_time, estimator_status_flags, [['cs_in_air'], ['cs_wind']],
             x_label='time (sec)', y_labels=['airborne', 'estimating wind'], annotation_text=[[], []],
             additional_annotation=[airborne_annotations, []],
             plot_title='EKF Control Status - Figure B', pdf_handle=pdf_pages)
@@ -214,15 +220,15 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
 
         # plot innovation_check_flags summary
         data_plot = CheckFlagsPlot(
-            status_time, innov_flags, [['vel_innov_fail', 'posh_innov_fail'], ['posv_innov_fail',
-                                                                               'hagl_innov_fail'],
-                                       ['magx_innov_fail', 'magy_innov_fail', 'magz_innov_fail',
-                                        'yaw_innov_fail'], ['tas_innov_fail'], ['sli_innov_fail'],
-                                       ['ofx_innov_fail',
-                                        'ofy_innov_fail']], x_label='time (sec)',
+            status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos',
+                                                                               'reject_hagl'],
+                                       ['reject_mag_x', 'reject_mag_y', 'reject_mag_z',
+                                        'reject_yaw'], ['reject_airspeed'], ['reject_sideslip'],
+                                       ['reject_optflow_x',
+                                        'reject_optflow_y']], x_label='time (sec)',
             y_labels=['failed', 'failed', 'failed', 'failed', 'failed', 'failed'],
             y_lim=(-0.1, 1.1),
-            legend=[['vel NED', 'pos NE'], ['hgt absolute', 'hgt above ground'],
+            legend=[['vel NE', 'pos NE'], ['vel D', 'hgt absolute', 'hgt above ground'],
                     ['mag_x', 'mag_y', 'mag_z', 'yaw'], ['airspeed'], ['sideslip'],
                     ['flow X', 'flow Y']],
             plot_title='EKF Innovation Test Fails', annotate=False, pdf_handle=pdf_pages)
@@ -344,33 +350,33 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
         data_plot.close()
 
 
-def detect_airtime(control_mode, status_time):
+def detect_airtime(estimator_status_flags, status_flags_time):
     # define flags for starting and finishing in air
     b_starts_in_air = False
     b_finishes_in_air = False
     # calculate in-air transition time
-    if (np.amin(control_mode['airborne']) < 0.5) and (np.amax(control_mode['airborne']) > 0.5):
-        in_air_transtion_time_arg = np.argmax(np.diff(control_mode['airborne']))
-        in_air_transition_time = status_time[in_air_transtion_time_arg]
-    elif (np.amax(control_mode['airborne']) > 0.5):
-        in_air_transition_time = np.amin(status_time)
+    if (np.amin(estimator_status_flags['cs_in_air']) < 0.5) and (np.amax(estimator_status_flags['cs_in_air']) > 0.5):
+        in_air_transtion_time_arg = np.argmax(np.diff(estimator_status_flags['cs_in_air']))
+        in_air_transition_time = status_flags_time[in_air_transtion_time_arg]
+    elif (np.amax(estimator_status_flags['cs_in_air']) > 0.5):
+        in_air_transition_time = np.amin(status_flags_time)
         print('log starts while in-air at ' + str(round(in_air_transition_time, 1)) + ' sec')
         b_starts_in_air = True
     else:
         in_air_transition_time = float('NaN')
         print('always on ground')
     # calculate on-ground transition time
-    if (np.amin(np.diff(control_mode['airborne'])) < 0.0):
-        on_ground_transition_time_arg = np.argmin(np.diff(control_mode['airborne']))
-        on_ground_transition_time = status_time[on_ground_transition_time_arg]
-    elif (np.amax(control_mode['airborne']) > 0.5):
-        on_ground_transition_time = np.amax(status_time)
+    if (np.amin(np.diff(estimator_status_flags['cs_in_air'])) < 0.0):
+        on_ground_transition_time_arg = np.argmin(np.diff(estimator_status_flags['cs_in_air']))
+        on_ground_transition_time = status_flags_time[on_ground_transition_time_arg]
+    elif (np.amax(estimator_status_flags['cs_in_air']) > 0.5):
+        on_ground_transition_time = np.amax(status_flags_time)
         print('log finishes while in-air at ' + str(round(on_ground_transition_time, 1)) + ' sec')
         b_finishes_in_air = True
     else:
         on_ground_transition_time = float('NaN')
         print('always on ground')
-    if (np.amax(np.diff(control_mode['airborne'])) > 0.5) and (np.amin(np.diff(control_mode['airborne'])) < -0.5):
+    if (np.amax(np.diff(estimator_status_flags['cs_in_air'])) > 0.5) and (np.amin(np.diff(estimator_status_flags['cs_in_air'])) < -0.5):
         if ((on_ground_transition_time - in_air_transition_time) > 0.0):
             in_air_duration = on_ground_transition_time - in_air_transition_time
         else:

Tools/flash_nxp/flash_ucan.jlink

@@ -0,0 +1,7 @@
+if SWD
+speed 1000
+r
+loadbin /Users/landon/git/px4/build/nxp_ucans32k146_nxp_demo/deploy/34.bin,0x6000
+r
+g
+q

Tools/flash_nxp/flash_ucan.sh

@@ -0,0 +1,3 @@
+#!/bin/zsh
+
+JLinkExe -device S32K146 -CommandFile /Users/landon/git/px4/Tools/flash_nxp/flash_ucan.jlink

boards/nxp/fmuk66-v3/init/rc.board_defaults

@@ -9,3 +9,11 @@ param set-default BAT1_A_PER_V 15.391030303
 rgbled_pwm start
 safety_button start
 
+# pre-arm stuff
+param set-default SYS_CTRL_ALLOC 1
+param set-default COM_PREARM_MODE 2
+
+# Cyphal stuff
+ifup can0
+cyphal start
+elm_lighting_module

boards/nxp/fmuk66-v3/nxp_demo.px4board

@@ -0,0 +1,12 @@
+CONFIG_DRIVERS_UAVCAN=n
+CONFIG_DRIVERS_CYPHAL=y
+CONFIG_CYPHAL_BMS_SUBSCRIBER=y
+CONFIG_CYPHAL_ESC_SUBSCRIBER=y
+CONFIG_CYPHAL_GNSS_SUBSCRIBER_0=y
+CONFIG_CYPHAL_GNSS_SUBSCRIBER_1=y
+CONFIG_CYPHAL_READINESS_PUBLISHER=y
+CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
+CONFIG_EXAMPLES_ELM_LIGHTING_MODULE=y
+CONFIG_EXAMPLES_LIGHTING_STATES_PUBLISHER=y
+#CONFIG_MODULES_MICRODDS_CLIENT=y
+#CONFIG_MODULES_MICRORTPS_BRIDGE=y

boards/nxp/ucans32k146/default.px4board

@@ -25,3 +25,4 @@ CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y

boards/nxp/ucans32k146/init/rc.board_defaults

@@ -3,7 +3,15 @@
 # board specific defaults
 #------------------------------------------------------------------------------
 
-pwm_out mode_pwm1 start
+# pre-arm stuff
+#param set-default SYS_CTRL_ALLOC 1
+
+#pwm_out start
+
+#control_allocator start
 
 ifup can0
 cyphal start
+lighting_state_converter start
+actuator_controls_driver start
+pca9685 start -X

boards/nxp/ucans32k146/nxp_demo.px4board

@@ -0,0 +1,9 @@
+CONFIG_DRIVERS_GPS=n
+CONFIG_DRIVERS_LIGHTS_APA102=y
+CONFIG_DRIVERS_PCA9685=y
+CONFIG_DRIVERS_PCA9685_PWM_OUT=y
+CONFIG_EXAMPLES_ACTUATOR_CONTROLS_DRIVER=y
+CONFIG_MODULES_LIGHTING_STATE_CONVERTER=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_MOTOR_TEST=y
+CONFIG_SYSTEMCMDS_UORB=y

boards/nxp/ucans32k146/src/board_config.h

@@ -106,7 +106,7 @@ __BEGIN_DECLS
  *      Defined in board.h
  */
 
-#define DIRECT_PWM_OUTPUT_CHANNELS  1
+#define DIRECT_PWM_OUTPUT_CHANNELS  2
 
 
 #define BOARD_HAS_LED_PWM              1

boards/nxp/ucans32k146/src/timer_config.cpp

@@ -110,6 +110,7 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 
 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::FTM2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::FTM2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
 };
 
 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =

boards/px4/fmu-v5/nuttx-config/include/board.h

@@ -397,27 +397,15 @@
 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8]  */
 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2       /* PB9  */
 
-#define GPIO_I2C1_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8)
-#define GPIO_I2C1_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN9)
-
 #define GPIO_I2C2_SCL GPIO_I2C2_SCL_2       /* PF1 */
 #define GPIO_I2C2_SDA GPIO_I2C2_SDA_2       /* PF0 */
 
-#define GPIO_I2C2_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN1)
-#define GPIO_I2C2_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN0)
-
 #define GPIO_I2C3_SCL GPIO_I2C3_SCL_2       /* PH7 */
 #define GPIO_I2C3_SDA GPIO_I2C3_SDA_2       /* PH8 */
 
-#define GPIO_I2C3_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN7)
-#define GPIO_I2C3_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8)
-
 #define GPIO_I2C4_SCL GPIO_I2C4_SCL_2       /* PF14 */
 #define GPIO_I2C4_SDA GPIO_I2C4_SDA_2       /* PF15 */
 
-#define GPIO_I2C4_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN14)
-#define GPIO_I2C4_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN15)
-
 /* SDMMC1
  *
  *      VDD 3.3

boards/px4/fmu-v5/src/board_config.h

@@ -459,7 +459,15 @@ static inline bool board_get_external_lockout_state(void)
 		GPIO_RSSI_IN_INIT,                \
 		GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \
 		GPIO_SAFETY_SWITCH_IN,            \
-		GPIO_nARMED_INIT                  \
+		GPIO_nARMED_INIT,                  \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SDA), \
 	}
 
 #define BOARD_ENABLE_CONSOLE_BUFFER

boards/px4/fmu-v5x/nuttx-config/include/board.h

@@ -394,27 +394,15 @@
 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_1       /* PB7  */
 
-#define GPIO_I2C1_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8)
-#define GPIO_I2C1_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7)
-
 #define GPIO_I2C2_SCL GPIO_I2C2_SCL_2       /* PF1 */
 #define GPIO_I2C2_SDA GPIO_I2C2_SDA_2       /* PF0 */
 
-#define GPIO_I2C2_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN1)
-#define GPIO_I2C2_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN0)
-
 #define GPIO_I2C3_SCL GPIO_I2C3_SCL_2       /* PH7 */
 #define GPIO_I2C3_SDA GPIO_I2C3_SDA_2       /* PH8 */
 
-#define GPIO_I2C3_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN7)
-#define GPIO_I2C3_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8)
-
 #define GPIO_I2C4_SCL GPIO_I2C4_SCL_2       /* PF14 */
 #define GPIO_I2C4_SDA GPIO_I2C4_SDA_2       /* PF15 */
 
-#define GPIO_I2C4_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN14)
-#define GPIO_I2C4_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN15)
-
 /* SDMMC2
  *
  *      VDD 3.3

boards/px4/fmu-v5x/src/board_config.h

@@ -423,7 +423,15 @@
 		GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \
 		GPIO_SAFETY_SWITCH_IN,            \
 		GPIO_PG6,                         \
-		GPIO_nARMED_INIT                  \
+		GPIO_nARMED_INIT,                  \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SDA), \
 	}
 
 #define BOARD_ENABLE_CONSOLE_BUFFER

msg/CMakeLists.txt

@@ -104,6 +104,7 @@ set(msg_files
 	landing_gear.msg
 	landing_target_innovations.msg
 	landing_target_pose.msg
+	lighting_states.msg
 	led_control.msg
 	log_message.msg
 	logger_status.msg

msg/actuator_controls.msg

@@ -1,5 +1,5 @@
 uint64 timestamp			# time since system start (microseconds)
-uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROLS = 16
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
 uint8 INDEX_ROLL = 0
 uint8 INDEX_PITCH = 1
@@ -20,7 +20,7 @@ uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3
 uint8 GROUP_INDEX_PAYLOAD = 6
 
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[9] control
+float32[16] control
 
 # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc

msg/estimator_aid_source_1d.msg

@@ -19,3 +19,4 @@ bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_source_1d
+# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt

msg/led_control.msg

@@ -13,6 +13,7 @@ uint8 COLOR_PURPLE = 5
 uint8 COLOR_AMBER = 6
 uint8 COLOR_CYAN = 7
 uint8 COLOR_WHITE = 8
+uint8 COLOR_DIM_RED = 9
 
 # LED modes definitions
 uint8 MODE_OFF = 0 # turn LED off
@@ -27,11 +28,11 @@ uint8 MODE_FLASH = 7 # two fast blinks (on/off) with timing as in MODE_BLINK_FAS
 uint8 MAX_PRIORITY = 2 # maxium priority (minimum is 0)
 
 
-uint8 led_mask # bitmask which LED(s) to control, set to 0xff for all
+uint16 led_mask # bitmask which LED(s) to control, set to 0xff for all
 uint8 color # see COLOR_*
 uint8 mode # see MODE_*
 uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite
                  # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20
 uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY)
 
-uint8 ORB_QUEUE_LENGTH = 8      # needs to match BOARD_MAX_LEDS
+uint8 ORB_QUEUE_LENGTH = 16      # needs to match BOARD_MAX_LEDS

msg/led_control.msg.old

@@ -0,0 +1,37 @@
+# LED control: control a single or multiple LED's.
+# These are the externally visible LED's, not the board LED's
+
+uint64 timestamp		# time since system start (microseconds)
+
+# colors
+uint8 COLOR_OFF = 0 # this is only used in the drivers
+uint8 COLOR_RED = 1
+uint8 COLOR_GREEN = 2
+uint8 COLOR_BLUE = 3
+uint8 COLOR_YELLOW = 4
+uint8 COLOR_PURPLE = 5
+uint8 COLOR_AMBER = 6
+uint8 COLOR_CYAN = 7
+uint8 COLOR_WHITE = 8
+
+# LED modes definitions
+uint8 MODE_OFF = 0 # turn LED off
+uint8 MODE_ON = 1  # turn LED on
+uint8 MODE_DISABLED = 2  # disable this priority (switch to lower priority setting)
+uint8 MODE_BLINK_SLOW = 3
+uint8 MODE_BLINK_NORMAL = 4
+uint8 MODE_BLINK_FAST = 5
+uint8 MODE_BREATHE = 6 # continuously increase & decrease brightness (solid color if driver does not support it)
+uint8 MODE_FLASH = 7 # two fast blinks (on/off) with timing as in MODE_BLINK_FAST and then off for a while
+
+uint8 MAX_PRIORITY = 2 # maxium priority (minimum is 0)
+
+
+uint8 led_mask # bitmask which LED(s) to control, set to 0xff for all
+uint8 color # see COLOR_*
+uint8 mode # see MODE_*
+uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite
+                 # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20
+uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY)
+
+uint8 ORB_QUEUE_LENGTH = 8      # needs to match BOARD_MAX_LEDS

msg/lighting_states.msg

@@ -0,0 +1,17 @@
+# LED States for El Mandadero and NXP Buggy3
+
+uint64 timestamp
+
+uint8 IDLE     = 0 # Dim Red
+uint8 BRAKE    = 1 # Bright Red
+uint8 REVERSE  = 2 # [Top] White
+uint8 TURN     = 3 # [Top] Flashing Yellow
+uint8 HAZARD   = 4 # Flashing Yellow
+uint8 NO_STATE = 255 # No state update
+
+# LED Panel IDs
+# 0 - Right Front
+# 1 - Left Front
+# 2 - Left Rear
+# 3 - Right Rear
+uint8[10] state

platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h

@@ -45,8 +45,8 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#define MAX_IO_TIMERS			1
-#define MAX_TIMER_IO_CHANNELS		1
+#define MAX_IO_TIMERS			2
+#define MAX_TIMER_IO_CHANNELS		2
 
 #define MAX_LED_TIMERS			1
 #define MAX_TIMER_LED_CHANNELS		3

src/drivers/cyphal/ParamManager.hpp

@@ -116,7 +116,7 @@ public:
 private:
 
 
-	const UavcanParamBinder _uavcan_params[13] {
+	const UavcanParamBinder _uavcan_params[15] {
 		{"uavcan.pub.esc.0.id",                "UCAN1_ESC_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.servo.0.id",              "UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.gps.0.id",                "UCAN1_GPS_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
@@ -130,6 +130,8 @@ private:
 		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.uorb.lighting_states.0.id",    "UCAN1_UORB_LS_S",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.uorb.lighting_states.0.id",    "UCAN1_UORB_LS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};

src/drivers/cyphal/PublicationManager.hpp

@@ -64,13 +64,18 @@
 #define CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER 0
 #endif
 
+#ifndef CONFIG_CYPHAL_UORB_LIGHTING_STATES_PUBLISHER
+#define CONFIG_CYPHAL_UORB_LIGHTING_STATES_PUBLISHER 0
+#endif
+
 /* Preprocessor calculation of publisher count */
 
 #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
 	CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_READINESS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
-	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
+	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER + \
+	CONFIG_CYPHAL_UORB_LIGHTING_STATES_PUBLISHER
 
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
@@ -159,6 +164,16 @@ private:
 			"uorb.sensor_gps",
 			0
 		},
+#endif
+#if CONFIG_CYPHAL_UORB_LIGHTING_STATES_PUBLISHER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new uORB_over_UAVCAN_Publisher<lighting_states_s>(handle, pmgr, ORB_ID(lighting_states));
+			},
+			"uorb.lighting_states",
+			0
+		},
 #endif
 	};
 };

src/drivers/cyphal/SubscriptionManager.hpp

@@ -61,13 +61,18 @@
 #define CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER 0
 #endif
 
+#ifndef CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER
+#define CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER 0
+#endif
+
 /* Preprocessor calculation of Subscribers count */
 
 #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
 	CONFIG_CYPHAL_BMS_SUBSCRIBER + \
-	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
+	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER + \
+	CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER
 
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
@@ -179,6 +184,16 @@ private:
 			"uorb.sensor_gps",
 			0
 		},
+#endif
+#if CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER
+		{
+			[](CanardInstance & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new uORB_over_UAVCAN_Subscriber<lighting_states_s>(handle, pmgr, ORB_ID(lighting_states));
+			},
+			"uorb.lighting_states",
+			0
+		},
 #endif
 	};
 };

src/drivers/cyphal/parameters.c

@@ -188,3 +188,21 @@ PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1);
  * @group Cyphal
  */
 PARAM_DEFINE_INT32(UCAN1_ACTR_PUB, -1);
+
+/**
+ * lighting_states uORB over Cyphal publication port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_UORB_LS_P, -1);
+
+/**
+ * lighting_states uORB over Cyphal publication port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_UORB_LS_S, -1);

src/drivers/drv_apa102.h

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_apa102.h
+ *
+ * APA102 LED driver interface.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+#include <systemlib/px4_macros.h>
+
+namespace apa102
+{
+class APA102LEDData
+{
+public:
+	enum eRGB {
+		eB = 0,
+		eR = 1,
+		eG = 2
+	};
+
+	typedef union {
+		uint8_t  grb[3];
+		uint32_t l;
+	} led_data_t;
+
+	led_data_t  data{};
+	APA102LEDData() {data.l = 0;}
+	APA102LEDData(APA102LEDData &r) {data.l = r.data.l;}
+
+	uint8_t &R() {return data.grb[eR];};
+	uint8_t &G() {return data.grb[eG];};
+	uint8_t &B() {return data.grb[eB];};
+};
+};

src/drivers/extreme_switch/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__extreme_switch
+	MAIN extreme_switch
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
+		#-O0             # uncomment when debugging
+	SRCS
+		ExtremeSwitch.cpp
+		ExtremeSwitch.hpp
+	DEPENDS
+		px4_work_queue
+	)

src/drivers/extreme_switch/ExtremeSwitch.cpp

@@ -0,0 +1,222 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ExtremeSwitch.hpp"
+
+ExtremeSwitch::ExtremeSwitch(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
+	SPI(DRV_DEVTYPE_UNUSED, MODULE_NAME, bus, device, spi_mode, bus_frequency),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
+{
+}
+
+ExtremeSwitch::~ExtremeSwitch()
+{
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
+}
+
+int ExtremeSwitch::init()
+{
+	/* Let's not do this yet
+	// execute Run() on every sensor_accel publication
+	if (!_sensor_accel_sub.registerCallback()) {
+		PX4_ERR("sensor_accel callback registration failed");
+		return false;
+	}
+	*/
+
+	int ret = SPI::init();
+
+	if(ret != OK) {
+		printf("SPI::init() failed\n");
+		DEVICE_DEBUG("SPI Init Failed");
+		return -EIO;
+	}
+
+	// alternatively, Run on fixed interval
+	ScheduleOnInterval(10000_us); // 2000 us interval, 200 Hz rate
+
+	return PX4_OK;
+}
+
+void ExtremeSwitch::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+	perf_count(_loop_interval_perf);
+	
+	uint8_t buf;
+	uint8_t rbuf;
+
+	printf("Starting loop\n\n");
+
+
+	printf("Transferring OCR register \n");
+	buf = 0b00000001;
+	transfer(&buf, &rbuf, sizeof(uint8_t));
+	printf("rbuf: 0x%X\n", rbuf);
+
+	px4_usleep(500000);
+
+	printf("Transferring ON command \n");
+	buf = 0b00010001;
+	transfer(&buf, &rbuf, sizeof(uint8_t));
+	printf("rbuf: 0x%X\n", rbuf);
+	
+	px4_usleep(100000);
+	
+	printf("Transferring OFF command \n");
+	buf = 0b00010000;
+	transfer(&buf, &rbuf, sizeof(uint8_t));
+	printf("rbuf: 0x%X\n", rbuf);
+
+	printf("Ending loop\n\n");
+
+	/*
+	 * Just send a few pulses to enable the switch
+	 * Wait a while between pulses
+	 */
+	/*
+	// Example
+	//  update vehicle_status to check arming state
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+			if (armed && !_armed) {
+				PX4_WARN("vehicle armed due to %d", vehicle_status.latest_arming_reason);
+
+			} else if (!armed && _armed) {
+				PX4_INFO("vehicle disarmed due to %d", vehicle_status.latest_disarming_reason);
+			}
+
+			_armed = armed;
+		}
+	}
+
+
+	// Example
+	//  grab latest accelerometer data
+	if (_sensor_accel_sub.updated()) {
+		sensor_accel_s accel;
+
+		if (_sensor_accel_sub.copy(&accel)) {
+			// DO WORK
+
+			// access parameter value (SYS_AUTOSTART)
+			if (_param_sys_autostart.get() == 1234) {
+				// do something if SYS_AUTOSTART is 1234
+			}
+		}
+	}
+
+
+	// Example
+	//  publish some data
+	orb_test_s data{};
+	data.val = 314159;
+	data.timestamp = hrt_absolute_time();
+	_orb_test_pub.publish(data);
+	*/
+
+
+	perf_end(_loop_perf);
+}
+
+int ExtremeSwitch::task_spawn(int argc, char *argv[])
+{
+	ExtremeSwitch *instance = new ExtremeSwitch(1, 0x10000000, 1000000, SPIDEV_MODE1);
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init() == PX4_OK) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int ExtremeSwitch::print_status()
+{
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_loop_interval_perf);
+	return 0;
+}
+
+int ExtremeSwitch::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int ExtremeSwitch::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Example of a simple module running out of a work queue.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("extreme_switch", "template");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int extreme_switch_main(int argc, char *argv[])
+{
+	return ExtremeSwitch::main(argc, argv);
+}

src/drivers/extreme_switch/ExtremeSwitch.hpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include <drivers/device/spi.h>
+
+using namespace time_literals;
+
+class ExtremeSwitch : public device::SPI, public px4::ScheduledWorkItem, public ModuleBase<ExtremeSwitch>
+{
+public:
+	ExtremeSwitch(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+	~ExtremeSwitch() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	int init();
+
+	int print_status() override;
+
+private:
+	void Run() override;
+
+	/*
+	// Publications
+	uORB::Publication<orb_test_s> _orb_test_pub{ORB_ID(orb_test)};
+
+	// Subscriptions
+	uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)};        // subscription that schedules ExtremeSwitch when updated
+	uORB::Subscription                 _vehicle_status_sub{ORB_ID(vehicle_status)};          // regular subscription for additional data
+	*/
+	
+
+	// Performance (perf) counters
+	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+
+	// Parameters
+
+
+	bool _armed{false};
+};

src/drivers/extreme_switch/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_EXTREME_SWITCH
+	bool "extreme_switch"
+	default n
+	---help---
+		Enable support for extreme_switch

src/drivers/lights/apa102/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__apa102
+	MAIN apa102
+	SRCS
+		apa102.cpp
+	DEPENDS
+		led
+	)

src/drivers/lights/apa102/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_LIGHTS_APA102
+	bool "apa102"
+	default n
+	---help---
+		Enable support for apa102

src/drivers/lights/apa102/apa102.cpp

@@ -0,0 +1,275 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file apa102.cpp
+ *
+ * Driver for the the apa102 class of RGB LED drivers.
+ * this driver is based on the PX4 led driver
+ *
+ */
+
+#include "apa102.hpp"
+
+// Constructor
+APA102::APA102(unsigned int number_of_packages, int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
+	SPI(DRV_DEVTYPE_UNUSED, MODULE_NAME, bus, device, spi_mode, bus_frequency),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
+	_number_of_packages(number_of_packages)
+{
+}
+
+// Deconstructor
+APA102::~APA102()
+{
+	//TODO: deinit SPI bus
+	//apa102_deinit();
+}
+
+// Init
+// This runs after calling the constructor
+int APA102::init()
+{
+	// Init SPI bus
+	int ret = SPI::init();
+
+	if (ret != OK) {
+		printf("SPI::init() failed\n");
+		DEVICE_DEBUG("SPI init failed");
+		return -EIO;
+	}
+
+	// Fill buffer with zeros
+	for (uint8_t i = 0; i < (_number_of_packages * 4) + 8; i++) {
+		buf[i] = 0x00;
+	}
+
+	// APA102LEDData is just the data format for the BRG LED
+	_leds = new apa102::APA102LEDData [_number_of_packages];
+
+	if (_leds == nullptr) {
+		return PX4_ERROR;
+	}
+
+	ScheduleNow();
+	return OK;
+}
+
+//
+int APA102::task_spawn(int argc, char *argv[])
+{
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	unsigned int number_of_packages = BOARD_MAX_LEDS;
+
+	while ((ch = px4_getopt(argc, argv, "n:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'n':
+			number_of_packages = atoi(myoptarg) + 1;
+
+			if (number_of_packages > BOARD_MAX_LEDS) {
+				number_of_packages = BOARD_MAX_LEDS;
+				PX4_INFO("Number of packages can not exceed BOARD_MAX_LEDS");
+			}
+
+			break;
+
+		default:
+			print_usage("unrecognized option");
+			return 1;
+		}
+	}
+
+	printf("Number of packages: %d\n", number_of_packages);
+	APA102 *instance = new APA102(number_of_packages, 1, 0, 4000000, SPIDEV_MODE0);
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init() == PX4_OK) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+	return PX4_ERROR;
+}
+
+int APA102::print_status()
+{
+
+	PX4_INFO("Controlling %i LEDs", _number_of_packages);
+
+	return 0;
+}
+
+int APA102::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module is responsible for driving interfasing to the Neopixel Serial LED
+
+### Examples
+It is typically started with:
+$ apa102 -n 8
+To drive all available leds.
+)DESCR_STR");
+
+PRINT_MODULE_USAGE_NAME("newpixel", "driver");
+PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+return 0;
+}
+
+int APA102::custom_command(int argc, char *argv[])
+{
+   return print_usage("unrecognized option");
+}
+
+/**
+ * Main loop function
+ * This will run periodically by the scheduler
+ */
+void APA102::Run()
+{
+  if (should_exit()) {
+      ScheduleClear();
+      exit_and_cleanup();
+      return;
+    }
+
+    // Structure for uORB data
+	LedControlData led_control_data;
+
+	// Get LED data from uORB (led_control)
+	if (_led_controller.update(led_control_data) == 1) {
+
+		// Loop through each LED
+	    for (unsigned int led = 1; led < math::min(_number_of_packages, arraySize(led_control_data.leds)); led++) {
+
+	    	// Set brightness
+	        uint8_t brightness = led_control_data.leds[led].brightness;
+	        
+	        /* Brightness is not 0-255, it is 5 bit, so 0-31. 256/32=8 */
+	        brightness = brightness / 8;
+	        //printf("BRIGHTNESS: %d\n", brightness);
+	        //printf("arraySize(%d)\n", arraySize(led_control_data.leds));
+
+	        // Use data from uORB to set specific APA102LEDData fields
+	        switch (led_control_data.leds[led].color) {
+	          case led_control_s::COLOR_RED:
+	            _leds[led].R() = 255; _leds[led].G() = 0; _leds[led].B() = 0;
+	            break;
+
+	          case led_control_s::COLOR_DIM_RED:
+	            _leds[led].R() = 16; _leds[led].G() = 0; _leds[led].B() = 0;
+	            break;
+
+	          case led_control_s::COLOR_GREEN:
+	            _leds[led].R() = 0; _leds[led].G() = 255; _leds[led].B() = 0;
+	            break;
+
+	          case led_control_s::COLOR_BLUE:
+	            _leds[led].R() = 0; _leds[led].G() = 0; _leds[led].B() = 255;
+	            break;
+
+	          case led_control_s::COLOR_AMBER: //make it the same as yellow
+	          case led_control_s::COLOR_YELLOW:
+	            _leds[led].R() = 255; _leds[led].G() = 255; _leds[led].B() = 0;
+	            break;
+
+	          case led_control_s::COLOR_PURPLE:
+	            _leds[led].R() = 255; _leds[led].G() = 0; _leds[led].B() = 255;
+	            break;
+
+	          case led_control_s::COLOR_CYAN:
+	            _leds[led].R() = 0; _leds[led].G() = 255; _leds[led].B() = 255;
+	            break;
+
+	          case led_control_s::COLOR_WHITE:
+	            _leds[led].R() = 255; _leds[led].G() = 255; _leds[led].B() = 255;
+	            break;
+
+	          default: // led_control_s::COLOR_OFF
+	            _leds[led].R() = 0; _leds[led].G() = 0; _leds[led].B() = 0;
+	            break;
+	        } // end switch
+
+			/* APA102 Frame
+			 * 0x000000000 [start frame] (this is already done in init)
+			 * 0xE0 + [brightness]
+			 * 0xXXXXXX [bgr]
+			 * 0xFFFFFFFF [end frame]
+			 */
+
+			for(uint8_t i = 1; i < _number_of_packages; i++)
+			{
+				buf[4+(4*(i-1))] = 0b11100000 + brightness;
+				buf[5+(4*(i-1))] = _leds[i].B();
+				buf[6+(4*(i-1))] = _leds[i].G();
+				buf[7+(4*(i-1))] = _leds[i].R();
+			} // end FOR loop
+
+			// Fill with end frame
+			buf[8+(4*_number_of_packages)] = 0xFF;
+			buf[9+(4*_number_of_packages)] = 0xFF;
+			buf[10+(4*_number_of_packages)] = 0xFF;
+			buf[11+(4*_number_of_packages)] = 0xFF;
+
+		} // end FOR loop
+
+		transfer(buf, rbuf, (_number_of_packages * 4) + 8);
+
+	} // end IF statement
+
+	ScheduleDelayed(_led_controller.maximum_update_interval());
+} // end FUNCTION
+
+// Main function
+// This runs when calling the command on the command line
+extern "C" __EXPORT int apa102_main(int argc, char *argv[])
+{
+	// This calls task_spawn
+  return APA102::main(argc, argv);
+}

src/drivers/lights/apa102/apa102.hpp

@@ -0,0 +1,54 @@
+#include <string.h>
+#include <px4_platform_common/px4_config.h>
+
+#include <lib/led/led.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <drivers/drv_apa102.h>
+#include <drivers/device/spi.h>
+
+class APA102 : public device::SPI, public px4::ScheduledWorkItem, public ModuleBase<APA102>
+{
+public:
+	APA102(unsigned int number_of_packages, int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+	virtual ~APA102();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	void Run() override;
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+
+	int			init();
+	int			status();
+
+private:
+	// We don't want to use BOARD_HAS_N_S_RGB_LED, SPI leds are external
+	unsigned int _number_of_packages;
+
+	uint8_t buf[(BOARD_MAX_LEDS * 4) + 8];
+	uint8_t rbuf[(BOARD_MAX_LEDS * 4) + 8];
+
+	// LedController is a driver that takes care of LEDs using the led_control ORB msg.
+	// Do we really want to use this?
+	LedController		_led_controller;
+
+	// ??
+	APA102(const APA102 &) = delete;
+	APA102 operator=(const APA102 &) = delete;
+
+	// What is this?
+	// _leds is the actual buffer I believe
+	apa102::APA102LEDData *_leds;
+};

src/drivers/pca9685/pca9685.cpp

@@ -93,18 +93,21 @@
 
 #define ADDR 0x40	// I2C adress
 
-#define PCA9685_PWMFREQ 60.0f
+#define PCA9685_PWMFREQ 400
 #define PCA9685_NCHANS 16 // total amount of pwm outputs
 
-#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096)
-#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f
+//#define PCA9685_PWMINT (1.0f / PCA9685_PWMFREQ) * 1000000
 
-#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2)
+#define PCA9685_PWMMIN (1000 / PCA9685_PWMINT) * 4096.0f // this is the 'minimum' pulse length count (out of 4096)
+#define PCA9685_PWMMAX (2000 / PCA9685_PWMINT) * 4096.0f // this is the 'maximum' pulse length count (out of 4096)
+
+#define PCA9685_PWMCENTER 308 //(PCA9685_PWMMAX + PCA9685_PWMMIN)/2
 #define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees
 				     PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG
 				     PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG
 				     */
-#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM
+#define PCA9685_SCALE (PCA9685_PWMMAX - PCA9685_PWMMIN) / 2 // scales from rad to PWM
+
 
 enum IOX_MODE {
 	IOX_MODE_ON,
@@ -232,7 +235,7 @@ PCA9685::RunImpl()
 			/* Subscribe to actuator control 2 (payload group for gimbal) */
 			_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
 			/* set the uorb update interval lower than the driver pwm interval */
-			orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);
+			orb_set_interval(_actuator_controls_sub, 1.05f);
 
 			_mode_on_initialized = true;
 		}
@@ -243,18 +246,40 @@ PCA9685::RunImpl()
 
 		if (updated) {
 			orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
+			//PX4_INFO("freq: %.2f, int: %.2f, max: %d, min: %d, pwmcenter: %d, control: %.2f, scale: %d", (double)PCA9685_PWMFREQ, (double)PCA9685_PWMINT, PCA9685_PWMMAX, PCA9685_PWMMIN,
+			// 	PCA9685_PWMCENTER, (double)_actuator_controls.control[0], PCA9685_SCALE);
+
+			/*
+			#define PCA9685_PWMFREQ 50.0f
+			#define PCA9685_NCHANS 16 // total amount of pwm outputs
+
+			//#define PCA9685_PWMINT (1.0f / PCA9685_PWMFREQ) * 1000000
+
+			#define PCA9685_PWMMIN (1000 / PCA9685_PWMINT) * 4096.0f // this is the 'minimum' pulse length count (out of 4096)
+			#define PCA9685_PWMMAX (2000 / PCA9685_PWMINT) * 4096.0f // this is the 'maximum' pulse length count (out of 4096)
+
+			#define PCA9685_PWMCENTER 308 //(PCA9685_PWMMAX + PCA9685_PWMMIN)/2
+
+			#define PCA9685_SCALE (PCA9685_PWMMAX - PCA9685_PWMMIN) / 2 // scales from rad to PWM
+			*/
+
+			float pwm_int = (1.0f / PCA9685_PWMFREQ) * 1000000;
+			float pwm_min = (1000 / pwm_int) * 4096;
+			float pwm_max = (2000 / pwm_int) * 4096;
+			float pwm_center = (pwm_max + pwm_min) / 2;
+			float pwm_scale = (pwm_max - pwm_min) / 2;
 
 			for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
 				/* Scale the controls to PWM, first multiply by pi to get rad,
 				 * the control[i] values are on the range -1 ... 1 */
-				uint16_t new_value = PCA9685_PWMCENTER +
-						     (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
-				DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
-					     (double)_actuator_controls.control[i]);
+				uint16_t new_value = pwm_center +
+						     (_actuator_controls.control[i] * pwm_scale);
+				//PX4_INFO("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
+				//	     (double)_actuator_controls.control[i]);
 
 				if (new_value != _current_values[i] &&
-				    new_value >= PCA9685_PWMMIN &&
-				    new_value <= PCA9685_PWMMAX) {
+				    new_value >= pwm_min &&
+				    new_value <= pwm_max) {
 					/* This value was updated, send the command to adjust the PWM value */
 					setPin(i, new_value);
 					_current_values[i] = new_value;
@@ -330,8 +355,8 @@ int
 PCA9685::setPWMFreq(float freq)
 {
 	int ret  = OK;
-	freq *= 0.9f;  /* Correct for overshoot in the frequency setting (see issue
-		https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */
+	//freq *= 0.9f;  /* Correct for overshoot in the frequency setting (see issue
+		//https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */
 	float prescaleval = 25000000;
 	prescaleval /= 4096;
 	prescaleval /= freq;

src/examples/actuator_controls_driver/ActuatorControlsDriver.cpp

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ActuatorControlsDriver.hpp"
+
+ActuatorControlsDriver::ActuatorControlsDriver() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
+{
+}
+
+ActuatorControlsDriver::~ActuatorControlsDriver()
+{
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
+}
+
+bool ActuatorControlsDriver::init()
+{
+	// execute Run() on every sensor_accel publication
+	/*
+	if (!_sensor_accel_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+	*/
+	// alternatively, Run on fixed interval
+	ScheduleOnInterval(500_ms); // 2000 us interval, 200 Hz rate
+
+	return true;
+}
+
+void ActuatorControlsDriver::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+	perf_count(_loop_interval_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+		updateParams(); // update module parameters (in DEFINE_PARAMETERS)
+	}
+
+
+	// Example
+	//  update vehicle_status to check arming state
+	/*
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+			if (armed && !_armed) {
+				PX4_WARN("vehicle armed due to %d", vehicle_status.latest_arming_reason);
+
+			} else if (!armed && _armed) {
+				PX4_INFO("vehicle disarmed due to %d", vehicle_status.latest_disarming_reason);
+			}
+
+			_armed = armed;
+		}
+	}
+	*/
+
+	// Example
+	//  grab latest accelerometer data
+	/*
+	if (_sensor_accel_sub.updated()) {
+		sensor_accel_s accel;
+
+		if (_sensor_accel_sub.copy(&accel)) {
+			// DO WORK
+
+			// access parameter value (SYS_AUTOSTART)
+			if (_param_sys_autostart.get() == 1234) {
+				// do something if SYS_AUTOSTART is 1234
+			}
+		}
+	}
+	*/
+
+	// Example
+	//  publish some data
+	if(loop > 10) loop = -10;
+
+	for(int i = 0; i < 16; i++) {
+		actuator_controls.control[i] = 0 + (loop * 0.1);
+	}
+
+	actuator_controls.timestamp = hrt_absolute_time();
+	_actuator_controls_pub.publish(actuator_controls);
+
+	loop = loop + 1;
+
+	perf_end(_loop_perf);
+}
+
+int ActuatorControlsDriver::task_spawn(int argc, char *argv[])
+{
+	ActuatorControlsDriver *instance = new ActuatorControlsDriver();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int ActuatorControlsDriver::print_status()
+{
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_loop_interval_perf);
+	return 0;
+}
+
+int ActuatorControlsDriver::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int ActuatorControlsDriver::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Example of a simple module running out of a work queue.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("actuator_controls_driver", "template");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int actuator_controls_driver_main(int argc, char *argv[])
+{
+	return ActuatorControlsDriver::main(argc, argv);
+}

src/examples/actuator_controls_driver/ActuatorControlsDriver.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/vehicle_status.h>
+
+using namespace time_literals;
+
+class ActuatorControlsDriver : public ModuleBase<ActuatorControlsDriver>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	ActuatorControlsDriver();
+	~ActuatorControlsDriver() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	int print_status() override;
+
+private:
+	void Run() override;
+
+	// Publications
+	uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_2)};
+
+	// Subscriptions
+
+	//uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)};        // subscription that schedules ActuatorControlsDriver when updated
+	uORB::SubscriptionInterval         _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
+	//uORB::Subscription                 _vehicle_status_sub{ORB_ID(vehicle_status)};          // regular subscription for additional data
+
+
+	// Performance (perf) counters
+	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */
+		(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */
+	)
+
+	actuator_controls_s actuator_controls;
+	int loop = 0;
+	//bool _armed{false};
+};

src/examples/actuator_controls_driver/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE examples__actuator_controls_driver
+	MAIN actuator_controls_driver
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
+		#-O0             # uncomment when debugging
+	SRCS
+		ActuatorControlsDriver.cpp
+		ActuatorControlsDriver.hpp
+	DEPENDS
+		px4_work_queue
+	)

src/examples/actuator_controls_driver/Kconfig

@@ -0,0 +1,5 @@
+menuconfig EXAMPLES_ACTUATOR_CONTROLS_DRIVER
+	bool "actuator_controls_driver"
+	default n
+	---help---
+		Enable support for actuator_controls_driver

src/examples/elm_lighting_module/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE examples__elm_lighting_module
+	MAIN elm_lighting_module
+	SRCS
+		elm_lighting_module.c
+	DEPENDS
+	)

src/examples/elm_lighting_module/Kconfig

@@ -0,0 +1,5 @@
+menuconfig EXAMPLES_ELM_LIGHTING_MODULE
+	bool "elm_lighting_module"
+	default n
+	---help---
+		Enable support for elm_lighting_module

src/examples/elm_lighting_module/elm_lighting_module.c

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file elm_lighting_module.c
+ * Minimal application example for PX4 autopilot
+ *
+ * @author Landon Haugh <landon.haugh@nxp.com>
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/posix.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <string.h>
+#include <math.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/lighting_states.h>
+
+__EXPORT int elm_lighting_module_main(int argc, char *argv[]);
+
+int elm_lighting_module_main(int argc, char *argv[])
+{
+	PX4_INFO("Hello Sky!");
+
+	/* advertise lighting_states topic */
+	struct lighting_states_s lighting_states;
+	memset(&lighting_states, 255, sizeof(lighting_states));
+	orb_advert_t lighting_states_pub = orb_advertise(ORB_ID(lighting_states), &lighting_states);
+
+/*
+	for (int i = 0; i < 8; i++) {
+		lighting_states.state[i] = 255;
+	}
+*/
+    for(int i = 0; i < 200; i++) {
+        px4_usleep(2000000);
+
+        // Idle state
+        printf("Updating state to IDLE\n");
+        lighting_states.state[0] = 0;
+        lighting_states.state[1] = 0;
+        lighting_states.state[2] = 0;
+        lighting_states.state[3] = 0;
+        lighting_states.state[4] = 2;
+        lighting_states.state[5] = 2;
+        lighting_states.state[6] = 2;
+        lighting_states.state[7] = 2;
+        lighting_states.timestamp = hrt_absolute_time();
+        orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states);
+        px4_usleep(2000000);
+        
+
+        // Braking state (NO OTHER STATES)
+        printf("Updating state to BRAKE\n");
+        lighting_states.state[0] = 1;
+        lighting_states.state[1] = 1;
+        lighting_states.state[2] = 1;
+        lighting_states.state[3] = 1;
+        lighting_states.state[4] = 2;
+        lighting_states.state[5] = 2;
+        lighting_states.state[6] = 2;
+        lighting_states.state[7] = 2;
+        lighting_states.timestamp = hrt_absolute_time();
+        orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states);
+        px4_usleep(2000000); 
+        
+
+        // Idle state while reversing
+        printf("Updating state to IDLE/REVERSE\n");
+        lighting_states.state[0] = 0;
+        lighting_states.state[1] = 2;
+        lighting_states.state[2] = 0;
+        lighting_states.state[3] = 2;
+        lighting_states.state[4] = 2;
+        lighting_states.state[5] = 2;
+        lighting_states.state[6] = 2;
+        lighting_states.state[7] = 2;
+        lighting_states.timestamp = hrt_absolute_time();
+        orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states);
+        px4_usleep(2000000);
+        
+
+        // Braking state while reversing
+        printf("Updating state to BRAKE/REVERSE\n");
+        lighting_states.state[0] = 1;
+        lighting_states.state[1] = 2;
+        lighting_states.state[2] = 1;
+        lighting_states.state[3] = 2;
+        lighting_states.state[4] = 2;
+        lighting_states.state[5] = 2;
+        lighting_states.state[6] = 2;
+        lighting_states.state[7] = 2;
+        lighting_states.timestamp = hrt_absolute_time();
+        orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states);
+        px4_usleep(2000000);
+        
+
+        // Idle state while turning
+        printf("Updating state to IDLE/TURN\n");
+        lighting_states.state[0] = 0;
+        lighting_states.state[1] = 3;
+        lighting_states.state[2] = 0;
+        lighting_states.state[3] = 0;
+        lighting_states.state[4] = 2;
+        lighting_states.state[5] = 3;
+        lighting_states.state[6] = 2;
+        lighting_states.state[7] = 2;
+        lighting_states.timestamp = hrt_absolute_time();
+        orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states);
+        px4_usleep(2000000);
+        
+
+        // Braking state while turning
+        printf("Updating state to BRAKE/TURN\n");
+        lighting_states.state[0] = 0;
+        lighting_states.state[1] = 0;
+        lighting_states.state[2] = 0;
+        lighting_states.state[3] = 3;
+        lighting_states.state[4] = 2;
+        lighting_states.state[5] = 2;
+        lighting_states.state[6] = 2;
+        lighting_states.state[7] = 3;
+        lighting_states.timestamp = hrt_absolute_time();
+        orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states);
+        px4_usleep(2000000);
+        
+
+        // Return to Idle
+        printf("Updating state to IDLE\n");
+        lighting_states.state[0] = 0;
+        lighting_states.state[1] = 0;
+        lighting_states.state[2] = 0;
+        lighting_states.state[3] = 0;
+        lighting_states.state[4] = 2;
+        lighting_states.state[5] = 2;
+        lighting_states.state[6] = 2;
+        lighting_states.state[7] = 2;
+        lighting_states.timestamp = hrt_absolute_time();
+        orb_publish(ORB_ID(lighting_states), lighting_states_pub, &lighting_states);
+        px4_usleep(2000000);
+    }
+
+	PX4_INFO("exiting");
+
+	return 0;
+}

src/lib/battery/battery_params_deprecated.c

@@ -43,7 +43,7 @@
  * @group Battery Calibration
  * @category system
  */
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.6f);
 
 /**
  * This parameter is deprecated. Please use BAT1_V_CHARGED instead.

src/lib/battery/module.yaml

@@ -21,7 +21,7 @@ parameters:
             reboot_required: true
             num_instances: *max_num_config_instances
             instance_start: 1
-            default: [3.5, 3.5]
+            default: [3.6, 3.6]
 
         BAT${i}_V_CHARGED:
             description:

src/modules/commander/Commander.hpp

@@ -391,16 +391,16 @@ private:
 
 	geofence_result_s	_geofence_result{};
 	vehicle_land_detected_s	_vehicle_land_detected{};
-	vtol_vehicle_status_s	_vtol_status{};
+	vtol_vehicle_status_s	_vtol_vehicle_status{};
 
 	hrt_abstime _last_wind_warning{0};
 
 	// commander publications
-	actuator_armed_s        _armed{};
-	commander_state_s       _internal_state{};
+	actuator_armed_s        _actuator_armed{};
+	commander_state_s       _commander_state{};
 	vehicle_control_mode_s  _vehicle_control_mode{};
-	vehicle_status_s        _status{};
-	vehicle_status_flags_s  _status_flags{};
+	vehicle_status_s        _vehicle_status{};
+	vehicle_status_flags_s  _vehicle_status_flags{};
 
 	Safety _safety{};
 
@@ -442,18 +442,18 @@ private:
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
 
 	// Publications
-	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};
+	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
 	uORB::Publication<commander_state_s>			_commander_state_pub{ORB_ID(commander_state)};
 	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
 	uORB::Publication<test_motor_s>				_test_motor_pub{ORB_ID(test_motor)};
 	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
-	uORB::Publication<vehicle_control_mode_s>		_control_mode_pub{ORB_ID(vehicle_control_mode)};
+	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
 	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
-	uORB::Publication<vehicle_status_s>			_status_pub{ORB_ID(vehicle_status)};
+	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};
 
-	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};
+	uORB::PublicationData<home_position_s>			_home_position_pub{ORB_ID(home_position)};
 
-	uORB::Publication<vehicle_command_ack_s>		_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	orb_advert_t _mavlink_log_pub{nullptr};
 

src/modules/ekf2/EKF/control.cpp

@@ -125,8 +125,8 @@ void Ekf::controlFusionModes()
 
 	if (_range_buffer) {
 		// Get range data from buffer and check validity
-		bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
-		_range_sensor.setDataReadiness(is_rng_data_ready);
+		_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
+		_range_sensor.setDataReadiness(_rng_data_ready);
 
 		// update range sensor angle parameters in case they have changed
 		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
@@ -779,16 +779,16 @@ void Ekf::checkVerticalAccelerationHealth()
 	bool are_vertical_pos_and_vel_independant = false;
 
 	if (_control_status.flags.gps) {
-		// velocity
-		auto &gps_vel = _aid_src_gnss_vel;
+		// GNSS velocity
+		const auto &gps_vel = _aid_src_gnss_vel;
 
 		if (gps_vel.time_last_fuse[2] > _vert_vel_fuse_time_us) {
 			_vert_vel_fuse_time_us = gps_vel.time_last_fuse[2];
 			_vert_vel_innov_ratio = gps_vel.innovation[2] / sqrtf(gps_vel.innovation_variance[2]);
 		}
 
-		// position
-		auto &gps_pos = _aid_src_gnss_pos;
+		// GNSS position
+		const auto &gps_pos = _aid_src_gnss_pos;
 
 		if (gps_pos.time_last_fuse[2] > _vert_pos_fuse_attempt_time_us) {
 			_vert_pos_fuse_attempt_time_us = gps_pos.time_last_fuse[2];
@@ -796,6 +796,16 @@ void Ekf::checkVerticalAccelerationHealth()
 		}
 	}
 
+	if (_control_status.flags.baro_hgt) {
+		// baro height
+		const auto &baro_hgt = _aid_src_baro_hgt;
+
+		if (baro_hgt.time_last_fuse > _vert_pos_fuse_attempt_time_us) {
+			_vert_pos_fuse_attempt_time_us = baro_hgt.time_last_fuse;
+			_vert_pos_innov_ratio = baro_hgt.innovation / sqrtf(baro_hgt.innovation_variance);
+		}
+	}
+
 	if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
 		if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
 			// If vertical position and velocity come from independent sensors then we can
@@ -941,21 +951,23 @@ void Ekf::controlHeightFusion()
 	updateBaroHgtOffset();
 	updateGroundEffect();
 
-	if (_control_status.flags.baro_hgt) {
+	if (_baro_data_ready) {
+		updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
 
-		if (_baro_data_ready && !_baro_hgt_faulty) {
-			fuseBaroHgt();
+		if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
+			fuseBaroHgt(_aid_src_baro_hgt);
 		}
+	}
 
-	} else if (_control_status.flags.gps_hgt) {
-		// handled by gps fusion
-	} else if (_control_status.flags.rng_hgt) {
+	if (_rng_data_ready) {
+		updateRngHgt(_aid_src_rng_hgt);
 
-		if (_range_sensor.isDataHealthy()) {
-			fuseRngHgt();
+		if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
+			fuseRngHgt(_aid_src_rng_hgt);
 		}
+	}
 
-	} else if (_control_status.flags.ev_hgt) {
+	if (_control_status.flags.ev_hgt) {
 
 		if (_control_status.flags.ev_hgt && _ev_data_ready) {
 			fuseEvHgt();

src/modules/ekf2/EKF/covariance.cpp

@@ -991,9 +991,9 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 			bool bad_vz_ev  = _control_status.flags.ev_vel && (down_dvel_bias * _ev_vel_innov(2) < 0.0f);
 
 			if (bad_vz_gps || bad_vz_ev) {
-				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _baro_hgt_innov < 0.0f);
+				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation    < 0.0f);
 				bool bad_z_gps  = _control_status.flags.gps_hgt  && (down_dvel_bias * _aid_src_gnss_pos.innovation[2] < 0.0f);
-				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _rng_hgt_innov < 0.0f);
+				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _aid_src_rng_hgt.innovation     < 0.0f);
 				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _ev_pos_innov(2) < 0.0f);
 
 

src/modules/ekf2/EKF/ekf.h

@@ -86,13 +86,13 @@ public:
 	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
 
-	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov; }
-	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var; }
-	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio; }
+	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
+	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
+	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
 
-	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov; }
-	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var; }
-	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio; }
+	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
+	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
+	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
 
 	void getAuxVelInnov(float aux_vel_innov[2]) const;
 	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
@@ -340,11 +340,14 @@ public:
 
 	const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
 
-	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
-	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
+	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
+	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
 
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
 
+	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
+	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
+
 private:
 
 	// set the internal states and status to their default value
@@ -386,6 +389,7 @@ private:
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 	bool _baro_data_ready{false};	///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
+	bool _rng_data_ready{false};
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
@@ -452,12 +456,6 @@ private:
 	Vector3f _ev_pos_innov{};	///< external vision position innovations (m)
 	Vector3f _ev_pos_innov_var{};	///< external vision position innovation variances (m**2)
 
-	float _baro_hgt_innov{};		///< baro hgt innovations (m)
-	float _baro_hgt_innov_var{};	///< baro hgt innovation variances (m**2)
-
-	float _rng_hgt_innov{};	///< range hgt innovations (m)
-	float _rng_hgt_innov_var{};	///< range hgt innovation variances (m**2)
-
 	Vector3f _aux_vel_innov{};	///< horizontal auxiliary velocity innovations: (m/sec)
 	Vector3f _aux_vel_innov_var{};	///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
 
@@ -493,11 +491,14 @@ private:
 	bool _inhibit_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
 	Vector2f _flow_compensated_XY_rad{};	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
-	estimator_aid_source_3d_s _aid_src_gnss_vel{};
-	estimator_aid_source_3d_s _aid_src_gnss_pos{};
+	estimator_aid_source_1d_s _aid_src_baro_hgt{};
+	estimator_aid_source_1d_s _aid_src_rng_hgt{};
 
 	estimator_aid_source_2d_s _aid_src_fake_pos{};
 
+	estimator_aid_source_3d_s _aid_src_gnss_vel{};
+	estimator_aid_source_3d_s _aid_src_gnss_pos{};
+
 	// output predictor states
 	Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)
 	Vector3f _vel_err_integ{};	///< integral of velocity tracking error (m)
@@ -644,10 +645,13 @@ private:
 	// fuse body frame drag specific forces for multi-rotor wind estimation
 	void fuseDrag(const dragSample &drag_sample);
 
-	void fuseBaroHgt();
-	void fuseRngHgt();
+	void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
+	void fuseRngHgt(estimator_aid_source_1d_s &range_hgt);
 	void fuseEvHgt();
 
+	void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt);
+	void updateRngHgt(estimator_aid_source_1d_s &rng_hgt);
+
 	// fuse single velocity and position measurement
 	bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
 
@@ -1063,7 +1067,7 @@ private:
 
 	void resetGpsDriftCheckFilters();
 
-	bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status)
+	bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status) const
 	{
 		if (status.timestamp_sample != 0) {
 			status.timestamp_sample = 0;
@@ -1077,7 +1081,7 @@ private:
 		return false;
 	}
 
-	void resetEstimatorAidStatus(estimator_aid_source_1d_s &status)
+	void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const
 	{
 		if (resetEstimatorAidStatusFlags(status)) {
 			status.observation = 0;
@@ -1090,7 +1094,7 @@ private:
 	}
 
 	template <typename T>
-	bool resetEstimatorAidStatusFlags(T &status)
+	bool resetEstimatorAidStatusFlags(T &status) const
 	{
 		if (status.timestamp_sample != 0) {
 			status.timestamp_sample = 0;
@@ -1108,7 +1112,7 @@ private:
 	}
 
 	template <typename T>
-	void resetEstimatorAidStatus(T &status)
+	void resetEstimatorAidStatus(T &status) const
 	{
 		if (resetEstimatorAidStatusFlags(status)) {
 			for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
@@ -1121,6 +1125,21 @@ private:
 			}
 		}
 	}
+
+	void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
+	{
+		status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
+		status.innovation_rejected = (status.test_ratio > 1.f);
+	}
+
+	template <typename T>
+	void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
+	{
+		for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
+			status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
+			status.innovation_rejected[i] = (status.test_ratio[i] > 1.f);
+		}
+	}
 };
 
 #endif // !EKF_EKF_H

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -941,13 +941,13 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 
 	// return the vertical position innovation test ratio
 	if (_control_status.flags.baro_hgt) {
-		hgt = math::max(sqrtf(_baro_hgt_test_ratio), FLT_MIN);
+		hgt = math::max(sqrtf(_aid_src_baro_hgt.test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.gps_hgt) {
 		hgt = math::max(sqrtf(_aid_src_gnss_pos.test_ratio[2]), FLT_MIN);
 
 	} else if (_control_status.flags.rng_hgt) {
-		hgt = math::max(sqrtf(_rng_hgt_test_ratio), FLT_MIN);
+		hgt = math::max(sqrtf(_aid_src_rng_hgt.test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.ev_hgt) {
 		hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);

src/modules/ekf2/EKF/estimator_interface.h

@@ -334,8 +334,6 @@ protected:
 	Vector2f _ev_vel_test_ratio{};		// EV velocity innovation consistency check ratios
 	Vector2f _ev_pos_test_ratio{};		// EV position innovation consistency check ratios
 	Vector2f _aux_vel_test_ratio{};		// Auxiliary horizontal velocity innovation consistency check ratio
-	float _baro_hgt_test_ratio{};	// baro height innovation consistency check ratios
-	float _rng_hgt_test_ratio{};		// range finder height innovation consistency check ratios
 	float _optflow_test_ratio{};		// Optical flow innovation consistency check ratio
 	float _tas_test_ratio{};		// tas innovation consistency check ratio
 	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -139,10 +139,12 @@ void Ekf::fuseFakePosition()
 
 		fake_pos.innovation[i] = _state.pos(i) - _last_known_posNE(i);
 		fake_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
-		fake_pos.test_ratio[i] = sq(fake_pos.innovation[i]) / (sq(innov_gate) * fake_pos.innovation_variance[i]);
+	}
 
-		fake_pos.innovation_rejected[i] = (fake_pos.test_ratio[i] > 1.f);
+	setEstimatorAidStatusTestRatio(fake_pos, innov_gate);
 
+	// fuse
+	for (int i = 0; i < 2; i++) {
 		// always protect against extreme values that could result in a NaN
 		fake_pos.fusion_enabled[i] = fake_pos.test_ratio[i] < sq(100.0f / innov_gate);
 

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -55,11 +55,10 @@ void Ekf::updateGpsVel(const gpsSample &gps_sample)
 
 		gps_vel.innovation[i] = _state.vel(i) - gps_sample.vel(i);
 		gps_vel.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i);
-		gps_vel.test_ratio[i] = sq(gps_vel.innovation[i]) / (sq(innov_gate) * gps_vel.innovation_variance[i]);
-
-		gps_vel.innovation_rejected[i] = (gps_vel.test_ratio[i] > 1.f);
 	}
 
+	setEstimatorAidStatusTestRatio(gps_vel, innov_gate);
+
 	// vz special case if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
 	if (_fault_status.flags.bad_acc_vertical && gps_vel.innovation_rejected[2]) {
@@ -109,11 +108,10 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 
 		gps_pos.innovation[i] = _state.pos(i) - position(i);
 		gps_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
-		gps_pos.test_ratio[i] = sq(gps_pos.innovation[i]) / (sq(innov_gate) * gps_pos.innovation_variance[i]);
-
-		gps_pos.innovation_rejected[i] = (gps_pos.test_ratio[i] > 1.f);
 	}
 
+	setEstimatorAidStatusTestRatio(gps_pos, innov_gate);
+
 	// z special case if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
 	if (_fault_status.flags.bad_acc_vertical && gps_pos.innovation_rejected[2]) {

src/modules/ekf2/EKF/height_fusion.cpp

@@ -38,12 +38,23 @@
 
 #include "ekf.h"
 
-void Ekf::fuseBaroHgt()
+void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt)
 {
-	// vertical position innovation - baro measurement has opposite sign to earth z axis
-	const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
+	// reset flags
+	resetEstimatorAidStatusFlags(baro_hgt);
 
-	_baro_hgt_innov = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
+	// innovation gate size
+	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
+
+	// observation variance - user parameter defined
+	float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
+
+	// vertical position innovation - baro measurement has opposite sign to earth z axis
+	baro_hgt.observation = -(_baro_sample_delayed.hgt - _baro_b_est.getBias() - _baro_hgt_offset);
+	baro_hgt.observation_variance = obs_var;
+
+	baro_hgt.innovation = _state.pos(2) - baro_hgt.observation;
+	baro_hgt.innovation_variance = P(9, 9) + obs_var;
 
 	// Compensate for positive static pressure transients (negative vertical position innovations)
 	// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@@ -52,40 +63,84 @@ void Ekf::fuseBaroHgt()
 		const float deadzone_start = 0.0f;
 		const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
 
-		if (_baro_hgt_innov < -deadzone_start) {
-			if (_baro_hgt_innov <= -deadzone_end) {
-				_baro_hgt_innov += deadzone_end;
+		if (baro_hgt.innovation < -deadzone_start) {
+			if (baro_hgt.innovation <= -deadzone_end) {
+				baro_hgt.innovation += deadzone_end;
 
 			} else {
-				_baro_hgt_innov = -deadzone_start;
+				baro_hgt.innovation = -deadzone_start;
 			}
 		}
 	}
 
-	// innovation gate size
-	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
+	setEstimatorAidStatusTestRatio(baro_hgt, innov_gate);
 
-	// observation variance - user parameter defined
-	float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
+	// special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && baro_hgt.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(baro_hgt.innovation_variance);
+		baro_hgt.innovation = math::constrain(baro_hgt.innovation, -innov_limit, innov_limit);
+		baro_hgt.innovation_rejected = false;
+	}
 
-	fuseVerticalPosition(_baro_hgt_innov, innov_gate, obs_var,
-			     _baro_hgt_innov_var, _baro_hgt_test_ratio);
+	baro_hgt.fusion_enabled = _control_status.flags.baro_hgt;
+
+	baro_hgt.timestamp_sample = baro_sample.time_us;
 }
 
-void Ekf::fuseRngHgt()
+void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt)
 {
-	// use range finder with tilt correction
-	_rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
-					  _params.rng_gnd_clearance)) - _rng_hgt_offset;
+	if (baro_hgt.fusion_enabled
+	    && !baro_hgt.innovation_rejected
+	    && fuseVelPosHeight(baro_hgt.innovation, baro_hgt.innovation_variance, 5)) {
 
-	// innovation gate size
-	float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
+		baro_hgt.fused = true;
+		baro_hgt.time_last_fuse = _time_last_imu;
+	}
+}
+
+void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
+{
+	// reset flags
+	resetEstimatorAidStatusFlags(rng_hgt);
 
 	// observation variance - user parameter defined
 	float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
 
-	fuseVerticalPosition(_rng_hgt_innov, innov_gate, obs_var,
-			     _rng_hgt_innov_var, _rng_hgt_test_ratio);
+	// innovation gate size
+	float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
+
+	// vertical position innovation, use range finder with tilt correction
+	rng_hgt.observation = (-math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance)) + _rng_hgt_offset;
+	rng_hgt.observation_variance = obs_var;
+
+	rng_hgt.innovation = _state.pos(2) - rng_hgt.observation;
+	rng_hgt.innovation_variance = P(9, 9) + obs_var;
+
+	setEstimatorAidStatusTestRatio(rng_hgt, innov_gate);
+
+	// special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && rng_hgt.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(rng_hgt.innovation_variance);
+		rng_hgt.innovation = math::constrain(rng_hgt.innovation, -innov_limit, innov_limit);
+		rng_hgt.innovation_rejected = false;
+	}
+
+	rng_hgt.fusion_enabled = _control_status.flags.rng_hgt;
+
+	rng_hgt.timestamp_sample = _range_sensor.getSampleAddress()->time_us;
+}
+
+void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
+{
+	if (rng_hgt.fusion_enabled
+	    && !rng_hgt.innovation_rejected
+	    && fuseVelPosHeight(rng_hgt.innovation, rng_hgt.innovation_variance, 5)) {
+
+		rng_hgt.fused = true;
+		rng_hgt.time_last_fuse = _time_last_imu;
+	}
 }
 
 void Ekf::fuseEvHgt()

src/modules/ekf2/EKF2.cpp

@@ -636,12 +636,19 @@ void EKF2::Run()
 
 void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 {
+	// baro height
+	PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
+
+	// RNG height
+	PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub);
+
+	// fake position
+	PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
+
 	// GNSS velocity & position
 	PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
 
-	// fake position
-	PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
 }
 
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)

src/modules/ekf2/EKF2.hpp

@@ -255,11 +255,14 @@ private:
 	hrt_abstime _last_sensor_bias_published{0};
 	hrt_abstime _last_gps_status_published{0};
 
-	hrt_abstime _status_gnss_vel_pub_last{0};
-	hrt_abstime _status_gnss_pos_pub_last{0};
+	hrt_abstime _status_baro_hgt_pub_last{0};
+	hrt_abstime _status_rng_hgt_pub_last{0};
 
 	hrt_abstime _status_fake_pos_pub_last{0};
 
+	hrt_abstime _status_gnss_vel_pub_last{0};
+	hrt_abstime _status_gnss_pos_pub_last{0};
+
 	float _last_baro_bias_published{};
 
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
@@ -319,11 +322,14 @@ private:
 	uORB::PublicationMulti<vehicle_odometry_s>           _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
 	uORB::PublicationMulti<yaw_estimator_status_s>       _yaw_est_pub{ORB_ID(yaw_estimator_status)};
 
-	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
-	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
+	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
+	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
 
 	uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
 
+	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
+	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
+
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;
 	uORB::PublicationMulti<vehicle_local_position_s>     _local_position_pub;

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

@@ -14,8 +14,8 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 1190000,1,-0.0112,-0.0137,-0.0166,0.0163,-0.0049,-0.155,0.00161,-0.000401,-0.0818,-1.25e-07,-6.6e-07,-2.4e-09,4.68e-07,-8.34e-08,-1.35e-08,0.192,0.00189,0.404,0,0,0,0,0,1.85e-06,0.00506,0.00506,0.00108,0.85,0.85,0.491,0.151,0.151,0.328,9.88e-07,9.88e-07,6.56e-07,3.99e-06,3.99e-06,3.99e-06,0,0,0,0,0,0,0,0
 1290000,1,-0.0113,-0.0139,-0.0166,0.0212,-0.00611,-0.169,0.00349,-0.000977,-0.098,-1.25e-07,-6.6e-07,-2.4e-09,4.68e-07,-8.34e-08,-1.35e-08,0.192,0.00189,0.404,0,0,0,0,0,2.06e-06,0.0056,0.0056,0.00108,1.03,1.03,0.5,0.214,0.214,0.402,9.88e-07,9.88e-07,5.97e-07,3.99e-06,3.99e-06,3.99e-06,0,0,0,0,0,0,0,0
 1390000,1,-0.0114,-0.0142,-0.0166,0.0275,-0.00825,-0.181,0.00597,-0.00168,-0.115,-1.25e-07,-6.6e-07,-2.4e-09,4.68e-07,-8.34e-08,-1.35e-08,0.192,0.00189,0.404,0,0,0,0,0,2.31e-06,0.00618,0.00619,0.00108,1.26,1.26,0.509,0.301,0.301,0.487,9.88e-07,9.88e-07,5.41e-07,3.99e-06,3.99e-06,3.99e-06,0,0,0,0,0,0,0,0
-1490000,1,-0.0114,-0.0143,-0.0166,0.0277,-0.00795,-0.196,0.00506,-0.00142,-0.134,-4.58e-07,-2.01e-06,-2.37e-09,1.16e-06,-2.53e-07,-3.54e-08,0.192,0.00189,0.404,0,0,0,0,0,2.42e-06,0.00637,0.00637,0.00108,1.01,1.01,0.519,0.189,0.189,0.582,9.62e-07,9.62e-07,4.87e-07,3.99e-06,3.99e-06,3.99e-06,0,0,0,0,0,0,0,0
-1590000,1,-0.0114,-0.0146,-0.0166,0.0346,-0.00911,-0.209,0.00815,-0.00227,-0.155,-4.58e-07,-2.01e-06,-2.37e-09,1.16e-06,-2.53e-07,-3.54e-08,0.192,0.00189,0.404,0,0,0,0,0,2.7e-06,0.00702,0.00702,0.00107,1.3,1.3,0.53,0.268,0.268,0.689,9.62e-07,9.62e-07,4.38e-07,3.99e-06,3.99e-06,3.99e-06,0,0,0,0,0,0,0,0
+1490000,1,-0.0114,-0.0143,-0.0166,0.0277,-0.00795,-0.196,0.00506,-0.00142,-0.134,-4.58e-07,-2.01e-06,-2.38e-09,1.16e-06,-2.53e-07,-3.54e-08,0.192,0.00189,0.404,0,0,0,0,0,2.42e-06,0.00637,0.00637,0.00108,1.01,1.01,0.519,0.189,0.189,0.582,9.62e-07,9.62e-07,4.87e-07,3.99e-06,3.99e-06,3.99e-06,0,0,0,0,0,0,0,0
+1590000,1,-0.0114,-0.0146,-0.0166,0.0346,-0.00911,-0.209,0.00815,-0.00227,-0.155,-4.58e-07,-2.01e-06,-2.38e-09,1.16e-06,-2.53e-07,-3.54e-08,0.192,0.00189,0.404,0,0,0,0,0,2.7e-06,0.00702,0.00702,0.00107,1.3,1.3,0.53,0.268,0.268,0.689,9.62e-07,9.62e-07,4.38e-07,3.99e-06,3.99e-06,3.99e-06,0,0,0,0,0,0,0,0
 1690000,1,-0.0115,-0.0145,-0.0166,0.0339,-0.0104,-0.221,0.00659,-0.00188,-0.176,-1.15e-06,-4.89e-06,2.07e-09,2.38e-06,-5.49e-07,-7.51e-08,0.192,0.00189,0.404,0,0,0,0,0,2.66e-06,0.00677,0.00677,0.00105,1.11,1.11,0.542,0.179,0.179,0.807,9.06e-07,9.06e-07,3.92e-07,3.98e-06,3.98e-06,3.99e-06,0,0,0,0,0,0,0,0
 1790000,1,-0.0115,-0.0148,-0.0167,0.0422,-0.0137,-0.236,0.0104,-0.00306,-0.199,-1.15e-06,-4.89e-06,2.07e-09,2.38e-06,-5.49e-07,-7.51e-08,0.192,0.00189,0.404,0,0,0,0,0,2.96e-06,0.00744,0.00744,0.00104,1.45,1.45,0.554,0.262,0.262,0.936,9.06e-07,9.06e-07,3.51e-07,3.98e-06,3.98e-06,3.99e-06,0,0,0,0,0,0,0,0
 1890000,1,-0.0113,-0.0144,-0.0167,0.0382,-0.0124,-0.25,0.00784,-0.00241,-0.223,-2.41e-06,-9.67e-06,1.8e-08,4.1e-06,-9.99e-07,-1.33e-07,0.192,0.00189,0.404,0,0,0,0,0,2.68e-06,0.00658,0.00658,0.00102,1.19,1.19,0.567,0.178,0.178,1.08,8.16e-07,8.16e-07,3.14e-07,3.97e-06,3.97e-06,3.99e-06,0,0,0,0,0,0,0,0
@@ -47,37 +47,37 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 4490000,1,-0.0101,-0.0116,-0.0168,0.0131,-0.0043,-0.602,0.0051,-0.00216,-1.33,-1.46e-05,-5.14e-05,2.32e-07,4.34e-06,-8.5e-07,-1.19e-07,0.192,0.00189,0.404,0,0,0,0,0,5.92e-07,0.00152,0.00152,0.000544,0.549,0.549,1.19,0.187,0.187,10.8,1.02e-07,1.02e-07,3.24e-08,3.92e-06,3.92e-06,3.99e-06,0,0,0,0,0,0,0,0
 4590000,1,-0.0101,-0.0114,-0.0168,0.0114,-0.00343,-0.617,0.00359,-0.00141,-1.39,-1.5e-05,-5.26e-05,2.4e-07,3.59e-06,-5.72e-07,-8.71e-08,0.192,0.00189,0.404,0,0,0,0,0,5.02e-07,0.00124,0.00124,0.000533,0.418,0.418,1.23,0.132,0.132,11.5,8.3e-08,8.31e-08,3.04e-08,3.91e-06,3.91e-06,3.99e-06,0,0,0,0,0,0,0,0
 4690000,1,-0.0101,-0.0114,-0.0168,0.0125,-0.00448,-0.631,0.00481,-0.00179,-1.45,-1.5e-05,-5.26e-05,2.4e-07,3.59e-06,-5.72e-07,-8.71e-08,0.192,0.00189,0.404,0,0,0,0,0,5.23e-07,0.00134,0.00134,0.000522,0.504,0.504,1.26,0.177,0.177,12.2,8.3e-08,8.31e-08,2.85e-08,3.91e-06,3.91e-06,3.99e-06,0,0,0,0,0,0,0,0
-4790000,1,-0.0101,-0.0112,-0.0168,0.0102,-0.00391,-0.645,0.00335,-0.00124,-1.52,-1.54e-05,-5.37e-05,2.47e-07,2.87e-06,-3.32e-07,-5.74e-08,0.192,0.00189,0.404,0,0,0,0,0,4.47e-07,0.0011,0.0011,0.000512,0.385,0.385,1.3,0.127,0.127,13,6.74e-08,6.74e-08,2.68e-08,3.9e-06,3.9e-06,3.99e-06,0,0,0,0,0,0,0,0
-4890000,1,-0.0101,-0.0113,-0.0168,0.0113,-0.00504,-0.66,0.00445,-0.00168,-1.58,-1.54e-05,-5.37e-05,2.47e-07,2.87e-06,-3.32e-07,-5.74e-08,0.192,0.00189,0.404,0,0,0,0,0,4.65e-07,0.00118,0.00118,0.000502,0.461,0.461,1.34,0.168,0.168,13.7,6.74e-08,6.74e-08,2.52e-08,3.9e-06,3.9e-06,3.99e-06,0,0,0,0,0,0,0,0
+4790000,1,-0.0101,-0.0112,-0.0168,0.0102,-0.00391,-0.645,0.00335,-0.00124,-1.52,-1.54e-05,-5.37e-05,2.47e-07,2.87e-06,-3.33e-07,-5.74e-08,0.192,0.00189,0.404,0,0,0,0,0,4.47e-07,0.0011,0.0011,0.000512,0.385,0.385,1.3,0.127,0.127,13,6.74e-08,6.74e-08,2.68e-08,3.9e-06,3.9e-06,3.99e-06,0,0,0,0,0,0,0,0
+4890000,1,-0.0101,-0.0113,-0.0168,0.0113,-0.00504,-0.66,0.00445,-0.00168,-1.58,-1.54e-05,-5.37e-05,2.47e-07,2.87e-06,-3.33e-07,-5.74e-08,0.192,0.00189,0.404,0,0,0,0,0,4.65e-07,0.00118,0.00118,0.000502,0.461,0.461,1.34,0.168,0.168,13.7,6.74e-08,6.74e-08,2.52e-08,3.9e-06,3.9e-06,3.99e-06,0,0,0,0,0,0,0,0
 4990000,1,-0.00999,-0.0112,-0.0168,0.01,-0.00373,-0.675,0.00313,-0.00121,-1.65,-1.57e-05,-5.46e-05,2.53e-07,2.19e-06,-9.75e-08,-2.96e-08,0.192,0.00189,0.404,0,0,0,0,0,4e-07,0.000965,0.000966,0.000492,0.353,0.353,1.37,0.121,0.121,14.6,5.44e-08,5.44e-08,2.38e-08,3.9e-06,3.9e-06,3.99e-06,0,0,0,0,0,0,0,0
 5090000,1,-0.01,-0.0112,-0.0168,0.0105,-0.00389,-0.69,0.00416,-0.00158,-1.72,-1.57e-05,-5.46e-05,2.53e-07,2.19e-06,-9.75e-08,-2.96e-08,0.192,0.00189,0.404,0,0,0,0,0,4.14e-07,0.00103,0.00103,0.000483,0.421,0.421,1.41,0.16,0.16,15.4,5.44e-08,5.44e-08,2.24e-08,3.9e-06,3.9e-06,3.99e-06,0,0,0,0,0,0,0,0
-5190000,1,-0.00997,-0.0111,-0.0168,0.00852,-0.00353,-0.703,0.00291,-0.00113,-1.79,-1.59e-05,-5.54e-05,2.58e-07,1.57e-06,1.19e-07,-4.11e-09,0.192,0.00189,0.404,0,0,0,0,0,3.58e-07,0.000848,0.000848,0.000474,0.323,0.323,1.45,0.116,0.116,16.3,4.38e-08,4.38e-08,2.12e-08,3.89e-06,3.89e-06,3.99e-06,0,0,0,0,0,0,0,0
-5290000,1,-0.00992,-0.0111,-0.0168,0.00778,-0.00312,-0.718,0.00373,-0.00148,-1.86,-1.59e-05,-5.54e-05,2.58e-07,1.57e-06,1.19e-07,-4.11e-09,0.192,0.00189,0.404,0,0,0,0,0,3.68e-07,0.000905,0.000905,0.000466,0.384,0.384,1.49,0.152,0.152,17.3,4.38e-08,4.38e-08,2e-08,3.89e-06,3.89e-06,3.99e-06,0,0,0,0,0,0,0,0
+5190000,1,-0.00997,-0.0111,-0.0168,0.00852,-0.00353,-0.703,0.00291,-0.00113,-1.79,-1.59e-05,-5.54e-05,2.58e-07,1.57e-06,1.19e-07,-4.12e-09,0.192,0.00189,0.404,0,0,0,0,0,3.58e-07,0.000848,0.000848,0.000474,0.323,0.323,1.45,0.116,0.116,16.3,4.38e-08,4.38e-08,2.12e-08,3.89e-06,3.89e-06,3.99e-06,0,0,0,0,0,0,0,0
+5290000,1,-0.00992,-0.0111,-0.0168,0.00778,-0.00312,-0.718,0.00373,-0.00148,-1.86,-1.59e-05,-5.54e-05,2.58e-07,1.57e-06,1.19e-07,-4.12e-09,0.192,0.00189,0.404,0,0,0,0,0,3.68e-07,0.000905,0.000905,0.000466,0.384,0.384,1.49,0.152,0.152,17.3,4.38e-08,4.38e-08,2e-08,3.89e-06,3.89e-06,3.99e-06,0,0,0,0,0,0,0,0
 5390000,1,-0.00981,-0.0111,-0.0167,0.00451,-0.00239,-0.732,0.00247,-0.001,-1.93,-1.62e-05,-5.6e-05,2.63e-07,1.05e-06,3.08e-07,1.7e-08,0.192,0.00189,0.404,0,0,0,0,0,3.23e-07,0.000744,0.000744,0.000457,0.295,0.295,1.53,0.111,0.111,18.3,3.52e-08,3.52e-08,1.9e-08,3.88e-06,3.88e-06,3.99e-06,0,0,0,0,0,0,0,0
 5490000,1,-0.00976,-0.0111,-0.0167,0.00459,-0.00238,-0.745,0.00293,-0.00126,-2,-1.62e-05,-5.6e-05,2.63e-07,1.05e-06,3.08e-07,1.7e-08,0.192,0.00189,0.404,0,0,0,0,0,3.3e-07,0.000792,0.000792,0.000449,0.349,0.349,1.58,0.145,0.145,19.3,3.52e-08,3.52e-08,1.8e-08,3.88e-06,3.88e-06,3.99e-06,0,0,0,0,0,0,0,0
 5590000,1,-0.00972,-0.0111,-0.0168,0.00519,-0.00237,-0.76,0.00341,-0.00148,-2.08,-1.62e-05,-5.6e-05,2.63e-07,1.05e-06,3.08e-07,1.7e-08,0.192,0.00189,0.404,0,0,0,0,0,3.38e-07,0.000841,0.000842,0.000442,0.41,0.41,1.62,0.188,0.188,20.4,3.52e-08,3.52e-08,1.7e-08,3.88e-06,3.88e-06,3.99e-06,0,0,0,0,0,0,0,0
 5690000,1,-0.00961,-0.011,-0.0167,0.00334,-0.000356,-0.775,0.00226,-0.000943,-2.16,-1.63e-05,-5.64e-05,2.66e-07,6.66e-07,4.59e-07,3.27e-08,0.192,0.00189,0.404,0,0,0,0,0,2.96e-07,0.000694,0.000694,0.000434,0.317,0.317,1.66,0.138,0.138,21.5,2.83e-08,2.83e-08,1.62e-08,3.88e-06,3.88e-06,3.99e-06,0,0,0,0,0,0,0,0
 5790000,1,-0.00954,-0.011,-0.0167,0.00364,0.00106,0,0.00258,-0.00091,-365,-1.63e-05,-5.64e-05,2.66e-07,6.66e-07,4.59e-07,3.27e-08,0.192,0.00189,0.404,0,0,0,0,0,3.02e-07,0.000735,0.000735,0.000427,0.371,0.371,10,0.178,0.178,4,2.83e-08,2.83e-08,1.53e-08,3.88e-06,3.88e-06,3.99e-06,0,0,0,0,0,0,0,0
-5890000,1,-0.00949,-0.011,-0.0167,0.00314,0.0023,0.00414,0.00175,-0.000382,-365,-1.64e-05,-5.67e-05,2.68e-07,3.88e-07,5.42e-07,8.18e-09,0.192,0.00189,0.404,0,0,0,0,0,2.7e-07,0.000609,0.000609,0.00042,0.288,0.288,9.76,0.131,0.131,0.471,2.27e-08,2.27e-08,1.46e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
-5990000,1,-0.00946,-0.011,-0.0167,0.0034,0.00409,0.0213,0.00206,-1.86e-05,-365,-1.64e-05,-5.67e-05,2.68e-07,3.86e-07,5.45e-07,-1.1e-07,0.192,0.00189,0.404,0,0,0,0,0,2.75e-07,0.000643,0.000643,0.000413,0.335,0.335,8.56,0.168,0.168,0.323,2.27e-08,2.27e-08,1.39e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
-6090000,1,-0.00942,-0.011,-0.0167,0.00324,0.0055,-0.00339,0.00146,0.00043,-365,-1.64e-05,-5.7e-05,2.67e-07,1.79e-07,5.24e-07,-2.28e-08,0.192,0.00189,0.404,0,0,0,0,0,2.47e-07,0.000536,0.000537,0.000407,0.261,0.261,6.77,0.125,0.125,0.331,1.83e-08,1.83e-08,1.32e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
-6190000,1,-0.00943,-0.0109,-0.0168,0.00331,0.00755,0.00222,0.0018,0.0011,-365,-1.64e-05,-5.7e-05,2.67e-07,1.73e-07,5.29e-07,-2.76e-07,0.192,0.00189,0.404,0,0,0,0,0,2.52e-07,0.000565,0.000565,0.0004,0.303,0.303,4.66,0.159,0.159,0.319,1.83e-08,1.83e-08,1.26e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
-6290000,1,-0.00945,-0.0109,-0.0168,0.00352,0.00715,-0.0059,0.00135,0.00124,-365,-1.63e-05,-5.71e-05,2.63e-07,-1.83e-10,3.98e-07,-3.37e-07,0.192,0.00189,0.404,0,0,0,0,0,2.3e-07,0.000475,0.000475,0.000394,0.237,0.237,3.09,0.119,0.119,0.295,1.48e-08,1.48e-08,1.2e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
-6390000,1,-0.00934,-0.0109,-0.0168,0.00395,0.00844,-0.0441,0.00173,0.00203,-365,-1.63e-05,-5.71e-05,2.63e-07,1.35e-08,3.85e-07,2.9e-07,0.192,0.00189,0.404,0,0,0,0,0,2.32e-07,0.000498,0.000499,0.000388,0.274,0.274,2.16,0.151,0.151,0.288,1.48e-08,1.48e-08,1.15e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
-6490000,1,-0.00939,-0.0109,-0.0168,0.00359,0.0077,-0.048,0.00134,0.00185,-365,-1.61e-05,-5.73e-05,2.58e-07,-1.54e-07,1.8e-07,-1.71e-07,0.192,0.00189,0.404,0,0,0,0,0,2.15e-07,0.000422,0.000422,0.000382,0.215,0.215,1.47,0.113,0.113,0.256,1.2e-08,1.2e-08,1.09e-08,3.86e-06,3.86e-06,3.99e-06,0,0,0,0,0,0,0,0
-6590000,1,-0.00939,-0.0109,-0.0169,0.00352,0.00841,-0.0946,0.0017,0.00262,-365,-1.61e-05,-5.73e-05,2.58e-07,-1.13e-07,1.43e-07,1.72e-06,0.192,0.00189,0.404,0,0,0,0,0,2.18e-07,0.000442,0.000442,0.000377,0.248,0.248,1.03,0.143,0.143,0.229,1.2e-08,1.2e-08,1.05e-08,3.86e-06,3.86e-06,3.99e-06,0,0,0,0,0,0,0,0
-6690000,1,-0.0094,-0.0109,-0.017,0.00346,0.00674,-0.0733,0.0013,0.00216,-365,-1.58e-05,-5.74e-05,2.52e-07,-3.26e-07,-4.55e-08,-1.49e-06,0.192,0.00189,0.404,0,0,0,0,0,2.03e-07,0.000378,0.000378,0.000371,0.196,0.196,0.746,0.108,0.108,0.206,9.76e-09,9.77e-09,1e-08,3.86e-06,3.86e-06,3.99e-06,0,0,0,0,0,0,0,0
-6790000,1,-0.00938,-0.0109,-0.017,0.00318,0.00814,-0.11,0.00165,0.0029,-365,-1.58e-05,-5.74e-05,2.52e-07,-2.73e-07,-9.31e-08,9.41e-07,0.192,0.00189,0.404,0,0,0,0,0,2.05e-07,0.000394,0.000394,0.000366,0.225,0.225,0.577,0.136,0.136,0.196,9.76e-09,9.77e-09,9.57e-09,3.86e-06,3.86e-06,3.99e-06,0,0,0,0,0,0,0,0
-6890000,0.706,0.0013,-0.0143,0.708,0.00244,0.00869,-0.121,0.00124,0.00242,-365,-1.56e-05,-5.75e-05,2.48e-07,-4.13e-07,-3.48e-07,4.85e-07,0.209,0.00206,0.432,0,0,0,0,0,0.00103,0.000339,0.000338,0.00132,0.176,0.176,0.442,0.104,0.104,0.178,7.97e-09,7.98e-09,9.28e-09,3.86e-06,3.86e-06,3.98e-06,0,0,0,0,0,0,0,0
+5890000,1,-0.00949,-0.011,-0.0167,0.00314,0.0023,0.00413,0.00175,-0.000382,-365,-1.64e-05,-5.67e-05,2.68e-07,3.88e-07,5.42e-07,8.19e-09,0.192,0.00189,0.404,0,0,0,0,0,2.7e-07,0.000609,0.000609,0.00042,0.288,0.288,9.76,0.131,0.131,0.471,2.27e-08,2.27e-08,1.46e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
+5990000,1,-0.00946,-0.011,-0.0167,0.0034,0.00409,0.0213,0.00206,-1.87e-05,-365,-1.64e-05,-5.67e-05,2.68e-07,3.86e-07,5.45e-07,-1.1e-07,0.192,0.00189,0.404,0,0,0,0,0,2.75e-07,0.000643,0.000643,0.000413,0.335,0.335,8.56,0.168,0.168,0.323,2.27e-08,2.27e-08,1.39e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
+6090000,1,-0.00942,-0.011,-0.0167,0.00324,0.0055,-0.0035,0.00146,0.00043,-365,-1.64e-05,-5.7e-05,2.67e-07,1.79e-07,5.24e-07,-2.21e-08,0.192,0.00189,0.404,0,0,0,0,0,2.47e-07,0.000536,0.000537,0.000407,0.261,0.261,6.77,0.125,0.125,0.331,1.83e-08,1.83e-08,1.32e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
+6190000,1,-0.00943,-0.0109,-0.0168,0.00331,0.00755,0.00215,0.0018,0.0011,-365,-1.64e-05,-5.7e-05,2.67e-07,1.73e-07,5.29e-07,-2.76e-07,0.192,0.00189,0.404,0,0,0,0,0,2.52e-07,0.000565,0.000565,0.0004,0.303,0.303,4.66,0.159,0.159,0.319,1.83e-08,1.83e-08,1.26e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
+6290000,1,-0.00945,-0.0109,-0.0168,0.00352,0.00715,-0.00589,0.00135,0.00124,-365,-1.63e-05,-5.71e-05,2.63e-07,-2.01e-10,3.98e-07,-3.38e-07,0.192,0.00189,0.404,0,0,0,0,0,2.3e-07,0.000475,0.000475,0.000394,0.237,0.237,3.09,0.119,0.119,0.295,1.48e-08,1.48e-08,1.2e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
+6390000,1,-0.00934,-0.0109,-0.0168,0.00395,0.00844,-0.0441,0.00173,0.00203,-365,-1.63e-05,-5.71e-05,2.63e-07,1.35e-08,3.85e-07,2.89e-07,0.192,0.00189,0.404,0,0,0,0,0,2.32e-07,0.000498,0.000499,0.000388,0.274,0.274,2.16,0.151,0.151,0.288,1.48e-08,1.48e-08,1.15e-08,3.87e-06,3.87e-06,3.99e-06,0,0,0,0,0,0,0,0
+6490000,1,-0.00939,-0.0109,-0.0168,0.00359,0.0077,-0.048,0.00134,0.00185,-365,-1.61e-05,-5.73e-05,2.58e-07,-1.54e-07,1.8e-07,-1.73e-07,0.192,0.00189,0.404,0,0,0,0,0,2.15e-07,0.000422,0.000422,0.000382,0.215,0.215,1.47,0.113,0.113,0.256,1.2e-08,1.2e-08,1.09e-08,3.86e-06,3.86e-06,3.99e-06,0,0,0,0,0,0,0,0
+6590000,1,-0.00939,-0.0109,-0.0169,0.00353,0.00841,-0.0945,0.0017,0.00262,-365,-1.61e-05,-5.73e-05,2.58e-07,-1.13e-07,1.43e-07,1.71e-06,0.192,0.00189,0.404,0,0,0,0,0,2.18e-07,0.000442,0.000442,0.000377,0.248,0.248,1.03,0.143,0.143,0.229,1.2e-08,1.2e-08,1.05e-08,3.86e-06,3.86e-06,3.99e-06,0,0,0,0,0,0,0,0
+6690000,1,-0.0094,-0.0109,-0.017,0.00346,0.00674,-0.0733,0.0013,0.00216,-365,-1.58e-05,-5.74e-05,2.52e-07,-3.26e-07,-4.56e-08,-1.49e-06,0.192,0.00189,0.404,0,0,0,0,0,2.03e-07,0.000378,0.000378,0.000371,0.196,0.196,0.746,0.108,0.108,0.206,9.76e-09,9.77e-09,1e-08,3.86e-06,3.86e-06,3.99e-06,0,0,0,0,0,0,0,0
+6790000,1,-0.00938,-0.0109,-0.017,0.00318,0.00814,-0.11,0.00165,0.0029,-365,-1.58e-05,-5.74e-05,2.52e-07,-2.73e-07,-9.31e-08,9.39e-07,0.192,0.00189,0.404,0,0,0,0,0,2.04e-07,0.000394,0.000394,0.000366,0.225,0.225,0.577,0.136,0.136,0.196,9.76e-09,9.77e-09,9.57e-09,3.86e-06,3.86e-06,3.99e-06,0,0,0,0,0,0,0,0
+6890000,0.706,0.0013,-0.0143,0.708,0.00244,0.00869,-0.121,0.00124,0.00242,-365,-1.56e-05,-5.75e-05,2.48e-07,-4.13e-07,-3.48e-07,4.83e-07,0.209,0.00206,0.432,0,0,0,0,0,0.00103,0.000339,0.000338,0.00132,0.176,0.176,0.442,0.104,0.104,0.178,7.97e-09,7.98e-09,9.28e-09,3.86e-06,3.86e-06,3.98e-06,0,0,0,0,0,0,0,0
 6990000,0.704,0.00139,-0.0142,0.71,0.0024,0.00926,-0.123,0.00147,0.00334,-365,-1.56e-05,-5.75e-05,1.96e-07,-4.79e-07,-2.87e-07,-2.51e-06,0.209,0.00206,0.432,0,0,0,0,0,0.000622,0.000339,0.000339,0.000783,0.178,0.178,0.347,0.128,0.128,0.163,7.97e-09,7.98e-09,9.28e-09,3.86e-06,3.86e-06,3.98e-06,0,0,0,0,0,0,0,0
 7090000,0.703,0.0014,-0.0142,0.711,0.00185,0.00855,-0.124,0.00169,0.00421,-365,-1.56e-05,-5.75e-05,1.16e-07,-5.58e-07,-2.14e-07,-6.11e-06,0.209,0.00206,0.432,0,0,0,0,0,0.000475,0.00034,0.000339,0.000593,0.184,0.184,0.287,0.156,0.156,0.156,7.97e-09,7.97e-09,9.28e-09,3.86e-06,3.86e-06,3.97e-06,0,0,0,0,0,0,0,0
 7190000,0.703,0.0014,-0.0141,0.711,0.000163,0.00836,-0.145,0.00179,0.00506,-365,-1.56e-05,-5.75e-05,1.08e-07,-5.09e-07,-2.57e-07,-3.87e-06,0.209,0.00206,0.432,0,0,0,0,0,0.000383,0.000341,0.000341,0.000473,0.193,0.193,0.236,0.188,0.188,0.144,7.97e-09,7.97e-09,9.28e-09,3.86e-06,3.86e-06,3.96e-06,0,0,0,0,0,0,0,0
-7290000,0.703,0.00142,-0.0141,0.711,-0.00132,0.00825,-0.145,0.00174,0.00587,-365,-1.56e-05,-5.75e-05,1.86e-07,-6.59e-07,-1.32e-07,-1.07e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000326,0.000342,0.000342,0.000399,0.205,0.205,0.198,0.223,0.223,0.134,7.96e-09,7.96e-09,9.27e-09,3.86e-06,3.86e-06,3.95e-06,0,0,0,0,0,0,0,0
+7290000,0.703,0.00142,-0.0141,0.711,-0.00132,0.00825,-0.145,0.00174,0.00587,-365,-1.56e-05,-5.75e-05,1.86e-07,-6.58e-07,-1.32e-07,-1.07e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000326,0.000342,0.000342,0.000399,0.205,0.205,0.198,0.223,0.223,0.134,7.96e-09,7.96e-09,9.27e-09,3.86e-06,3.86e-06,3.95e-06,0,0,0,0,0,0,0,0
 7390000,0.703,0.00143,-0.014,0.711,-0.00145,0.00941,-0.157,0.00158,0.00678,-365,-1.56e-05,-5.75e-05,2.7e-07,-6.93e-07,-1.07e-07,-1.22e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000292,0.000344,0.000344,0.000355,0.221,0.221,0.174,0.263,0.263,0.13,7.96e-09,7.96e-09,9.27e-09,3.86e-06,3.86e-06,3.94e-06,0,0,0,0,0,0,0,0
-7490000,0.703,0.00149,-0.014,0.711,-0.00306,0.0095,-0.16,0.00133,0.00771,-365,-1.56e-05,-5.75e-05,3.27e-07,-8.61e-07,3.5e-08,-2e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000264,0.000346,0.000346,0.000318,0.24,0.24,0.152,0.307,0.307,0.122,7.94e-09,7.95e-09,9.26e-09,3.86e-06,3.86e-06,3.92e-06,0,0,0,0,0,0,0,0
-7590000,0.704,0.00149,-0.0139,0.71,-0.00496,0.0104,-0.165,0.000962,0.00872,-365,-1.56e-05,-5.76e-05,5.48e-07,-1.06e-06,1.92e-07,-2.88e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000243,0.000349,0.000349,0.00029,0.263,0.262,0.136,0.358,0.358,0.115,7.94e-09,7.95e-09,9.25e-09,3.86e-06,3.86e-06,3.9e-06,0,0,0,0,0,0,0,0
+7490000,0.703,0.00149,-0.014,0.711,-0.00306,0.0095,-0.16,0.00133,0.00771,-365,-1.56e-05,-5.75e-05,3.27e-07,-8.61e-07,3.49e-08,-2e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000264,0.000346,0.000346,0.000318,0.24,0.24,0.152,0.307,0.307,0.122,7.94e-09,7.95e-09,9.26e-09,3.86e-06,3.86e-06,3.92e-06,0,0,0,0,0,0,0,0
+7590000,0.704,0.00149,-0.0139,0.71,-0.00496,0.0104,-0.165,0.000962,0.00872,-365,-1.56e-05,-5.76e-05,5.48e-07,-1.06e-06,1.91e-07,-2.88e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000243,0.000349,0.000349,0.00029,0.263,0.262,0.136,0.358,0.358,0.115,7.94e-09,7.95e-09,9.25e-09,3.86e-06,3.86e-06,3.9e-06,0,0,0,0,0,0,0,0
 7690000,0.704,0.00157,-0.0138,0.71,-0.00677,0.0109,-0.161,0.000372,0.00977,-365,-1.56e-05,-5.76e-05,5.72e-07,-1.51e-06,5.79e-07,-4.94e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000228,0.000352,0.000352,0.000271,0.289,0.289,0.126,0.414,0.414,0.112,7.94e-09,7.95e-09,9.24e-09,3.86e-06,3.86e-06,3.88e-06,0,0,0,0,0,0,0,0
-7790000,0.704,0.00159,-0.0138,0.711,-0.00815,0.0114,-0.16,-0.000355,0.0108,-365,-1.56e-05,-5.76e-05,2.37e-07,-1.95e-06,9.71e-07,-6.96e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000215,0.000355,0.000355,0.000253,0.318,0.318,0.116,0.475,0.475,0.107,7.92e-09,7.93e-09,9.22e-09,3.86e-06,3.86e-06,3.84e-06,0,0,0,0,0,0,0,0
+7790000,0.704,0.00159,-0.0138,0.711,-0.00815,0.0114,-0.16,-0.000355,0.0108,-365,-1.56e-05,-5.76e-05,2.37e-07,-1.95e-06,9.7e-07,-6.96e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000215,0.000355,0.000355,0.000253,0.318,0.318,0.116,0.475,0.475,0.107,7.92e-09,7.93e-09,9.22e-09,3.86e-06,3.86e-06,3.84e-06,0,0,0,0,0,0,0,0
 7890000,0.704,0.0016,-0.0138,0.71,-0.0109,0.013,-0.157,-0.0013,0.0121,-365,-1.56e-05,-5.76e-05,4.56e-07,-2.52e-06,1.44e-06,-9.52e-05,0.209,0.00206,0.432,0,0,0,0,0,0.000204,0.000359,0.000359,0.000239,0.351,0.351,0.109,0.546,0.546,0.102,7.92e-09,7.93e-09,9.2e-09,3.86e-06,3.86e-06,3.8e-06,0,0,0,0,0,0,0,0
 7990000,0.704,0.00163,-0.0138,0.71,-0.0127,0.0138,-0.163,-0.00248,0.0133,-365,-1.56e-05,-5.76e-05,6.86e-07,-2.77e-06,1.66e-06,-0.000108,0.209,0.00206,0.432,0,0,0,0,0,0.000196,0.000363,0.000363,0.000227,0.387,0.386,0.104,0.622,0.622,0.0981,7.9e-09,7.9e-09,9.17e-09,3.86e-06,3.86e-06,3.76e-06,0,0,0,0,0,0,0,0
 8090000,0.704,0.00163,-0.0138,0.71,-0.0146,0.0149,-0.175,-0.00386,0.0148,-366,-1.56e-05,-5.76e-05,1.05e-06,-2.82e-06,1.69e-06,-0.00011,0.209,0.00206,0.432,0,0,0,0,0,0.00019,0.000368,0.000368,0.000219,0.427,0.427,0.102,0.71,0.71,0.097,7.9e-09,7.9e-09,9.15e-09,3.86e-06,3.86e-06,3.71e-06,0,0,0,0,0,0,0,0
@@ -90,19 +90,19 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 8790000,0.704,0.00172,-0.0136,0.71,-0.0301,0.0228,-0.152,-0.0189,0.0269,-366,-1.56e-05,-5.76e-05,8.65e-07,-9e-06,7.29e-06,-0.00041,0.209,0.00206,0.432,0,0,0,0,0,0.000164,0.000407,0.000407,0.000181,0.792,0.791,0.0954,1.64,1.64,0.0867,7.72e-09,7.72e-09,8.8e-09,3.85e-06,3.85e-06,3.14e-06,0,0,0,0,0,0,0,0
 8890000,0.704,0.00176,-0.0136,0.71,-0.0323,0.0234,-0.151,-0.0221,0.0292,-366,-1.56e-05,-5.76e-05,7.16e-07,-9.86e-06,8e-06,-0.000449,0.209,0.00206,0.432,0,0,0,0,0,0.000162,0.000414,0.000414,0.000178,0.862,0.861,0.0949,1.86,1.86,0.0857,7.72e-09,7.72e-09,8.73e-09,3.85e-06,3.85e-06,3.02e-06,0,0,0,0,0,0,0,0
 8990000,0.704,0.00181,-0.0135,0.71,-0.0339,0.0235,-0.142,-0.025,0.031,-365,-1.56e-05,-5.76e-05,2.64e-07,-1.09e-05,9.26e-06,-0.000509,0.209,0.00206,0.432,0,0,0,0,0,0.000161,0.00042,0.00042,0.000176,0.922,0.921,0.0956,2.05,2.05,0.0868,7.65e-09,7.66e-09,8.66e-09,3.84e-06,3.84e-06,2.91e-06,0,0,0,0,0,0,0,0
-9090000,0.704,0.00185,-0.0136,0.71,-0.0366,0.0242,-0.141,-0.0285,0.0334,-366,-1.56e-05,-5.76e-05,-6.78e-09,-1.15e-05,9.77e-06,-0.000537,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000428,0.000428,0.000173,0.999,0.998,0.0949,2.3,2.3,0.086,7.65e-09,7.66e-09,8.58e-09,3.84e-06,3.84e-06,2.78e-06,0,0,0,0,0,0,0,0
+9090000,0.704,0.00185,-0.0136,0.71,-0.0366,0.0242,-0.141,-0.0285,0.0334,-366,-1.56e-05,-5.76e-05,-6.81e-09,-1.15e-05,9.77e-06,-0.000537,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000428,0.000428,0.000173,0.999,0.998,0.0949,2.3,2.3,0.086,7.65e-09,7.66e-09,8.58e-09,3.84e-06,3.84e-06,2.78e-06,0,0,0,0,0,0,0,0
 9190000,0.704,0.00186,-0.0136,0.71,-0.0378,0.0245,-0.141,-0.0315,0.035,-366,-1.55e-05,-5.76e-05,1.15e-06,-1.19e-05,1.05e-05,-0.000569,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000433,0.000433,0.000171,1.06,1.06,0.094,2.52,2.52,0.0853,7.58e-09,7.59e-09,8.48e-09,3.83e-06,3.83e-06,2.65e-06,0,0,0,0,0,0,0,0
 9290000,0.704,0.00185,-0.0136,0.71,-0.0393,0.0255,-0.137,-0.0355,0.0376,-366,-1.55e-05,-5.76e-05,1.32e-06,-1.29e-05,1.13e-05,-0.000615,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000442,0.000442,0.000169,1.14,1.14,0.0929,2.83,2.83,0.0847,7.58e-09,7.59e-09,8.39e-09,3.83e-06,3.83e-06,2.52e-06,0,0,0,0,0,0,0,0
 9390000,0.704,0.00181,-0.0135,0.71,-0.0401,0.0268,-0.135,-0.0383,0.0391,-366,-1.55e-05,-5.75e-05,1.33e-06,-1.32e-05,1.23e-05,-0.000649,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000445,0.000445,0.000168,1.2,1.2,0.0928,3.07,3.07,0.086,7.5e-09,7.51e-09,8.3e-09,3.82e-06,3.82e-06,2.4e-06,0,0,0,0,0,0,0,0
 9490000,0.704,0.00182,-0.0135,0.71,-0.0425,0.0272,-0.13,-0.0426,0.0418,-366,-1.55e-05,-5.76e-05,2.24e-06,-1.4e-05,1.29e-05,-0.000686,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000455,0.000455,0.000167,1.29,1.29,0.0912,3.44,3.44,0.0854,7.5e-09,7.51e-09,8.19e-09,3.82e-06,3.82e-06,2.27e-06,0,0,0,0,0,0,0,0
 9590000,0.704,0.00183,-0.0135,0.71,-0.044,0.0276,-0.127,-0.0453,0.043,-366,-1.54e-05,-5.75e-05,2.52e-06,-1.42e-05,1.41e-05,-0.000723,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000458,0.000458,0.000166,1.35,1.35,0.0894,3.7,3.69,0.0848,7.42e-09,7.42e-09,8.08e-09,3.81e-06,3.81e-06,2.13e-06,0,0,0,0,0,0,0,0
-9690000,0.704,0.00191,-0.0135,0.71,-0.0463,0.0296,-0.119,-0.0499,0.0459,-366,-1.54e-05,-5.75e-05,1.66e-06,-1.53e-05,1.49e-05,-0.00077,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000468,0.000468,0.000165,1.45,1.44,0.0886,4.12,4.12,0.086,7.42e-09,7.42e-09,7.97e-09,3.81e-06,3.81e-06,2.02e-06,0,0,0,0,0,0,0,0
+9690000,0.704,0.00191,-0.0135,0.71,-0.0463,0.0296,-0.119,-0.0499,0.0459,-366,-1.54e-05,-5.75e-05,1.66e-06,-1.52e-05,1.49e-05,-0.00077,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000468,0.000468,0.000165,1.45,1.44,0.0886,4.12,4.12,0.086,7.42e-09,7.42e-09,7.97e-09,3.81e-06,3.81e-06,2.02e-06,0,0,0,0,0,0,0,0
 9790000,0.705,0.0019,-0.0134,0.71,-0.0467,0.0314,-0.109,-0.0547,0.0491,-365,-1.54e-05,-5.75e-05,2.29e-06,-1.65e-05,1.59e-05,-0.000825,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000479,0.000479,0.000164,1.55,1.55,0.0864,4.59,4.58,0.0853,7.42e-09,7.42e-09,7.85e-09,3.81e-06,3.81e-06,1.89e-06,0,0,0,0,0,0,0,0
 9890000,0.704,0.00191,-0.0134,0.71,-0.0478,0.0315,-0.106,-0.057,0.05,-365,-1.54e-05,-5.75e-05,2.24e-06,-1.63e-05,1.7e-05,-0.00085,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.00048,0.00048,0.000163,1.59,1.59,0.084,4.87,4.87,0.0846,7.33e-09,7.33e-09,7.72e-09,3.79e-06,3.79e-06,1.76e-06,0,0,0,0,0,0,0,0
-9990000,0.704,0.00196,-0.0134,0.71,-0.0498,0.0319,-0.1,-0.062,0.0533,-365,-1.54e-05,-5.75e-05,1.83e-06,-1.71e-05,1.77e-05,-0.000888,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000491,0.000491,0.000163,1.7,1.7,0.0827,5.4,5.4,0.0857,7.33e-09,7.33e-09,7.61e-09,3.79e-06,3.79e-06,1.66e-06,0,0,0,0,0,0,0,0
+9990000,0.704,0.00196,-0.0134,0.71,-0.0498,0.0319,-0.1,-0.062,0.0533,-365,-1.54e-05,-5.75e-05,1.83e-06,-1.71e-05,1.76e-05,-0.000888,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000491,0.000491,0.000163,1.7,1.7,0.0827,5.4,5.4,0.0857,7.33e-09,7.33e-09,7.61e-09,3.79e-06,3.79e-06,1.66e-06,0,0,0,0,0,0,0,0
 10090000,0.705,0.00197,-0.0134,0.71,-0.0494,0.0306,-0.096,-0.0637,0.0536,-365,-1.53e-05,-5.75e-05,2.07e-06,-1.68e-05,1.9e-05,-0.000914,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000491,0.000491,0.000162,1.74,1.74,0.0801,5.67,5.67,0.0848,7.24e-09,7.24e-09,7.47e-09,3.77e-06,3.77e-06,1.54e-06,0,0,0,0,0,0,0,0
 10190000,0.704,0.00198,-0.0134,0.71,-0.0512,0.0329,-0.0957,-0.0687,0.0568,-366,-1.54e-05,-5.74e-05,8.27e-07,-1.71e-05,1.93e-05,-0.000928,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000502,0.000502,0.000161,1.85,1.85,0.0774,6.28,6.27,0.0839,7.24e-09,7.24e-09,7.33e-09,3.77e-06,3.77e-06,1.44e-06,0,0,0,0,0,0,0,0
-10290000,0.704,0.00193,-0.0134,0.71,-0.0504,0.0315,-0.0832,-0.0697,0.0565,-365,-1.53e-05,-5.74e-05,6.94e-08,-1.73e-05,2.13e-05,-0.000983,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.0005,0.0005,0.000161,1.87,1.87,0.0758,6.51,6.51,0.0847,7.14e-09,7.15e-09,7.21e-09,3.74e-06,3.74e-06,1.35e-06,0,0,0,0,0,0,0,0
+10290000,0.704,0.00193,-0.0134,0.71,-0.0504,0.0315,-0.0832,-0.0697,0.0565,-365,-1.53e-05,-5.74e-05,6.93e-08,-1.73e-05,2.13e-05,-0.000983,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.0005,0.0005,0.000161,1.87,1.87,0.0758,6.51,6.51,0.0847,7.14e-09,7.15e-09,7.21e-09,3.74e-06,3.74e-06,1.35e-06,0,0,0,0,0,0,0,0
 10390000,0.704,0.00191,-0.0133,0.71,0.00936,-0.0198,0.00845,0.000863,-0.00177,-365,-1.53e-05,-5.74e-05,1.5e-07,-1.8e-05,2.18e-05,-0.00101,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000512,0.000512,0.00016,0.252,0.252,0.25,0.252,0.252,0.0753,7.14e-09,7.15e-09,7.07e-09,3.74e-06,3.74e-06,1.26e-06,0,0,0,0,0,0,0,0
 10490000,0.704,0.00195,-0.0133,0.71,0.00799,-0.0184,0.0138,0.00171,-0.00366,-365,-1.53e-05,-5.74e-05,-5.75e-07,-1.87e-05,2.24e-05,-0.00105,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000525,0.000525,0.00016,0.258,0.258,0.248,0.259,0.259,0.0714,7.14e-09,7.15e-09,6.92e-09,3.74e-06,3.74e-06,1.19e-06,0,0,0,0,0,0,0,0
 10590000,0.704,0.00204,-0.0134,0.71,0.00751,-0.0078,0.0178,0.00181,-0.000838,-365,-1.53e-05,-5.72e-05,-4.85e-07,-2.11e-05,2.26e-05,-0.00106,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.00053,0.00053,0.00016,0.132,0.132,0.169,0.131,0.131,0.0672,7.1e-09,7.11e-09,6.77e-09,3.73e-06,3.73e-06,1.14e-06,0,0,0,0,0,0,0,0
@@ -112,13 +112,13 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 10990000,0.704,0.00202,-0.0136,0.71,0.00547,0.000295,0.00905,0.00459,-0.00245,-365,-1.47e-05,-5.68e-05,3.06e-07,-2.79e-05,2.81e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000516,0.000515,0.000159,0.0901,0.0901,0.0927,0.0729,0.0729,0.0653,6.73e-09,6.73e-09,6.21e-09,3.68e-06,3.68e-06,1.04e-06,0,0,0,0,0,0,0,0
 11090000,0.704,0.00202,-0.0135,0.71,0.00393,0.00198,0.0124,0.00507,-0.00238,-365,-1.47e-05,-5.68e-05,1.13e-06,-2.81e-05,2.82e-05,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000528,0.000528,0.000158,0.108,0.108,0.0871,0.0794,0.0794,0.0671,6.73e-09,6.73e-09,6.05e-09,3.68e-06,3.68e-06,1.03e-06,0,0,0,0,0,0,0,0
 11190000,0.704,0.00195,-0.0137,0.71,0.00838,0.0048,0.00301,0.00646,-0.0031,-365,-1.41e-05,-5.67e-05,5.45e-07,-3.1e-05,3.31e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000479,0.000479,0.000158,0.0889,0.0888,0.0709,0.0629,0.0629,0.0641,6.38e-09,6.39e-09,5.9e-09,3.64e-06,3.64e-06,1.01e-06,0,0,0,0,0,0,0,0
-11290000,0.704,0.00204,-0.0138,0.71,0.00769,0.00648,0.00233,0.00728,-0.00253,-365,-1.42e-05,-5.67e-05,-4.52e-07,-3.1e-05,3.32e-05,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000491,0.000491,0.000158,0.109,0.109,0.0668,0.0699,0.0699,0.0665,6.38e-09,6.39e-09,5.77e-09,3.64e-06,3.64e-06,1.01e-06,0,0,0,0,0,0,0,0
-11390000,0.704,0.00209,-0.0136,0.71,0.00372,0.00613,-0.0027,0.00534,-0.00222,-365,-1.45e-05,-5.68e-05,-1.25e-06,-2.84e-05,3.06e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000431,0.000431,0.000158,0.0893,0.0893,0.0559,0.0572,0.0572,0.0636,5.99e-09,5.99e-09,5.62e-09,3.59e-06,3.59e-06,1e-06,0,0,0,0,0,0,0,0
-11490000,0.704,0.00211,-0.0136,0.71,0.000894,0.00835,-0.00232,0.00558,-0.0015,-365,-1.45e-05,-5.68e-05,-2.77e-06,-2.84e-05,3.07e-05,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000442,0.000442,0.000157,0.109,0.109,0.052,0.0647,0.0647,0.0645,5.99e-09,5.99e-09,5.47e-09,3.59e-06,3.59e-06,9.94e-07,0,0,0,0,0,0,0,0
-11590000,0.704,0.00203,-0.0136,0.71,-0.00296,0.00787,-0.00771,0.00433,-0.00159,-365,-1.46e-05,-5.71e-05,-3.09e-06,-2.57e-05,3.07e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.00038,0.00038,0.000158,0.0888,0.0888,0.045,0.0538,0.0538,0.0628,5.58e-09,5.59e-09,5.34e-09,3.56e-06,3.56e-06,9.9e-07,0,0,0,0,0,0,0,0
+11290000,0.704,0.00204,-0.0138,0.71,0.00769,0.00648,0.00232,0.00728,-0.00253,-365,-1.42e-05,-5.67e-05,-4.53e-07,-3.1e-05,3.32e-05,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000491,0.000491,0.000158,0.109,0.109,0.0668,0.0699,0.0699,0.0665,6.38e-09,6.39e-09,5.77e-09,3.64e-06,3.64e-06,1.01e-06,0,0,0,0,0,0,0,0
+11390000,0.704,0.00209,-0.0136,0.71,0.00372,0.00613,-0.00271,0.00534,-0.00222,-365,-1.45e-05,-5.68e-05,-1.25e-06,-2.84e-05,3.06e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000431,0.000431,0.000158,0.0893,0.0893,0.0559,0.0572,0.0572,0.0636,5.99e-09,5.99e-09,5.62e-09,3.59e-06,3.59e-06,1e-06,0,0,0,0,0,0,0,0
+11490000,0.704,0.00211,-0.0136,0.71,0.000894,0.00835,-0.00233,0.00558,-0.0015,-365,-1.45e-05,-5.68e-05,-2.77e-06,-2.84e-05,3.07e-05,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000442,0.000442,0.000157,0.109,0.109,0.052,0.0647,0.0647,0.0645,5.99e-09,5.99e-09,5.47e-09,3.59e-06,3.59e-06,9.94e-07,0,0,0,0,0,0,0,0
+11590000,0.704,0.00203,-0.0136,0.71,-0.00296,0.00787,-0.00772,0.00433,-0.00159,-365,-1.46e-05,-5.71e-05,-3.09e-06,-2.57e-05,3.07e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.00038,0.00038,0.000158,0.0888,0.0888,0.045,0.0538,0.0538,0.0628,5.58e-09,5.59e-09,5.34e-09,3.56e-06,3.56e-06,9.9e-07,0,0,0,0,0,0,0,0
 11690000,0.704,0.00201,-0.0136,0.71,-0.00604,0.0105,-0.0121,0.00386,-0.000701,-365,-1.46e-05,-5.71e-05,-3.55e-06,-2.56e-05,3.06e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.00039,0.00039,0.000157,0.108,0.108,0.0421,0.0618,0.0618,0.0632,5.58e-09,5.59e-09,5.2e-09,3.56e-06,3.56e-06,9.86e-07,0,0,0,0,0,0,0,0
 11790000,0.704,0.00208,-0.0135,0.71,-0.0112,0.0109,-0.0139,0.00173,0.000401,-365,-1.48e-05,-5.7e-05,-3.61e-06,-2.53e-05,2.94e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000334,0.000334,0.000157,0.0866,0.0866,0.0368,0.0518,0.0518,0.0607,5.21e-09,5.22e-09,5.05e-09,3.52e-06,3.53e-06,9.81e-07,0,0,0,0,0,0,0,0
-11890000,0.704,0.00209,-0.0135,0.71,-0.0129,0.0117,-0.015,0.00054,0.00153,-365,-1.48e-05,-5.7e-05,-4.35e-06,-2.53e-05,2.95e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000343,0.000343,0.000157,0.104,0.104,0.0348,0.0601,0.0601,0.0608,5.21e-09,5.22e-09,4.91e-09,3.52e-06,3.53e-06,9.78e-07,0,0,0,0,0,0,0,0
+11890000,0.704,0.00209,-0.0135,0.71,-0.0129,0.0117,-0.015,0.000541,0.00153,-365,-1.48e-05,-5.7e-05,-4.35e-06,-2.53e-05,2.95e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000343,0.000343,0.000157,0.104,0.104,0.0348,0.0601,0.0601,0.0608,5.21e-09,5.22e-09,4.91e-09,3.52e-06,3.53e-06,9.78e-07,0,0,0,0,0,0,0,0
 11990000,0.704,0.00211,-0.0135,0.71,-0.0143,0.0122,-0.0201,-0.000414,0.00219,-365,-1.47e-05,-5.71e-05,-4.16e-06,-2.49e-05,3.03e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000296,0.000296,0.000157,0.0831,0.0831,0.0315,0.0507,0.0507,0.0594,4.89e-09,4.89e-09,4.79e-09,3.5e-06,3.5e-06,9.73e-07,0,0,0,0,0,0,0,0
 12090000,0.704,0.00212,-0.0135,0.71,-0.0158,0.0142,-0.0257,-0.00191,0.00348,-365,-1.47e-05,-5.71e-05,-3.44e-06,-2.48e-05,3.02e-05,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000304,0.000304,0.000156,0.0988,0.0988,0.0301,0.0591,0.0591,0.0593,4.89e-09,4.89e-09,4.65e-09,3.5e-06,3.5e-06,9.71e-07,0,0,0,0,0,0,0,0
 12190000,0.704,0.0018,-0.0136,0.71,-0.0096,0.0118,-0.0208,0.00116,0.00193,-365,-1.4e-05,-5.76e-05,-3.16e-06,-2.28e-05,3.6e-05,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000265,0.000265,0.000156,0.0788,0.0788,0.0276,0.05,0.05,0.0572,4.61e-09,4.62e-09,4.52e-09,3.49e-06,3.49e-06,9.63e-07,0,0,0,0,0,0,0,0
@@ -129,22 +129,22 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 12690000,0.704,0.00158,-0.0134,0.71,-0.0159,0.0118,-0.031,-0.00457,0.00271,-365,-1.4e-05,-5.83e-05,-4.47e-06,-1.93e-05,3.79e-05,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.00023,0.00023,0.000155,0.0806,0.0806,0.0246,0.0576,0.0576,0.0543,4.17e-09,4.17e-09,3.92e-09,3.47e-06,3.47e-06,9.32e-07,0,0,0,0,0,0,0,0
 12790000,0.704,0.00162,-0.0132,0.71,-0.0208,0.00879,-0.0344,-0.00774,0.00147,-365,-1.43e-05,-5.86e-05,-4.16e-06,-1.8e-05,3.74e-05,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000211,0.000211,0.000154,0.0654,0.0654,0.024,0.049,0.049,0.0527,3.99e-09,3.99e-09,3.8e-09,3.47e-06,3.47e-06,9.15e-07,0,0,0,0,0,0,0,0
 12890000,0.704,0.00158,-0.0132,0.71,-0.022,0.00868,-0.0335,-0.00987,0.00232,-365,-1.43e-05,-5.86e-05,-3.98e-06,-1.81e-05,3.75e-05,-0.00109,0.209,0.00206,0.432,0,0,0,0,0,0.000159,0.000217,0.000217,0.000154,0.0751,0.0751,0.0247,0.0573,0.0573,0.0532,3.99e-09,3.99e-09,3.7e-09,3.47e-06,3.47e-06,9.12e-07,0,0,0,0,0,0,0,0
-12990000,0.704,0.00125,-0.0135,0.71,-0.00984,0.0063,-0.0341,-0.00133,0.00132,-365,-1.33e-05,-5.89e-05,-3.14e-06,-1.89e-05,4.04e-05,-0.0011,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000201,0.000201,0.000154,0.0614,0.0614,0.0244,0.0488,0.0488,0.0518,3.82e-09,3.82e-09,3.59e-09,3.47e-06,3.47e-06,8.91e-07,0,0,0,0,0,0,0,0
+12990000,0.704,0.00125,-0.0135,0.71,-0.00984,0.0063,-0.0341,-0.00133,0.00132,-365,-1.33e-05,-5.89e-05,-3.14e-06,-1.89e-05,4.04e-05,-0.00109,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000201,0.000201,0.000154,0.0614,0.0614,0.0244,0.0488,0.0488,0.0518,3.82e-09,3.82e-09,3.59e-09,3.47e-06,3.47e-06,8.91e-07,0,0,0,0,0,0,0,0
 13090000,0.704,0.00126,-0.0134,0.71,-0.0107,0.00646,-0.0343,-0.00235,0.00196,-365,-1.33e-05,-5.89e-05,-3.91e-06,-1.89e-05,4.05e-05,-0.0011,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000207,0.000207,0.000153,0.0702,0.0702,0.0252,0.0569,0.0569,0.0517,3.82e-09,3.82e-09,3.49e-09,3.47e-06,3.47e-06,8.85e-07,0,0,0,0,0,0,0,0
-13190000,0.704,0.00103,-0.0135,0.71,-0.00165,0.00592,-0.0311,0.00402,0.0013,-365,-1.26e-05,-5.91e-05,-3.82e-06,-1.98e-05,4.17e-05,-0.00112,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000194,0.000194,0.000153,0.0578,0.0578,0.025,0.0486,0.0486,0.0504,3.67e-09,3.67e-09,3.38e-09,3.47e-06,3.47e-06,8.6e-07,0,0,0,0,0,0,0,0
+13190000,0.704,0.00103,-0.0135,0.71,-0.00165,0.00592,-0.0312,0.00402,0.0013,-365,-1.26e-05,-5.91e-05,-3.82e-06,-1.98e-05,4.17e-05,-0.00112,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.000194,0.000194,0.000153,0.0578,0.0578,0.025,0.0486,0.0486,0.0504,3.67e-09,3.67e-09,3.38e-09,3.47e-06,3.47e-06,8.6e-07,0,0,0,0,0,0,0,0
 13290000,0.704,0.00104,-0.0135,0.71,-0.00209,0.00674,-0.0278,0.00384,0.00194,-365,-1.26e-05,-5.91e-05,-3.06e-06,-2e-05,4.19e-05,-0.00113,0.209,0.00206,0.432,0,0,0,0,0,0.000158,0.0002,0.0002,0.000153,0.0658,0.0658,0.0261,0.0566,0.0566,0.0511,3.67e-09,3.67e-09,3.29e-09,3.47e-06,3.47e-06,8.54e-07,0,0,0,0,0,0,0,0
 13390000,0.704,0.000886,-0.0135,0.71,-0.00101,0.00579,-0.0244,0.00294,0.00119,-365,-1.24e-05,-5.93e-05,-2.42e-06,-2.05e-05,4.15e-05,-0.00114,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000189,0.000189,0.000152,0.0546,0.0546,0.026,0.0484,0.0484,0.05,3.52e-09,3.52e-09,3.19e-09,3.47e-06,3.47e-06,8.26e-07,0,0,0,0,0,0,0,0
 13490000,0.704,0.000914,-0.0135,0.71,-0.00168,0.00568,-0.023,0.00283,0.00175,-365,-1.24e-05,-5.93e-05,-1.9e-06,-2.06e-05,4.16e-05,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000195,0.000195,0.000152,0.0619,0.0619,0.027,0.0562,0.0562,0.0502,3.52e-09,3.52e-09,3.09e-09,3.47e-06,3.47e-06,8.18e-07,0,0,0,0,0,0,0,0
 13590000,0.704,0.000857,-0.0134,0.71,-0.0012,0.00597,-0.0253,0.00204,0.00116,-365,-1.23e-05,-5.94e-05,-2.28e-06,-2.09e-05,4.07e-05,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000186,0.000186,0.000152,0.0517,0.0517,0.027,0.0482,0.0482,0.0498,3.38e-09,3.38e-09,3.01e-09,3.47e-06,3.47e-06,7.86e-07,0,0,0,0,0,0,0,0
 13690000,0.704,0.000831,-0.0134,0.71,-0.00069,0.00772,-0.0299,0.00193,0.00182,-365,-1.23e-05,-5.94e-05,-1.49e-06,-2.09e-05,4.07e-05,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000157,0.000191,0.000191,0.000151,0.0586,0.0586,0.0281,0.0559,0.0559,0.0501,3.38e-09,3.38e-09,2.92e-09,3.47e-06,3.47e-06,7.77e-07,0,0,0,0,0,0,0,0
 13790000,0.704,0.000721,-0.0133,0.71,9.19e-05,0.00364,-0.0313,0.00314,-0.00049,-365,-1.21e-05,-5.98e-05,-1.49e-06,-2.24e-05,4.01e-05,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000183,0.000183,0.000151,0.0493,0.0493,0.0279,0.048,0.048,0.0493,3.24e-09,3.24e-09,2.83e-09,3.47e-06,3.47e-06,7.42e-07,0,0,0,0,0,0,0,0
-13890000,0.704,0.000689,-0.0133,0.71,0.000425,0.00355,-0.0356,0.00316,-0.000152,-365,-1.21e-05,-5.98e-05,-1.02e-06,-2.23e-05,4e-05,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000189,0.000189,0.00015,0.0557,0.0557,0.0292,0.0555,0.0555,0.0503,3.24e-09,3.24e-09,2.76e-09,3.47e-06,3.47e-06,7.32e-07,0,0,0,0,0,0,0,0
+13890000,0.704,0.000689,-0.0133,0.71,0.000425,0.00355,-0.0356,0.00316,-0.000152,-365,-1.21e-05,-5.98e-05,-1.03e-06,-2.23e-05,4e-05,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000189,0.000189,0.00015,0.0557,0.0557,0.0292,0.0555,0.0555,0.0503,3.24e-09,3.24e-09,2.76e-09,3.47e-06,3.47e-06,7.32e-07,0,0,0,0,0,0,0,0
 13990000,0.704,0.000623,-0.0133,0.71,0.000882,0.00111,-0.0348,0.00413,-0.00188,-365,-1.19e-05,-6.01e-05,-9.06e-07,-2.4e-05,3.93e-05,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000156,0.000181,0.000181,0.00015,0.0472,0.0472,0.0288,0.0478,0.0478,0.0495,3.1e-09,3.1e-09,2.68e-09,3.46e-06,3.46e-06,6.95e-07,0,0,0,0,0,0,0,0
-14090000,0.704,0.000605,-0.0133,0.71,0.000777,0.00126,-0.0361,0.00419,-0.00178,-365,-1.19e-05,-6.01e-05,-6.95e-08,-2.39e-05,3.92e-05,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000155,0.000187,0.000187,0.000149,0.0533,0.0533,0.0298,0.0551,0.0551,0.05,3.1e-09,3.1e-09,2.6e-09,3.46e-06,3.46e-06,6.83e-07,0,0,0,0,0,0,0,0
+14090000,0.704,0.000605,-0.0133,0.71,0.000777,0.00126,-0.0361,0.00419,-0.00178,-365,-1.19e-05,-6.01e-05,-6.97e-08,-2.39e-05,3.92e-05,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000155,0.000187,0.000187,0.000149,0.0533,0.0533,0.0298,0.0551,0.0551,0.05,3.1e-09,3.1e-09,2.6e-09,3.46e-06,3.46e-06,6.83e-07,0,0,0,0,0,0,0,0
 14190000,0.705,0.0005,-0.0134,0.71,0.0043,0.000704,-0.0377,0.00642,-0.00152,-365,-1.15e-05,-6.02e-05,3.69e-07,-2.43e-05,3.68e-05,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000155,0.00018,0.00018,0.000149,0.0454,0.0454,0.0295,0.0476,0.0476,0.0499,2.95e-09,2.95e-09,2.53e-09,3.46e-06,3.46e-06,6.47e-07,0,0,0,0,0,0,0,0
 14290000,0.705,0.00051,-0.0133,0.709,0.00491,0.00148,-0.0367,0.00688,-0.00143,-365,-1.15e-05,-6.02e-05,6.87e-07,-2.44e-05,3.69e-05,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000155,0.000185,0.000185,0.000149,0.0512,0.0512,0.0304,0.0548,0.0548,0.0505,2.95e-09,2.95e-09,2.45e-09,3.46e-06,3.46e-06,6.34e-07,0,0,0,0,0,0,0,0
 14390000,0.705,0.00042,-0.0133,0.709,0.00689,0.00238,-0.0386,0.00836,-0.00123,-365,-1.12e-05,-6.02e-05,1.5e-06,-2.48e-05,3.48e-05,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000154,0.000179,0.000179,0.000148,0.0439,0.0439,0.0297,0.0474,0.0474,0.0498,2.81e-09,2.81e-09,2.38e-09,3.45e-06,3.45e-06,5.96e-07,0,0,0,0,0,0,0,0
-14490000,0.705,0.000406,-0.0133,0.709,0.00673,0.00361,-0.0419,0.00902,-0.000928,-365,-1.12e-05,-6.02e-05,1.73e-06,-2.47e-05,3.46e-05,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000154,0.000184,0.000184,0.000148,0.0495,0.0495,0.0305,0.0545,0.0545,0.0505,2.81e-09,2.81e-09,2.31e-09,3.45e-06,3.45e-06,5.82e-07,0,0,0,0,0,0,0,0
+14490000,0.705,0.000406,-0.0133,0.709,0.00673,0.00361,-0.042,0.00902,-0.000928,-365,-1.12e-05,-6.02e-05,1.73e-06,-2.47e-05,3.46e-05,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000154,0.000184,0.000184,0.000148,0.0495,0.0495,0.0305,0.0545,0.0545,0.0505,2.81e-09,2.81e-09,2.31e-09,3.45e-06,3.45e-06,5.82e-07,0,0,0,0,0,0,0,0
 14590000,0.705,0.000392,-0.0131,0.709,0.00351,0.00204,-0.0422,0.00568,-0.00234,-365,-1.16e-05,-6.05e-05,1.73e-06,-2.68e-05,3.77e-05,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000154,0.000177,0.000177,0.000148,0.0427,0.0427,0.0299,0.0472,0.0472,0.0503,2.66e-09,2.67e-09,2.25e-09,3.44e-06,3.44e-06,5.47e-07,0,0,0,0,0,0,0,0
 14690000,0.705,0.000352,-0.0131,0.709,0.00475,-0.000877,-0.0387,0.00613,-0.00227,-365,-1.16e-05,-6.05e-05,2.29e-06,-2.7e-05,3.79e-05,-0.00117,0.209,0.00206,0.432,0,0,0,0,0,0.000153,0.000183,0.000182,0.000147,0.0481,0.0481,0.0305,0.0541,0.0541,0.051,2.66e-09,2.67e-09,2.18e-09,3.44e-06,3.44e-06,5.33e-07,0,0,0,0,0,0,0,0
 14790000,0.705,0.000371,-0.0129,0.709,0.00178,-0.00242,-0.0348,0.00343,-0.00329,-365,-1.19e-05,-6.07e-05,2.41e-06,-2.88e-05,4.1e-05,-0.00119,0.209,0.00206,0.432,0,0,0,0,0,0.000153,0.000176,0.000176,0.000147,0.0416,0.0416,0.0295,0.047,0.047,0.0502,2.52e-09,2.52e-09,2.12e-09,3.43e-06,3.43e-06,4.98e-07,0,0,0,0,0,0,0,0
@@ -161,32 +161,32 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 15890000,0.705,0.000199,-0.0128,0.709,0.00295,-0.00273,-0.0283,-0.000959,-0.00302,-365,-1.24e-05,-6.07e-05,2.83e-06,-3.01e-05,4.55e-05,-0.00123,0.209,0.00206,0.432,0,0,0,0,0,0.000148,0.00017,0.00017,0.000142,0.0432,0.0432,0.026,0.0527,0.0527,0.052,1.79e-09,1.79e-09,1.56e-09,3.36e-06,3.36e-06,2.87e-07,0,0,0,0,0,0,0,0
 15990000,0.705,0.000138,-0.0128,0.709,0.00295,-0.00364,-0.0235,-0.000917,-0.00379,-365,-1.24e-05,-6.08e-05,3.34e-06,-3.2e-05,4.64e-05,-0.00124,0.209,0.00206,0.432,0,0,0,0,0,0.000148,0.000163,0.000163,0.000142,0.038,0.038,0.0248,0.0461,0.0461,0.051,1.65e-09,1.65e-09,1.52e-09,3.34e-06,3.34e-06,2.67e-07,0,0,0,0,0,0,0,0
 16090000,0.705,0.000139,-0.0128,0.709,0.00462,-0.00382,-0.0199,-0.00055,-0.00419,-365,-1.24e-05,-6.09e-05,4.04e-06,-3.21e-05,4.66e-05,-0.00125,0.209,0.00206,0.432,0,0,0,0,0,0.000148,0.000167,0.000167,0.000141,0.0427,0.0427,0.0248,0.0526,0.0526,0.0514,1.65e-09,1.65e-09,1.48e-09,3.34e-06,3.34e-06,2.57e-07,0,0,0,0,0,0,0,0
-16190000,0.705,0.000157,-0.0128,0.709,0.00474,-0.00307,-0.0184,-0.000653,-0.00339,-365,-1.25e-05,-6.08e-05,4.11e-06,-3.1e-05,4.78e-05,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000147,0.00016,0.00016,0.000141,0.0375,0.0375,0.0238,0.046,0.046,0.051,1.52e-09,1.52e-09,1.44e-09,3.32e-06,3.32e-06,2.4e-07,0,0,0,0,0,0,0,0
-16290000,0.706,0.000176,-0.0128,0.708,0.00628,-0.00385,-0.0197,-9.62e-05,-0.00373,-365,-1.25e-05,-6.08e-05,4.75e-06,-3.1e-05,4.77e-05,-0.00125,0.209,0.00206,0.432,0,0,0,0,0,0.000147,0.000164,0.000164,0.000141,0.0421,0.0421,0.0237,0.0525,0.0525,0.0513,1.52e-09,1.52e-09,1.4e-09,3.32e-06,3.32e-06,2.31e-07,0,0,0,0,0,0,0,0
+16190000,0.705,0.000157,-0.0128,0.709,0.00474,-0.00307,-0.0185,-0.000653,-0.00339,-365,-1.25e-05,-6.08e-05,4.11e-06,-3.1e-05,4.77e-05,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000147,0.00016,0.00016,0.000141,0.0375,0.0375,0.0238,0.046,0.046,0.051,1.52e-09,1.52e-09,1.44e-09,3.32e-06,3.32e-06,2.4e-07,0,0,0,0,0,0,0,0
+16290000,0.706,0.000176,-0.0128,0.708,0.00628,-0.00385,-0.0197,-9.63e-05,-0.00373,-365,-1.25e-05,-6.08e-05,4.75e-06,-3.1e-05,4.77e-05,-0.00125,0.209,0.00206,0.432,0,0,0,0,0,0.000147,0.000164,0.000164,0.000141,0.0421,0.0421,0.0237,0.0525,0.0525,0.0513,1.52e-09,1.52e-09,1.4e-09,3.32e-06,3.32e-06,2.31e-07,0,0,0,0,0,0,0,0
 16390000,0.706,0.000158,-0.0128,0.708,0.00529,-0.00411,-0.0187,-0.000322,-0.00298,-365,-1.26e-05,-6.07e-05,4.52e-06,-2.98e-05,4.93e-05,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000146,0.000157,0.000157,0.00014,0.0371,0.0371,0.0226,0.0459,0.0459,0.0503,1.39e-09,1.39e-09,1.37e-09,3.3e-06,3.3e-06,2.15e-07,0,0,0,0,0,0,0,0
 16490000,0.706,0.000176,-0.0128,0.708,0.00441,-0.00363,-0.0216,0.000135,-0.00336,-365,-1.26e-05,-6.07e-05,4.62e-06,-2.97e-05,4.92e-05,-0.00125,0.209,0.00206,0.432,0,0,0,0,0,0.000146,0.00016,0.00016,0.00014,0.0416,0.0416,0.0227,0.0523,0.0523,0.0513,1.39e-09,1.39e-09,1.33e-09,3.3e-06,3.3e-06,2.08e-07,0,0,0,0,0,0,0,0
-16590000,0.706,0.000418,-0.0128,0.708,0.000946,-0.000919,-0.0219,-0.00271,-2.83e-06,-365,-1.31e-05,-6.02e-05,4.68e-06,-2.36e-05,5.51e-05,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000146,0.000154,0.000153,0.00014,0.0366,0.0366,0.0217,0.0458,0.0458,0.0502,1.27e-09,1.27e-09,1.3e-09,3.28e-06,3.28e-06,1.94e-07,0,0,0,0,0,0,0,0
+16590000,0.706,0.000418,-0.0128,0.708,0.000946,-0.000919,-0.0219,-0.00271,-2.82e-06,-365,-1.31e-05,-6.02e-05,4.68e-06,-2.36e-05,5.51e-05,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000146,0.000154,0.000153,0.00014,0.0366,0.0366,0.0217,0.0458,0.0458,0.0502,1.27e-09,1.27e-09,1.3e-09,3.28e-06,3.28e-06,1.94e-07,0,0,0,0,0,0,0,0
 16690000,0.706,0.000409,-0.0127,0.708,0.00107,-0.00042,-0.0183,-0.00259,-6.62e-05,-365,-1.31e-05,-6.02e-05,4.37e-06,-2.37e-05,5.53e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000145,0.000157,0.000156,0.000139,0.041,0.041,0.0215,0.0522,0.0522,0.0504,1.27e-09,1.27e-09,1.26e-09,3.28e-06,3.28e-06,1.86e-07,0,0,0,0,0,0,0,0
 16790000,0.706,0.000543,-0.0127,0.708,-0.00219,0.00177,-0.0173,-0.00488,0.0026,-365,-1.35e-05,-5.98e-05,4.38e-06,-1.86e-05,6.01e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000145,0.00015,0.00015,0.000139,0.0361,0.0361,0.0207,0.0458,0.0458,0.0501,1.16e-09,1.16e-09,1.24e-09,3.26e-06,3.26e-06,1.75e-07,0,0,0,0,0,0,0,0
 16890000,0.706,0.000562,-0.0127,0.708,-0.00254,0.00266,-0.0146,-0.00511,0.00281,-365,-1.35e-05,-5.98e-05,4.2e-06,-1.87e-05,6.02e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000145,0.000153,0.000153,0.000139,0.0404,0.0404,0.0205,0.0521,0.0521,0.0502,1.16e-09,1.16e-09,1.2e-09,3.26e-06,3.26e-06,1.68e-07,0,0,0,0,0,0,0,0
 16990000,0.706,0.000499,-0.0126,0.709,-0.00239,0.000624,-0.0138,-0.00546,0.000937,-365,-1.36e-05,-6e-05,3.83e-06,-2.19e-05,6.17e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000144,0.000146,0.000146,0.000138,0.0356,0.0356,0.0196,0.0457,0.0457,0.0492,1.05e-09,1.06e-09,1.17e-09,3.24e-06,3.25e-06,1.58e-07,0,0,0,0,0,0,0,0
 17090000,0.706,0.000467,-0.0126,0.709,-0.00166,0.00161,-0.0137,-0.00566,0.00102,-365,-1.36e-05,-6e-05,3.87e-06,-2.19e-05,6.17e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000144,0.000149,0.000149,0.000138,0.0398,0.0398,0.0194,0.052,0.052,0.0494,1.06e-09,1.06e-09,1.14e-09,3.24e-06,3.25e-06,1.52e-07,0,0,0,0,0,0,0,0
-17190000,0.706,0.000452,-0.0126,0.708,-0.00112,0.00155,-0.0143,-0.00589,-0.000449,-365,-1.37e-05,-6.02e-05,4.08e-06,-2.47e-05,6.32e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000143,0.000143,0.000143,0.000137,0.035,0.035,0.0187,0.0456,0.0456,0.049,9.59e-10,9.59e-10,1.12e-09,3.23e-06,3.23e-06,1.43e-07,0,0,0,0,0,0,0,0
-17290000,0.706,0.000432,-0.0125,0.709,0.000926,0.00262,-0.0098,-0.0059,-0.000256,-365,-1.37e-05,-6.02e-05,3.77e-06,-2.47e-05,6.33e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000143,0.000145,0.000145,0.000137,0.0391,0.0391,0.0185,0.0519,0.0519,0.0491,9.59e-10,9.59e-10,1.09e-09,3.23e-06,3.23e-06,1.38e-07,0,0,0,0,0,0,0,0
+17190000,0.706,0.000452,-0.0126,0.708,-0.00112,0.00155,-0.0143,-0.00589,-0.000449,-365,-1.37e-05,-6.02e-05,4.08e-06,-2.46e-05,6.32e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000143,0.000143,0.000143,0.000137,0.035,0.035,0.0187,0.0456,0.0456,0.049,9.59e-10,9.59e-10,1.12e-09,3.23e-06,3.23e-06,1.43e-07,0,0,0,0,0,0,0,0
+17290000,0.706,0.000432,-0.0125,0.709,0.000925,0.00262,-0.0098,-0.0059,-0.000256,-365,-1.37e-05,-6.02e-05,3.77e-06,-2.47e-05,6.33e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000143,0.000145,0.000145,0.000137,0.0391,0.0391,0.0185,0.0519,0.0519,0.0491,9.59e-10,9.59e-10,1.09e-09,3.23e-06,3.23e-06,1.38e-07,0,0,0,0,0,0,0,0
 17390000,0.706,0.000394,-0.0125,0.708,0.00168,0.00176,-0.00775,-0.0049,-0.00155,-365,-1.36e-05,-6.04e-05,4.09e-06,-2.76e-05,6.32e-05,-0.00129,0.209,0.00206,0.432,0,0,0,0,0,0.000143,0.000139,0.000139,0.000137,0.0344,0.0344,0.0177,0.0455,0.0455,0.0481,8.7e-10,8.7e-10,1.06e-09,3.21e-06,3.21e-06,1.29e-07,0,0,0,0,0,0,0,0
 17490000,0.706,0.000391,-0.0125,0.708,0.00216,0.00135,-0.00595,-0.00473,-0.00141,-365,-1.36e-05,-6.04e-05,4.12e-06,-2.77e-05,6.32e-05,-0.00129,0.209,0.00206,0.432,0,0,0,0,0,0.000142,0.000142,0.000142,0.000136,0.0383,0.0383,0.0177,0.0518,0.0518,0.0488,8.7e-10,8.7e-10,1.04e-09,3.21e-06,3.21e-06,1.25e-07,0,0,0,0,0,0,0,0
-17590000,0.706,0.000301,-0.0124,0.708,0.00352,0.00015,-0.00041,-0.00395,-0.00252,-365,-1.36e-05,-6.06e-05,4.2e-06,-3.01e-05,6.34e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000142,0.000136,0.000136,0.000136,0.0338,0.0338,0.017,0.0455,0.0455,0.0478,7.89e-10,7.89e-10,1.01e-09,3.19e-06,3.19e-06,1.18e-07,0,0,0,0,0,0,0,0
-17690000,0.706,0.000272,-0.0124,0.708,0.00436,0.000877,-0.000986,-0.00355,-0.00249,-365,-1.36e-05,-6.06e-05,4.32e-06,-3.01e-05,6.34e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000142,0.000138,0.000138,0.000136,0.0376,0.0376,0.0167,0.0517,0.0517,0.0478,7.89e-10,7.89e-10,9.86e-10,3.19e-06,3.19e-06,1.14e-07,0,0,0,0,0,0,0,0
+17590000,0.706,0.000301,-0.0124,0.708,0.00352,0.00015,-0.000412,-0.00395,-0.00252,-365,-1.36e-05,-6.06e-05,4.2e-06,-3.01e-05,6.34e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000142,0.000136,0.000136,0.000136,0.0338,0.0338,0.017,0.0455,0.0455,0.0478,7.89e-10,7.89e-10,1.01e-09,3.19e-06,3.19e-06,1.18e-07,0,0,0,0,0,0,0,0
+17690000,0.706,0.000272,-0.0124,0.708,0.00436,0.000877,-0.000987,-0.00355,-0.00249,-365,-1.36e-05,-6.06e-05,4.32e-06,-3.01e-05,6.34e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000142,0.000138,0.000138,0.000136,0.0376,0.0376,0.0167,0.0517,0.0517,0.0478,7.89e-10,7.89e-10,9.86e-10,3.19e-06,3.19e-06,1.14e-07,0,0,0,0,0,0,0,0
 17790000,0.706,0.000178,-0.0124,0.708,0.00703,0.000567,-0.00219,-0.00231,-0.00213,-365,-1.34e-05,-6.06e-05,5e-06,-3.01e-05,6.06e-05,-0.00129,0.209,0.00206,0.432,0,0,0,0,0,0.000141,0.000133,0.000133,0.000135,0.0331,0.0331,0.0162,0.0454,0.0454,0.0475,7.15e-10,7.15e-10,9.65e-10,3.18e-06,3.18e-06,1.08e-07,0,0,0,0,0,0,0,0
-17890000,0.706,0.000188,-0.0124,0.708,0.00851,-0.000176,-0.00205,-0.00153,-0.00208,-365,-1.34e-05,-6.06e-05,5.25e-06,-3.01e-05,6.06e-05,-0.00129,0.209,0.00206,0.432,0,0,0,0,0,0.000141,0.000135,0.000135,0.000135,0.0368,0.0368,0.016,0.0516,0.0516,0.0475,7.15e-10,7.15e-10,9.41e-10,3.18e-06,3.18e-06,1.04e-07,0,0,0,0,0,0,0,0
-17990000,0.706,0.000129,-0.0125,0.708,0.0103,-0.00193,-0.000714,-0.000758,-0.0018,-365,-1.33e-05,-6.06e-05,5.15e-06,-2.98e-05,5.9e-05,-0.00129,0.209,0.00206,0.432,0,0,0,0,0,0.00014,0.00013,0.000129,0.000135,0.0325,0.0325,0.0154,0.0453,0.0453,0.0466,6.47e-10,6.48e-10,9.18e-10,3.16e-06,3.17e-06,9.82e-08,0,0,0,0,0,0,0,0
-18090000,0.706,0.000134,-0.0125,0.708,0.0109,-0.00209,0.00168,0.000307,-0.00204,-365,-1.34e-05,-6.06e-05,4.7e-06,-2.99e-05,5.91e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.00014,0.000131,0.000131,0.000134,0.036,0.036,0.0153,0.0514,0.0514,0.0471,6.47e-10,6.48e-10,8.99e-10,3.16e-06,3.17e-06,9.52e-08,0,0,0,0,0,0,0,0
+17890000,0.706,0.000188,-0.0124,0.708,0.00851,-0.000176,-0.00206,-0.00153,-0.00208,-365,-1.34e-05,-6.06e-05,5.25e-06,-3.01e-05,6.06e-05,-0.00129,0.209,0.00206,0.432,0,0,0,0,0,0.000141,0.000135,0.000135,0.000135,0.0368,0.0368,0.016,0.0516,0.0516,0.0475,7.15e-10,7.15e-10,9.41e-10,3.18e-06,3.18e-06,1.04e-07,0,0,0,0,0,0,0,0
+17990000,0.706,0.000129,-0.0125,0.708,0.0103,-0.00193,-0.000715,-0.000758,-0.0018,-365,-1.33e-05,-6.06e-05,5.15e-06,-2.98e-05,5.9e-05,-0.00129,0.209,0.00206,0.432,0,0,0,0,0,0.00014,0.00013,0.000129,0.000135,0.0325,0.0325,0.0154,0.0453,0.0453,0.0466,6.47e-10,6.48e-10,9.18e-10,3.16e-06,3.17e-06,9.82e-08,0,0,0,0,0,0,0,0
+18090000,0.706,0.000134,-0.0125,0.708,0.0109,-0.00209,0.00168,0.000307,-0.00204,-365,-1.34e-05,-6.06e-05,4.69e-06,-2.99e-05,5.91e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.00014,0.000131,0.000131,0.000134,0.036,0.036,0.0153,0.0514,0.0514,0.0471,6.47e-10,6.48e-10,8.99e-10,3.16e-06,3.17e-06,9.52e-08,0,0,0,0,0,0,0,0
 18190000,0.706,9.82e-05,-0.0124,0.708,0.0116,-0.00104,0.00308,0.00123,-0.00158,-365,-1.34e-05,-6.05e-05,4.94e-06,-2.92e-05,5.93e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.00014,0.000126,0.000126,0.000134,0.0318,0.0318,0.0147,0.0452,0.0452,0.0463,5.87e-10,5.87e-10,8.78e-10,3.15e-06,3.15e-06,9.02e-08,0,0,0,0,0,0,0,0
 18290000,0.706,4e-05,-0.0124,0.708,0.0116,-0.00158,0.00426,0.00239,-0.00171,-365,-1.34e-05,-6.05e-05,4.74e-06,-2.93e-05,5.93e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000139,0.000128,0.000128,0.000134,0.0352,0.0352,0.0145,0.0513,0.0513,0.0462,5.87e-10,5.87e-10,8.57e-10,3.15e-06,3.15e-06,8.71e-08,0,0,0,0,0,0,0,0
-18390000,0.706,5.21e-05,-0.0124,0.708,0.013,4.77e-05,0.00551,0.00299,-0.0013,-365,-1.34e-05,-6.05e-05,5.08e-06,-2.88e-05,5.98e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000139,0.000124,0.000123,0.000133,0.0311,0.0311,0.014,0.0451,0.0451,0.0454,5.32e-10,5.32e-10,8.36e-10,3.14e-06,3.14e-06,8.27e-08,0,0,0,0,0,0,0,0
+18390000,0.706,5.21e-05,-0.0124,0.708,0.013,4.76e-05,0.00551,0.00299,-0.0013,-365,-1.34e-05,-6.05e-05,5.08e-06,-2.88e-05,5.98e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000139,0.000124,0.000123,0.000133,0.0311,0.0311,0.014,0.0451,0.0451,0.0454,5.32e-10,5.32e-10,8.36e-10,3.14e-06,3.14e-06,8.27e-08,0,0,0,0,0,0,0,0
 18490000,0.706,6.76e-05,-0.0124,0.708,0.0138,0.000479,0.00517,0.00438,-0.00127,-365,-1.34e-05,-6.05e-05,5.15e-06,-2.88e-05,5.98e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000139,0.000125,0.000125,0.000133,0.0344,0.0344,0.0139,0.0511,0.0511,0.0458,5.32e-10,5.32e-10,8.19e-10,3.14e-06,3.14e-06,8.03e-08,0,0,0,0,0,0,0,0
 18590000,0.706,6.82e-05,-0.0123,0.708,0.0129,0.000693,0.00344,0.00332,-0.00109,-365,-1.36e-05,-6.05e-05,5.56e-06,-2.87e-05,6.27e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000139,0.000121,0.000121,0.000133,0.0304,0.0304,0.0134,0.045,0.045,0.045,4.82e-10,4.83e-10,8e-10,3.13e-06,3.13e-06,7.64e-08,0,0,0,0,0,0,0,0
-18690000,0.706,3.76e-05,-0.0123,0.708,0.0132,1.33e-05,0.00161,0.00462,-0.00103,-365,-1.36e-05,-6.05e-05,5.42e-06,-2.87e-05,6.27e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000138,0.000122,0.000122,0.000132,0.0335,0.0335,0.0133,0.0509,0.0509,0.0449,4.82e-10,4.83e-10,7.82e-10,3.13e-06,3.13e-06,7.39e-08,0,0,0,0,0,0,0,0
+18690000,0.706,3.76e-05,-0.0123,0.708,0.0132,1.33e-05,0.0016,0.00462,-0.00103,-365,-1.36e-05,-6.05e-05,5.42e-06,-2.87e-05,6.27e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000138,0.000122,0.000122,0.000132,0.0335,0.0335,0.0133,0.0509,0.0509,0.0449,4.82e-10,4.83e-10,7.82e-10,3.13e-06,3.13e-06,7.39e-08,0,0,0,0,0,0,0,0
 18790000,0.706,6.4e-05,-0.0123,0.708,0.0117,0.000296,0.00138,0.00353,-0.00082,-365,-1.38e-05,-6.05e-05,5.27e-06,-2.86e-05,6.53e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000138,0.000118,0.000118,0.000132,0.0297,0.0297,0.0129,0.0449,0.0449,0.0447,4.38e-10,4.38e-10,7.66e-10,3.12e-06,3.12e-06,7.08e-08,0,0,0,0,0,0,0,0
 18890000,0.706,8.76e-05,-0.0123,0.708,0.0123,0.000806,0.00204,0.00473,-0.000731,-365,-1.37e-05,-6.05e-05,5.66e-06,-2.86e-05,6.53e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000138,0.00012,0.00012,0.000132,0.0327,0.0327,0.0128,0.0508,0.0508,0.0446,4.38e-10,4.38e-10,7.48e-10,3.12e-06,3.12e-06,6.85e-08,0,0,0,0,0,0,0,0
 18990000,0.707,7.3e-05,-0.0123,0.708,0.0136,0.00169,0.000837,0.00613,-0.000613,-365,-1.37e-05,-6.05e-05,5.82e-06,-2.87e-05,6.44e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000137,0.000116,0.000116,0.000132,0.029,0.029,0.0123,0.0448,0.0448,0.0438,3.98e-10,3.98e-10,7.31e-10,3.11e-06,3.11e-06,6.54e-08,0,0,0,0,0,0,0,0
@@ -194,11 +194,11 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 19190000,0.707,5.9e-05,-0.0121,0.708,0.0143,0.00228,0.00399,0.00838,-0.000362,-365,-1.37e-05,-6.05e-05,5.94e-06,-2.9e-05,6.41e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000137,0.000114,0.000114,0.000131,0.0283,0.0283,0.0119,0.0446,0.0446,0.0435,3.62e-10,3.62e-10,7.01e-10,3.1e-06,3.1e-06,6.09e-08,0,0,0,0,0,0,0,0
 19290000,0.707,8.2e-05,-0.0121,0.708,0.0146,0.00154,0.00673,0.00979,-0.000153,-365,-1.37e-05,-6.05e-05,5.73e-06,-2.9e-05,6.41e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000136,0.000115,0.000115,0.000131,0.0311,0.0311,0.0117,0.0504,0.0504,0.0433,3.62e-10,3.62e-10,6.85e-10,3.1e-06,3.1e-06,5.91e-08,0,0,0,0,0,0,0,0
 19390000,0.707,9.08e-05,-0.012,0.708,0.0122,0.000596,0.0106,0.00784,-0.000191,-365,-1.39e-05,-6.06e-05,5.88e-06,-2.92e-05,6.7e-05,-0.00131,0.209,0.00206,0.432,0,0,0,0,0,0.000136,0.000111,0.000111,0.000131,0.0276,0.0276,0.0115,0.0445,0.0445,0.0431,3.3e-10,3.3e-10,6.72e-10,3.09e-06,3.09e-06,5.69e-08,0,0,0,0,0,0,0,0
-19490000,0.707,0.000114,-0.012,0.707,0.0112,-0.000104,0.00696,0.00899,-0.000169,-365,-1.39e-05,-6.06e-05,6.17e-06,-2.92e-05,6.7e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000136,0.000113,0.000112,0.00013,0.0303,0.0303,0.0113,0.0502,0.0502,0.043,3.3e-10,3.3e-10,6.57e-10,3.09e-06,3.09e-06,5.53e-08,0,0,0,0,0,0,0,0
+19490000,0.707,0.000114,-0.012,0.707,0.0112,-0.000104,0.00696,0.00899,-0.00017,-365,-1.39e-05,-6.06e-05,6.17e-06,-2.92e-05,6.7e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000136,0.000113,0.000112,0.00013,0.0303,0.0303,0.0113,0.0502,0.0502,0.043,3.3e-10,3.3e-10,6.57e-10,3.09e-06,3.09e-06,5.53e-08,0,0,0,0,0,0,0,0
 19590000,0.707,0.00016,-0.0119,0.707,0.00929,-0.00115,0.00631,0.00726,-0.000211,-365,-1.4e-05,-6.06e-05,6.55e-06,-2.92e-05,6.94e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000135,0.00011,0.000109,0.00013,0.0269,0.0269,0.011,0.0444,0.0444,0.0423,3.01e-10,3.01e-10,6.43e-10,3.08e-06,3.08e-06,5.31e-08,0,0,0,0,0,0,0,0
 19690000,0.707,0.00016,-0.012,0.707,0.00964,-0.00333,0.00785,0.00821,-0.00044,-365,-1.4e-05,-6.06e-05,6.32e-06,-2.92e-05,6.94e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000135,0.000111,0.00011,0.00013,0.0295,0.0295,0.0109,0.05,0.05,0.0422,3.01e-10,3.01e-10,6.29e-10,3.08e-06,3.08e-06,5.16e-08,0,0,0,0,0,0,0,0
 19790000,0.707,0.000225,-0.0119,0.707,0.00735,-0.00418,0.00835,0.00665,-0.000354,-365,-1.41e-05,-6.05e-05,6.33e-06,-2.87e-05,7.14e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000135,0.000108,0.000108,0.000129,0.0263,0.0263,0.0106,0.0442,0.0442,0.042,2.75e-10,2.75e-10,6.17e-10,3.07e-06,3.08e-06,5e-08,0,0,0,0,0,0,0,0
-19890000,0.707,0.000172,-0.0119,0.707,0.0061,-0.00445,0.00953,0.00733,-0.0008,-365,-1.41e-05,-6.05e-05,6.75e-06,-2.87e-05,7.13e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000135,0.000109,0.000109,0.000129,0.0288,0.0288,0.0105,0.0498,0.0498,0.0419,2.75e-10,2.76e-10,6.04e-10,3.07e-06,3.08e-06,5e-08,0,0,0,0,0,0,0,0
+19890000,0.707,0.000172,-0.0119,0.707,0.0061,-0.00445,0.00953,0.00733,-0.0008,-365,-1.41e-05,-6.05e-05,6.74e-06,-2.87e-05,7.13e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000135,0.000109,0.000109,0.000129,0.0288,0.0288,0.0105,0.0498,0.0498,0.0419,2.75e-10,2.76e-10,6.04e-10,3.07e-06,3.08e-06,5e-08,0,0,0,0,0,0,0,0
 19990000,0.707,0.000155,-0.0119,0.707,0.00368,-0.00516,0.0123,0.00596,-0.000671,-365,-1.42e-05,-6.05e-05,7.3e-06,-2.81e-05,7.26e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000134,0.000106,0.000106,0.000129,0.0256,0.0256,0.0102,0.0441,0.0441,0.0412,2.52e-10,2.52e-10,5.91e-10,3.07e-06,3.07e-06,5e-08,0,0,0,0,0,0,0,0
 20090000,0.707,0.00015,-0.0119,0.707,0.00344,-0.0071,0.0126,0.00632,-0.00127,-365,-1.42e-05,-6.05e-05,7.76e-06,-2.81e-05,7.26e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000134,0.000107,0.000107,0.000129,0.028,0.028,0.0102,0.0495,0.0495,0.0415,2.52e-10,2.52e-10,5.8e-10,3.07e-06,3.07e-06,5e-08,0,0,0,0,0,0,0,0
 20190000,0.707,0.00025,-0.0118,0.707,0.00114,-0.00779,0.0149,0.00406,-0.000998,-365,-1.43e-05,-6.05e-05,7.95e-06,-2.7e-05,7.47e-05,-0.0013,0.209,0.00206,0.432,0,0,0,0,0,0.000134,0.000104,0.000104,0.000128,0.025,0.025,0.00991,0.0439,0.0439,0.0409,2.31e-10,2.32e-10,5.68e-10,3.06e-06,3.06e-06,5e-08,0,0,0,0,0,0,0,0
@@ -221,9 +221,9 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 21890000,0.708,0.000508,-0.012,0.706,-0.0065,-0.0116,0.0147,-0.00071,-0.0018,-365,-1.42e-05,-5.95e-05,7.98e-06,-9.69e-06,7.33e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.00013,9.57e-05,9.56e-05,0.000124,0.0224,0.0224,0.00818,0.0473,0.0473,0.0376,1.26e-10,1.26e-10,4.1e-10,3.03e-06,3.03e-06,5e-08,0,0,0,0,0,0,0,0
 21990000,0.708,0.000554,-0.0121,0.706,-0.00697,-0.00894,0.0155,-0.00159,0.00158,-365,-1.42e-05,-5.93e-05,7.93e-06,-6.19e-06,7.35e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000129,9.44e-05,9.43e-05,0.000124,0.0202,0.0202,0.00811,0.0423,0.0423,0.0375,1.17e-10,1.17e-10,4.03e-10,3.03e-06,3.03e-06,5e-08,0,0,0,0,0,0,0,0
 22090000,0.708,0.000565,-0.0121,0.706,-0.00732,-0.00807,0.0139,-0.0023,0.000743,-365,-1.42e-05,-5.93e-05,7.85e-06,-6.22e-06,7.35e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000129,9.48e-05,9.48e-05,0.000124,0.0219,0.0219,0.0081,0.0471,0.0471,0.0375,1.17e-10,1.18e-10,3.96e-10,3.03e-06,3.03e-06,5e-08,0,0,0,0,0,0,0,0
-22190000,0.708,0.000536,-0.0121,0.706,-0.0071,-0.00718,0.0143,-0.00192,0.000684,-365,-1.42e-05,-5.92e-05,7.86e-06,-5.69e-06,7.28e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000129,9.36e-05,9.36e-05,0.000124,0.0197,0.0197,0.00799,0.0421,0.0421,0.037,1.1e-10,1.1e-10,3.89e-10,3.02e-06,3.03e-06,5e-08,0,0,0,0,0,0,0,0
-22290000,0.708,0.000576,-0.0121,0.706,-0.00846,-0.00791,0.0143,-0.00268,-7.89e-05,-365,-1.42e-05,-5.92e-05,7.7e-06,-5.71e-06,7.29e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000129,9.41e-05,9.4e-05,0.000123,0.0214,0.0214,0.00799,0.0468,0.0468,0.0369,1.1e-10,1.1e-10,3.82e-10,3.02e-06,3.03e-06,5e-08,0,0,0,0,0,0,0,0
-22390000,0.708,0.00055,-0.0121,0.706,-0.009,-0.0074,0.0161,-0.0023,-8.37e-05,-365,-1.42e-05,-5.92e-05,7.75e-06,-5.14e-06,7.21e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000128,9.29e-05,9.29e-05,0.000123,0.0193,0.0193,0.00793,0.0419,0.0419,0.0369,1.03e-10,1.03e-10,3.76e-10,3.02e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
+22190000,0.708,0.000536,-0.0121,0.706,-0.0071,-0.00718,0.0143,-0.00192,0.000684,-365,-1.42e-05,-5.92e-05,7.86e-06,-5.68e-06,7.28e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000129,9.36e-05,9.36e-05,0.000124,0.0197,0.0197,0.00799,0.0421,0.0421,0.037,1.1e-10,1.1e-10,3.89e-10,3.02e-06,3.03e-06,5e-08,0,0,0,0,0,0,0,0
+22290000,0.708,0.000576,-0.0121,0.706,-0.00846,-0.00791,0.0143,-0.00268,-7.9e-05,-365,-1.42e-05,-5.92e-05,7.7e-06,-5.71e-06,7.29e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000129,9.41e-05,9.4e-05,0.000123,0.0214,0.0214,0.00799,0.0468,0.0468,0.0369,1.1e-10,1.1e-10,3.82e-10,3.02e-06,3.03e-06,5e-08,0,0,0,0,0,0,0,0
+22390000,0.708,0.00055,-0.0121,0.706,-0.009,-0.0074,0.0161,-0.0023,-8.38e-05,-365,-1.42e-05,-5.92e-05,7.75e-06,-5.14e-06,7.21e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000128,9.29e-05,9.29e-05,0.000123,0.0193,0.0193,0.00793,0.0419,0.0419,0.0369,1.03e-10,1.03e-10,3.76e-10,3.02e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
 22490000,0.708,0.000555,-0.012,0.706,-0.00967,-0.00731,0.0173,-0.00323,-0.000839,-365,-1.42e-05,-5.92e-05,7.68e-06,-5.12e-06,7.21e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000128,9.34e-05,9.33e-05,0.000123,0.0209,0.0209,0.00794,0.0466,0.0466,0.0368,1.03e-10,1.04e-10,3.69e-10,3.02e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
 22590000,0.708,0.000535,-0.0121,0.706,-0.00936,-0.00685,0.0164,-0.0035,0.00023,-365,-1.41e-05,-5.91e-05,7.67e-06,-3.93e-06,7.1e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000128,9.23e-05,9.22e-05,0.000123,0.0189,0.0189,0.00784,0.0418,0.0418,0.0364,9.73e-11,9.74e-11,3.62e-10,3.02e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
 22690000,0.708,0.000571,-0.0121,0.706,-0.0106,-0.00658,0.0176,-0.00449,-0.000437,-365,-1.41e-05,-5.91e-05,7.75e-06,-3.94e-06,7.1e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000128,9.27e-05,9.26e-05,0.000123,0.0205,0.0205,0.0079,0.0464,0.0464,0.0367,9.74e-11,9.75e-11,3.57e-10,3.02e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
@@ -232,21 +232,21 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 22990000,0.708,0.000547,-0.0121,0.706,-0.0124,-0.00548,0.0214,-0.00748,-0.000774,-365,-1.41e-05,-5.91e-05,7.32e-06,-3.08e-06,7e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000127,9.12e-05,9.11e-05,0.000122,0.0182,0.0182,0.00779,0.0414,0.0414,0.0362,8.68e-11,8.69e-11,3.39e-10,3.02e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
 23090000,0.708,0.000512,-0.012,0.706,-0.0132,-0.00547,0.022,-0.00878,-0.00131,-365,-1.41e-05,-5.91e-05,7.03e-06,-3.03e-06,7e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000127,9.15e-05,9.14e-05,0.000122,0.0196,0.0196,0.00782,0.0459,0.0459,0.0362,8.69e-11,8.7e-11,3.33e-10,3.02e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
 23190000,0.708,0.000578,-0.012,0.706,-0.0146,-0.00645,0.0236,-0.0121,-0.00118,-365,-1.41e-05,-5.91e-05,6.97e-06,-2.55e-06,7.11e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000127,9.06e-05,9.06e-05,0.000122,0.0179,0.0179,0.00774,0.0412,0.0412,0.0359,8.23e-11,8.23e-11,3.27e-10,3.01e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
-23290000,0.708,0.000517,-0.012,0.706,-0.0153,-0.00769,0.024,-0.0135,-0.00189,-365,-1.41e-05,-5.91e-05,6.95e-06,-2.58e-06,7.12e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000127,9.1e-05,9.09e-05,0.000121,0.0193,0.0193,0.00781,0.0457,0.0457,0.0361,8.24e-11,8.24e-11,3.22e-10,3.01e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
+23290000,0.708,0.000517,-0.012,0.706,-0.0153,-0.00769,0.0239,-0.0135,-0.0019,-365,-1.41e-05,-5.91e-05,6.95e-06,-2.58e-06,7.12e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000127,9.1e-05,9.09e-05,0.000121,0.0193,0.0193,0.00781,0.0457,0.0457,0.0361,8.24e-11,8.24e-11,3.22e-10,3.01e-06,3.02e-06,5e-08,0,0,0,0,0,0,0,0
 23390000,0.708,0.000607,-0.0119,0.706,-0.0162,-0.00793,0.0215,-0.016,-0.00168,-365,-1.42e-05,-5.9e-05,6.89e-06,-2.06e-06,7.2e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000126,9.02e-05,9.01e-05,0.000121,0.0175,0.0175,0.00774,0.041,0.041,0.0358,7.81e-11,7.81e-11,3.17e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
 23490000,0.708,0.003,-0.00951,0.706,-0.0233,-0.00877,-0.012,-0.018,-0.00253,-365,-1.42e-05,-5.9e-05,6.97e-06,-2.1e-06,7.2e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000126,9.05e-05,9.04e-05,0.000121,0.0189,0.0189,0.00778,0.0454,0.0454,0.0358,7.82e-11,7.82e-11,3.12e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
-23590000,0.708,0.00823,-0.0017,0.706,-0.0336,-0.00749,-0.0435,-0.0167,-0.00122,-365,-1.41e-05,-5.9e-05,6.82e-06,-3.96e-07,7.03e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000126,8.97e-05,8.96e-05,0.000121,0.0173,0.0173,0.00771,0.0409,0.0409,0.0355,7.43e-11,7.43e-11,3.06e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
-23690000,0.707,0.00786,0.00407,0.707,-0.0648,-0.016,-0.094,-0.0215,-0.00232,-365,-1.41e-05,-5.9e-05,6.77e-06,-3.17e-07,7.02e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000126,9e-05,8.99e-05,0.000121,0.0187,0.0187,0.00779,0.0452,0.0452,0.0358,7.44e-11,7.44e-11,3.02e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
-23790000,0.707,0.00493,0.00072,0.707,-0.0887,-0.0272,-0.148,-0.0207,-0.00167,-365,-1.39e-05,-5.89e-05,6.78e-06,1.55e-06,6.52e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000125,8.92e-05,8.91e-05,0.000121,0.0172,0.0172,0.00773,0.0407,0.0407,0.0355,7.07e-11,7.08e-11,2.97e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
-23890000,0.707,0.0023,-0.00536,0.708,-0.105,-0.0362,-0.201,-0.0306,-0.00487,-365,-1.39e-05,-5.89e-05,6.69e-06,1.68e-06,6.53e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000125,8.95e-05,8.94e-05,0.000121,0.0186,0.0186,0.00777,0.045,0.045,0.0354,7.08e-11,7.09e-11,2.92e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
+23590000,0.708,0.00823,-0.0017,0.706,-0.0336,-0.00749,-0.0435,-0.0167,-0.00122,-365,-1.41e-05,-5.9e-05,6.82e-06,-3.95e-07,7.03e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000126,8.97e-05,8.96e-05,0.000121,0.0173,0.0173,0.00771,0.0409,0.0409,0.0355,7.43e-11,7.43e-11,3.06e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
+23690000,0.707,0.00786,0.00407,0.707,-0.0648,-0.016,-0.094,-0.0215,-0.00232,-365,-1.41e-05,-5.9e-05,6.77e-06,-3.16e-07,7.02e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000126,9e-05,8.99e-05,0.000121,0.0187,0.0187,0.00779,0.0452,0.0452,0.0358,7.44e-11,7.44e-11,3.02e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
+23790000,0.707,0.00493,0.00072,0.707,-0.0887,-0.0272,-0.148,-0.0207,-0.00167,-365,-1.39e-05,-5.89e-05,6.78e-06,1.56e-06,6.52e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000125,8.92e-05,8.91e-05,0.000121,0.0172,0.0172,0.00773,0.0407,0.0407,0.0355,7.07e-11,7.08e-11,2.97e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
+23890000,0.707,0.0023,-0.00536,0.708,-0.105,-0.0362,-0.201,-0.0306,-0.00487,-365,-1.39e-05,-5.89e-05,6.69e-06,1.68e-06,6.52e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000125,8.95e-05,8.94e-05,0.000121,0.0186,0.0186,0.00777,0.045,0.045,0.0354,7.08e-11,7.09e-11,2.92e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
 23990000,0.707,0.000895,-0.00996,0.708,-0.106,-0.0395,-0.254,-0.0341,-0.0081,-366,-1.37e-05,-5.89e-05,6.73e-06,2.64e-06,6.18e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000125,8.87e-05,8.86e-05,0.00012,0.017,0.017,0.00775,0.0406,0.0406,0.0355,6.75e-11,6.75e-11,2.88e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
 24090000,0.707,0.00218,-0.00869,0.708,-0.108,-0.0398,-0.302,-0.0448,-0.0121,-366,-1.37e-05,-5.89e-05,6.82e-06,2.61e-06,6.18e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000125,8.9e-05,8.89e-05,0.00012,0.0184,0.0184,0.00779,0.0449,0.0449,0.0355,6.76e-11,6.76e-11,2.83e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
 24190000,0.707,0.00325,-0.00644,0.708,-0.11,-0.0408,-0.35,-0.0463,-0.0141,-366,-1.36e-05,-5.88e-05,6.82e-06,4.44e-06,5.72e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000125,8.82e-05,8.81e-05,0.00012,0.0168,0.0168,0.00773,0.0404,0.0404,0.0352,6.45e-11,6.45e-11,2.79e-10,3e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
 24290000,0.707,0.00374,-0.00562,0.708,-0.121,-0.0447,-0.405,-0.0579,-0.0184,-366,-1.36e-05,-5.88e-05,6.72e-06,4.57e-06,5.73e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.84e-05,8.83e-05,0.00012,0.0182,0.0182,0.00782,0.0447,0.0447,0.0355,6.46e-11,6.46e-11,2.75e-10,3e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
 24390000,0.706,0.00379,-0.00583,0.708,-0.129,-0.052,-0.457,-0.0638,-0.0299,-366,-1.35e-05,-5.89e-05,6.53e-06,1.87e-06,5.44e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.76e-05,8.75e-05,0.00012,0.0167,0.0167,0.00776,0.0403,0.0403,0.0352,6.17e-11,6.17e-11,2.7e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
-24490000,0.706,0.00466,-0.00168,0.708,-0.143,-0.0573,-0.507,-0.0773,-0.0353,-366,-1.35e-05,-5.89e-05,6.47e-06,2.01e-06,5.44e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.79e-05,8.78e-05,0.00012,0.018,0.018,0.00781,0.0445,0.0445,0.0352,6.18e-11,6.18e-11,2.66e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
-24590000,0.707,0.00511,0.00194,0.708,-0.157,-0.0684,-0.558,-0.0808,-0.0446,-366,-1.33e-05,-5.89e-05,6.6e-06,7.52e-07,4.82e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.7e-05,8.69e-05,0.00012,0.0165,0.0165,0.00779,0.0402,0.0402,0.0353,5.92e-11,5.92e-11,2.62e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
-24690000,0.707,0.00515,0.00289,0.708,-0.182,-0.0822,-0.641,-0.0977,-0.0521,-366,-1.33e-05,-5.9e-05,6.69e-06,5.55e-07,4.82e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.73e-05,8.71e-05,0.000119,0.0179,0.0179,0.00784,0.0444,0.0444,0.0353,5.93e-11,5.93e-11,2.58e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
+24490000,0.706,0.00466,-0.00168,0.708,-0.143,-0.0573,-0.507,-0.0773,-0.0353,-366,-1.35e-05,-5.89e-05,6.47e-06,2.02e-06,5.44e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.79e-05,8.78e-05,0.00012,0.018,0.018,0.00781,0.0445,0.0445,0.0352,6.18e-11,6.18e-11,2.66e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
+24590000,0.707,0.00511,0.00194,0.708,-0.157,-0.0684,-0.558,-0.0808,-0.0446,-366,-1.33e-05,-5.89e-05,6.6e-06,7.54e-07,4.82e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.7e-05,8.69e-05,0.00012,0.0165,0.0165,0.00779,0.0402,0.0402,0.0353,5.92e-11,5.92e-11,2.62e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
+24690000,0.707,0.00515,0.00289,0.708,-0.182,-0.0822,-0.641,-0.0977,-0.0521,-366,-1.33e-05,-5.9e-05,6.69e-06,5.56e-07,4.82e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.73e-05,8.71e-05,0.000119,0.0179,0.0179,0.00784,0.0444,0.0444,0.0353,5.93e-11,5.93e-11,2.58e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 24790000,0.706,0.00486,0.00152,0.708,-0.198,-0.0945,-0.724,-0.105,-0.0632,-366,-1.3e-05,-5.89e-05,6.51e-06,4.59e-06,4.07e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000124,8.64e-05,8.63e-05,0.000119,0.0165,0.0164,0.00778,0.0401,0.0401,0.035,5.68e-11,5.68e-11,2.54e-10,2.99e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 24890000,0.706,0.00661,0.00321,0.708,-0.221,-0.106,-0.748,-0.126,-0.0732,-366,-1.3e-05,-5.89e-05,6.38e-06,4.8e-06,4.07e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000123,8.66e-05,8.65e-05,0.000119,0.0177,0.0177,0.00783,0.0443,0.0443,0.0351,5.69e-11,5.69e-11,2.5e-10,2.99e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 24990000,0.706,0.00842,0.00481,0.708,-0.238,-0.114,-0.805,-0.129,-0.0812,-366,-1.27e-05,-5.88e-05,6.19e-06,1.3e-05,2.69e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000123,8.57e-05,8.56e-05,0.000119,0.0163,0.0162,0.00781,0.04,0.04,0.0351,5.46e-11,5.46e-11,2.47e-10,2.99e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
@@ -254,22 +254,22 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 25190000,0.706,0.00818,0.00279,0.708,-0.291,-0.137,-0.904,-0.173,-0.119,-366,-1.26e-05,-5.89e-05,6.07e-06,1e-05,2.2e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000123,8.51e-05,8.49e-05,0.000119,0.0161,0.016,0.00781,0.0399,0.0399,0.0349,5.26e-11,5.26e-11,2.4e-10,2.99e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
 25290000,0.706,0.0101,0.00961,0.708,-0.321,-0.146,-0.958,-0.204,-0.133,-366,-1.26e-05,-5.89e-05,6.05e-06,1.01e-05,2.2e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000123,8.53e-05,8.51e-05,0.000119,0.0173,0.0173,0.0079,0.044,0.044,0.0353,5.27e-11,5.27e-11,2.36e-10,2.99e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
 25390000,0.706,0.0114,0.016,0.708,-0.351,-0.166,-1.01,-0.216,-0.153,-366,-1.23e-05,-5.89e-05,6.06e-06,1.21e-05,7.73e-06,-0.00129,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.44e-05,8.42e-05,0.000118,0.0159,0.0159,0.00784,0.0398,0.0398,0.035,5.07e-11,5.07e-11,2.33e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
-25490000,0.706,0.0116,0.0172,0.707,-0.4,-0.19,-1.06,-0.253,-0.171,-366,-1.23e-05,-5.89e-05,6.11e-06,1.19e-05,7.82e-06,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.46e-05,8.44e-05,0.000118,0.0172,0.0171,0.00789,0.0439,0.0439,0.0351,5.08e-11,5.08e-11,2.29e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
-25590000,0.707,0.011,0.0153,0.707,-0.439,-0.219,-1.12,-0.28,-0.208,-367,-1.21e-05,-5.91e-05,6.09e-06,9.35e-06,-6.82e-07,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.37e-05,8.36e-05,0.000118,0.0158,0.0157,0.00788,0.0397,0.0397,0.0351,4.9e-11,4.9e-11,2.26e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
-25690000,0.706,0.0146,0.022,0.707,-0.488,-0.239,-1.17,-0.326,-0.231,-367,-1.21e-05,-5.91e-05,6.08e-06,9.23e-06,-4.68e-07,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.39e-05,8.38e-05,0.000118,0.017,0.0169,0.00793,0.0437,0.0437,0.0352,4.91e-11,4.91e-11,2.23e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
+25490000,0.706,0.0116,0.0172,0.707,-0.4,-0.19,-1.06,-0.253,-0.171,-366,-1.23e-05,-5.89e-05,6.11e-06,1.19e-05,7.82e-06,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.46e-05,8.45e-05,0.000118,0.0172,0.0171,0.00789,0.0439,0.0439,0.0351,5.08e-11,5.08e-11,2.29e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
+25590000,0.707,0.011,0.0153,0.707,-0.439,-0.219,-1.12,-0.28,-0.208,-367,-1.21e-05,-5.91e-05,6.08e-06,9.35e-06,-6.84e-07,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.37e-05,8.36e-05,0.000118,0.0158,0.0157,0.00788,0.0397,0.0397,0.0351,4.9e-11,4.9e-11,2.26e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
+25690000,0.706,0.0146,0.022,0.707,-0.488,-0.239,-1.17,-0.326,-0.231,-367,-1.21e-05,-5.91e-05,6.08e-06,9.23e-06,-4.7e-07,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.39e-05,8.38e-05,0.000118,0.017,0.0169,0.00793,0.0437,0.0437,0.0352,4.91e-11,4.91e-11,2.23e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
 25790000,0.706,0.0171,0.0282,0.707,-0.533,-0.266,-1.22,-0.343,-0.26,-367,-1.16e-05,-5.9e-05,6.15e-06,1.7e-05,-2.47e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.3e-05,8.29e-05,0.000118,0.0157,0.0155,0.00787,0.0396,0.0396,0.0349,4.74e-11,4.74e-11,2.2e-10,2.97e-06,2.97e-06,5e-08,0,0,0,0,0,0,0,0
 25890000,0.706,0.0174,0.0286,0.707,-0.604,-0.296,-1.27,-0.4,-0.288,-367,-1.16e-05,-5.9e-05,6.26e-06,1.67e-05,-2.48e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000122,8.32e-05,8.3e-05,0.000118,0.0169,0.0167,0.00796,0.0436,0.0436,0.0353,4.75e-11,4.75e-11,2.17e-10,2.97e-06,2.97e-06,5e-08,0,0,0,0,0,0,0,0
 25990000,0.706,0.0163,0.0254,0.707,-0.656,-0.332,-1.32,-0.439,-0.344,-367,-1.12e-05,-5.92e-05,6.28e-06,1.33e-05,-4.03e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000121,8.23e-05,8.22e-05,0.000117,0.0156,0.0154,0.0079,0.0395,0.0395,0.0351,4.59e-11,4.59e-11,2.14e-10,2.97e-06,2.97e-06,5e-08,0,0,0,0,0,0,0,0
 26090000,0.702,0.021,0.0353,0.711,-0.723,-0.359,-1.34,-0.508,-0.378,-367,-1.13e-05,-5.92e-05,6.07e-06,1.36e-05,-3.95e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.000121,8.25e-05,8.24e-05,0.000118,0.0167,0.0165,0.00796,0.0435,0.0435,0.0351,4.6e-11,4.6e-11,2.11e-10,2.97e-06,2.97e-06,5e-08,0,0,0,0,0,0,0,0
-26190000,0.701,0.0232,0.0446,0.712,-0.776,-0.394,-1.31,-0.533,-0.422,-367,-1.06e-05,-5.91e-05,6.07e-06,2.45e-05,-7.55e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.00012,8.16e-05,8.15e-05,0.000118,0.0153,0.0151,0.0079,0.0394,0.0394,0.0349,4.46e-11,4.46e-11,2.08e-10,2.96e-06,2.96e-06,5e-08,0,0,0,0,0,0,0,0
+26190000,0.701,0.0232,0.0446,0.712,-0.776,-0.394,-1.31,-0.533,-0.422,-367,-1.06e-05,-5.91e-05,6.07e-06,2.46e-05,-7.55e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.00012,8.16e-05,8.15e-05,0.000118,0.0153,0.0151,0.0079,0.0394,0.0394,0.0349,4.46e-11,4.46e-11,2.08e-10,2.96e-06,2.96e-06,5e-08,0,0,0,0,0,0,0,0
 26290000,0.701,0.0241,0.0469,0.711,-0.869,-0.436,-1.3,-0.616,-0.463,-368,-1.06e-05,-5.91e-05,5.97e-06,2.46e-05,-7.48e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.00012,8.18e-05,8.17e-05,0.000118,0.0164,0.0161,0.00799,0.0434,0.0433,0.0352,4.47e-11,4.47e-11,2.05e-10,2.96e-06,2.96e-06,5e-08,0,0,0,0,0,0,0,0
 26390000,0.702,0.023,0.0435,0.711,-0.945,-0.492,-1.31,-0.679,-0.548,-368,-1.03e-05,-5.94e-05,5.98e-06,1.26e-05,-8.74e-05,-0.00128,0.209,0.00206,0.432,0,0,0,0,0,0.00012,8.11e-05,8.09e-05,0.000117,0.0151,0.0148,0.00793,0.0393,0.0392,0.035,4.34e-11,4.34e-11,2.02e-10,2.96e-06,2.96e-06,5e-08,0,0,0,0,0,0,0,0
 26490000,0.702,0.0308,0.0593,0.709,-1.04,-0.531,-1.31,-0.778,-0.599,-368,-1.03e-05,-5.94e-05,5.94e-06,1.25e-05,-8.68e-05,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.00012,8.12e-05,8.11e-05,0.000117,0.0163,0.0158,0.00799,0.0432,0.0431,0.0351,4.35e-11,4.34e-11,1.99e-10,2.96e-06,2.96e-06,5e-08,0,0,0,0,0,0,0,0
 26590000,0.702,0.0369,0.0752,0.707,-1.14,-0.586,-1.3,-0.822,-0.665,-368,-9.5e-06,-5.93e-05,5.58e-06,2.15e-05,-0.00012,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.00012,8.05e-05,8.04e-05,0.000117,0.0151,0.0146,0.00797,0.0392,0.0391,0.0351,4.23e-11,4.22e-11,1.97e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
 26690000,0.703,0.0381,0.078,0.706,-1.28,-0.648,-1.29,-0.943,-0.727,-368,-9.49e-06,-5.93e-05,5.65e-06,2.11e-05,-0.00012,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000119,8.06e-05,8.06e-05,0.000116,0.0163,0.0156,0.00803,0.0431,0.043,0.0352,4.23e-11,4.23e-11,1.94e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
-26790000,0.704,0.0358,0.0725,0.705,-1.4,-0.73,-1.29,-1.04,-0.854,-368,-9.05e-06,-5.98e-05,5.48e-06,5.89e-08,-0.000139,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000119,8e-05,7.99e-05,0.000116,0.0151,0.0144,0.00797,0.0391,0.039,0.035,4.12e-11,4.12e-11,1.91e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
-26890000,0.704,0.0447,0.0944,0.703,-1.54,-0.789,-1.3,-1.18,-0.93,-368,-9.05e-06,-5.98e-05,5.52e-06,-3.57e-07,-0.000138,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000119,8.01e-05,8.01e-05,0.000116,0.0164,0.0154,0.00807,0.0429,0.0428,0.0353,4.13e-11,4.13e-11,1.89e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
-26990000,0.703,0.051,0.116,0.699,-1.68,-0.871,-1.28,-1.24,-1.03,-368,-7.89e-06,-5.97e-05,5.42e-06,7.09e-06,-0.000186,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000119,7.93e-05,7.96e-05,0.000115,0.0153,0.0142,0.00802,0.039,0.0388,0.0351,4.03e-11,4.02e-11,1.86e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
+26790000,0.704,0.0358,0.0725,0.705,-1.4,-0.73,-1.29,-1.04,-0.854,-368,-9.05e-06,-5.98e-05,5.48e-06,6.01e-08,-0.000139,-0.00127,0.209,0.00206,0.432,0,0,0,0,0,0.000119,8e-05,7.99e-05,0.000116,0.0151,0.0144,0.00797,0.0391,0.039,0.035,4.12e-11,4.12e-11,1.91e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
+26890000,0.704,0.0447,0.0944,0.703,-1.54,-0.789,-1.3,-1.18,-0.93,-368,-9.05e-06,-5.98e-05,5.52e-06,-3.56e-07,-0.000138,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000119,8.01e-05,8.01e-05,0.000116,0.0164,0.0154,0.00807,0.0429,0.0428,0.0353,4.13e-11,4.13e-11,1.89e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
+26990000,0.703,0.051,0.116,0.699,-1.68,-0.871,-1.28,-1.24,-1.03,-368,-7.89e-06,-5.97e-05,5.42e-06,7.1e-06,-0.000186,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000119,7.93e-05,7.96e-05,0.000115,0.0153,0.0142,0.00802,0.039,0.0388,0.0351,4.03e-11,4.02e-11,1.86e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
 27090000,0.704,0.0519,0.12,0.698,-1.88,-0.962,-1.25,-1.42,-1.12,-369,-7.89e-06,-5.97e-05,5.35e-06,7.08e-06,-0.000184,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000119,7.94e-05,7.98e-05,0.000115,0.0167,0.0153,0.00809,0.0428,0.0426,0.0352,4.04e-11,4.03e-11,1.84e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
 27190000,0.706,0.0485,0.109,0.698,-2.08,-1.03,-1.23,-1.62,-1.2,-369,-7.86e-06,-5.94e-05,5.44e-06,1.61e-05,-0.00018,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000118,7.93e-05,7.94e-05,0.000114,0.0171,0.0154,0.00809,0.0453,0.045,0.0353,4e-11,3.98e-11,1.82e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
 27290000,0.707,0.0428,0.0941,0.699,-2.24,-1.1,-1.22,-1.84,-1.31,-369,-7.85e-06,-5.94e-05,5.45e-06,1.56e-05,-0.000179,-0.00126,0.209,0.00206,0.432,0,0,0,0,0,0.000118,7.95e-05,7.94e-05,0.000114,0.0185,0.0165,0.00816,0.0498,0.0494,0.0353,4.01e-11,3.99e-11,1.79e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0
@@ -289,7 +289,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 28690000,0.71,0.000108,0.000302,0.704,-2.62,-1.23,0.972,-5.63,-2.92,-370,-9.13e-06,-5.77e-05,4.89e-06,-2.77e-05,-0.000197,-0.00119,0.209,0.00206,0.432,0,0,0,0,0,0.00011,8.04e-05,8.01e-05,0.000108,0.0222,0.0215,0.00899,0.113,0.111,0.0362,3.75e-11,3.67e-11,1.5e-10,2.93e-06,2.91e-06,5e-08,0,0,0,0,0,0,0,0
 28790000,0.709,-0.000208,1.86e-05,0.705,-2.58,-1.21,0.976,-5.94,-3.03,-370,-9.63e-06,-5.76e-05,4.82e-06,-5.36e-05,-0.000254,-0.00118,0.209,0.00206,0.432,0,0,0,0,0,0.000109,8.06e-05,8.02e-05,0.000107,0.0213,0.0209,0.00893,0.114,0.112,0.036,3.7e-11,3.61e-11,1.49e-10,2.91e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 28890000,0.709,-0.000222,0.000243,0.705,-2.51,-1.19,0.965,-6.2,-3.15,-370,-9.63e-06,-5.76e-05,4.78e-06,-5.82e-05,-0.000242,-0.00117,0.209,0.00206,0.432,0,0,0,0,0,0.000109,8.07e-05,8.03e-05,0.000107,0.0224,0.0221,0.00906,0.123,0.121,0.0364,3.71e-11,3.62e-11,1.47e-10,2.91e-06,2.89e-06,5e-08,0,0,0,0,0,0,0,0
-28990000,0.708,-6.17e-05,0.000642,0.706,-2.49,-1.17,0.959,-6.53,-3.26,-370,-1.03e-05,-5.75e-05,4.65e-06,-7.51e-05,-0.000311,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000108,8.09e-05,8.03e-05,0.000107,0.0216,0.0215,0.00899,0.124,0.122,0.0361,3.67e-11,3.57e-11,1.45e-10,2.9e-06,2.88e-06,5e-08,0,0,0,0,0,0,0,0
+28990000,0.708,-6.16e-05,0.000643,0.706,-2.49,-1.17,0.959,-6.53,-3.26,-370,-1.03e-05,-5.75e-05,4.65e-06,-7.51e-05,-0.000311,-0.00116,0.209,0.00206,0.432,0,0,0,0,0,0.000108,8.09e-05,8.03e-05,0.000107,0.0216,0.0215,0.00899,0.124,0.122,0.0361,3.67e-11,3.57e-11,1.45e-10,2.9e-06,2.88e-06,5e-08,0,0,0,0,0,0,0,0
 29090000,0.708,9.56e-05,0.00105,0.706,-2.42,-1.16,0.95,-6.77,-3.37,-369,-1.03e-05,-5.75e-05,4.56e-06,-8.02e-05,-0.000298,-0.00115,0.209,0.00206,0.432,0,0,0,0,0,0.000108,8.1e-05,8.04e-05,0.000107,0.0227,0.0228,0.00907,0.133,0.131,0.0362,3.68e-11,3.58e-11,1.44e-10,2.9e-06,2.88e-06,5e-08,0,0,0,0,0,0,0,0
 29190000,0.708,0.000294,0.00145,0.706,-2.38,-1.14,0.943,-7.06,-3.48,-369,-1.07e-05,-5.74e-05,4.61e-06,-9.7e-05,-0.00032,-0.00114,0.209,0.00206,0.432,0,0,0,0,0,0.000108,8.11e-05,8.04e-05,0.000107,0.0218,0.0221,0.00905,0.134,0.132,0.0363,3.63e-11,3.52e-11,1.42e-10,2.89e-06,2.86e-06,5e-08,0,0,0,0,0,0,0,0
 29290000,0.708,0.000652,0.00231,0.706,-2.33,-1.13,0.968,-7.29,-3.6,-369,-1.07e-05,-5.74e-05,4.5e-06,-0.000103,-0.000304,-0.00114,0.209,0.00206,0.432,0,0,0,0,0,0.000108,8.13e-05,8.05e-05,0.000106,0.023,0.0234,0.00912,0.143,0.141,0.0364,3.64e-11,3.53e-11,1.41e-10,2.89e-06,2.86e-06,5e-08,0,0,0,0,0,0,0,0
@@ -302,13 +302,13 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 29990000,0.708,0.00287,0.00767,0.706,-2.11,-1.05,0.911,-9.04,-4.32,-369,-1.22e-05,-5.71e-05,3.73e-06,-0.000194,-0.000285,-0.00108,0.209,0.00206,0.432,0,0,0,0,0,0.000107,8.22e-05,8.04e-05,0.000105,0.023,0.024,0.00907,0.173,0.171,0.0364,3.45e-11,3.34e-11,1.3e-10,2.86e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 30090000,0.709,0.00287,0.00762,0.706,-2.08,-1.04,0.897,-9.25,-4.43,-369,-1.22e-05,-5.71e-05,3.61e-06,-0.0002,-0.000269,-0.00107,0.209,0.00206,0.432,0,0,0,0,0,0.000107,8.23e-05,8.04e-05,0.000104,0.0243,0.0253,0.00912,0.184,0.182,0.0365,3.46e-11,3.35e-11,1.29e-10,2.86e-06,2.79e-06,5e-08,0,0,0,0,0,0,0,0
 30190000,0.709,0.00293,0.0075,0.705,-2.06,-1.03,0.883,-9.5,-4.52,-369,-1.25e-05,-5.7e-05,3.61e-06,-0.000213,-0.000271,-0.00106,0.209,0.00206,0.432,0,0,0,0,0,0.000106,8.24e-05,8.03e-05,0.000104,0.0233,0.0243,0.00907,0.183,0.181,0.0366,3.41e-11,3.3e-11,1.27e-10,2.86e-06,2.78e-06,5e-08,0,0,0,0,0,0,0,0
-30290000,0.709,0.00283,0.00733,0.705,-2.03,-1.02,0.871,-9.7,-4.62,-368,-1.25e-05,-5.7e-05,3.53e-06,-0.000218,-0.000261,-0.00106,0.209,0.00206,0.432,0,0,0,0,0,0.000106,8.25e-05,8.03e-05,0.000104,0.0246,0.0256,0.00912,0.194,0.192,0.0367,3.42e-11,3.31e-11,1.26e-10,2.86e-06,2.78e-06,5e-08,0,0,0,0,0,0,0,0
+30290000,0.709,0.00283,0.00733,0.705,-2.03,-1.02,0.87,-9.7,-4.62,-368,-1.25e-05,-5.7e-05,3.53e-06,-0.000218,-0.000261,-0.00106,0.209,0.00206,0.432,0,0,0,0,0,0.000106,8.25e-05,8.03e-05,0.000104,0.0246,0.0256,0.00912,0.194,0.192,0.0367,3.42e-11,3.31e-11,1.26e-10,2.86e-06,2.78e-06,5e-08,0,0,0,0,0,0,0,0
 30390000,0.709,0.00285,0.00718,0.705,-2.01,-1.01,0.853,-9.93,-4.72,-368,-1.27e-05,-5.7e-05,3.47e-06,-0.00023,-0.000248,-0.00105,0.209,0.00206,0.432,0,0,0,0,0,0.000106,8.26e-05,8.01e-05,0.000103,0.0236,0.0245,0.00902,0.193,0.191,0.0364,3.36e-11,3.25e-11,1.25e-10,2.85e-06,2.77e-06,5e-08,0,0,0,0,0,0,0,0
 30490000,0.709,0.00273,0.00694,0.705,-1.97,-1.01,0.838,-10.1,-4.82,-368,-1.27e-05,-5.7e-05,3.48e-06,-0.000234,-0.000239,-0.00105,0.209,0.00206,0.432,0,0,0,0,0,0.000106,8.27e-05,8.02e-05,0.000103,0.0248,0.0258,0.00911,0.205,0.203,0.0368,3.37e-11,3.26e-11,1.23e-10,2.85e-06,2.76e-06,5e-08,0,0,0,0,0,0,0,0
 30590000,0.709,0.00265,0.00665,0.705,-1.96,-0.997,0.797,-10.4,-4.91,-368,-1.3e-05,-5.7e-05,3.49e-06,-0.000243,-0.000231,-0.00104,0.209,0.00206,0.432,0,0,0,0,0,0.000106,8.28e-05,8e-05,0.000103,0.0238,0.0247,0.00901,0.203,0.201,0.0366,3.32e-11,3.21e-11,1.22e-10,2.85e-06,2.75e-06,5e-08,0,0,0,0,0,0,0,0
 30690000,0.709,0.0025,0.00635,0.705,-1.93,-0.989,0.79,-10.6,-5.01,-368,-1.3e-05,-5.7e-05,3.46e-06,-0.000249,-0.000216,-0.00104,0.209,0.00206,0.432,0,0,0,0,0,0.000106,8.29e-05,8e-05,0.000103,0.0251,0.0261,0.00904,0.215,0.213,0.0367,3.33e-11,3.22e-11,1.21e-10,2.85e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0
 30790000,0.709,0.00245,0.00603,0.705,-1.91,-0.978,0.781,-10.8,-5.1,-368,-1.31e-05,-5.69e-05,3.31e-06,-0.000257,-0.00021,-0.00103,0.209,0.00206,0.432,0,0,0,0,0,0.000106,8.29e-05,7.97e-05,0.000102,0.024,0.0249,0.00899,0.213,0.211,0.0367,3.27e-11,3.18e-11,1.19e-10,2.84e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0
-30890000,0.709,0.00228,0.00561,0.705,-1.88,-0.971,0.768,-11,-5.2,-368,-1.31e-05,-5.69e-05,3.31e-06,-0.000262,-0.000198,-0.00103,0.209,0.00206,0.432,0,0,0,0,0,0.000105,8.31e-05,7.98e-05,0.000102,0.0253,0.0262,0.00902,0.224,0.223,0.0368,3.28e-11,3.19e-11,1.18e-10,2.84e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0
+30890000,0.709,0.00228,0.00561,0.705,-1.88,-0.971,0.768,-11,-5.2,-368,-1.31e-05,-5.69e-05,3.31e-06,-0.000262,-0.000197,-0.00103,0.209,0.00206,0.432,0,0,0,0,0,0.000105,8.31e-05,7.98e-05,0.000102,0.0253,0.0262,0.00902,0.224,0.223,0.0368,3.28e-11,3.19e-11,1.18e-10,2.84e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0
 30990000,0.709,0.00225,0.00515,0.705,-1.86,-0.961,0.759,-11.2,-5.29,-368,-1.34e-05,-5.68e-05,3.23e-06,-0.000271,-0.000187,-0.00102,0.209,0.00206,0.432,0,0,0,0,0,0.000105,8.3e-05,7.95e-05,0.000101,0.0242,0.025,0.00891,0.222,0.22,0.0365,3.23e-11,3.14e-11,1.17e-10,2.84e-06,2.72e-06,5e-08,0,0,0,0,0,0,0,0
 31090000,0.709,0.00204,0.00467,0.705,-1.83,-0.954,0.747,-11.4,-5.38,-368,-1.34e-05,-5.68e-05,3.16e-06,-0.000278,-0.000172,-0.00102,0.209,0.00206,0.432,0,0,0,0,0,0.000105,8.32e-05,7.95e-05,0.000101,0.0255,0.0263,0.00899,0.234,0.232,0.037,3.24e-11,3.15e-11,1.16e-10,2.84e-06,2.71e-06,5e-08,0,0,0,0,0,0,0,0
 31190000,0.709,0.00198,0.00446,0.705,-1.81,-0.946,0.735,-11.6,-5.48,-368,-1.35e-05,-5.68e-05,3.12e-06,-0.000292,-0.000143,-0.00101,0.209,0.00206,0.432,0,0,0,0,0,0.000105,8.31e-05,7.92e-05,0.000101,0.0243,0.025,0.00888,0.232,0.23,0.0367,3.19e-11,3.1e-11,1.15e-10,2.84e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
@@ -316,15 +316,15 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 31390000,0.709,0.00162,0.00348,0.705,-1.76,-0.929,0.732,-12,-5.67,-368,-1.37e-05,-5.68e-05,3.05e-06,-0.000308,-0.000109,-0.000999,0.209,0.00206,0.432,0,0,0,0,0,0.000104,8.32e-05,7.89e-05,0.0001,0.0244,0.025,0.0088,0.241,0.239,0.0365,3.14e-11,3.07e-11,1.12e-10,2.84e-06,2.69e-06,5e-08,0,0,0,0,0,0,0,0
 31490000,0.709,0.00142,0.00288,0.705,-1.73,-0.921,0.726,-12.1,-5.76,-368,-1.37e-05,-5.68e-05,2.94e-06,-0.000315,-9.35e-05,-0.000994,0.209,0.00206,0.432,0,0,0,0,0,0.000104,8.34e-05,7.89e-05,0.0001,0.0257,0.0263,0.00887,0.254,0.252,0.0369,3.15e-11,3.08e-11,1.11e-10,2.83e-06,2.68e-06,5e-08,0,0,0,0,0,0,0,0
 31590000,0.709,0.00136,0.00255,0.705,-1.7,-0.908,0.721,-12.3,-5.84,-368,-1.39e-05,-5.68e-05,2.95e-06,-0.000324,-7.52e-05,-0.000988,0.209,0.00206,0.432,0,0,0,0,0,0.000104,8.33e-05,7.86e-05,9.96e-05,0.0245,0.025,0.00876,0.251,0.249,0.0366,3.1e-11,3.04e-11,1.1e-10,2.83e-06,2.68e-06,5e-08,0,0,0,0,0,0,0,0
-31690000,0.709,0.00111,0.0019,0.705,-1.68,-0.9,0.725,-12.5,-5.93,-368,-1.39e-05,-5.68e-05,2.95e-06,-0.00033,-6.14e-05,-0.000984,0.209,0.00206,0.432,0,0,0,0,0,0.000104,8.34e-05,7.86e-05,9.95e-05,0.0258,0.0263,0.00878,0.263,0.261,0.0366,3.11e-11,3.05e-11,1.09e-10,2.83e-06,2.67e-06,5e-08,0,0,0,0,0,0,0,0
+31690000,0.709,0.00111,0.0019,0.705,-1.68,-0.9,0.725,-12.5,-5.93,-368,-1.39e-05,-5.68e-05,2.94e-06,-0.00033,-6.14e-05,-0.000984,0.209,0.00206,0.432,0,0,0,0,0,0.000104,8.34e-05,7.86e-05,9.95e-05,0.0258,0.0263,0.00878,0.263,0.261,0.0366,3.11e-11,3.05e-11,1.09e-10,2.83e-06,2.67e-06,5e-08,0,0,0,0,0,0,0,0
 31790000,0.709,0.000913,0.00129,0.705,-1.66,-0.89,0.723,-12.7,-6.02,-368,-1.41e-05,-5.67e-05,2.94e-06,-0.00034,-4.19e-05,-0.000977,0.209,0.00206,0.432,0,0,0,0,0,0.000104,8.33e-05,7.83e-05,9.91e-05,0.0246,0.025,0.00872,0.26,0.258,0.0367,3.07e-11,3.01e-11,1.08e-10,2.83e-06,2.66e-06,5e-08,0,0,0,0,0,0,0,0
 31890000,0.709,0.00067,0.000603,0.705,-1.63,-0.879,0.718,-12.9,-6.1,-367,-1.41e-05,-5.67e-05,2.91e-06,-0.000346,-2.73e-05,-0.000972,0.209,0.00206,0.432,0,0,0,0,0,0.000103,8.35e-05,7.83e-05,9.9e-05,0.0259,0.0263,0.00874,0.273,0.271,0.0367,3.08e-11,3.02e-11,1.07e-10,2.83e-06,2.66e-06,5e-08,0,0,0,0,0,0,0,0
-31990000,0.709,0.000534,0.000152,0.705,-1.6,-0.867,0.712,-13,-6.18,-367,-1.42e-05,-5.67e-05,2.83e-06,-0.000356,-6.79e-06,-0.000966,0.209,0.00206,0.432,0,0,0,0,0,0.000103,8.33e-05,7.79e-05,9.85e-05,0.0247,0.025,0.00863,0.27,0.268,0.0364,3.03e-11,2.98e-11,1.06e-10,2.83e-06,2.65e-06,5e-08,0,0,0,0,0,0,0,0
-32090000,0.709,0.000248,-0.00056,0.705,-1.57,-0.857,0.718,-13.2,-6.27,-367,-1.42e-05,-5.67e-05,2.78e-06,-0.000364,9.83e-06,-0.000961,0.209,0.00206,0.432,0,0,0,0,0,0.000103,8.35e-05,7.8e-05,9.84e-05,0.0259,0.0262,0.0087,0.282,0.28,0.0368,3.04e-11,2.99e-11,1.05e-10,2.83e-06,2.64e-06,5e-08,0,0,0,0,0,0,0,0
-32190000,0.709,3.62e-05,-0.00133,0.705,-1.55,-0.848,0.716,-13.4,-6.35,-367,-1.43e-05,-5.66e-05,2.6e-06,-0.000374,3.17e-05,-0.000953,0.209,0.00206,0.432,0,0,0,0,0,0.000103,8.34e-05,7.76e-05,9.8e-05,0.0247,0.0249,0.00859,0.279,0.277,0.0365,2.99e-11,2.96e-11,1.04e-10,2.82e-06,2.64e-06,5e-08,0,0,0,0,0,0,0,0
+31990000,0.709,0.000534,0.000152,0.705,-1.6,-0.867,0.712,-13,-6.18,-367,-1.42e-05,-5.67e-05,2.83e-06,-0.000356,-6.78e-06,-0.000966,0.209,0.00206,0.432,0,0,0,0,0,0.000103,8.33e-05,7.79e-05,9.85e-05,0.0247,0.025,0.00863,0.27,0.268,0.0364,3.03e-11,2.98e-11,1.06e-10,2.83e-06,2.65e-06,5e-08,0,0,0,0,0,0,0,0
+32090000,0.709,0.000248,-0.00056,0.705,-1.57,-0.857,0.718,-13.2,-6.27,-367,-1.42e-05,-5.67e-05,2.78e-06,-0.000364,9.84e-06,-0.000961,0.209,0.00206,0.432,0,0,0,0,0,0.000103,8.35e-05,7.8e-05,9.84e-05,0.0259,0.0262,0.0087,0.282,0.28,0.0368,3.04e-11,2.99e-11,1.05e-10,2.83e-06,2.64e-06,5e-08,0,0,0,0,0,0,0,0
+32190000,0.709,3.63e-05,-0.00133,0.705,-1.55,-0.848,0.716,-13.4,-6.35,-367,-1.43e-05,-5.66e-05,2.6e-06,-0.000374,3.17e-05,-0.000953,0.209,0.00206,0.432,0,0,0,0,0,0.000103,8.34e-05,7.76e-05,9.8e-05,0.0247,0.0249,0.00859,0.279,0.277,0.0365,2.99e-11,2.96e-11,1.04e-10,2.82e-06,2.64e-06,5e-08,0,0,0,0,0,0,0,0
 32290000,0.709,-0.000194,-0.00205,0.705,-1.51,-0.838,0.71,-13.5,-6.43,-367,-1.43e-05,-5.66e-05,2.58e-06,-0.000382,4.93e-05,-0.000948,0.209,0.00206,0.432,0,0,0,0,0,0.000103,8.35e-05,7.76e-05,9.79e-05,0.026,0.0261,0.00861,0.292,0.29,0.0366,3e-11,2.97e-11,1.03e-10,2.82e-06,2.63e-06,5e-08,0,0,0,0,0,0,0,0
 32390000,0.709,-0.000378,-0.00268,0.705,-1.48,-0.826,0.709,-13.7,-6.51,-367,-1.44e-05,-5.66e-05,2.61e-06,-0.000387,5.92e-05,-0.000944,0.209,0.00206,0.432,0,0,0,0,0,0.000102,8.34e-05,7.73e-05,9.75e-05,0.0247,0.0248,0.00855,0.288,0.286,0.0366,2.96e-11,2.93e-11,1.02e-10,2.82e-06,2.63e-06,5e-08,0,0,0,0,0,0,0,0
-32490000,0.709,-0.000498,-0.00291,0.705,-1.45,-0.814,0.714,-13.8,-6.6,-367,-1.44e-05,-5.66e-05,2.61e-06,-0.000392,7.17e-05,-0.00094,0.209,0.00206,0.432,0,0,0,0,0,0.000102,8.35e-05,7.73e-05,9.74e-05,0.026,0.026,0.00857,0.301,0.299,0.0366,2.97e-11,2.94e-11,1.01e-10,2.82e-06,2.62e-06,5e-08,0,0,0,0,0,0,0,0
+32490000,0.709,-0.000498,-0.00291,0.705,-1.45,-0.814,0.714,-13.8,-6.6,-367,-1.44e-05,-5.66e-05,2.61e-06,-0.000392,7.18e-05,-0.00094,0.209,0.00206,0.432,0,0,0,0,0,0.000102,8.35e-05,7.73e-05,9.74e-05,0.026,0.026,0.00857,0.301,0.299,0.0366,2.97e-11,2.94e-11,1.01e-10,2.82e-06,2.62e-06,5e-08,0,0,0,0,0,0,0,0
 32590000,0.709,-0.000494,-0.00312,0.705,-1.43,-0.804,0.711,-14,-6.68,-367,-1.45e-05,-5.66e-05,2.54e-06,-0.000396,8.02e-05,-0.000937,0.209,0.00206,0.432,0,0,0,0,0,0.000102,8.33e-05,7.7e-05,9.7e-05,0.0247,0.0247,0.00846,0.298,0.295,0.0363,2.92e-11,2.91e-11,9.97e-11,2.82e-06,2.61e-06,5e-08,0,0,0,0,0,0,0,0
 32690000,0.709,-0.000535,-0.0032,0.705,-1.39,-0.794,0.707,-14.1,-6.76,-367,-1.45e-05,-5.66e-05,2.52e-06,-0.000399,8.69e-05,-0.000935,0.209,0.00206,0.432,0,0,0,0,0,0.000102,8.35e-05,7.7e-05,9.69e-05,0.026,0.026,0.00848,0.311,0.308,0.0364,2.93e-11,2.92e-11,9.88e-11,2.82e-06,2.61e-06,5e-08,0,0,0,0,0,0,0,0
 32790000,0.709,-0.000498,-0.00314,0.705,-1.37,-0.783,0.703,-14.3,-6.83,-367,-1.45e-05,-5.66e-05,2.5e-06,-0.000404,9.69e-05,-0.000932,0.209,0.00206,0.432,0,0,0,0,0,0.000101,8.33e-05,7.67e-05,9.65e-05,0.0248,0.0247,0.00843,0.307,0.304,0.0364,2.89e-11,2.88e-11,9.79e-11,2.82e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0

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

@@ -2,9 +2,9 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 10000,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 90000,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 190000,0.979,-0.00865,-0.0138,0.205,-0.000572,-0.00011,-0.00742,-2.83e-05,-2.2e-05,-0.0351,-2.69e-14,7.98e-14,2.03e-15,2.99e-11,-2.9e-11,1.29e-09,0.203,0.0109,0.434,0,0,0,0,0,0.000143,0.00247,0.00247,0.00324,25,25,0.563,100,100,0.8,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-290000,0.979,-0.00873,-0.0141,0.205,0.000802,-0.000466,-0.0239,-6.92e-05,-2.29e-05,-0.0538,5.72e-12,-9.05e-13,-1.79e-13,1.07e-09,-1.03e-09,4.57e-08,0.203,0.0109,0.434,0,0,0,0,0,9.44e-05,0.00253,0.00253,0.00213,25,25,0.562,101,101,0.337,1e-06,1e-06,9.97e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+290000,0.979,-0.00873,-0.0141,0.205,0.000802,-0.000466,-0.0239,-6.92e-05,-2.29e-05,-0.0538,5.71e-12,-9.05e-13,-1.79e-13,1.07e-09,-1.03e-09,4.57e-08,0.203,0.0109,0.434,0,0,0,0,0,9.44e-05,0.00253,0.00253,0.00213,25,25,0.562,101,101,0.337,1e-06,1e-06,9.97e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 390000,0.979,-0.00874,-0.0144,0.205,0.00384,-0.000207,-0.0425,0.000107,7.76e-06,-0.0658,-2.92e-11,-1.07e-10,-1.39e-12,8.73e-09,-8.37e-09,3.69e-07,0.203,0.0109,0.434,0,0,0,0,0,7.22e-05,0.00263,0.00263,0.00162,24.8,24.8,0.557,0.29,0.29,0.209,1e-06,1e-06,9.88e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-490000,0.979,-0.00876,-0.0148,0.205,0.00668,-0.00219,-0.0639,0.000628,-0.000111,-0.0812,-2.88e-10,-5.71e-10,-3.06e-12,2.75e-08,-2.63e-08,1.16e-06,0.203,0.0109,0.434,0,0,0,0,0,6.04e-05,0.00279,0.00279,0.00135,24.9,24.9,0.544,0.739,0.739,0.159,1e-06,1e-06,9.73e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+490000,0.979,-0.00876,-0.0148,0.205,0.00668,-0.00219,-0.0639,0.000628,-0.000111,-0.0812,-2.87e-10,-5.71e-10,-3.06e-12,2.75e-08,-2.63e-08,1.15e-06,0.203,0.0109,0.434,0,0,0,0,0,6.04e-05,0.00279,0.00279,0.00135,24.9,24.9,0.544,0.739,0.739,0.159,1e-06,1e-06,9.73e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 590000,0.979,-0.00877,-0.015,0.205,0.00456,-0.00282,-0.0851,0.000297,-0.000135,-0.0938,-6.02e-09,-7.53e-09,5.54e-11,6.35e-08,-5.84e-08,2.02e-06,0.203,0.0109,0.434,0,0,0,0,0,5.37e-05,0.00299,0.00299,0.0012,7.8,7.8,0.527,0.267,0.267,0.141,1e-06,1e-06,9.49e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 690000,0.979,-0.0088,-0.0153,0.205,0.00888,-0.00425,-0.0976,0.000996,-0.000484,-0.103,-6.06e-09,-7.57e-09,5.59e-11,6.46e-08,-5.94e-08,2.07e-06,0.203,0.0109,0.434,0,0,0,0,0,5.01e-05,0.00324,0.00324,0.00111,7.87,7.87,0.497,0.556,0.556,0.13,1e-06,1e-06,9.16e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 790000,0.979,-0.0088,-0.0156,0.205,0.00942,-0.00187,-0.11,0.000559,-0.000205,-0.114,-3.56e-08,-3.46e-08,4.58e-10,1.03e-07,-1.01e-07,2.23e-06,0.203,0.0109,0.434,0,0,0,0,0,4.82e-05,0.00353,0.00353,0.00106,2.63,2.63,0.46,0.218,0.218,0.125,9.99e-07,9.99e-07,8.75e-07,4e-06,4e-06,3.99e-06,0,0,0,0,0,0,0,0
@@ -18,21 +18,21 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 1590000,0.979,-0.00863,-0.0173,0.205,0.0533,-0.00359,-0.228,0.0128,-0.00094,-0.256,-1.42e-06,-2.63e-06,1.57e-08,1.6e-06,-9.28e-07,4.67e-06,0.203,0.0109,0.434,0,0,0,0,0,4.83e-05,0.007,0.007,0.00102,1.3,1.3,0.182,0.268,0.268,0.118,9.62e-07,9.62e-07,4.38e-07,3.99e-06,3.99e-06,3.87e-06,0,0,0,0,0,0,0,0
 1690000,0.979,-0.00854,-0.017,0.205,0.0494,-0.00235,-0.244,0.0101,-0.000622,-0.28,-3.52e-06,-6.61e-06,4.8e-08,3.3e-06,-1.82e-06,4.59e-06,0.203,0.0109,0.434,0,0,0,0,0,4.77e-05,0.00675,0.00675,0.00101,1.1,1.1,0.163,0.179,0.179,0.114,9.06e-07,9.06e-07,3.92e-07,3.98e-06,3.98e-06,3.83e-06,0,0,0,0,0,0,0,0
 1790000,0.979,-0.00865,-0.0173,0.205,0.0604,-0.00313,-0.257,0.0157,-0.000918,-0.305,-3.52e-06,-6.61e-06,4.8e-08,3.3e-06,-1.82e-06,4.59e-06,0.203,0.0109,0.434,0,0,0,0,0,4.75e-05,0.00743,0.00743,0.000993,1.44,1.44,0.147,0.262,0.262,0.11,9.06e-07,9.06e-07,3.51e-07,3.98e-06,3.98e-06,3.8e-06,0,0,0,0,0,0,0,0
-1890000,0.979,-0.00832,-0.0166,0.205,0.0501,7.36e-05,-0.269,0.0114,-0.000469,-0.331,-6.76e-06,-1.3e-05,1.05e-07,5.58e-06,-2.98e-06,4.46e-06,0.203,0.0109,0.434,0,0,0,0,0,4.63e-05,0.00657,0.00657,0.000975,1.18,1.18,0.137,0.178,0.178,0.109,8.15e-07,8.16e-07,3.15e-07,3.97e-06,3.97e-06,3.76e-06,0,0,0,0,0,0,0,0
-1990000,0.979,-0.0084,-0.0169,0.205,0.0585,0.000588,-0.282,0.0168,-0.000493,-0.359,-6.76e-06,-1.3e-05,1.05e-07,5.58e-06,-2.98e-06,4.46e-06,0.203,0.0109,0.434,0,0,0,0,0,4.58e-05,0.00722,0.00722,0.000955,1.54,1.54,0.126,0.266,0.266,0.105,8.15e-07,8.16e-07,2.82e-07,3.97e-06,3.97e-06,3.72e-06,0,0,0,0,0,0,0,0
-2090000,0.979,-0.00819,-0.0161,0.205,0.0446,0.00161,-0.296,0.0111,-0.000118,-0.388,-1.04e-05,-2.08e-05,1.73e-07,7.87e-06,-4.05e-06,4.31e-06,0.203,0.0109,0.434,0,0,0,0,0,4.42e-05,0.00588,0.00588,0.000934,1.18,1.18,0.118,0.177,0.177,0.102,7.03e-07,7.03e-07,2.52e-07,3.96e-06,3.96e-06,3.67e-06,0,0,0,0,0,0,0,0
-2190000,0.979,-0.00811,-0.0164,0.205,0.0523,0.00169,-0.293,0.0162,0.000103,-0.4,-1.03e-05,-2.05e-05,1.71e-07,7.17e-06,-3.42e-06,-2.46e-05,0.203,0.0109,0.434,0,0,0,0,0,4.36e-05,0.00645,0.00645,0.000912,1.51,1.51,0.113,0.267,0.267,0.101,7.03e-07,7.03e-07,2.27e-07,3.96e-06,3.96e-06,3.62e-06,0,0,0,0,0,0,0,0
+1890000,0.979,-0.00832,-0.0166,0.205,0.0501,7.35e-05,-0.269,0.0114,-0.000469,-0.331,-6.76e-06,-1.3e-05,1.05e-07,5.58e-06,-2.98e-06,4.46e-06,0.203,0.0109,0.434,0,0,0,0,0,4.63e-05,0.00657,0.00657,0.000975,1.18,1.18,0.137,0.178,0.178,0.109,8.15e-07,8.16e-07,3.15e-07,3.97e-06,3.97e-06,3.76e-06,0,0,0,0,0,0,0,0
+1990000,0.979,-0.0084,-0.0169,0.205,0.0585,0.000588,-0.282,0.0168,-0.000494,-0.359,-6.76e-06,-1.3e-05,1.05e-07,5.58e-06,-2.98e-06,4.46e-06,0.203,0.0109,0.434,0,0,0,0,0,4.58e-05,0.00722,0.00722,0.000955,1.54,1.54,0.126,0.266,0.266,0.105,8.15e-07,8.16e-07,2.82e-07,3.97e-06,3.97e-06,3.72e-06,0,0,0,0,0,0,0,0
+2090000,0.979,-0.00819,-0.0161,0.205,0.0446,0.0016,-0.296,0.0111,-0.000118,-0.388,-1.04e-05,-2.08e-05,1.73e-07,7.87e-06,-4.05e-06,4.31e-06,0.203,0.0109,0.434,0,0,0,0,0,4.42e-05,0.00588,0.00588,0.000934,1.18,1.18,0.118,0.177,0.177,0.102,7.03e-07,7.03e-07,2.52e-07,3.96e-06,3.96e-06,3.67e-06,0,0,0,0,0,0,0,0
+2190000,0.979,-0.00811,-0.0164,0.205,0.0523,0.00169,-0.293,0.0162,0.000102,-0.4,-1.03e-05,-2.05e-05,1.71e-07,7.17e-06,-3.42e-06,-2.46e-05,0.203,0.0109,0.434,0,0,0,0,0,4.36e-05,0.00645,0.00645,0.000912,1.51,1.51,0.113,0.267,0.267,0.101,7.03e-07,7.03e-07,2.27e-07,3.96e-06,3.96e-06,3.62e-06,0,0,0,0,0,0,0,0
 2290000,0.979,-0.00794,-0.0156,0.205,0.037,0.00274,-0.275,0.0102,0.000239,-0.394,-1.33e-05,-2.77e-05,2.28e-07,7.55e-06,-2.91e-06,-8.4e-05,0.203,0.0109,0.434,0,0,0,0,0,4.18e-05,0.00497,0.00497,0.00089,1.09,1.09,0.107,0.173,0.173,0.0977,5.9e-07,5.91e-07,2.04e-07,3.95e-06,3.95e-06,3.56e-06,0,0,0,0,0,0,0,0
 2390000,0.979,-0.00796,-0.0158,0.205,0.0446,0.00236,-0.263,0.0146,0.000477,-0.391,-1.32e-05,-2.73e-05,2.24e-07,6.19e-06,-1.67e-06,-0.000142,0.203,0.0109,0.434,0,0,0,0,0,4.11e-05,0.00545,0.00545,0.000867,1.38,1.39,0.103,0.259,0.259,0.0949,5.9e-07,5.91e-07,1.84e-07,3.95e-06,3.95e-06,3.49e-06,0,0,0,0,0,0,0,0
 2490000,0.979,-0.00769,-0.015,0.205,0.0284,0.00337,-0.259,0.00872,0.000394,-0.4,-1.57e-05,-3.37e-05,2.7e-07,6.44e-06,-1.33e-06,-0.000178,0.203,0.0109,0.434,0,0,0,0,0,3.94e-05,0.00411,0.00411,0.000845,0.963,0.963,0.101,0.166,0.166,0.0946,4.92e-07,4.93e-07,1.66e-07,3.95e-06,3.95e-06,3.43e-06,0,0,0,0,0,0,0,0
-2590000,0.979,-0.00779,-0.0152,0.205,0.0323,0.00208,-0.247,0.012,0.000675,-0.397,-1.56e-05,-3.34e-05,2.68e-07,5.1e-06,-9.66e-08,-0.000238,0.203,0.0109,0.434,0,0,0,0,0,3.86e-05,0.00452,0.00452,0.000823,1.21,1.21,0.0981,0.245,0.245,0.0923,4.92e-07,4.93e-07,1.5e-07,3.95e-06,3.95e-06,3.35e-06,0,0,0,0,0,0,0,0
+2590000,0.979,-0.00779,-0.0152,0.205,0.0323,0.00208,-0.247,0.012,0.000675,-0.397,-1.56e-05,-3.34e-05,2.68e-07,5.1e-06,-9.65e-08,-0.000238,0.203,0.0109,0.434,0,0,0,0,0,3.86e-05,0.00452,0.00452,0.000823,1.21,1.21,0.0981,0.245,0.245,0.0923,4.92e-07,4.93e-07,1.5e-07,3.95e-06,3.95e-06,3.35e-06,0,0,0,0,0,0,0,0
 2690000,0.979,-0.00772,-0.0146,0.205,0.0208,0.00295,-0.235,0.00706,0.000467,-0.394,-1.73e-05,-3.82e-05,2.99e-07,4.12e-06,1.04e-06,-0.000299,0.203,0.0109,0.434,0,0,0,0,0,3.7e-05,0.00342,0.00342,0.000802,0.836,0.836,0.096,0.157,0.157,0.0903,4.12e-07,4.12e-07,1.36e-07,3.95e-06,3.95e-06,3.26e-06,0,0,0,0,0,0,0,0
-2790000,0.979,-0.00762,-0.0147,0.205,0.0256,0.00351,-0.226,0.00965,0.000814,-0.393,-1.72e-05,-3.79e-05,2.97e-07,2.86e-06,2.2e-06,-0.000356,0.203,0.0109,0.434,0,0,0,0,0,3.62e-05,0.00376,0.00376,0.000781,1.05,1.05,0.0943,0.228,0.228,0.0885,4.12e-07,4.12e-07,1.23e-07,3.95e-06,3.95e-06,3.17e-06,0,0,0,0,0,0,0,0
+2790000,0.979,-0.00762,-0.0147,0.205,0.0256,0.00351,-0.226,0.00965,0.000813,-0.393,-1.72e-05,-3.79e-05,2.97e-07,2.86e-06,2.2e-06,-0.000356,0.203,0.0109,0.434,0,0,0,0,0,3.62e-05,0.00376,0.00376,0.000781,1.05,1.05,0.0943,0.228,0.228,0.0885,4.12e-07,4.12e-07,1.23e-07,3.95e-06,3.95e-06,3.17e-06,0,0,0,0,0,0,0,0
 2890000,0.979,-0.00759,-0.0143,0.205,0.0181,0.00184,-0.222,0.00574,0.000506,-0.399,-1.85e-05,-4.16e-05,3.18e-07,1.91e-06,3.08e-06,-0.000398,0.203,0.0109,0.434,0,0,0,0,0,3.48e-05,0.0029,0.0029,0.000761,0.727,0.727,0.0942,0.148,0.148,0.0889,3.47e-07,3.47e-07,1.12e-07,3.95e-06,3.95e-06,3.08e-06,0,0,0,0,0,0,0,0
 2990000,0.979,-0.00756,-0.0144,0.205,0.0208,0.00137,-0.207,0.00795,0.000696,-0.392,-1.84e-05,-4.14e-05,3.17e-07,3.11e-07,4.56e-06,-0.000471,0.203,0.0109,0.434,0,0,0,0,0,3.41e-05,0.00319,0.00319,0.000741,0.904,0.904,0.0929,0.211,0.211,0.0875,3.47e-07,3.47e-07,1.02e-07,3.95e-06,3.95e-06,2.98e-06,0,0,0,0,0,0,0,0
 3090000,0.979,-0.0075,-0.0141,0.205,0.017,-0.000483,-0.203,0.00489,0.000335,-0.395,-1.94e-05,-4.43e-05,3.34e-07,-9.68e-07,5.59e-06,-0.000516,0.203,0.0109,0.434,0,0,0,0,0,3.29e-05,0.00252,0.00252,0.000722,0.638,0.638,0.0919,0.139,0.139,0.0862,2.94e-07,2.94e-07,9.36e-08,3.95e-06,3.95e-06,2.87e-06,0,0,0,0,0,0,0,0
-3190000,0.979,-0.00749,-0.0143,0.205,0.0201,-0.00148,-0.195,0.00684,0.00019,-0.398,-1.94e-05,-4.42e-05,3.34e-07,-2.01e-06,6.55e-06,-0.000563,0.203,0.0109,0.434,0,0,0,0,0,3.23e-05,0.00277,0.00277,0.000704,0.791,0.791,0.092,0.196,0.196,0.087,2.94e-07,2.94e-07,8.58e-08,3.95e-06,3.95e-06,2.78e-06,0,0,0,0,0,0,0,0
-3290000,0.979,-0.00737,-0.0139,0.205,0.0145,-0.00116,-0.183,0.00435,1.63e-06,-0.394,-2.04e-05,-4.65e-05,3.52e-07,-3.87e-06,8.03e-06,-0.000628,0.203,0.0109,0.434,0,0,0,0,0,3.12e-05,0.00222,0.00222,0.000686,0.567,0.567,0.0909,0.131,0.131,0.086,2.49e-07,2.49e-07,7.87e-08,3.94e-06,3.94e-06,2.66e-06,0,0,0,0,0,0,0,0
+3190000,0.979,-0.00749,-0.0143,0.205,0.0201,-0.00147,-0.195,0.00684,0.00019,-0.398,-1.94e-05,-4.42e-05,3.34e-07,-2.01e-06,6.55e-06,-0.000563,0.203,0.0109,0.434,0,0,0,0,0,3.23e-05,0.00277,0.00277,0.000704,0.791,0.791,0.092,0.196,0.196,0.087,2.94e-07,2.94e-07,8.58e-08,3.95e-06,3.95e-06,2.78e-06,0,0,0,0,0,0,0,0
+3290000,0.979,-0.00737,-0.0139,0.205,0.0145,-0.00116,-0.183,0.00435,1.62e-06,-0.394,-2.04e-05,-4.65e-05,3.52e-07,-3.87e-06,8.03e-06,-0.000628,0.203,0.0109,0.434,0,0,0,0,0,3.12e-05,0.00222,0.00222,0.000686,0.567,0.567,0.0909,0.131,0.131,0.086,2.49e-07,2.49e-07,7.87e-08,3.94e-06,3.94e-06,2.66e-06,0,0,0,0,0,0,0,0
 3390000,0.979,-0.00721,-0.014,0.205,0.0157,0.000219,-0.174,0.00588,-6.31e-05,-0.391,-2.04e-05,-4.64e-05,3.52e-07,-5.16e-06,9.2e-06,-0.000686,0.203,0.0109,0.434,0,0,0,0,0,3.05e-05,0.00244,0.00244,0.000669,0.702,0.702,0.0898,0.183,0.183,0.0851,2.49e-07,2.49e-07,7.24e-08,3.94e-06,3.94e-06,2.54e-06,0,0,0,0,0,0,0,0
 3490000,0.979,-0.00715,-0.014,0.205,0.0195,0.00301,-0.172,0.00774,9.49e-05,-0.398,-2.04e-05,-4.64e-05,3.51e-07,-5.81e-06,9.79e-06,-0.000716,0.203,0.0109,0.434,0,0,0,0,0,2.99e-05,0.00268,0.00268,0.000653,0.86,0.86,0.0898,0.254,0.254,0.0861,2.49e-07,2.49e-07,6.68e-08,3.94e-06,3.94e-06,2.44e-06,0,0,0,0,0,0,0,0
 3590000,0.979,-0.00699,-0.0136,0.205,0.0157,0.00279,-0.167,0.00528,0.00032,-0.399,-2.12e-05,-4.83e-05,3.66e-07,-7.38e-06,1.09e-05,-0.000762,0.203,0.0109,0.434,0,0,0,0,0,2.89e-05,0.00218,0.00218,0.000637,0.632,0.632,0.0884,0.171,0.171,0.0853,2.11e-07,2.11e-07,6.16e-08,3.94e-06,3.94e-06,2.31e-06,0,0,0,0,0,0,0,0
@@ -60,7 +60,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 5790000,0.979,-0.00622,-0.0125,0.205,-0.00438,0.0179,-0.0875,-0.00107,0.00511,-0.448,-2.04e-05,-5.72e-05,3.25e-07,-2.33e-05,1.99e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.82e-05,0.000745,0.000745,0.000409,0.367,0.367,0.0468,0.177,0.177,0.0749,2.85e-08,2.85e-08,1.53e-08,3.88e-06,3.88e-06,5.42e-07,0,0,0,0,0,0,0,0
 5890000,0.979,-0.00624,-0.0126,0.205,-0.0019,0.0166,-0.0869,-0.000853,0.00444,-0.457,-1.98e-05,-5.73e-05,3.09e-07,-2.34e-05,1.93e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.79e-05,0.000618,0.000619,0.000402,0.285,0.285,0.0449,0.131,0.131,0.0737,2.29e-08,2.29e-08,1.46e-08,3.87e-06,3.87e-06,5.04e-07,0,0,0,0,0,0,0,0
 5990000,0.979,-0.00621,-0.0127,0.205,-0.00154,0.0181,-0.0868,-0.000999,0.00615,-0.465,-1.98e-05,-5.73e-05,3.09e-07,-2.34e-05,1.93e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.76e-05,0.000653,0.000653,0.000396,0.332,0.332,0.0431,0.167,0.167,0.0725,2.29e-08,2.29e-08,1.39e-08,3.87e-06,3.87e-06,4.7e-07,0,0,0,0,0,0,0,0
-6090000,0.979,-0.00631,-0.0126,0.205,-0.00193,0.0157,-0.0885,-0.000744,0.00497,-0.474,-1.91e-05,-5.75e-05,2.93e-07,-2.36e-05,1.87e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.73e-05,0.000546,0.000546,0.00039,0.259,0.259,0.0419,0.124,0.124,0.0726,1.85e-08,1.85e-08,1.32e-08,3.87e-06,3.87e-06,4.41e-07,0,0,0,0,0,0,0,0
+6090000,0.979,-0.00631,-0.0126,0.205,-0.00193,0.0157,-0.0884,-0.000744,0.00497,-0.474,-1.91e-05,-5.75e-05,2.93e-07,-2.36e-05,1.87e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.73e-05,0.000546,0.000546,0.00039,0.259,0.259,0.0419,0.124,0.124,0.0726,1.85e-08,1.85e-08,1.32e-08,3.87e-06,3.87e-06,4.41e-07,0,0,0,0,0,0,0,0
 6190000,0.979,-0.00633,-0.0126,0.205,-0.00319,0.0177,-0.0899,-0.000947,0.00666,-0.483,-1.91e-05,-5.75e-05,2.93e-07,-2.36e-05,1.87e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.7e-05,0.000575,0.000575,0.000383,0.3,0.3,0.0402,0.159,0.159,0.0714,1.85e-08,1.85e-08,1.26e-08,3.87e-06,3.87e-06,4.12e-07,0,0,0,0,0,0,0,0
 6290000,0.979,-0.00641,-0.0126,0.205,-0.00474,0.0159,-0.0915,-0.000858,0.00526,-0.492,-1.85e-05,-5.76e-05,2.76e-07,-2.38e-05,1.8e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.67e-05,0.000484,0.000484,0.000378,0.235,0.235,0.0385,0.119,0.119,0.0703,1.49e-08,1.5e-08,1.2e-08,3.87e-06,3.87e-06,3.84e-07,0,0,0,0,0,0,0,0
 6390000,0.979,-0.00635,-0.0125,0.205,-0.00313,0.0175,-0.0933,-0.00127,0.00693,-0.501,-1.85e-05,-5.76e-05,2.76e-07,-2.38e-05,1.8e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.65e-05,0.000508,0.000508,0.000372,0.272,0.272,0.0375,0.15,0.15,0.0704,1.49e-08,1.5e-08,1.15e-08,3.87e-06,3.87e-06,3.62e-07,0,0,0,0,0,0,0,0
@@ -94,9 +94,9 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 9190000,0.982,-0.00596,-0.0123,0.187,-0.0058,0.0708,-0.127,-0.0179,0.086,-0.818,-1.6e-05,-5.79e-05,1.21e-06,-2.38e-05,1.72e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.77e-06,0.000375,0.000375,0.000151,0.95,0.95,0.0155,2.07,2.07,0.0519,6.35e-09,6.36e-09,7.97e-09,3.84e-06,3.84e-06,8.1e-08,0,0,0,0,0,0,0,0
 9290000,0.982,-0.00581,-0.0121,0.187,-0.00399,0.0736,-0.128,-0.0183,0.0932,-0.831,-1.6e-05,-5.79e-05,1.34e-06,-2.38e-05,1.72e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.62e-06,0.000381,0.000381,0.000147,1.03,1.03,0.0151,2.34,2.34,0.0512,6.35e-09,6.36e-09,7.9e-09,3.84e-06,3.84e-06,7.77e-08,0,0,0,0,0,0,0,0
 9390000,0.982,-0.00573,-0.0121,0.187,-0.00411,0.0746,-0.128,-0.0182,0.0981,-0.843,-1.59e-05,-5.79e-05,8.93e-07,-2.36e-05,1.8e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.53e-06,0.000382,0.000382,0.000145,1.09,1.09,0.0148,2.58,2.58,0.0512,6.28e-09,6.28e-09,7.83e-09,3.84e-06,3.84e-06,7.49e-08,0,0,0,0,0,0,0,0
-9490000,0.982,-0.00575,-0.0122,0.187,-0.00469,0.0765,-0.129,-0.0187,0.106,-0.856,-1.59e-05,-5.8e-05,1.08e-06,-2.36e-05,1.8e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.42e-06,0.000389,0.000389,0.000142,1.18,1.18,0.0144,2.91,2.91,0.0506,6.28e-09,6.29e-09,7.75e-09,3.84e-06,3.84e-06,7.19e-08,0,0,0,0,0,0,0,0
+9490000,0.982,-0.00575,-0.0122,0.187,-0.00469,0.0765,-0.129,-0.0187,0.106,-0.856,-1.59e-05,-5.8e-05,1.08e-06,-2.36e-05,1.8e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.42e-06,0.000389,0.000389,0.000142,1.18,1.18,0.0144,2.91,2.91,0.0506,6.28e-09,6.28e-09,7.75e-09,3.84e-06,3.84e-06,7.19e-08,0,0,0,0,0,0,0,0
 9590000,0.982,-0.00582,-0.0122,0.187,-0.00493,0.076,-0.13,-0.0186,0.11,-0.869,-1.59e-05,-5.8e-05,1.59e-07,-2.34e-05,1.91e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.32e-06,0.000388,0.000388,0.000139,1.24,1.24,0.014,3.17,3.17,0.05,6.2e-09,6.2e-09,7.66e-09,3.83e-06,3.83e-06,6.91e-08,0,0,0,0,0,0,0,0
-9690000,0.982,-0.00582,-0.0121,0.187,-0.00548,0.0785,-0.128,-0.0191,0.117,-0.882,-1.59e-05,-5.8e-05,-8.91e-08,-2.34e-05,1.91e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.27e-06,0.000395,0.000395,0.000138,1.33,1.33,0.0138,3.56,3.56,0.05,6.2e-09,6.2e-09,7.58e-09,3.83e-06,3.83e-06,6.68e-08,0,0,0,0,0,0,0,0
+9690000,0.982,-0.00582,-0.0121,0.187,-0.00548,0.0785,-0.128,-0.0191,0.117,-0.882,-1.59e-05,-5.8e-05,-8.9e-08,-2.34e-05,1.91e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.27e-06,0.000395,0.000395,0.000138,1.33,1.33,0.0138,3.56,3.56,0.05,6.2e-09,6.2e-09,7.58e-09,3.83e-06,3.83e-06,6.68e-08,0,0,0,0,0,0,0,0
 9790000,0.982,-0.00579,-0.0121,0.187,-0.00462,0.0816,-0.131,-0.0196,0.125,-0.895,-1.59e-05,-5.79e-05,-1.02e-06,-2.35e-05,1.92e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.2e-06,0.000403,0.000403,0.000136,1.43,1.43,0.0135,3.99,3.99,0.0493,6.2e-09,6.2e-09,7.48e-09,3.83e-06,3.83e-06,6.42e-08,0,0,0,0,0,0,0,0
 9890000,0.982,-0.00579,-0.012,0.187,-0.00241,0.0807,-0.13,-0.0192,0.128,-0.908,-1.58e-05,-5.8e-05,-8.34e-07,-2.31e-05,2.05e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.13e-06,0.0004,0.0004,0.000134,1.48,1.48,0.0131,4.29,4.29,0.0487,6.11e-09,6.11e-09,7.38e-09,3.82e-06,3.82e-06,6.19e-08,0,0,0,0,0,0,0,0
 9990000,0.982,-0.00574,-0.0121,0.188,-0.00111,0.0851,-0.0989,-0.0191,0.141,-0.849,-1.58e-05,-5.79e-05,-1.61e-06,-2.38e-05,1.92e-05,-0.00131,0.204,0.00201,0.435,0,0,0,0,0,5.11e-06,0.000408,0.000408,0.000134,1.59,1.59,0.0129,4.79,4.79,0.0488,6.11e-09,6.11e-09,7.29e-09,3.82e-06,3.82e-06,5.99e-08,0,0,0,0,0,0,0,0
@@ -114,7 +114,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 11190000,0.982,-0.00578,-0.0123,0.188,0.00185,0.0168,0.207,0.000862,-0.0017,-0.243,-1.49e-05,-5.77e-05,-5.28e-06,-2.3e-05,2.44e-05,-0.0016,0.204,0.00201,0.435,0,0,0,0,0,4.91e-06,0.000398,0.000398,0.00013,0.0823,0.0823,0.0701,0.0627,0.0627,0.0596,5.56e-09,5.56e-09,5.85e-09,3.68e-06,3.68e-06,5e-08,0,0,0,0,0,0,0,0
 11290000,0.982,-0.00575,-0.0123,0.188,0.00158,0.0181,0.223,0.001,0.000108,-0.206,-1.49e-05,-5.77e-05,-5.63e-06,-2.3e-05,2.43e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.94e-06,0.000407,0.000407,0.00013,0.0989,0.099,0.0657,0.0694,0.0694,0.063,5.56e-09,5.56e-09,5.73e-09,3.68e-06,3.68e-06,5e-08,0,0,0,0,0,0,0,0
 11390000,0.982,-0.00586,-0.0123,0.188,0.000542,0.0163,0.209,0.000676,-0.000587,-0.195,-1.45e-05,-5.79e-05,-5.61e-06,-1.98e-05,3.06e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.91e-06,0.000367,0.000368,0.00013,0.0823,0.0823,0.0546,0.0569,0.0569,0.0608,5.36e-09,5.36e-09,5.59e-09,3.63e-06,3.63e-06,5e-08,0,0,0,0,0,0,0,0
-11490000,0.982,-0.00578,-0.0122,0.188,-0.000727,0.0177,0.224,0.000599,0.00113,-0.158,-1.45e-05,-5.79e-05,-5.54e-06,-1.98e-05,3.05e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.9e-06,0.000376,0.000376,0.00013,0.0996,0.0996,0.0504,0.064,0.064,0.0622,5.36e-09,5.36e-09,5.46e-09,3.63e-06,3.63e-06,5e-08,0,0,0,0,0,0,0,0
+11490000,0.982,-0.00578,-0.0122,0.188,-0.000726,0.0177,0.224,0.000599,0.00113,-0.158,-1.45e-05,-5.79e-05,-5.54e-06,-1.98e-05,3.05e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.9e-06,0.000376,0.000376,0.00013,0.0996,0.0996,0.0504,0.064,0.064,0.0622,5.36e-09,5.36e-09,5.46e-09,3.63e-06,3.63e-06,5e-08,0,0,0,0,0,0,0,0
 11590000,0.982,-0.00607,-0.0122,0.188,0.00157,0.0146,0.213,0.000628,9.52e-05,-0.147,-1.41e-05,-5.8e-05,-5.58e-06,-1.76e-05,3.67e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.91e-06,0.000333,0.000333,0.00013,0.0822,0.0822,0.0432,0.0535,0.0535,0.0609,5.13e-09,5.14e-09,5.34e-09,3.58e-06,3.58e-06,5e-08,0,0,0,0,0,0,0,0
 11690000,0.982,-0.00604,-0.0122,0.188,0.00175,0.0188,0.219,0.000809,0.00174,-0.123,-1.4e-05,-5.8e-05,-5.35e-06,-1.76e-05,3.67e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.9e-06,0.000342,0.000342,0.00013,0.099,0.099,0.0398,0.0611,0.0611,0.0616,5.13e-09,5.14e-09,5.21e-09,3.58e-06,3.58e-06,5e-08,0,0,0,0,0,0,0,0
 11790000,0.982,-0.0064,-0.0121,0.188,0.00099,0.0134,0.211,0.000517,-0.00108,-0.113,-1.33e-05,-5.84e-05,-4.42e-06,-1.35e-05,4.55e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.88e-06,0.000301,0.000301,0.00013,0.0809,0.0809,0.0343,0.0515,0.0515,0.0593,4.91e-09,4.92e-09,5.08e-09,3.54e-06,3.54e-06,5e-08,0,0,0,0,0,0,0,0
@@ -135,10 +135,10 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 13290000,0.982,-0.00714,-0.0116,0.187,0.00329,-0.00289,0.152,0.00122,-0.00424,-0.0287,-1.18e-05,-5.94e-05,-7.39e-07,-8.76e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.71e-06,0.000199,0.000199,0.000126,0.0643,0.0643,0.0101,0.0562,0.0562,0.0465,3.76e-09,3.76e-09,3.39e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13390000,0.982,-0.00708,-0.0117,0.187,0.00251,-0.00135,0.148,0.000831,-0.00327,-0.0311,-1.19e-05,-5.94e-05,-1.02e-06,-9.1e-06,5.63e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.68e-06,0.00019,0.00019,0.000125,0.0536,0.0536,0.00943,0.0482,0.0482,0.0452,3.63e-09,3.63e-09,3.29e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13490000,0.982,-0.00706,-0.0116,0.187,0.00298,0.000544,0.145,0.00112,-0.00324,-0.0338,-1.19e-05,-5.94e-05,-7.29e-07,-9.25e-06,5.64e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.66e-06,0.000196,0.000196,0.000125,0.0607,0.0607,0.00904,0.0559,0.0559,0.0445,3.63e-09,3.63e-09,3.19e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
-13590000,0.982,-0.00707,-0.0117,0.187,0.00735,-1.49e-05,0.143,0.00393,-0.00275,-0.0368,-1.18e-05,-5.91e-05,-9.05e-07,-8.96e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.66e-06,0.000187,0.000187,0.000125,0.0509,0.0509,0.00862,0.0481,0.0481,0.0439,3.49e-09,3.5e-09,3.11e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
+13590000,0.982,-0.00707,-0.0117,0.187,0.00735,-1.5e-05,0.143,0.00393,-0.00275,-0.0368,-1.18e-05,-5.91e-05,-9.06e-07,-8.96e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.66e-06,0.000187,0.000187,0.000125,0.0509,0.0509,0.00862,0.0481,0.0481,0.0439,3.49e-09,3.5e-09,3.11e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13690000,0.982,-0.00702,-0.0116,0.187,0.00735,-0.00124,0.142,0.00465,-0.00281,-0.0336,-1.18e-05,-5.91e-05,-3.87e-07,-9.06e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.63e-06,0.000193,0.000193,0.000124,0.0576,0.0576,0.00832,0.0556,0.0556,0.0432,3.49e-09,3.5e-09,3.02e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13790000,0.982,-0.00697,-0.0118,0.187,0.0142,0.00254,0.138,0.00814,-0.000595,-0.0374,-1.19e-05,-5.87e-05,-5.78e-07,-7.59e-06,5.66e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.6e-06,0.000186,0.000186,0.000123,0.0487,0.0487,0.00794,0.0479,0.0479,0.0421,3.36e-09,3.36e-09,2.93e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
-13890000,0.982,-0.00688,-0.0117,0.187,0.0153,0.00344,0.138,0.00959,-0.00029,-0.0338,-1.18e-05,-5.87e-05,5.08e-08,-7.71e-06,5.66e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.59e-06,0.000191,0.000191,0.000123,0.055,0.055,0.00777,0.0553,0.0553,0.0419,3.36e-09,3.36e-09,2.85e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
+13890000,0.982,-0.00688,-0.0117,0.187,0.0153,0.00344,0.138,0.00959,-0.00029,-0.0338,-1.18e-05,-5.87e-05,5.07e-08,-7.71e-06,5.66e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.59e-06,0.000191,0.000191,0.000123,0.055,0.055,0.00777,0.0553,0.0553,0.0419,3.36e-09,3.36e-09,2.85e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13990000,0.982,-0.00694,-0.0114,0.187,0.0148,0.00342,0.133,0.00728,-0.00193,-0.0384,-1.17e-05,-5.91e-05,7.03e-07,-9.92e-06,5.6e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.56e-06,0.000185,0.000185,0.000122,0.0467,0.0467,0.00748,0.0477,0.0477,0.0409,3.22e-09,3.22e-09,2.77e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 14090000,0.982,-0.00695,-0.0114,0.187,0.0124,-0.000798,0.133,0.00873,-0.00182,-0.0425,-1.17e-05,-5.91e-05,-6.09e-07,-1.02e-05,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.54e-06,0.00019,0.00019,0.000121,0.0527,0.0527,0.00733,0.0549,0.0549,0.0403,3.22e-09,3.22e-09,2.69e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 14190000,0.982,-0.00692,-0.0113,0.187,0.00986,0.000255,0.13,0.00797,-0.00139,-0.0444,-1.18e-05,-5.92e-05,-1.19e-06,-1.09e-05,5.68e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.53e-06,0.000184,0.000184,0.000121,0.0451,0.0451,0.00715,0.0475,0.0475,0.0398,3.08e-09,3.08e-09,2.62e-09,3.45e-06,3.45e-06,5e-08,0,0,0,0,0,0,0,0
@@ -202,13 +202,13 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 19990000,0.982,-0.00666,-0.0101,0.187,-0.00742,-0.000841,0.0263,0.00502,-0.000326,-0.254,-1.51e-05,-5.96e-05,1e-06,-4.94e-05,0.000134,-0.00136,0.204,0.00201,0.435,0,0,0,0,0,3.08e-06,0.000105,0.000105,8.3e-05,0.0258,0.0258,0.0083,0.0441,0.0441,0.0353,2.55e-10,2.55e-10,6.14e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
 20090000,0.982,-0.00667,-0.0101,0.186,-0.00702,-0.00301,0.0257,0.00427,-0.000528,-0.254,-1.52e-05,-5.96e-05,9.65e-07,-4.96e-05,0.000134,-0.00136,0.204,0.00201,0.435,0,0,0,0,0,3.07e-06,0.000106,0.000106,8.27e-05,0.0282,0.0282,0.00837,0.0496,0.0496,0.0357,2.55e-10,2.56e-10,6.03e-10,3.01e-06,3.01e-06,5e-08,0,0,0,0,0,0,0,0
 20190000,0.982,-0.00667,-0.0102,0.187,-0.0058,-0.000548,0.0254,0.00549,-0.000333,-0.259,-1.51e-05,-5.95e-05,7.09e-07,-4.89e-05,0.000134,-0.00136,0.204,0.00201,0.435,0,0,0,0,0,3.04e-06,0.000104,0.000104,8.21e-05,0.0251,0.0251,0.00829,0.044,0.044,0.0355,2.34e-10,2.34e-10,5.9e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
-20290000,0.982,-0.00665,-0.0103,0.187,-0.00913,-0.00156,0.0245,0.00477,-0.000376,-0.262,-1.52e-05,-5.95e-05,6.16e-07,-4.94e-05,0.000135,-0.00136,0.204,0.00201,0.435,0,0,0,0,0,3.02e-06,0.000104,0.000104,8.15e-05,0.0275,0.0275,0.00832,0.0494,0.0494,0.0356,2.34e-10,2.34e-10,5.78e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
+20290000,0.982,-0.00665,-0.0103,0.187,-0.00913,-0.00156,0.0245,0.00477,-0.000375,-0.262,-1.52e-05,-5.95e-05,6.16e-07,-4.94e-05,0.000135,-0.00136,0.204,0.00201,0.435,0,0,0,0,0,3.02e-06,0.000104,0.000104,8.15e-05,0.0275,0.0275,0.00832,0.0494,0.0494,0.0356,2.34e-10,2.34e-10,5.78e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 20390000,0.982,-0.00662,-0.0103,0.187,-0.00962,-0.000489,0.0238,0.00585,-0.000214,-0.264,-1.51e-05,-5.94e-05,7.51e-07,-4.82e-05,0.000135,-0.00135,0.204,0.00201,0.435,0,0,0,0,0,3.01e-06,0.000102,0.000102,8.12e-05,0.0245,0.0245,0.00827,0.0438,0.0438,0.0357,2.15e-10,2.15e-10,5.67e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 20490000,0.982,-0.00661,-0.0102,0.186,-0.0144,-0.00139,0.0226,0.00464,-0.00028,-0.27,-1.51e-05,-5.94e-05,7.19e-07,-4.88e-05,0.000135,-0.00135,0.204,0.00201,0.435,0,0,0,0,0,2.99e-06,0.000103,0.000103,8.06e-05,0.0267,0.0267,0.0083,0.0491,0.0491,0.0357,2.15e-10,2.15e-10,5.55e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 20590000,0.982,-0.0066,-0.0102,0.186,-0.0132,-0.00243,0.0212,0.00589,-0.000237,-0.275,-1.51e-05,-5.93e-05,9.66e-07,-4.71e-05,0.000135,-0.00135,0.204,0.00201,0.435,0,0,0,0,0,2.96e-06,0.000101,0.0001,8e-05,0.0239,0.0239,0.00821,0.0436,0.0436,0.0355,1.98e-10,1.98e-10,5.44e-10,2.99e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
 20690000,0.982,-0.00651,-0.0102,0.186,-0.0151,-0.00114,0.0214,0.00449,-0.000362,-0.278,-1.51e-05,-5.93e-05,7.37e-07,-4.76e-05,0.000135,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.95e-06,0.000101,0.000101,7.97e-05,0.0261,0.0261,0.00829,0.0489,0.0489,0.0359,1.98e-10,1.98e-10,5.34e-10,2.99e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
 20790000,0.982,-0.00591,-0.0102,0.186,-0.0174,0.00133,0.00537,0.00381,-0.000264,-0.283,-1.51e-05,-5.92e-05,8.08e-07,-4.67e-05,0.000135,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.93e-06,9.92e-05,9.91e-05,7.91e-05,0.0234,0.0234,0.0082,0.0434,0.0434,0.0356,1.83e-10,1.83e-10,5.23e-10,2.98e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
-20890000,0.982,0.00321,-0.00615,0.186,-0.0238,0.0134,-0.114,0.00169,0.000517,-0.293,-1.51e-05,-5.92e-05,8.04e-07,-4.69e-05,0.000135,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.9e-06,9.98e-05,9.98e-05,7.86e-05,0.0258,0.0258,0.00823,0.0487,0.0487,0.0357,1.83e-10,1.83e-10,5.13e-10,2.98e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
+20890000,0.982,0.00321,-0.00615,0.186,-0.0238,0.0134,-0.114,0.00169,0.000517,-0.293,-1.51e-05,-5.92e-05,8.03e-07,-4.69e-05,0.000135,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.9e-06,9.98e-05,9.98e-05,7.86e-05,0.0258,0.0258,0.00823,0.0487,0.0487,0.0357,1.83e-10,1.83e-10,5.13e-10,2.98e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
 20990000,0.982,0.00653,-0.00273,0.186,-0.0345,0.0313,-0.254,0.00122,0.00104,-0.311,-1.49e-05,-5.92e-05,7.54e-07,-4.47e-05,0.000132,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.88e-06,9.78e-05,9.77e-05,7.8e-05,0.0234,0.0234,0.00814,0.0433,0.0433,0.0354,1.69e-10,1.69e-10,5.03e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
 21090000,0.982,0.00494,-0.00311,0.187,-0.0482,0.0474,-0.373,-0.00291,0.00502,-0.345,-1.49e-05,-5.92e-05,7.37e-07,-4.48e-05,0.000132,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.88e-06,9.84e-05,9.83e-05,7.77e-05,0.0258,0.0258,0.00822,0.0485,0.0485,0.0358,1.69e-10,1.69e-10,4.94e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
 21190000,0.982,0.00206,-0.00479,0.187,-0.0517,0.0513,-0.5,-0.00221,0.00397,-0.384,-1.46e-05,-5.9e-05,7.96e-07,-3.99e-05,0.000122,-0.00135,0.204,0.00201,0.435,0,0,0,0,0,2.87e-06,9.62e-05,9.62e-05,7.72e-05,0.0234,0.0234,0.00813,0.0432,0.0432,0.0356,1.56e-10,1.56e-10,4.84e-10,2.97e-06,2.97e-06,5e-08,0,0,0,0,0,0,0,0
@@ -224,7 +224,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 22190000,0.982,-0.00579,-0.00887,0.187,-0.00629,0.0139,-1.38,0.00479,0.0134,-1.5,-1.39e-05,-5.84e-05,1.44e-06,-2.65e-05,0.000111,-0.00135,0.204,0.00201,0.435,0,0,0,0,0,2.73e-06,8.73e-05,8.73e-05,7.28e-05,0.0214,0.0214,0.00802,0.0427,0.0427,0.0355,1.08e-10,1.08e-10,4.02e-10,2.91e-06,2.92e-06,5e-08,0,0,0,0,0,0,0,0
 22290000,0.982,-0.0065,-0.00912,0.187,-0.00118,0.00872,-1.38,0.0044,0.0145,-1.65,-1.39e-05,-5.84e-05,1.34e-06,-2.71e-05,0.000112,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.71e-06,8.77e-05,8.76e-05,7.23e-05,0.0231,0.0231,0.00806,0.0476,0.0476,0.0355,1.08e-10,1.08e-10,3.95e-10,2.91e-06,2.92e-06,5e-08,0,0,0,0,0,0,0,0
 22390000,0.982,-0.00686,-0.00928,0.187,0.00414,-0.000935,-1.38,0.0118,0.00462,-1.79,-1.38e-05,-5.85e-05,1.14e-06,-3.13e-05,0.000111,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.71e-06,8.58e-05,8.58e-05,7.2e-05,0.0206,0.0206,0.00802,0.0425,0.0425,0.0356,1.01e-10,1.01e-10,3.88e-10,2.9e-06,2.91e-06,5e-08,0,0,0,0,0,0,0,0
-22490000,0.982,-0.007,-0.00974,0.187,0.008,-0.00689,-1.39,0.0124,0.00419,-1.93,-1.38e-05,-5.85e-05,9.92e-07,-3.15e-05,0.000111,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.69e-06,8.62e-05,8.61e-05,7.15e-05,0.0222,0.0222,0.00806,0.0474,0.0474,0.0356,1.01e-10,1.01e-10,3.81e-10,2.9e-06,2.91e-06,5e-08,0,0,0,0,0,0,0,0
+22490000,0.982,-0.007,-0.00974,0.187,0.008,-0.00688,-1.39,0.0124,0.00419,-1.93,-1.38e-05,-5.85e-05,9.92e-07,-3.15e-05,0.000111,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.69e-06,8.62e-05,8.61e-05,7.15e-05,0.0222,0.0222,0.00806,0.0474,0.0474,0.0356,1.01e-10,1.01e-10,3.81e-10,2.9e-06,2.91e-06,5e-08,0,0,0,0,0,0,0,0
 22590000,0.982,-0.00693,-0.0104,0.187,0.0176,-0.0159,-1.38,0.0245,-0.00484,-2.08,-1.37e-05,-5.84e-05,1.16e-06,-2.88e-05,0.00011,-0.00133,0.204,0.00201,0.435,0,0,0,0,0,2.68e-06,8.45e-05,8.44e-05,7.11e-05,0.0199,0.0199,0.00799,0.0423,0.0423,0.0353,9.46e-11,9.47e-11,3.75e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 22690000,0.982,-0.00684,-0.0106,0.188,0.0197,-0.0203,-1.39,0.0264,-0.00669,-2.22,-1.37e-05,-5.84e-05,1.07e-06,-2.94e-05,0.000111,-0.00133,0.204,0.00201,0.435,0,0,0,0,0,2.67e-06,8.48e-05,8.48e-05,7.08e-05,0.0215,0.0215,0.00807,0.0471,0.0471,0.0357,9.47e-11,9.48e-11,3.69e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 22790000,0.982,-0.00684,-0.011,0.187,0.026,-0.0282,-1.39,0.0367,-0.0168,-2.37,-1.36e-05,-5.84e-05,9.31e-07,-3.16e-05,0.000112,-0.00133,0.204,0.00201,0.435,0,0,0,0,0,2.65e-06,8.33e-05,8.33e-05,7.04e-05,0.0193,0.0193,0.00799,0.0421,0.0421,0.0354,8.92e-11,8.92e-11,3.62e-10,2.89e-06,2.89e-06,5e-08,0,0,0,0,0,0,0,0
@@ -249,7 +249,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 24690000,0.983,-0.0105,-0.0133,0.185,0.0616,-0.0616,0.0629,0.169,-0.117,-3.91,-1.44e-05,-5.84e-05,1.95e-06,-3.4e-05,9.94e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.37e-06,7.95e-05,7.94e-05,6.38e-05,0.0161,0.0161,0.00801,0.0436,0.0436,0.0354,5.9e-11,5.9e-11,2.66e-10,2.86e-06,2.86e-06,5e-08,0,0,0,0,0,0,0,0
 24790000,0.983,-0.0105,-0.0126,0.185,0.0583,-0.0592,0.0546,0.171,-0.113,-3.91,-1.46e-05,-5.84e-05,1.98e-06,-3.42e-05,9.31e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.35e-06,7.94e-05,7.94e-05,6.34e-05,0.0149,0.0149,0.00794,0.0394,0.0394,0.0352,5.69e-11,5.69e-11,2.62e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
 24890000,0.983,-0.0104,-0.0121,0.185,0.0567,-0.0625,0.0441,0.177,-0.119,-3.91,-1.46e-05,-5.84e-05,2.06e-06,-3.44e-05,9.34e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.34e-06,7.96e-05,7.96e-05,6.3e-05,0.016,0.016,0.00799,0.0434,0.0434,0.0352,5.7e-11,5.7e-11,2.58e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
-24990000,0.983,-0.0101,-0.0119,0.185,0.0477,-0.058,0.037,0.177,-0.111,-3.91,-1.49e-05,-5.83e-05,2.05e-06,-3.63e-05,8.56e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.33e-06,7.96e-05,7.96e-05,6.28e-05,0.0148,0.0148,0.00796,0.0393,0.0393,0.0353,5.5e-11,5.5e-11,2.54e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
+24990000,0.983,-0.0101,-0.0119,0.185,0.0477,-0.058,0.037,0.177,-0.111,-3.91,-1.49e-05,-5.83e-05,2.05e-06,-3.62e-05,8.56e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.33e-06,7.96e-05,7.96e-05,6.28e-05,0.0148,0.0148,0.00796,0.0393,0.0393,0.0353,5.5e-11,5.5e-11,2.54e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
 25090000,0.983,-0.0105,-0.012,0.185,0.0442,-0.0572,0.0344,0.182,-0.116,-3.91,-1.49e-05,-5.83e-05,2.01e-06,-3.63e-05,8.6e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.32e-06,7.98e-05,7.98e-05,6.25e-05,0.0159,0.0159,0.00801,0.0432,0.0432,0.0353,5.51e-11,5.51e-11,2.5e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
 25190000,0.983,-0.0107,-0.0118,0.185,0.0393,-0.0505,0.0344,0.182,-0.108,-3.91,-1.51e-05,-5.83e-05,1.9e-06,-3.88e-05,8.06e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.31e-06,7.98e-05,7.98e-05,6.21e-05,0.0147,0.0147,0.00794,0.0391,0.0391,0.0351,5.32e-11,5.32e-11,2.47e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
 25290000,0.983,-0.0108,-0.0111,0.185,0.0344,-0.0517,0.0292,0.185,-0.113,-3.91,-1.51e-05,-5.83e-05,1.73e-06,-3.86e-05,8.05e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.3e-06,8e-05,8e-05,6.19e-05,0.0158,0.0158,0.00803,0.043,0.043,0.0355,5.33e-11,5.33e-11,2.43e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
@@ -262,7 +262,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 25990000,0.983,-0.0103,-0.00958,0.186,-0.0112,-0.0218,0.0127,0.163,-0.0859,-3.91,-1.58e-05,-5.83e-05,1.19e-06,-3.91e-05,5.96e-05,-0.00124,0.204,0.00201,0.435,0,0,0,0,0,2.23e-06,8.06e-05,8.06e-05,5.99e-05,0.0143,0.0143,0.00798,0.0387,0.0387,0.0353,4.7e-11,4.7e-11,2.2e-10,2.84e-06,2.84e-06,5e-08,0,0,0,0,0,0,0,0
 26090000,0.983,-0.01,-0.00957,0.186,-0.0164,-0.0215,0.0113,0.162,-0.0881,-3.91,-1.58e-05,-5.83e-05,1.27e-06,-3.91e-05,5.96e-05,-0.00124,0.204,0.00201,0.435,0,0,0,0,0,2.21e-06,8.08e-05,8.08e-05,5.95e-05,0.0153,0.0153,0.00802,0.0425,0.0425,0.0353,4.71e-11,4.71e-11,2.17e-10,2.84e-06,2.84e-06,5e-08,0,0,0,0,0,0,0,0
 26190000,0.983,-0.00992,-0.0101,0.186,-0.023,-0.0149,0.00648,0.151,-0.081,-3.92,-1.59e-05,-5.83e-05,1.32e-06,-3.86e-05,5.74e-05,-0.00123,0.204,0.00201,0.435,0,0,0,0,0,2.2e-06,8.08e-05,8.08e-05,5.92e-05,0.0142,0.0142,0.00796,0.0386,0.0386,0.0351,4.57e-11,4.57e-11,2.14e-10,2.84e-06,2.84e-06,5e-08,0,0,0,0,0,0,0,0
-26290000,0.983,-0.00993,-0.0104,0.186,-0.0245,-0.0135,0.000746,0.149,-0.0825,-3.92,-1.59e-05,-5.83e-05,1.18e-06,-3.84e-05,5.73e-05,-0.00123,0.204,0.00201,0.435,0,0,0,0,0,2.2e-06,8.1e-05,8.1e-05,5.9e-05,0.0152,0.0152,0.00804,0.0424,0.0424,0.0354,4.58e-11,4.58e-11,2.11e-10,2.84e-06,2.84e-06,5e-08,0,0,0,0,0,0,0,0
+26290000,0.983,-0.00993,-0.0104,0.186,-0.0245,-0.0135,0.000745,0.149,-0.0825,-3.92,-1.59e-05,-5.83e-05,1.18e-06,-3.84e-05,5.73e-05,-0.00123,0.204,0.00201,0.435,0,0,0,0,0,2.2e-06,8.1e-05,8.1e-05,5.9e-05,0.0152,0.0152,0.00804,0.0424,0.0424,0.0354,4.58e-11,4.58e-11,2.11e-10,2.84e-06,2.84e-06,5e-08,0,0,0,0,0,0,0,0
 26390000,0.983,-0.00935,-0.0103,0.186,-0.0306,-0.0064,0.00471,0.136,-0.0744,-3.92,-1.6e-05,-5.84e-05,1.05e-06,-3.62e-05,5.44e-05,-0.00123,0.204,0.00201,0.435,0,0,0,0,0,2.18e-06,8.1e-05,8.1e-05,5.87e-05,0.0141,0.0141,0.00797,0.0385,0.0385,0.0352,4.45e-11,4.45e-11,2.08e-10,2.84e-06,2.84e-06,5e-08,0,0,0,0,0,0,0,0
 26490000,0.983,-0.0091,-0.0102,0.185,-0.0338,-0.00325,0.0142,0.132,-0.0749,-3.92,-1.6e-05,-5.84e-05,9.8e-07,-3.62e-05,5.44e-05,-0.00123,0.204,0.00201,0.435,0,0,0,0,0,2.17e-06,8.12e-05,8.12e-05,5.84e-05,0.0151,0.0151,0.00802,0.0423,0.0423,0.0352,4.46e-11,4.46e-11,2.05e-10,2.84e-06,2.84e-06,5e-08,0,0,0,0,0,0,0,0
 26590000,0.983,-0.00854,-0.0104,0.186,-0.0354,0.00469,0.0142,0.122,-0.0678,-3.92,-1.61e-05,-5.84e-05,8.56e-07,-3.53e-05,5.22e-05,-0.00123,0.204,0.00201,0.435,0,0,0,0,0,2.16e-06,8.12e-05,8.11e-05,5.83e-05,0.014,0.014,0.00799,0.0384,0.0384,0.0353,4.33e-11,4.33e-11,2.03e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
@@ -309,7 +309,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 30690000,0.983,-0.00624,-0.0124,0.186,-0.0524,0.0405,0.758,-0.0704,0.0539,-1.55,-1.41e-05,-5.73e-05,8.6e-07,-2.37e-05,2.86e-05,-0.00111,0.204,0.00201,0.435,0,0,0,0,0,1.85e-06,8.4e-05,8.4e-05,4.91e-05,0.0135,0.0135,0.008,0.0405,0.0405,0.0353,3.04e-11,3.05e-11,1.24e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 30790000,0.983,-0.00593,-0.012,0.185,-0.0454,0.035,0.757,-0.0631,0.0524,-1.48,-1.4e-05,-5.72e-05,8.66e-07,-2.07e-05,2.7e-05,-0.00111,0.204,0.00201,0.435,0,0,0,0,0,1.84e-06,8.37e-05,8.37e-05,4.9e-05,0.0126,0.0126,0.00797,0.0371,0.0371,0.0354,3e-11,3e-11,1.23e-10,2.82e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 30890000,0.983,-0.00529,-0.0119,0.186,-0.0458,0.0321,0.753,-0.0676,0.0558,-1.42,-1.4e-05,-5.72e-05,7.85e-07,-2.04e-05,2.63e-05,-0.00111,0.204,0.00201,0.435,0,0,0,0,0,1.83e-06,8.39e-05,8.39e-05,4.88e-05,0.0135,0.0135,0.00802,0.0405,0.0405,0.0354,3.01e-11,3.01e-11,1.21e-10,2.82e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
-30990000,0.983,-0.00548,-0.0119,0.186,-0.0381,0.0265,0.755,-0.0576,0.0486,-1.35,-1.39e-05,-5.72e-05,7.77e-07,-1.71e-05,2.13e-05,-0.0011,0.204,0.00201,0.435,0,0,0,0,0,1.83e-06,8.36e-05,8.36e-05,4.86e-05,0.0126,0.0126,0.00795,0.037,0.037,0.0352,2.96e-11,2.96e-11,1.2e-10,2.82e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
+30990000,0.983,-0.00548,-0.0119,0.186,-0.0381,0.0265,0.754,-0.0576,0.0486,-1.35,-1.39e-05,-5.72e-05,7.77e-07,-1.71e-05,2.13e-05,-0.0011,0.204,0.00201,0.435,0,0,0,0,0,1.83e-06,8.36e-05,8.36e-05,4.86e-05,0.0126,0.0126,0.00795,0.037,0.037,0.0352,2.96e-11,2.96e-11,1.2e-10,2.82e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31090000,0.982,-0.00562,-0.012,0.186,-0.0371,0.0256,0.753,-0.0613,0.0512,-1.28,-1.39e-05,-5.72e-05,7.25e-07,-1.7e-05,2.11e-05,-0.0011,0.204,0.00201,0.435,0,0,0,0,0,1.83e-06,8.38e-05,8.38e-05,4.85e-05,0.0134,0.0134,0.00803,0.0405,0.0405,0.0355,2.97e-11,2.97e-11,1.19e-10,2.82e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31190000,0.983,-0.00581,-0.0121,0.186,-0.0324,0.0209,0.755,-0.0528,0.046,-1.21,-1.38e-05,-5.71e-05,8.7e-07,-1.38e-05,1.75e-05,-0.0011,0.204,0.00201,0.435,0,0,0,0,0,1.82e-06,8.35e-05,8.35e-05,4.83e-05,0.0125,0.0125,0.00796,0.037,0.037,0.0353,2.93e-11,2.93e-11,1.18e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31290000,0.983,-0.00604,-0.0122,0.186,-0.0301,0.0183,0.759,-0.0558,0.048,-1.14,-1.38e-05,-5.71e-05,9.47e-07,-1.33e-05,1.67e-05,-0.0011,0.204,0.00201,0.435,0,0,0,0,0,1.81e-06,8.37e-05,8.37e-05,4.81e-05,0.0134,0.0134,0.008,0.0404,0.0404,0.0353,2.94e-11,2.94e-11,1.16e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
@@ -317,8 +317,8 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 31490000,0.982,-0.00555,-0.0123,0.186,-0.0241,0.00935,0.754,-0.0494,0.0433,-0.993,-1.37e-05,-5.7e-05,8.45e-07,-1.1e-05,1.39e-05,-0.00109,0.204,0.00201,0.435,0,0,0,0,0,1.8e-06,8.35e-05,8.35e-05,4.78e-05,0.0134,0.0134,0.00802,0.0404,0.0404,0.0354,2.9e-11,2.9e-11,1.14e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31590000,0.983,-0.00542,-0.0128,0.186,-0.02,0.00712,0.758,-0.0384,0.0389,-0.922,-1.36e-05,-5.7e-05,9.33e-07,-6.86e-06,1.15e-05,-0.00109,0.204,0.00201,0.435,0,0,0,0,0,1.79e-06,8.32e-05,8.31e-05,4.76e-05,0.0125,0.0125,0.00795,0.0369,0.037,0.0352,2.86e-11,2.86e-11,1.13e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31690000,0.983,-0.00541,-0.0133,0.185,-0.0222,0.00606,0.754,-0.0406,0.0395,-0.854,-1.36e-05,-5.7e-05,1.05e-06,-6.22e-06,1.08e-05,-0.00109,0.204,0.00201,0.435,0,0,0,0,0,1.79e-06,8.33e-05,8.33e-05,4.74e-05,0.0133,0.0133,0.00799,0.0404,0.0404,0.0353,2.87e-11,2.87e-11,1.12e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
-31790000,0.983,-0.00563,-0.0139,0.185,-0.013,0.00349,0.754,-0.029,0.0376,-0.783,-1.36e-05,-5.69e-05,1.12e-06,-9.72e-07,1.07e-05,-0.00108,0.204,0.00201,0.435,0,0,0,0,0,1.78e-06,8.3e-05,8.29e-05,4.73e-05,0.0125,0.0125,0.00796,0.0369,0.0369,0.0353,2.83e-11,2.83e-11,1.11e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
-31890000,0.983,-0.00536,-0.0136,0.185,-0.00985,0.00122,0.752,-0.0301,0.0378,-0.716,-1.36e-05,-5.69e-05,1.17e-06,-3.51e-07,1.01e-05,-0.00108,0.204,0.00201,0.435,0,0,0,0,0,1.78e-06,8.31e-05,8.31e-05,4.71e-05,0.0133,0.0133,0.00801,0.0403,0.0403,0.0354,2.84e-11,2.84e-11,1.1e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
+31790000,0.983,-0.00563,-0.0139,0.185,-0.013,0.00349,0.754,-0.029,0.0376,-0.783,-1.36e-05,-5.69e-05,1.12e-06,-9.71e-07,1.07e-05,-0.00108,0.204,0.00201,0.435,0,0,0,0,0,1.78e-06,8.3e-05,8.29e-05,4.73e-05,0.0125,0.0125,0.00796,0.0369,0.0369,0.0353,2.83e-11,2.83e-11,1.11e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
+31890000,0.983,-0.00536,-0.0136,0.185,-0.00985,0.00122,0.752,-0.0301,0.0378,-0.716,-1.36e-05,-5.69e-05,1.17e-06,-3.5e-07,1.01e-05,-0.00108,0.204,0.00201,0.435,0,0,0,0,0,1.78e-06,8.31e-05,8.31e-05,4.71e-05,0.0133,0.0133,0.00801,0.0403,0.0403,0.0354,2.84e-11,2.84e-11,1.1e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31990000,0.983,-0.00563,-0.0132,0.185,-0.00189,0.000554,0.748,-0.0181,0.0347,-0.651,-1.36e-05,-5.68e-05,1.13e-06,4.26e-06,9.25e-06,-0.00107,0.204,0.00201,0.435,0,0,0,0,0,1.77e-06,8.27e-05,8.27e-05,4.69e-05,0.0124,0.0124,0.00794,0.0369,0.0369,0.0351,2.8e-11,2.8e-11,1.08e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32090000,0.983,-0.00599,-0.0129,0.185,-0.00251,-0.00283,0.75,-0.0183,0.0346,-0.581,-1.36e-05,-5.68e-05,1.14e-06,4.61e-06,9.04e-06,-0.00107,0.204,0.00201,0.435,0,0,0,0,0,1.77e-06,8.29e-05,8.29e-05,4.68e-05,0.0133,0.0133,0.00802,0.0403,0.0403,0.0355,2.81e-11,2.81e-11,1.08e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32190000,0.983,-0.00617,-0.0132,0.185,0.00289,-0.00605,0.75,-0.00708,0.0332,-0.515,-1.36e-05,-5.67e-05,1.1e-06,8.82e-06,1.06e-05,-0.00107,0.204,0.00201,0.435,0,0,0,0,0,1.76e-06,8.25e-05,8.25e-05,4.66e-05,0.0124,0.0124,0.00795,0.0369,0.0369,0.0352,2.77e-11,2.77e-11,1.06e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
@@ -329,7 +329,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 32690000,0.983,-0.00897,-0.0113,0.185,0.0311,-0.0793,-0.13,0.0229,0.0124,-0.408,-1.36e-05,-5.66e-05,1.16e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.73e-06,8.14e-05,8.14e-05,4.57e-05,0.0187,0.0187,0.00736,0.0406,0.0406,0.0351,2.72e-11,2.72e-11,1.01e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32790000,0.983,-0.00866,-0.0113,0.185,0.0297,-0.0768,-0.131,0.0327,0.0105,-0.425,-1.37e-05,-5.65e-05,1.24e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.72e-06,7.87e-05,7.87e-05,4.57e-05,0.0196,0.0196,0.00713,0.0372,0.0372,0.0351,2.68e-11,2.68e-11,1.01e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32890000,0.983,-0.00862,-0.0114,0.185,0.0293,-0.0827,-0.133,0.0357,0.00253,-0.441,-1.37e-05,-5.65e-05,1.28e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.72e-06,7.88e-05,7.88e-05,4.55e-05,0.0236,0.0236,0.00697,0.041,0.041,0.035,2.69e-11,2.69e-11,9.97e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
-32990000,0.983,-0.00835,-0.0113,0.185,0.0268,-0.0787,-0.132,0.0436,-0.00068,-0.455,-1.38e-05,-5.65e-05,1.36e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.71e-06,7.44e-05,7.44e-05,4.53e-05,0.0243,0.0243,0.00673,0.0376,0.0376,0.0347,2.65e-11,2.65e-11,9.87e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
+32990000,0.983,-0.00835,-0.0113,0.185,0.0268,-0.0787,-0.132,0.0436,-0.000681,-0.455,-1.38e-05,-5.65e-05,1.36e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.71e-06,7.44e-05,7.44e-05,4.53e-05,0.0243,0.0243,0.00673,0.0376,0.0376,0.0347,2.65e-11,2.65e-11,9.87e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 33090000,0.983,-0.00831,-0.0113,0.185,0.0227,-0.0825,-0.129,0.0461,-0.00872,-0.463,-1.38e-05,-5.65e-05,1.34e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.71e-06,7.45e-05,7.45e-05,4.52e-05,0.029,0.029,0.00661,0.0418,0.0418,0.0349,2.66e-11,2.66e-11,9.79e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 33190000,0.983,-0.008,-0.0113,0.185,0.019,-0.0781,-0.127,0.0523,-0.0106,-0.47,-1.38e-05,-5.65e-05,1.29e-06,1.23e-05,7.5e-06,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.7e-06,6.85e-05,6.85e-05,4.5e-05,0.0295,0.0296,0.00641,0.0382,0.0382,0.0346,2.62e-11,2.62e-11,9.7e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 33290000,0.983,-0.00805,-0.0113,0.185,0.0157,-0.0794,-0.126,0.054,-0.0185,-0.479,-1.38e-05,-5.65e-05,1.39e-06,1.23e-05,7.51e-06,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.69e-06,6.86e-05,6.86e-05,4.49e-05,0.0357,0.0357,0.00629,0.0428,0.0428,0.0344,2.63e-11,2.63e-11,9.61e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0

src/modules/lighting_state_converter/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__lighting_state_converter
+	MAIN lighting_state_converter
+	SRCS
+		lighting_state_converter.cpp
+	)
+

src/modules/lighting_state_converter/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_LIGHTING_STATE_CONVERTER
+	bool "lighting_state_converter"
+	default n
+	---help---
+		Enable support for lighting_state_converter

src/modules/lighting_state_converter/lighting_state_converter.cpp

@@ -0,0 +1,271 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "lighting_state_converter.h"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/posix.h>
+
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_combined.h>
+
+
+int LightingStateConverter::print_status()
+{
+	PX4_INFO("Running");
+	// TODO: print additional runtime information about the state of the module
+
+	return 0;
+}
+
+int LightingStateConverter::custom_command(int argc, char *argv[])
+{
+	/*
+	if (!is_running()) {
+		print_usage("not running");
+		return 1;
+	}
+
+	// additional custom commands can be handled like this:
+	if (!strcmp(argv[0], "do-something")) {
+		get_instance()->do_something();
+		return 0;
+	}
+	 */
+
+	return print_usage("unknown command");
+}
+
+
+int LightingStateConverter::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("module",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_DEFAULT,
+				      1024,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	return 0;
+}
+
+LightingStateConverter *LightingStateConverter::instantiate(int argc, char *argv[])
+{
+	int example_param = 0;
+	bool example_flag = false;
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+
+	// parse CLI arguments
+	while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'p':
+			example_param = (int)strtol(myoptarg, nullptr, 10);
+			break;
+
+		case 'f':
+			example_flag = true;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return nullptr;
+	}
+
+	LightingStateConverter *instance = new LightingStateConverter(example_param, example_flag);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+	}
+
+	return instance;
+}
+
+LightingStateConverter::LightingStateConverter(int example_param, bool example_flag)
+	: ModuleParams(nullptr)
+{
+}
+
+void LightingStateConverter::run()
+{
+	// Example: run the loop synchronized to the sensor_combined topic publication
+	int lighting_states_sub = orb_subscribe(ORB_ID(lighting_states));
+
+	px4_pollfd_struct_t fds[1];
+	fds[0].fd = lighting_states_sub;
+	fds[0].events = POLLIN;
+
+	// initialize parameters
+	parameters_update(true);
+
+	_lighting_id_no = _param_lighting_id_no.get();
+
+	while (!should_exit()) {
+
+		// wait for up to 1000ms for data
+		int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000);
+
+		if (pret == 0) {
+			// Timeout: let the loop run anyway, don't do `continue` here
+			//PX4_ERR("Timeout on poll");
+
+		} else if (pret < 0) {
+			// this is undesirable but not much we can do
+			PX4_ERR("poll error %d, %d", pret, errno);
+			px4_usleep(50000);
+			continue;
+
+		} else if (fds[0].revents & POLLIN) {
+
+			struct lighting_states_s lighting_states;
+			orb_copy(ORB_ID(lighting_states), lighting_states_sub, &lighting_states);
+			// TODO: do something with the data...
+
+			printf("Received state data. Contents: \n");
+			for(int i = 0; i < 10; i++) {
+				printf("ID: %d State: %d\n", i, lighting_states.state[i]);
+			}
+
+			for (int i = (_lighting_id_no - 1)*2; i < (_lighting_id_no*2); i++) {
+				printf("_lighting_id_no: %d\n", i);
+				if (lighting_states.state[i] != 255) {
+					led_control_s led_control;
+					if(i == (_lighting_id_no - 1)*2) led_control.led_mask = 0b0000011110000010;
+					if(i == (_lighting_id_no*2)-1) led_control.led_mask = 0b0000000001111100;
+
+					if (lighting_states.state[i] == 0) {
+						led_control.color = led_control_s::COLOR_DIM_RED;
+						led_control.mode = led_control_s::MODE_ON;
+					}
+
+					if (lighting_states.state[i] == 1) {
+						led_control.color = led_control_s::COLOR_RED;
+						led_control.mode = led_control_s::MODE_ON;
+					}
+
+					if (lighting_states.state[i] == 2) {
+						led_control.color = led_control_s::COLOR_WHITE;
+						led_control.mode = led_control_s::MODE_ON;
+					}
+
+					if (lighting_states.state[i] == 3) {
+						led_control.color = led_control_s::COLOR_YELLOW;
+						led_control.mode = led_control_s::MODE_BLINK_NORMAL;
+						led_control.num_blinks = 0;
+					}
+
+					if (lighting_states.state[i] == 4) {
+						led_control.color = led_control_s::COLOR_YELLOW;
+						led_control.mode = led_control_s::MODE_BREATHE;
+						led_control.num_blinks = 0;
+					}
+
+					led_control.timestamp = hrt_absolute_time();
+					_led_control_pub.publish(led_control);
+				}
+			}
+		}
+
+		parameters_update();
+	}
+
+	orb_unsubscribe(lighting_states_sub);
+}
+
+void LightingStateConverter::parameters_update(bool force)
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s update;
+		_parameter_update_sub.copy(&update);
+
+		// update parameters from storage
+		updateParams();
+	}
+}
+
+int LightingStateConverter::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Section that describes the provided module functionality.
+
+This is a template for a module running as a task in the background with start/stop/status functionality.
+
+### Implementation
+Section describing the high-level implementation of this module.
+
+### Examples
+CLI usage example:
+$ module start -f -p 42
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("module", "template");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
+	PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+int lighting_state_converter_main(int argc, char *argv[])
+{
+	return LightingStateConverter::main(argc, argv);
+}

src/modules/lighting_state_converter/lighting_state_converter.h

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/lighting_states.h>
+#include <uORB/topics/led_control.h>
+
+using namespace time_literals;
+
+extern "C" __EXPORT int lighting_state_converter_main(int argc, char *argv[]);
+
+
+class LightingStateConverter : public ModuleBase<LightingStateConverter>, public ModuleParams
+{
+public:
+	LightingStateConverter(int example_param, bool example_flag);
+
+	virtual ~LightingStateConverter() = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static LightingStateConverter *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+
+	/**
+	 * Check for parameter changes and update them if needed.
+	 * @param parameter_update_sub uorb subscription to parameter_update
+	 * @param force for a parameter update
+	 */
+	void parameters_update(bool force = false);
+
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::LIGHTING_ID_NO>) _param_lighting_id_no   /**< example parameter */
+	)
+
+	int _lighting_id_no;
+
+	// Subscriptions
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Publication<led_control_s> _led_control_pub{ORB_ID(led_control)};
+	uORB::Subscription _lighting_states_sub{ORB_ID(lighting_states)};
+
+};
+

src/modules/lighting_state_converter/parameters.c

@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* Lighting ID number.
+*
+* @boolean
+* @reboot_required false
+* @group id
+*/
+PARAM_DEFINE_INT32(LIGHTING_ID_NO, 1);

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -192,7 +192,7 @@ MulticopterRateControl::Run()
 				vehicle_rates_setpoint.yaw            = _rates_setpoint(2);
 				vehicle_rates_setpoint.thrust_body[0] = 0.0f;
 				vehicle_rates_setpoint.thrust_body[1] = 0.0f;
-				vehicle_rates_setpoint.thrust_body[2] = _thrust_setpoint;
+				vehicle_rates_setpoint.thrust_body[2] = -_thrust_setpoint;
 				vehicle_rates_setpoint.timestamp      = hrt_absolute_time();
 
 				_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);

src/modules/navigator/mission_block.cpp

@@ -74,6 +74,8 @@ MissionBlock::is_mission_item_reached()
 {
 	/* handle non-navigation or indefinite waypoints */
 
+	hrt_abstime now = hrt_absolute_time();
+
 	switch (_mission_item.nav_cmd) {
 	case NAV_CMD_DO_SET_SERVO:
 		return true;
@@ -108,6 +110,8 @@ MissionBlock::is_mission_item_reached()
 	case NAV_CMD_SET_CAMERA_MODE:
 	case NAV_CMD_SET_CAMERA_ZOOM:
 	case NAV_CMD_SET_CAMERA_FOCUS:
+	case NAV_CMD_DO_CHANGE_SPEED:
+	case NAV_CMD_DO_SET_HOME:
 		return true;
 
 	case NAV_CMD_DO_VTOL_TRANSITION:
@@ -125,17 +129,21 @@ MissionBlock::is_mission_item_reached()
 			return false;
 		}
 
-	case NAV_CMD_DO_CHANGE_SPEED:
-	case NAV_CMD_DO_SET_HOME:
-		return true;
+	case NAV_CMD_DELAY:
+		// Set reached flags directly such that only the delay time is considered
+		_waypoint_position_reached = true;
+		_waypoint_yaw_reached = true;
+
+		// Set timestamp when entering only (it's reset to 0 for every waypoint)
+		if (_time_wp_reached == 0) {
+			_time_wp_reached = now;
+		}
 
 	default:
 		/* do nothing, this is a 3D waypoint */
 		break;
 	}
 
-	hrt_abstime now = hrt_absolute_time();
-
 	if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) {
 
 		float dist = -1.0f;
@@ -268,11 +276,6 @@ MissionBlock::is_mission_item_reached()
 				}
 			}
 
-		} else if (_mission_item.nav_cmd == NAV_CMD_DELAY) {
-			_waypoint_position_reached = true;
-			_waypoint_yaw_reached = true;
-			_time_wp_reached = now;
-
 		} else {
 
 			float acceptance_radius = _navigator->get_acceptance_radius();

src/modules/simulator/simulator_mavlink.cpp

@@ -148,12 +148,6 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
 			is_fixed_wing = false;
 			break;
 
-		case MAV_TYPE_VTOL_TAILSITTER:
-			// this is the tricopter VTOL / quad plane with 3 motors and 2 servos
-			pos_thrust_motors_count = 3;
-			is_fixed_wing = false;
-			break;
-
 		case MAV_TYPE_OCTOROTOR:
 			pos_thrust_motors_count = 8;
 			is_fixed_wing = false;