FailureDetector: check if ESCs have current

And increase the timeout to 300ms, as some ESCs only update with 10Hz.
TEST PositionControl: use aux2 to switch between thrust vs attitude
2026-05-25 20:37:35 +08:00 · 2022-06-03 09:53:56 +02:00 · 2022-06-03 09:52:45 +02:00 · 2022-05-31 14:33:27 +02:00 · 2022-05-31 14:16:33 +02:00 · 2022-05-23 15:37:48 +02:00
118 changed files with 1530 additions and 4016 deletions
@@ -864,15 +864,12 @@ void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_vel" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_variances" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovations" || true'
@@ -32,4 +32,3 @@
 ############################################################################

 add_subdirectory(init.d)
-add_subdirectory(mixers)
@@ -33,5 +33,4 @@

 px4_add_romfs_files(
 	rcS
-	rc.output_defaults
 )
@@ -1,32 +0,0 @@
-#!/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
@@ -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
@@ -1,36 +0,0 @@
-############################################################################
-#
-#   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
-)
@@ -1,12 +0,0 @@
-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
@@ -95,9 +95,10 @@ fi

 if param compare SYS_AUTOCONFIG 1
 then
-	# Reset params except Airframe, RC calibration, sensor calibration, flight modes, total flight time, and next flight UUID.
-	param reset_all SYS_AUTOSTART RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT*
 	set AUTOCNF yes
+
+	# Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal, next flight UUID
+	param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT*
 fi

 # multi-instance setup
@@ -195,6 +196,14 @@ fi

 . "$autostart_file"

+#
+# If autoconfig parameter was set, reset it and save parameters.
+#
+if [ $AUTOCNF = yes ]
+then
+	param set SYS_AUTOCONFIG 0
+fi
+
 # Simulator IMU data provided at 250 Hz
 param set IMU_INTEG_RATE 250

@@ -55,5 +55,4 @@ px4_add_romfs_files(
 	rc.vehicle_setup
 	rc.vtol_apps
 	rc.vtol_defaults
-	rc.output_defaults
 )
@@ -1,21 +0,0 @@
-#!/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
@@ -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
@@ -1,25 +0,0 @@
-#!/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
@@ -21,6 +21,7 @@ set +e
 # it wastes flash
 #
 set R /
+set AUTOCNF no
 set FCONFIG /fs/microsd/etc/config.txt
 set FEXTRAS /fs/microsd/etc/extras.txt
 set FRC /fs/microsd/etc/rc.txt
@@ -176,12 +177,13 @@ else
 	fi

 	#
-	# If the airframe has been previously reset SYS_AUTCONFIG will have been set to 1 and other params will be reset on the next boot.
+	# Set AUTOCNF flag to use it in AUTOSTART scripts.
 	#
 	if param greater SYS_AUTOCONFIG 0
 	then
-		# Reset params except Airframe, RC calibration, sensor calibration, flight modes, total flight time, and next flight UUID.
-		param reset_all SYS_AUTOSTART RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT*
+		# Wipe out params except RC*, flight modes, total flight time, calibration parameters, next flight UUID
+		param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT*
+		set AUTOCNF yes
 	fi

 	#
@@ -272,6 +274,14 @@ else
 		. $FCONFIG
 	fi

+	#
+	# If autoconfig parameter was set, reset it and save parameters.
+	#
+	if [ $AUTOCNF = yes ]
+	then
+		param set SYS_AUTOCONFIG 0
+	fi
+
 	#
 	# Check if UAVCAN is enabled, default to it for ESCs.
 	#
@@ -555,6 +565,7 @@ fi
 # Unset all script parameters to free RAM.
 #
 unset R
+unset AUTOCNF
 unset FCONFIG
 unset FEXTRAS
 unset FRC
@@ -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_gps_check_fail_flags
+from analysis.post_processing import get_estimator_check_flags

 def analyse_ekf(
        ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0,
@@ -40,11 +40,6 @@ 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:
@@ -66,14 +61,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)}

-    gps_fail_flags = get_gps_check_fail_flags(estimator_status)
+    control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)

    sensor_checks, innov_fail_checks = find_checks_that_apply(
-        estimator_status_flags, estimator_status,
+        control_mode, estimator_status,
        pos_checks_when_sensors_not_fused=pos_checks_when_sensors_not_fused)

    metrics = calculate_ecl_ekf_metrics(
-        ulog, estimator_status_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects,
+        ulog, innov_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(
@@ -83,12 +78,12 @@ def analyse_ekf(


 def find_checks_that_apply(
-    estimator_status_flags: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\
+    control_mode: 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 estimator_status_flags:
+    :param control_mode:
    :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
@@ -102,7 +97,7 @@ def find_checks_that_apply(
    innov_fail_checks.append('posv')

    # Magnetometer Sensor Checks
-    if (np.amax(estimator_status_flags['cs_yaw_align']) > 0.5):
+    if (np.amax(control_mode['yaw_aligned']) > 0.5):
        sensor_checks.append('mag')

        innov_fail_checks.append('magx')
@@ -111,14 +106,13 @@ def find_checks_that_apply(
        innov_fail_checks.append('yaw')

    # Velocity Sensor Checks
-    if (np.amax(estimator_status_flags['cs_gps']) > 0.5):
+    if (np.amax(control_mode['using_gps']) > 0.5):
        sensor_checks.append('vel')
-        innov_fail_checks.append('velh')
-        innov_fail_checks.append('velv')
+        innov_fail_checks.append('vel')

    # Position Sensor Checks
-    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)):
+    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)):
        sensor_checks.append('pos')
        innov_fail_checks.append('posh')

@@ -134,7 +128,7 @@ def find_checks_that_apply(
        innov_fail_checks.append('hagl')

    # optical flow sensor checks
-    if (np.amax(estimator_status_flags['cs_opt_flow']) > 0.5):
+    if (np.amax(control_mode['using_optflow']) > 0.5):
        innov_fail_checks.append('ofx')
        innov_fail_checks.append('ofy')

@@ -123,8 +123,7 @@ def perform_sensor_innov_checks(
                                              ('magy', 'magy_fail_percentage', 'mag'),
                                              ('magz', 'magz_fail_percentage', 'mag'),
                                              ('yaw', 'yaw_fail_percentage', 'yaw'),
-                                              ('velh', 'vel_fail_percentage', 'vel'),
-                                              ('velv', 'vel_fail_percentage', 'vel'),
+                                              ('vel', 'vel_fail_percentage', 'vel'),
                                              ('posh', 'pos_fail_percentage', 'pos'),
                                              ('tas', 'tas_fail_percentage', 'tas'),
                                              ('hagl', 'hagl_fail_percentage', 'hagl'),
@@ -11,7 +11,7 @@ import numpy as np
 from analysis.detectors import InAirDetector

 def calculate_ecl_ekf_metrics(
-        ulog: ULog, estimator_status_flags: Dict[str, float], innov_fail_checks: List[str],
+        ulog: ULog, innov_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(
-        estimator_status_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
+        innov_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(
-        estimator_status_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
+        innov_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
        in_air_no_ground_effects: InAirDetector) -> dict:
    """
-    :param estimator_status_flags:
+    :param innov_flags:
    :param innov_fail_checks:
    :param in_air:
    :param in_air_no_ground_effects:
@@ -103,18 +103,17 @@ def calculate_innov_fail_metrics(
    innov_fail_metrics = dict()

    # calculate innovation check fail metrics
-    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')]:
+    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')]:

        # only run innov fail checks, if they apply.
        if signal_id in innov_fail_checks:
@@ -126,7 +125,7 @@ def calculate_innov_fail_metrics(
                in_air_detector = in_air

            innov_fail_metrics[result] = calculate_stat_from_signal(
-                estimator_status_flags, 'estimator_status_flags', signal, in_air_detector,
+                innov_flags, 'estimator_status', signal, in_air_detector,
                lambda x: 100.0 * np.mean(x > 0.5))

    return innov_fail_metrics
@@ -153,7 +152,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 [('gyro_coning_vibration', 'imu_coning'),
+                for signal, result in [('delta_angle_coning_metric', 'imu_coning'),
                                       ('gyro_vibration_metric', 'imu_hfgyro'),
                                       ('accel_vibration_metric', 'imu_hfaccel')]:

@@ -7,6 +7,115 @@ 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:
@@ -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_gps_check_fail_flags
+from analysis.post_processing import magnetic_field_estimates_from_states, get_estimator_check_flags
 from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
    CheckFlagsPlot
 from analysis.detectors import PreconditionError
@@ -33,11 +33,6 @@ 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:
@@ -73,13 +68,12 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
    except:
        raise PreconditionError('could not find innovation data')

-    gps_fail_flags = get_gps_check_fail_flags(estimator_status)
+    control_mode, innov_flags, gps_fail_flags = get_estimator_check_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(estimator_status_flags, status_flags_time)
+    on_ground_transition_time = detect_airtime(control_mode, status_time)

    with PdfPages(output_plot_filename) as pdf_pages:

@@ -179,9 +173,9 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str

        # plot control mode summary A
        data_plot = ControlModeSummaryPlot(
-            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']],
+            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']],
            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',
@@ -194,7 +188,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(estimator_status_flags['cs_in_air'])) > -0.5:
+        if np.amin(np.diff(control_mode['airborne'])) > -0.5:
            airborne_annotations.append(
                (on_ground_transition_time, 'air to ground transition not detected'))
        else:
@@ -203,7 +197,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(estimator_status_flags['cs_in_air'])) < 0.5:
+        if np.amax(np.diff(control_mode['airborne'])) < 0.5:
            airborne_annotations.append(
                (in_air_transition_time, 'ground to air transition not detected'))
        else:
@@ -211,7 +205,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_flags_time, estimator_status_flags, [['cs_in_air'], ['cs_wind']],
+            status_time, control_mode, [['airborne'], ['estimating_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)
@@ -220,15 +214,15 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str

        # plot innovation_check_flags summary
        data_plot = CheckFlagsPlot(
-            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)',
+            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)',
            y_labels=['failed', 'failed', 'failed', 'failed', 'failed', 'failed'],
            y_lim=(-0.1, 1.1),
-            legend=[['vel NE', 'pos NE'], ['vel D', 'hgt absolute', 'hgt above ground'],
+            legend=[['vel NED', 'pos NE'], ['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)
@@ -350,33 +344,33 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
        data_plot.close()


-def detect_airtime(estimator_status_flags, status_flags_time):
+def detect_airtime(control_mode, status_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(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)
+    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)
        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(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)
+    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)
        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(estimator_status_flags['cs_in_air'])) > 0.5) and (np.amin(np.diff(estimator_status_flags['cs_in_air'])) < -0.5):
+    if (np.amax(np.diff(control_mode['airborne'])) > 0.5) and (np.amin(np.diff(control_mode['airborne'])) < -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:
@@ -1,7 +0,0 @@
-if SWD
-speed 1000
-r
-loadbin /Users/landon/git/px4/build/nxp_ucans32k146_nxp_demo/deploy/34.bin,0x6000
-r
-g
-q
@@ -1,3 +0,0 @@
-#!/bin/zsh
-
-JLinkExe -device S32K146 -CommandFile /Users/landon/git/px4/Tools/flash_nxp/flash_ucan.jlink
@@ -136,8 +136,11 @@ class firmware(object):

        self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))

-        # pad image to 4-byte length
-        while ((len(self.image) % 4) != 0):
+        # PX4 bootloader in theory requires only 4-byte padding,
+        # but a bug exists with the flash block caching where if the data
+        # does not fill up the block, it will not be written (left as all 1s).
+        # So pad up to the flash block size (8 words).
+        while ((len(self.image) % (4 * 8)) != 0):
            self.image.extend(b'\xff')

    def property(self, propname):
@@ -9,11 +9,3 @@ 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
@@ -1,12 +0,0 @@
-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
@@ -25,4 +25,3 @@ CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
@@ -3,15 +3,7 @@
 # board specific defaults
 #------------------------------------------------------------------------------

-# pre-arm stuff
-#param set-default SYS_CTRL_ALLOC 1
-
-#pwm_out start
-
-#control_allocator start
+pwm_out mode_pwm1 start

 ifup can0
 cyphal start
-lighting_state_converter start
-actuator_controls_driver start
-pca9685 start -X
@@ -1,9 +0,0 @@
-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
@@ -106,7 +106,7 @@ __BEGIN_DECLS
 *      Defined in board.h
 */

-#define DIRECT_PWM_OUTPUT_CHANNELS  2
+#define DIRECT_PWM_OUTPUT_CHANNELS  1


 #define BOARD_HAS_LED_PWM              1
@@ -110,7 +110,6 @@ 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 =
@@ -397,15 +397,27 @@
 #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
@@ -459,15 +459,7 @@ 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,                  \
-		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), \
+		GPIO_nARMED_INIT                  \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -394,15 +394,27 @@
 #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
@@ -423,15 +423,7 @@
 		GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \
 		GPIO_SAFETY_SWITCH_IN,            \
 		GPIO_PG6,                         \
-		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), \
+		GPIO_nARMED_INIT                  \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -77,9 +77,6 @@ set(msg_files
 	estimator_states.msg
 	estimator_status.msg
 	estimator_status_flags.msg
-	estimator_aid_source_1d.msg
-	estimator_aid_source_2d.msg
-	estimator_aid_source_3d.msg
 	event.msg
 	follow_target.msg
 	failure_detector_status.msg
@@ -104,7 +101,6 @@ 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
@@ -1,5 +1,5 @@
 uint64 timestamp			# time since system start (microseconds)
-uint8 NUM_ACTUATOR_CONTROLS = 16
+uint8 NUM_ACTUATOR_CONTROLS = 8
 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[16] control
+float32[9] 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
@@ -1,22 +0,0 @@
-uint64 timestamp             # time since system start (microseconds)
-uint64 timestamp_sample      # the timestamp of the raw data (microseconds)
-
-uint8 estimator_instance
-
-uint32 device_id
-
-uint64 time_last_fuse
-
-float32 observation
-float32 observation_variance
-
-float32 innovation
-float32 innovation_variance
-float32 test_ratio
-
-bool fusion_enabled          # true when measurements are being fused
-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
@@ -1,22 +0,0 @@
-uint64 timestamp             # time since system start (microseconds)
-uint64 timestamp_sample      # the timestamp of the raw data (microseconds)
-
-uint8 estimator_instance
-
-uint32 device_id
-
-uint64[2] time_last_fuse
-
-float32[2] observation
-float32[2] observation_variance
-
-float32[2] innovation
-float32[2] innovation_variance
-float32[2] test_ratio
-
-bool[2] fusion_enabled       # true when measurements are being fused
-bool[2] innovation_rejected  # true if the observation has been rejected
-bool[2] fused                # true if the sample was successfully fused
-
-# TOPICS estimator_aid_source_2d
-# TOPICS estimator_aid_src_fake_pos
@@ -1,22 +0,0 @@
-uint64 timestamp             # time since system start (microseconds)
-uint64 timestamp_sample      # the timestamp of the raw data (microseconds)
-
-uint8 estimator_instance
-
-uint32 device_id
-
-uint64[3] time_last_fuse
-
-float32[3] observation
-float32[3] observation_variance
-
-float32[3] innovation
-float32[3] innovation_variance
-float32[3] test_ratio
-
-bool[3] fusion_enabled       # true when measurements are being fused
-bool[3] innovation_rejected  # true if the observation has been rejected
-bool[3] fused                # true if the sample was successfully fused
-
-# TOPICS estimator_aid_source_3d
-# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
@@ -16,10 +16,6 @@ bool starting_vision_pos_fusion         #  9 - true when the filter starts using
 bool starting_vision_vel_fusion         # 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
 bool starting_vision_yaw_fusion         # 11 - true when the filter starts using vision system yaw  measurements to correct the state estimates
 bool yaw_aligned_to_imu_gps             # 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
-bool reset_hgt_to_baro                  # 13 - true when the vertical position state is reset to the baro measurement
-bool reset_hgt_to_gps                   # 14 - true when the vertical position state is reset to the gps measurement
-bool reset_hgt_to_rng                   # 15 - true when the vertical position state is reset to the rng measurement
-bool reset_hgt_to_ev                    # 16 - true when the vertical position state is reset to the ev measurement

 # warning events
 uint32 warning_event_changes            # number of warning event changes
@@ -13,7 +13,6 @@ 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
@@ -28,11 +27,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)


-uint16 led_mask # bitmask which LED(s) to control, set to 0xff for all
+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 = 16      # needs to match BOARD_MAX_LEDS
+uint8 ORB_QUEUE_LENGTH = 8      # needs to match BOARD_MAX_LEDS
@@ -1,37 +0,0 @@
-# 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
@@ -1,17 +0,0 @@
-# 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
@@ -45,8 +45,8 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#define MAX_IO_TIMERS			2
-#define MAX_TIMER_IO_CHANNELS		2
+#define MAX_IO_TIMERS			1
+#define MAX_TIMER_IO_CHANNELS		1

 #define MAX_LED_TIMERS			1
 #define MAX_TIMER_LED_CHANNELS		3
@@ -116,7 +116,7 @@ public:
 private:


-	const UavcanParamBinder _uavcan_params[15] {
+	const UavcanParamBinder _uavcan_params[13] {
 		{"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,8 +130,6 @@ 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"},
 	};
@@ -64,18 +64,13 @@
 #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_LIGHTING_STATES_PUBLISHER
+	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER

 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
@@ -164,16 +159,6 @@ 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
 	};
 };
@@ -61,18 +61,13 @@
 #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_LIGHTING_STATES_SUBSCRIBER
+	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER

 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
@@ -184,16 +179,6 @@ 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
 	};
 };
@@ -188,21 +188,3 @@ 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);
@@ -1,71 +0,0 @@
-/****************************************************************************
- *
- *   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];};
-};
-};
@@ -1,45 +0,0 @@
-############################################################################
-#
-#   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
-	)
@@ -1,222 +0,0 @@
-/****************************************************************************
- *
- *   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);
-}
@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   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};
-};
@@ -1,5 +0,0 @@
-menuconfig DRIVERS_EXTREME_SWITCH
-	bool "extreme_switch"
-	default n
-	---help---
-		Enable support for extreme_switch
@@ -274,22 +274,20 @@ void ICM42688P::RunImpl()
 				}
 			}

-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
-				    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
-				    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
-				   ) {
-					_last_config_check_timestamp = now;
-					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-					_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
-					_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
+			// check configuration registers periodically or immediately following any failure
+			if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+			    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
+			    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
+			   ) {
+				_last_config_check_timestamp = now;
+				_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+				_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+				_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;

-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
+			} else {
+				// register check failed, force reset
+				perf_count(_bad_register_perf);
+				Reset();
 			}
 		}

@@ -1,41 +0,0 @@
-############################################################################
-#
-#   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
-	)
@@ -1,5 +0,0 @@
-menuconfig DRIVERS_LIGHTS_APA102
-	bool "apa102"
-	default n
-	---help---
-		Enable support for apa102
@@ -1,275 +0,0 @@
-/****************************************************************************
- *
- *   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);
-}
@@ -1,54 +0,0 @@
-#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;
-};
@@ -93,21 +93,18 @@

 #define ADDR 0x40	// I2C adress

-#define PCA9685_PWMFREQ 400
+#define PCA9685_PWMFREQ 60.0f
 #define PCA9685_NCHANS 16 // total amount of pwm outputs

-//#define PCA9685_PWMINT (1.0f / PCA9685_PWMFREQ) * 1000000
+#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_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_PWMCENTER ((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_PWMMIN) / 2 // scales from rad to PWM
-
+#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM

 enum IOX_MODE {
 	IOX_MODE_ON,
@@ -235,7 +232,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, 1.05f);
+			orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);

 			_mode_on_initialized = true;
 		}
@@ -246,40 +243,18 @@ 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 = 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]);
+				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]);

 				if (new_value != _current_values[i] &&
-				    new_value >= pwm_min &&
-				    new_value <= pwm_max) {
+				    new_value >= PCA9685_PWMMIN &&
+				    new_value <= PCA9685_PWMMAX) {
 					/* This value was updated, send the command to adjust the PWM value */
 					setPin(i, new_value);
 					_current_values[i] = new_value;
@@ -355,8 +330,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;
@@ -1,196 +0,0 @@
-/****************************************************************************
- *
- *   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);
-}
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   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};
-};
@@ -1,45 +0,0 @@
-############################################################################
-#
-#   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
-	)
@@ -1,5 +0,0 @@
-menuconfig EXAMPLES_ACTUATOR_CONTROLS_DRIVER
-	bool "actuator_controls_driver"
-	default n
-	---help---
-		Enable support for actuator_controls_driver
@@ -1,39 +0,0 @@
-############################################################################
-#
-#   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
-	)
@@ -1,5 +0,0 @@
-menuconfig EXAMPLES_ELM_LIGHTING_MODULE
-	bool "elm_lighting_module"
-	default n
-	---help---
-		Enable support for elm_lighting_module
@@ -1,180 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}
@@ -198,7 +198,7 @@ void Battery::estimateStateOfCharge(const float voltage_v, const float current_a
 	float cell_voltage = voltage_v / _params.n_cells;

 	// correct battery voltage locally for load drop to avoid estimation fluctuations
-	if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) {
+	if (_params.r_internal >= 0.f) {
 		cell_voltage += _params.r_internal * current_a;

 	} else {
@@ -43,7 +43,7 @@
 * @group Battery Calibration
 * @category system
 */
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.6f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);

 /**
 * This parameter is deprecated. Please use BAT1_V_CHARGED instead.
@@ -21,7 +21,7 @@ parameters:
            reboot_required: true
            num_instances: *max_num_config_instances
            instance_start: 1
-            default: [3.6, 3.6]
+            default: [3.5, 3.5]

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

        BAT${i}_R_INTERNAL:
            description:
@@ -71,12 +71,12 @@ parameters:
            unit: Ohm
            min: -1.0
            max: 0.2
-            decimal: 4
-            increment: 0.0005
+            decimal: 2
+            increment: 0.01
            reboot_required: true
            num_instances: *max_num_config_instances
            instance_start: 1
-            default: [0.005, 0.005]
+            default: [-1.0, -1.0]

        BAT${i}_N_CELLS:
            description:
@@ -391,16 +391,16 @@ private:

 	geofence_result_s	_geofence_result{};
 	vehicle_land_detected_s	_vehicle_land_detected{};
-	vtol_vehicle_status_s	_vtol_vehicle_status{};
+	vtol_vehicle_status_s	_vtol_status{};

 	hrt_abstime _last_wind_warning{0};

 	// commander publications
-	actuator_armed_s        _actuator_armed{};
-	commander_state_s       _commander_state{};
+	actuator_armed_s        _armed{};
+	commander_state_s       _internal_state{};
 	vehicle_control_mode_s  _vehicle_control_mode{};
-	vehicle_status_s        _vehicle_status{};
-	vehicle_status_flags_s  _vehicle_status_flags{};
+	vehicle_status_s        _status{};
+	vehicle_status_flags_s  _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>			_actuator_armed_pub{ORB_ID(actuator_armed)};
+	uORB::Publication<actuator_armed_s>			_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>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
+	uORB::Publication<vehicle_control_mode_s>		_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>			_vehicle_status_pub{ORB_ID(vehicle_status)};
+	uORB::Publication<vehicle_status_s>			_status_pub{ORB_ID(vehicle_status)};

-	uORB::PublicationData<home_position_s>			_home_position_pub{ORB_ID(home_position)};
+	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};

-	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_command_ack_s>		_command_ack_pub{ORB_ID(vehicle_command_ack)};

 	orb_advert_t _mavlink_log_pub{nullptr};

@@ -221,8 +221,8 @@ void FailureDetector::updateAttitudeStatus()
 		const float max_roll(fabsf(math::radians(max_roll_deg)));
 		const float max_pitch(fabsf(math::radians(max_pitch_deg)));

-		const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
-		const bool pitch_status = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch);
+		const bool roll_status = (max_roll > FLT_EPSILON) && (fabsf(roll) > max_roll);
+		const bool pitch_status = (max_pitch > FLT_EPSILON) && (fabsf(pitch) > max_pitch);

 		hrt_abstime time_now = hrt_absolute_time();

@@ -384,7 +384,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,

 			// Check for telemetry timeout
 			const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp;
-			const bool esc_timed_out = telemetry_age > 100_ms;  // TODO: magic number
+			const bool esc_timed_out = telemetry_age > 300_ms;

 			const bool esc_was_valid = _motor_failure_esc_valid_current_mask & (1 << i_esc);
 			const bool esc_timeout_currently_flagged = _motor_failure_esc_timed_out_mask & (1 << i_esc);
@@ -399,35 +399,40 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 			}

 			// Check if ESC current is too low
-			float esc_throttle = 0.f;
-
-			if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
-				esc_throttle = fabsf(actuator_motors.control[i_esc]);
+			if (cur_esc_report.esc_current > FLT_EPSILON) {
+				_motor_failure_escs_have_current = true;
 			}

-			const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
-			const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
-						     _param_fd_motor_current2throttle_thres.get();
+			if (_motor_failure_escs_have_current) {
+				float esc_throttle = 0.f;

-			if (throttle_above_threshold && current_too_low && !esc_timed_out) {
-				if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
-					_motor_failure_undercurrent_start_time[i_esc] = time_now;
+				if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
+					esc_throttle = fabsf(actuator_motors.control[i_esc]);
 				}

-			} else {
-				if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
-					_motor_failure_undercurrent_start_time[i_esc] = 0;
+				const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
+				const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
+							     _param_fd_motor_current2throttle_thres.get();
+
+				if (throttle_above_threshold && current_too_low && !esc_timed_out) {
+					if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
+						_motor_failure_undercurrent_start_time[i_esc] = time_now;
+					}
+
+				} else {
+					if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
+						_motor_failure_undercurrent_start_time[i_esc] = 0;
+					}
 				}
+
+				if (_motor_failure_undercurrent_start_time[i_esc] != 0
+				    && (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
+				    && (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
+					// Set flag
+					_motor_failure_esc_under_current_mask |= (1 << i_esc);
+
+				} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
 			}
-
-			if (_motor_failure_undercurrent_start_time[i_esc] != 0
-			    && (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
-			    && (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
-				// Set flag
-				_motor_failure_esc_under_current_mask |= (1 << i_esc);
-
-			} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
-
 		}

 		bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
@@ -129,6 +129,7 @@ private:
 	uint8_t _motor_failure_esc_valid_current_mask{};  // ESC 1-8, true if ESC telemetry was valid at some point
 	uint8_t _motor_failure_esc_timed_out_mask{};      // ESC telemetry no longer available -> failure
 	uint8_t _motor_failure_esc_under_current_mask{};  // ESC drawing too little current -> failure
+	bool _motor_failure_escs_have_current{false}; // true if some ESC had non-zero current (some don't support it)
 	hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};

 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -53,7 +53,7 @@
 *
 * Setting this parameter to 0 disables the check
 *
- * @min 60
+ * @min 0
 * @max 180
 * @unit deg
 * @group Failure Detector
@@ -71,7 +71,7 @@ PARAM_DEFINE_INT32(FD_FAIL_R, 60);
 *
 * Setting this parameter to 0 disables the check
 *
- * @min 60
+ * @min 0
 * @max 180
 * @unit deg
 * @group Failure Detector
@@ -556,6 +556,18 @@ mixer:
                            label: 'Direction CCW'
                            function: 'spin-dir'
                            show_as: 'true-if-positive'
+                          - name: 'CA_ROTOR${i}_AX'
+                            label: 'Axis X'
+                            function: 'axisx'
+                            advanced: true
+                          - name: 'CA_ROTOR${i}_AY'
+                            label: 'Axis Y'
+                            function: 'axisy'
+                            advanced: true
+                          - name: 'CA_ROTOR${i}_AZ'
+                            label: 'Axis Z'
+                            function: 'axisz'
+                            advanced: true

            1: # Fixed Wing
                title: 'Fixed Wing'
@@ -550,10 +550,6 @@ union information_event_status_u {
 		bool starting_vision_vel_fusion : 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
 		bool starting_vision_yaw_fusion : 1; ///< 11 - true when the filter starts using vision system yaw  measurements to correct the state estimates
 		bool yaw_aligned_to_imu_gps     : 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
-		bool reset_hgt_to_baro          : 1; ///< 13 - true when the vertical position state is reset to the baro measurement
-		bool reset_hgt_to_gps           : 1; ///< 14 - true when the vertical position state is reset to the gps measurement
-		bool reset_hgt_to_rng           : 1; ///< 15 - true when the vertical position state is reset to the rng measurement
-		bool reset_hgt_to_ev            : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
 	} flags;
 	uint32_t value;
 };
@@ -125,8 +125,8 @@ void Ekf::controlFusionModes()

 	if (_range_buffer) {
 		// Get range data from buffer and check validity
-		_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
-		_range_sensor.setDataReadiness(_rng_data_ready);
+		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);

 		// update range sensor angle parameters in case they have changed
 		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
@@ -778,34 +778,6 @@ void Ekf::checkVerticalAccelerationHealth()
 	bool is_inertial_nav_falling = false;
 	bool are_vertical_pos_and_vel_independant = false;

-	if (_control_status.flags.gps) {
-		// 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]);
-		}
-
-		// 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];
-			_vert_pos_innov_ratio = gps_pos.innovation[2] / sqrtf(gps_pos.innovation_variance[2]);
-		}
-	}
-
-	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
@@ -951,23 +923,25 @@ void Ekf::controlHeightFusion()
 	updateBaroHgtOffset();
 	updateGroundEffect();

-	if (_baro_data_ready) {
-		updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
+	if (_control_status.flags.baro_hgt) {

-		if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
-			fuseBaroHgt(_aid_src_baro_hgt);
+		if (_baro_data_ready && !_baro_hgt_faulty) {
+			fuseBaroHgt();
 		}
-	}

-	if (_rng_data_ready) {
-		updateRngHgt(_aid_src_rng_hgt);
+	} else if (_control_status.flags.gps_hgt) {

-		if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-			fuseRngHgt(_aid_src_rng_hgt);
+		if (_gps_data_ready) {
+			fuseGpsHgt();
 		}
-	}

-	if (_control_status.flags.ev_hgt) {
+	} else if (_control_status.flags.rng_hgt) {
+
+		if (_range_sensor.isDataHealthy()) {
+			fuseRngHgt();
+		}
+
+	} else if (_control_status.flags.ev_hgt) {

 		if (_control_status.flags.ev_hgt && _ev_data_ready) {
 			fuseEvHgt();
@@ -984,24 +984,13 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		const float down_dvel_bias = _state.delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));

 		// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
-		bool bad_acc_bias = false;
-		if (fabsf(down_dvel_bias) > dVel_bias_lim) {
-
-			bool bad_vz_gps = _control_status.flags.gps    && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
-			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 * _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 * _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);
-
-
-				if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {
-					bad_acc_bias = true;
-				}
-			}
-		}
+		bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
+				     && ((down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
+					 || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel))
+				     && ((down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
+					 || (down_dvel_bias * _baro_hgt_innov < 0.0f && _control_status.flags.baro_hgt)
+					 || (down_dvel_bias * _rng_hgt_innov < 0.0f && _control_status.flags.rng_hgt)
+					 || (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt)));

 		// record the pass/fail
 		if (!bad_acc_bias) {
@@ -48,10 +48,6 @@
 #include "EKFGSF_yaw.h"
 #include "baro_bias_estimator.hpp"

-#include <uORB/topics/estimator_aid_source_1d.h>
-#include <uORB/topics/estimator_aid_source_2d.h>
-#include <uORB/topics/estimator_aid_source_3d.h>
-
 class Ekf final : public EstimatorInterface
 {
 public:
@@ -86,13 +82,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 = _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 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 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 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 getAuxVelInnov(float aux_vel_innov[2]) const;
 	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
@@ -340,14 +336,6 @@ public:

 	const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }

-	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
@@ -389,7 +377,6 @@ 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
@@ -407,6 +394,7 @@ private:
 	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_arsp_fuse{0};	///< time the last fusion of airspeed measurements were performed (uSec)
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
+	uint64_t _time_last_fake_pos_fuse{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
 	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
 	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
@@ -450,12 +438,24 @@ private:
 	float _vert_vel_innov_ratio{0.f};		///< standard deviation of vertical velocity innovation
 	uint64_t _vert_vel_fuse_time_us{0};	///< last system time in usec time vertical velocity measurement fuson was attempted

+	Vector3f _gps_vel_innov{};	///< GPS velocity innovations (m/sec)
+	Vector3f _gps_vel_innov_var{};	///< GPS velocity innovation variances ((m/sec)**2)
+
+	Vector3f _gps_pos_innov{};	///< GPS position innovations (m)
+	Vector3f _gps_pos_innov_var{};	///< GPS position innovation variances (m**2)
+
 	Vector3f _ev_vel_innov{};	///< external vision velocity innovations (m/sec)
 	Vector3f _ev_vel_innov_var{};	///< external vision velocity innovation variances ((m/sec)**2)

 	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)

@@ -491,14 +491,6 @@ 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_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)
@@ -529,12 +521,8 @@ private:

 	// Variables used to perform in flight resets and switch between height sources
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
-
+	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
 	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
-	float _gps_hgt_offset{0.0f};		///< GPS height reading at the local NED origin (m)
-	float _rng_hgt_offset{0.0f};		///< Range height reading at the local NED origin (m)
-	float _ev_hgt_offset{0.0f};		///< EV height reading at the local NED origin (m)
-
 	float _baro_hgt_bias{0.0f};
 	float _baro_hgt_bias_var{1.f};

@@ -645,13 +633,11 @@ private:
 	// fuse body frame drag specific forces for multi-rotor wind estimation
 	void fuseDrag(const dragSample &drag_sample);

-	void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
-	void fuseRngHgt(estimator_aid_source_1d_s &range_hgt);
+	void fuseBaroHgt();
+	void fuseGpsHgt();
+	void fuseRngHgt();
 	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);

@@ -690,16 +676,12 @@ private:
 				  Vector3f &innov_var, Vector2f &test_ratio);

 	bool fuseHorizontalPosition(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
-				    Vector3f &innov_var, Vector2f &test_ratiov);
+				    Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);

 	bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
 				  float &innov_var, float &test_ratio);

-	void updateGpsVel(const gpsSample &gps_sample);
-	void fuseGpsVel();
-
-	void updateGpsPos(const gpsSample &gps_sample);
-	void fuseGpsPos();
+	void fuseGpsVelPos();

 	// calculate optical flow body angular rate compensation
 	// returns false if bias corrected body rate data is unavailable
@@ -1066,80 +1048,6 @@ private:
 	bool isYawEmergencyEstimateAvailable() const;

 	void resetGpsDriftCheckFilters();
-
-	bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status) const
-	{
-		if (status.timestamp_sample != 0) {
-			status.timestamp_sample = 0;
-			status.fusion_enabled = false;
-			status.innovation_rejected = false;
-			status.fused = false;
-
-			return true;
-		}
-
-		return false;
-	}
-
-	void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const
-	{
-		if (resetEstimatorAidStatusFlags(status)) {
-			status.observation = 0;
-			status.observation_variance = 0;
-
-			status.innovation = 0;
-			status.innovation_variance = 0;
-			status.test_ratio = 0;
-		}
-	}
-
-	template <typename T>
-	bool resetEstimatorAidStatusFlags(T &status) const
-	{
-		if (status.timestamp_sample != 0) {
-			status.timestamp_sample = 0;
-
-			for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
-				status.fusion_enabled[i] = false;
-				status.innovation_rejected[i] = false;
-				status.fused[i] = false;
-			}
-
-			return true;
-		}
-
-		return false;
-	}
-
-	template <typename T>
-	void resetEstimatorAidStatus(T &status) const
-	{
-		if (resetEstimatorAidStatusFlags(status)) {
-			for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
-				status.observation[i] = 0;
-				status.observation_variance[i] = 0;
-
-				status.innovation[i] = 0;
-				status.innovation_variance[i] = 0;
-				status.test_ratio[i] = 0;
-			}
-		}
-	}
-
-	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
@@ -241,10 +241,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos)

 void Ekf::resetHeightToBaro()
 {
-	ECL_INFO("reset height to baro");
-	_information_events.flags.reset_hgt_to_baro = true;
-
-	resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_hgt_offset));
+	resetVerticalPositionTo(-_baro_sample_delayed.hgt + _baro_hgt_offset);

 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
@@ -252,9 +249,6 @@ void Ekf::resetHeightToBaro()

 void Ekf::resetHeightToGps()
 {
-	ECL_INFO("reset height to GPS");
-	_information_events.flags.reset_hgt_to_gps = true;
-
 	const float z_pos_before_reset = _state.pos(2);
 	resetVerticalPositionTo(-_gps_sample_delayed.hgt + _gps_alt_ref);

@@ -267,9 +261,6 @@ void Ekf::resetHeightToGps()

 void Ekf::resetHeightToRng()
 {
-	ECL_INFO("reset height to RNG");
-	_information_events.flags.reset_hgt_to_rng = true;
-
 	float dist_bottom;

 	if (_control_status.flags.in_air) {
@@ -282,7 +273,7 @@ void Ekf::resetHeightToRng()

 	// update the state and associated variance
 	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(-dist_bottom + _rng_hgt_offset);
+	resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset);

 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
@@ -293,9 +284,6 @@ void Ekf::resetHeightToRng()

 void Ekf::resetHeightToEv()
 {
-	ECL_INFO("reset height to EV");
-	_information_events.flags.reset_hgt_to_ev = true;
-
 	const float z_pos_before_reset = _state.pos(2);
 	resetVerticalPositionTo(_ev_sample_delayed.pos(2));

@@ -364,8 +352,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 	}

 	// check for excessive horizontal GPS velocity innovations
-	const float gps_vel_test_ratio = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
-	const bool badVelInnov = (gps_vel_test_ratio > 1.0f) && _control_status.flags.gps;
+	const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;

 	// calculate GPS course over ground angle
 	const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
@@ -606,33 +593,30 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const

 void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2],  float &vpos) const
 {
-	hvel[0] = _aid_src_gnss_vel.innovation[0];
-	hvel[1] = _aid_src_gnss_vel.innovation[1];
-	vvel    = _aid_src_gnss_vel.innovation[2];
-
-	hpos[0] = _aid_src_gnss_pos.innovation[0];
-	hpos[1] = _aid_src_gnss_pos.innovation[1];
-	vpos    = _aid_src_gnss_pos.innovation[2];
+	hvel[0] = _gps_vel_innov(0);
+	hvel[1] = _gps_vel_innov(1);
+	vvel    = _gps_vel_innov(2);
+	hpos[0] = _gps_pos_innov(0);
+	hpos[1] = _gps_pos_innov(1);
+	vpos    = _gps_pos_innov(2);
 }

 void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)  const
 {
-	hvel[0] = _aid_src_gnss_vel.innovation_variance[0];
-	hvel[1] = _aid_src_gnss_vel.innovation_variance[1];
-	vvel    = _aid_src_gnss_vel.innovation_variance[2];
-
-	hpos[0] = _aid_src_gnss_pos.innovation_variance[0];
-	hpos[1] = _aid_src_gnss_pos.innovation_variance[1];
-	vpos    = _aid_src_gnss_pos.innovation_variance[2];
+	hvel[0] = _gps_vel_innov_var(0);
+	hvel[1] = _gps_vel_innov_var(1);
+	vvel    = _gps_vel_innov_var(2);
+	hpos[0] = _gps_pos_innov_var(0);
+	hpos[1] = _gps_pos_innov_var(1);
+	vpos    = _gps_pos_innov_var(2);
 }

 void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
 {
-	hvel = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
-	vvel = _aid_src_gnss_vel.test_ratio[2];
-
-	hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
-	vpos = _aid_src_gnss_pos.test_ratio[2];
+	hvel = _gps_vel_test_ratio(0);
+	vvel = _gps_vel_test_ratio(1);
+	hpos = _gps_pos_test_ratio(0);
+	vpos = _gps_pos_test_ratio(1);
 }

 void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
@@ -744,7 +728,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_control_status.flags.inertial_dead_reckoning) {
 		if (_control_status.flags.gps) {
-			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
+			hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
 		}

 		if (_control_status.flags.ev_pos) {
@@ -766,7 +750,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_deadreckon_time_exceeded && _control_status.flags.gps) {
-		hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
+		hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
 	}

 	*ekf_eph = hpos_err;
@@ -790,7 +774,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 		}

 		if (_control_status.flags.gps) {
-			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
+			vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));

 		} else if (_control_status.flags.ev_pos) {
 			vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
@@ -917,10 +901,10 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	pos = NAN;

 	if (_control_status.flags.gps) {
-		float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max());
+		float gps_vel = sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1)));
 		vel = math::max(gps_vel, FLT_MIN);

-		float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max());
+		float gps_pos = sqrtf(_gps_pos_test_ratio(0));
 		pos = math::max(gps_pos, FLT_MIN);
 	}

@@ -941,13 +925,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(_aid_src_baro_hgt.test_ratio), FLT_MIN);
+		hgt = math::max(sqrtf(_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);
+		hgt = math::max(sqrtf(_gps_pos_test_ratio(1)), FLT_MIN);

 	} else if (_control_status.flags.rng_hgt) {
-		hgt = math::max(sqrtf(_aid_src_rng_hgt.test_ratio), FLT_MIN);
+		hgt = math::max(sqrtf(_rng_hgt_test_ratio), FLT_MIN);

 	} else if (_control_status.flags.ev_hgt) {
 		hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);
@@ -981,8 +965,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
-	const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
-	const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
+	const bool gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f);
+	const bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f);
 	const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f);
 	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
 	soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
@@ -1016,7 +1000,6 @@ void Ekf::update_deadreckoning_status()
 				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));

 	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
-
 	const bool airDataAiding = _control_status.flags.wind &&
 				   isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&
 				   isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max);
@@ -1271,8 +1254,7 @@ void Ekf::startBaroHgtFusion()

 		// We don't need to set a height sensor offset
 		// since we track a separate _baro_hgt_offset
-
-		ECL_INFO("starting baro height fusion");
+		_hgt_sensor_offset = 0.0f;
 	}
 }

@@ -1280,19 +1262,17 @@ void Ekf::startGpsHgtFusion()
 {
 	if (!_control_status.flags.gps_hgt) {
 		if (_control_status.flags.rng_hgt) {
-			// switch out of range aid
+			// swith out of range aid
 			// calculate height sensor offset such that current
 			// measurement matches our current height estimate
-			_gps_hgt_offset = (_gps_sample_delayed.hgt - _gps_alt_ref) + _state.pos(2);
+			_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);

 		} else {
-			_gps_hgt_offset = 0.f;
+			_hgt_sensor_offset = 0.f;
 			resetHeightToGps();
 		}

 		setControlGPSHeight();
-
-		ECL_INFO("starting GPS height fusion");
 	}
 }

@@ -1303,14 +1283,12 @@ void Ekf::startRngHgtFusion()

 		// Range finder is the primary height source, the ground is now the datum used
 		// to compute the local vertical position
-		_rng_hgt_offset = 0.f;
+		_hgt_sensor_offset = 0.f;

 		if (!_control_status_prev.flags.ev_hgt) {
 			// EV and range finders are using the same height datum
 			resetHeightToRng();
 		}
-
-		ECL_INFO("starting RNG height fusion");
 	}
 }

@@ -1321,9 +1299,7 @@ void Ekf::startRngAidHgtFusion()

 		// calculate height sensor offset such that current
 		// measurement matches our current height estimate
-		_rng_hgt_offset = _terrain_vpos;
-
-		ECL_INFO("starting RNG aid height fusion");
+		_hgt_sensor_offset = _terrain_vpos;
 	}
 }

@@ -1336,8 +1312,6 @@ void Ekf::startEvHgtFusion()
 			// EV and range finders are using the same height datum
 			resetHeightToEv();
 		}
-
-		ECL_INFO("starting EV height fusion");
 	}
 }

@@ -1583,8 +1557,6 @@ void Ekf::stopGpsFusion()
 	if (_control_status.flags.gps) {
 		stopGpsPosFusion();
 		stopGpsVelFusion();
-
-		_control_status.flags.gps = false;
 	}

 	if (_control_status.flags.gps_yaw) {
@@ -1598,27 +1570,27 @@ void Ekf::stopGpsFusion()

 void Ekf::stopGpsPosFusion()
 {
-	ECL_INFO("stopping GPS position fusion");
+	_control_status.flags.gps = false;

 	if (_control_status.flags.gps_hgt) {
-		ECL_INFO("stopping GPS height fusion");
 		startBaroHgtFusion();
 	}

-	resetEstimatorAidStatus(_aid_src_gnss_pos);
+	_gps_pos_innov.setZero();
+	_gps_pos_innov_var.setZero();
+	_gps_pos_test_ratio.setZero();
 }

 void Ekf::stopGpsVelFusion()
 {
-	ECL_INFO("stopping GPS velocity fusion");
-
-	resetEstimatorAidStatus(_aid_src_gnss_vel);
+	_gps_vel_innov.setZero();
+	_gps_vel_innov_var.setZero();
+	_gps_vel_test_ratio.setZero();
 }

 void Ekf::startGpsYawFusion()
 {
-	if (!_control_status.flags.gps_yaw && resetYawToGps()) {
-		ECL_INFO("starting GPS yaw fusion");
+	if (resetYawToGps()) {
 		_control_status.flags.yaw_align = true;
 		_control_status.flags.mag_dec = false;
 		stopEvYawFusion();
@@ -1626,14 +1598,12 @@ void Ekf::startGpsYawFusion()
 		stopMag3DFusion();
 		_control_status.flags.gps_yaw = true;
 	}
+
 }

 void Ekf::stopGpsYawFusion()
 {
-	if (_control_status.flags.gps_yaw) {
-		ECL_INFO("stopping GPS yaw fusion");
-		_control_status.flags.gps_yaw = false;
-	}
+	_control_status.flags.gps_yaw = false;
 }

 void Ekf::startEvPosFusion()
@@ -329,11 +329,15 @@ protected:

 	// innovation consistency check monitoring ratios
 	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
-	AlphaFilter<float> _yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
+	AlphaFilter<float>_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 	Vector3f _mag_test_ratio{};		// magnetometer XYZ innovation consistency check ratios
+	Vector2f _gps_vel_test_ratio{};		// GPS velocity innovation consistency check ratios
+	Vector2f _gps_pos_test_ratio{};		// GPS position innovation consistency check ratios
 	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
@@ -40,14 +40,9 @@

 void Ekf::controlFakePosFusion()
 {
-	auto &fake_pos = _aid_src_fake_pos;
-
-	// clear
-	resetEstimatorAidStatusFlags(fake_pos);
-
 	// If we aren't doing any aiding, fake position measurements at the last known position to constrain drift
 	// During intial tilt aligment, fake position is used to perform a "quasi-stationary" leveling of the EKF
-	const bool fake_pos_data_ready = isTimedOut(fake_pos.time_last_fuse[0], (uint64_t)2e5); // Fuse fake position at a limited rate
+	const bool fake_pos_data_ready = isTimedOut(_time_last_fake_pos_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate

 	if (fake_pos_data_ready) {
 		const bool continuing_conditions_passing = !isHorizontalAidingActive();
@@ -57,7 +52,7 @@ void Ekf::controlFakePosFusion()
 			if (continuing_conditions_passing) {
 				fuseFakePosition();

-				const bool is_fusion_failing = isTimedOut(fake_pos.time_last_fuse[0], (uint64_t)4e5);
+				const bool is_fusion_failing = isTimedOut(_time_last_fake_pos_fuse, (uint64_t)4e5);

 				if (is_fusion_failing) {
 					resetFakePosFusion();
@@ -84,7 +79,6 @@ void Ekf::controlFakePosFusion()
 void Ekf::startFakePosFusion()
 {
 	if (!_using_synthetic_position) {
-		ECL_INFO("start fake position fusion");
 		_using_synthetic_position = true;
 		_fuse_hpos_as_odom = false; // TODO: needed?
 		resetFakePosFusion();
@@ -93,68 +87,39 @@ void Ekf::startFakePosFusion()

 void Ekf::resetFakePosFusion()
 {
-	ECL_INFO("reset fake position fusion");
 	_last_known_posNE = _state.pos.xy();
-
 	resetHorizontalPositionToLastKnown();
 	resetHorizontalVelocityToZero();
-
-	_aid_src_fake_pos.time_last_fuse[0] = _time_last_imu;
-	_aid_src_fake_pos.time_last_fuse[1] = _time_last_imu;
+	_time_last_fake_pos_fuse = _time_last_imu;
 }

 void Ekf::stopFakePosFusion()
 {
-	if (_using_synthetic_position) {
-		ECL_INFO("stop fake position fusion");
-		_using_synthetic_position = false;
-
-		resetEstimatorAidStatus(_aid_src_fake_pos);
-	}
+	_using_synthetic_position = false;
 }

 void Ekf::fuseFakePosition()
 {
-	Vector2f obs_var;
+	Vector3f fake_pos_obs_var;

 	if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-		obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
+		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));

 	} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 		// Accelerate tilt fine alignment by fusing more
 		// aggressively when the vehicle is at rest
-		obs_var(0) = obs_var(1) = sq(0.01f);
+		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.01f);

 	} else {
-		obs_var(0) = obs_var(1) = sq(0.5f);
+		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f);
 	}

-	const float innov_gate = 3.f;
+	_gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE;

-	auto &fake_pos = _aid_src_fake_pos;
+	const float fake_pos_innov_gate = 3.f;

-	for (int i = 0; i < 2; i++) {
-		fake_pos.observation[i] = _last_known_posNE(i);
-		fake_pos.observation_variance[i] = obs_var(i);
-
-		fake_pos.innovation[i] = _state.pos(i) - _last_known_posNE(i);
-		fake_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
+	if (fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var,
+	                           _gps_pos_innov_var, _gps_pos_test_ratio, true)) {
+		_time_last_fake_pos_fuse = _time_last_imu;
 	}
-
-	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);
-
-		if (fake_pos.fusion_enabled[i] && !fake_pos.innovation_rejected[i]) {
-			if (fuseVelPosHeight(fake_pos.innovation[i], fake_pos.innovation_variance[i], 3 + i)) {
-				fake_pos.fused[i] = true;
-				fake_pos.time_last_fuse[i] = _time_last_imu;
-			}
-		}
-	}
-
-	fake_pos.timestamp_sample = _time_last_imu;
 }
@@ -48,14 +48,6 @@ void Ekf::controlGpsFusion()

 	// Check for new GPS data that has fallen behind the fusion time horizon
 	if (_gps_data_ready) {
-
-		// reset flags
-		resetEstimatorAidStatusFlags(_aid_src_gnss_vel);
-		resetEstimatorAidStatusFlags(_aid_src_gnss_pos);
-
-		updateGpsVel(_gps_sample_delayed);
-		updateGpsPos(_gps_sample_delayed);
-
 		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
 		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);

@@ -75,8 +67,7 @@ void Ekf::controlGpsFusion()
 				if (continuing_conditions_passing
 				    || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {

-					fuseGpsVel();
-					fuseGpsPos();
+					fuseGpsVelPos();

 					if (shouldResetGpsFusion()) {
 						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
@@ -39,143 +39,39 @@
 /* #include <mathlib/mathlib.h> */
 #include "ekf.h"

-void Ekf::updateGpsVel(const gpsSample &gps_sample)
+void Ekf::fuseGpsVelPos()
 {
-	const float vel_var = sq(gps_sample.sacc);
-	const Vector3f obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
-
-	// innovation gate size
-	const float innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);
-
-	auto &gps_vel = _aid_src_gnss_vel;
-
-	for (int i = 0; i < 3; i++) {
-		gps_vel.observation[i] = gps_sample.vel(i);
-		gps_vel.observation_variance[i] = obs_var(i);
-
-		gps_vel.innovation[i] = _state.vel(i) - gps_sample.vel(i);
-		gps_vel.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i);
-	}
-
-	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]) {
-		const float innov_limit = innov_gate * sqrtf(gps_vel.innovation_variance[2]);
-		gps_vel.innovation[2] = math::constrain(gps_vel.innovation[2], -innov_limit, innov_limit);
-		gps_vel.innovation_rejected[2] = false;
-	}
-
-	gps_vel.timestamp_sample = gps_sample.time_us;
-}
-
-void Ekf::updateGpsPos(const gpsSample &gps_sample)
-{
-	Vector3f position;
-	position(0) = gps_sample.pos(0);
-	position(1) = gps_sample.pos(1);
-
-	// vertical position - gps measurement has opposite sign to earth z axis
-	position(2) = -(gps_sample.hgt - _gps_alt_ref - _gps_hgt_offset);
+	Vector3f gps_pos_obs_var;

 	const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);

-	Vector3f obs_var;
-
 	if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 		// if we are using other sources of aiding, then relax the upper observation
 		// noise limit which prevents bad GPS perturbing the position estimate
-		obs_var(0) = obs_var(1) = sq(fmaxf(gps_sample.hacc, lower_limit));
+		gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit));

 	} else {
 		// if we are not using another source of aiding, then we are reliant on the GPS
 		// observations to constrain attitude errors and must limit the observation noise value.
 		float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
-		obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit));
+		gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
 	}

-	obs_var(2) = getGpsHeightVariance();

-	// innovation gate size
-	float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);

-	auto &gps_pos = _aid_src_gnss_pos;
+	const float vel_var = sq(_gps_sample_delayed.sacc);
+	const Vector3f gps_vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};

-	for (int i = 0; i < 3; i++) {
-		gps_pos.observation[i] = position(i);
-		gps_pos.observation_variance[i] = obs_var(i);
+	// calculate innovations
+	_gps_vel_innov = _state.vel - _gps_sample_delayed.vel;
+	_gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos;

-		gps_pos.innovation[i] = _state.pos(i) - position(i);
-		gps_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
-	}
+	// set innovation gate size
+	const float pos_innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
+	const float vel_innov_gate = fmaxf(_params.gps_vel_innov_gate, 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]) {
-		const float innov_limit = innov_gate * sqrtf(gps_pos.innovation_variance[2]);
-		gps_pos.innovation[2] = math::constrain(gps_pos.innovation[2], -innov_limit, innov_limit);
-		gps_pos.innovation_rejected[2] = false;
-	}
-
-	gps_pos.timestamp_sample = gps_sample.time_us;
-}
-
-void Ekf::fuseGpsVel()
-{
-	// velocity
-	auto &gps_vel = _aid_src_gnss_vel;
-
-	// vx & vy
-	gps_vel.fusion_enabled[0] = true;
-	gps_vel.fusion_enabled[1] = true;
-
-	if (!gps_vel.innovation_rejected[0] && !gps_vel.innovation_rejected[1]) {
-		for (int i = 0; i < 2; i++) {
-			if (fuseVelPosHeight(gps_vel.innovation[i], gps_vel.innovation_variance[i], i)) {
-				gps_vel.fused[i] = true;
-				gps_vel.time_last_fuse[i] = _time_last_imu;
-			}
-		}
-	}
-
-	// vz
-	gps_vel.fusion_enabled[2] = true;
-
-	if (gps_vel.fusion_enabled[2] && !gps_vel.innovation_rejected[2]) {
-		if (fuseVelPosHeight(gps_vel.innovation[2], gps_vel.innovation_variance[2], 2)) {
-			gps_vel.fused[2] = true;
-			gps_vel.time_last_fuse[2] = _time_last_imu;
-		}
-	}
-}
-
-void Ekf::fuseGpsPos()
-{
-	auto &gps_pos = _aid_src_gnss_pos;
-
-	// x & y
-	gps_pos.fusion_enabled[0] = true;
-	gps_pos.fusion_enabled[1] = true;
-
-	if (!gps_pos.innovation_rejected[0] && !gps_pos.innovation_rejected[1]) {
-		for (int i = 0; i < 2; i++) {
-			if (fuseVelPosHeight(gps_pos.innovation[i], gps_pos.innovation_variance[i], 3 + i)) {
-				gps_pos.fused[i] = true;
-				gps_pos.time_last_fuse[i] = _time_last_imu;
-			}
-		}
-	}
-
-	// z
-	gps_pos.fusion_enabled[2] = _control_status.flags.gps_hgt;
-
-	if (gps_pos.fusion_enabled[2] && !gps_pos.innovation_rejected[2]) {
-		if (fuseVelPosHeight(gps_pos.innovation[2], gps_pos.innovation_variance[2], 5)) {
-			gps_pos.fused[2] = true;
-			gps_pos.time_last_fuse[2] = _time_last_imu;
-		}
-	}
+	// fuse GPS measurement
+	fuseHorizontalVelocity(_gps_vel_innov, vel_innov_gate, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
+	fuseVerticalVelocity(_gps_vel_innov, vel_innov_gate, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
+	fuseHorizontalPosition(_gps_pos_innov, pos_innov_gate, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
 }
@@ -38,23 +38,12 @@

 #include "ekf.h"

-void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt)
+void Ekf::fuseBaroHgt()
 {
-	// reset flags
-	resetEstimatorAidStatusFlags(baro_hgt);
-
-	// 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;
+	const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();

-	baro_hgt.innovation = _state.pos(2) - baro_hgt.observation;
-	baro_hgt.innovation_variance = P(9, 9) + obs_var;
+	_baro_hgt_innov = _state.pos(2) + unbiased_baro - _baro_hgt_offset;

 	// Compensate for positive static pressure transients (negative vertical position innovations)
 	// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@@ -63,84 +52,55 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s
 		const float deadzone_start = 0.0f;
 		const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;

-		if (baro_hgt.innovation < -deadzone_start) {
-			if (baro_hgt.innovation <= -deadzone_end) {
-				baro_hgt.innovation += deadzone_end;
+		if (_baro_hgt_innov < -deadzone_start) {
+			if (_baro_hgt_innov <= -deadzone_end) {
+				_baro_hgt_innov += deadzone_end;

 			} else {
-				baro_hgt.innovation = -deadzone_start;
+				_baro_hgt_innov = -deadzone_start;
 			}
 		}
 	}

-	setEstimatorAidStatusTestRatio(baro_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 && 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;
-	}
-
-	baro_hgt.fusion_enabled = _control_status.flags.baro_hgt;
-
-	baro_hgt.timestamp_sample = baro_sample.time_us;
-}
-
-void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt)
-{
-	if (baro_hgt.fusion_enabled
-	    && !baro_hgt.innovation_rejected
-	    && fuseVelPosHeight(baro_hgt.innovation, baro_hgt.innovation_variance, 5)) {
-
-		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);
+	// innovation gate size
+	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);

 	// observation variance - user parameter defined
-	float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
+	float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
+
+	fuseVerticalPosition(_baro_hgt_innov, innov_gate, obs_var,
+			     _baro_hgt_innov_var, _baro_hgt_test_ratio);
+}
+
+void Ekf::fuseGpsHgt()
+{
+	// vertical position innovation - gps measurement has opposite sign to earth z axis
+	_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
+
+	// innovation gate size
+	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
+
+	float obs_var = getGpsHeightVariance();
+
+	// _gps_pos_test_ratio(1) is the vertical test ratio
+	fuseVerticalPosition(_gps_pos_innov(2), innov_gate, obs_var,
+			     _gps_pos_innov_var(2), _gps_pos_test_ratio(1));
+}
+
+void Ekf::fuseRngHgt()
+{
+	// use range finder with tilt correction
+	_rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
+					  _params.rng_gnd_clearance)) - _hgt_sensor_offset;

 	// 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;
+	// observation variance - user parameter defined
+	float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);

-	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;
-	}
+	fuseVerticalPosition(_rng_hgt_innov, innov_gate, obs_var,
+			     _rng_hgt_innov_var, _rng_hgt_test_ratio);
 }

 void Ekf::fuseEvHgt()
@@ -174,20 +174,12 @@ void Ekf::fuseMag(const Vector3f &mag)
 	for (uint8_t index = 0; index <= 2; index++) {
 		_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));

-		const bool innov_check_fail = (_mag_test_ratio(index) > 1.0f);
-
-		if (innov_check_fail) {
+		if (_mag_test_ratio(index) > 1.0f) {
 			all_innovation_checks_passed = false;
-		}
-
-		if (index == 0) {
-			_innov_check_fail_status.flags.reject_mag_x = innov_check_fail;
-
-		} else if (index == 1) {
-			_innov_check_fail_status.flags.reject_mag_y = innov_check_fail;
+			_innov_check_fail_status.value |= (1 << (index + 3));

 		} else {
-			_innov_check_fail_status.flags.reject_mag_z = innov_check_fail;
+			_innov_check_fail_status.value &= ~(1 << (index + 3));
 		}
 	}

@@ -236,29 +236,25 @@ void Ekf::fuseOptFlow()


 	// run the innovation consistency check and record result
-	bool all_innovation_checks_passed = true;
+	bool flow_fail = false;
 	float test_ratio[2];
 	test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0));
 	test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1));
 	_optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]);

 	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
-		const bool innov_check_fail = (test_ratio[obs_index] > 1.0f);
-
-		if (innov_check_fail) {
-			all_innovation_checks_passed = false;
-		}
-
-		if (obs_index == 0) {
-			_innov_check_fail_status.flags.reject_optflow_X = innov_check_fail;
+		if (test_ratio[obs_index] > 1.0f) {
+			flow_fail = true;
+			_innov_check_fail_status.value |= (1 << (obs_index + 10));

 		} else {
-			_innov_check_fail_status.flags.reject_optflow_Y = innov_check_fail;
+			_innov_check_fail_status.value &= ~(1 << (obs_index + 10));
+
 		}
 	}

 	// if either axis fails we abort the fusion
-	if (!all_innovation_checks_passed) {
+	if (flow_fail) {
 		return;

 	}
@@ -102,7 +102,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, co
 }

 bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
-				 Vector3f &innov_var, Vector2f &test_ratio)
+				 Vector3f &innov_var, Vector2f &test_ratio, bool inhibit_gate)
 {

 	innov_var(0) = P(7, 7) + obs_var(0);
@@ -112,7 +112,12 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate,

 	const bool innov_check_pass = test_ratio(0) <= 1.0f;

-	if (innov_check_pass) {
+	if (innov_check_pass || inhibit_gate) {
+		if (inhibit_gate && test_ratio(0) > sq(100.0f / innov_gate)) {
+			// always protect against extreme values that could result in a NaN
+			return false;
+		}
+
 		_innov_check_fail_status.flags.reject_hor_pos = false;

 		bool fuse_x = fuseVelPosHeight(innov(0), innov_var(0), 3);
@@ -614,8 +614,6 @@ void EKF2::Run()
 			UpdateMagCalibration(now);
 			PublishSensorBias(now);

-			PublishAidSourceStatus(now);
-
 		} else {
 			// ekf no update
 			perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
@@ -634,23 +632,6 @@ void EKF2::Run()
 	ScheduleDelayed(100_ms);
 }

-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);
-
-}
-
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 {
 	if (_ekf.attitude_valid()) {
@@ -732,10 +713,6 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 		event_flags.starting_vision_vel_fusion          = _ekf.information_event_flags().starting_vision_vel_fusion;
 		event_flags.starting_vision_yaw_fusion          = _ekf.information_event_flags().starting_vision_yaw_fusion;
 		event_flags.yaw_aligned_to_imu_gps              = _ekf.information_event_flags().yaw_aligned_to_imu_gps;
-		event_flags.reset_hgt_to_baro                   = _ekf.information_event_flags().reset_hgt_to_baro;
-		event_flags.reset_hgt_to_gps                    = _ekf.information_event_flags().reset_hgt_to_gps;
-		event_flags.reset_hgt_to_rng                    = _ekf.information_event_flags().reset_hgt_to_rng;
-		event_flags.reset_hgt_to_ev                     = _ekf.information_event_flags().reset_hgt_to_ev;

 		event_flags.warning_event_changes               = _filter_warning_event_changes;
 		event_flags.gps_quality_poor                    = _ekf.warning_event_flags().gps_quality_poor;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Beat Küng	767cb3dc79	FailureDetector: check if ESCs have current And increase the timeout to 300ms, as some ESCs only update with 10Hz.	2022-06-03 09:53:56 +02:00
Beat Küng	b7c721f3f6	TEST PositionControl: use aux2 to switch between thrust vs attitude	2022-06-03 09:52:45 +02:00
Beat Küng	588e1572e3	mc_rate_control: pass through 3D thrust	2022-05-31 14:33:27 +02:00
Beat Küng	923b5b15bd	TEST: allow omnicopter to rotate via AUX1 & use 3D thrust in pos/mission mode	2022-05-31 14:16:33 +02:00
Beat Küng	1decf904d7	failure_detector: allow disabling attitude failure (as already documented)	2022-05-23 15:37:48 +02:00
Beat Küng	49956f2c44	control_allocator: show motor axis for MC (as advanced)	2022-05-23 15:37:14 +02:00
Alex Mikhalev	8267f4fd1e	scripts: Hotfix px_uploader.py to avoid PX4BL bug Signed-off-by: Alex Mikhalev <alex@corvus-robotics.com>	2022-05-23 15:37:06 +02:00