make demo work with pca.

* match vehicle_status

* match home_position

* match vehicle_command_ack

* match actuator_armed

* match vehicle_control_mode

* match commander_state

.ci/Jenkinsfile-hardware

@@ -864,12 +864,15 @@ 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'

ROMFS/cannode/CMakeLists.txt

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

ROMFS/cannode/init.d/CMakeLists.txt

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

ROMFS/cannode/init.d/rc.output_defaults

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

ROMFS/cannode/init.d/rcS

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

ROMFS/cannode/mixers/CMakeLists.txt

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

ROMFS/cannode/mixers/IO_pass_ucan.main.mix

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

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -95,10 +95,9 @@ 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
@@ -196,14 +195,6 @@ 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
 

ROMFS/px4fmu_common/init.d/CMakeLists.txt

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

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

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

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

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

ROMFS/px4fmu_common/init.d/rc.output_defaults

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

ROMFS/px4fmu_common/init.d/rcS

@@ -21,7 +21,6 @@ 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
@@ -177,13 +176,12 @@ else
 	fi
 
 	#
-	# Set AUTOCNF flag to use it in AUTOSTART scripts.
+	# 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.
 	#
 	if param greater SYS_AUTOCONFIG 0
 	then
-		# 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
+		# 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*
 	fi
 
 	#
@@ -274,14 +272,6 @@ 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.
 	#
@@ -565,7 +555,6 @@ fi
 # Unset all script parameters to free RAM.
 #
 unset R
-unset AUTOCNF
 unset FCONFIG
 unset FEXTRAS
 unset FRC

Tools/ecl_ekf/analyse_logdata_ekf.py

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

Tools/ecl_ekf/analysis/checks.py

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

Tools/ecl_ekf/analysis/metrics.py

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

Tools/ecl_ekf/analysis/post_processing.py

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

Tools/ecl_ekf/plotting/pdf_report.py

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

Tools/flash_nxp/flash_ucan.jlink

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

Tools/flash_nxp/flash_ucan.sh

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

Tools/px_uploader.py

@@ -136,11 +136,8 @@ class firmware(object):
 
         self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))
 
-        # 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):
+        # pad image to 4-byte length
+        while ((len(self.image) % 4) != 0):
             self.image.extend(b'\xff')
 
     def property(self, propname):

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

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

boards/nxp/fmuk66-v3/nxp_demo.px4board

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

boards/nxp/ucans32k146/default.px4board

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

boards/nxp/ucans32k146/init/rc.board_defaults

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

boards/nxp/ucans32k146/nxp_demo.px4board

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

boards/nxp/ucans32k146/src/board_config.h

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

boards/nxp/ucans32k146/src/timer_config.cpp

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

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

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

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

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

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

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

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

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

msg/CMakeLists.txt

@@ -77,6 +77,9 @@ 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
@@ -101,6 +104,7 @@ set(msg_files
 	landing_gear.msg
 	landing_target_innovations.msg
 	landing_target_pose.msg
+	lighting_states.msg
 	led_control.msg
 	log_message.msg
 	logger_status.msg

msg/actuator_controls.msg

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

msg/estimator_aid_source_1d.msg

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

msg/estimator_aid_source_2d.msg

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

msg/estimator_aid_source_3d.msg

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

msg/estimator_event_flags.msg

@@ -16,6 +16,10 @@ 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

msg/led_control.msg

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

msg/led_control.msg.old

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

msg/lighting_states.msg

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

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

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

src/drivers/cyphal/ParamManager.hpp

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

src/drivers/cyphal/PublicationManager.hpp

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

src/drivers/cyphal/SubscriptionManager.hpp

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

src/drivers/cyphal/parameters.c

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

src/drivers/drv_apa102.h

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

src/drivers/extreme_switch/CMakeLists.txt

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

src/drivers/extreme_switch/ExtremeSwitch.cpp

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

src/drivers/extreme_switch/ExtremeSwitch.hpp

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

src/drivers/extreme_switch/Kconfig

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

src/drivers/imu/invensense/icm42688p/ICM42688P.cpp

@@ -274,20 +274,22 @@ void ICM42688P::RunImpl()
 				}
 			}
 
-			// 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;
+			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;
 
-			} else {
-				// register check failed, force reset
-				perf_count(_bad_register_perf);
-				Reset();
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
 			}
 		}
 

src/drivers/lights/apa102/CMakeLists.txt

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

src/drivers/lights/apa102/Kconfig

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

src/drivers/lights/apa102/apa102.cpp

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

src/drivers/lights/apa102/apa102.hpp

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

src/drivers/pca9685/pca9685.cpp

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

src/examples/actuator_controls_driver/ActuatorControlsDriver.cpp

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

src/examples/actuator_controls_driver/ActuatorControlsDriver.hpp

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

src/examples/actuator_controls_driver/CMakeLists.txt

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

src/examples/actuator_controls_driver/Kconfig

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

src/examples/elm_lighting_module/CMakeLists.txt

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

src/examples/elm_lighting_module/Kconfig

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

src/examples/elm_lighting_module/elm_lighting_module.c

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

src/lib/battery/battery.cpp

@@ -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) {
+	if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) {
 		cell_voltage += _params.r_internal * current_a;
 
 	} else {

src/lib/battery/battery_params_deprecated.c

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

src/lib/battery/module.yaml

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

src/modules/commander/Commander.hpp

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

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -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 > FLT_EPSILON) && (fabsf(roll) > max_roll);
-		const bool pitch_status = (max_pitch > FLT_EPSILON) && (fabsf(pitch) > max_pitch);
+		const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
+		const bool pitch_status = (max_pitch > 0.0f) && (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 > 300_ms;
+			const bool esc_timed_out = telemetry_age > 100_ms;  // TODO: magic number
 
 			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,40 +399,35 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 			}
 
 			// Check if ESC current is too low
-			if (cur_esc_report.esc_current > FLT_EPSILON) {
-				_motor_failure_escs_have_current = true;
+			float esc_throttle = 0.f;
+
+			if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
+				esc_throttle = fabsf(actuator_motors.control[i_esc]);
 			}
 
-			if (_motor_failure_escs_have_current) {
-				float esc_throttle = 0.f;
+			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 (PX4_ISFINITE(actuator_motors.control[i_esc])) {
-					esc_throttle = fabsf(actuator_motors.control[i_esc]);
+			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;
 				}
 
-				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;
-					}
+			} 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);

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -129,7 +129,6 @@ 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)};

src/modules/commander/failure_detector/failure_detector_params.c

@@ -53,7 +53,7 @@
  *
  * Setting this parameter to 0 disables the check
  *
- * @min 0
+ * @min 60
  * @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 0
+ * @min 60
  * @max 180
  * @unit deg
  * @group Failure Detector

src/modules/control_allocator/module.yaml

@@ -556,18 +556,6 @@ 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'

src/modules/ekf2/EKF/common.h

@@ -550,6 +550,10 @@ 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;
 };

src/modules/ekf2/EKF/control.cpp

@@ -125,8 +125,8 @@ void Ekf::controlFusionModes()
 
 	if (_range_buffer) {
 		// Get range data from buffer and check validity
-		bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
-		_range_sensor.setDataReadiness(is_rng_data_ready);
+		_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
+		_range_sensor.setDataReadiness(_rng_data_ready);
 
 		// update range sensor angle parameters in case they have changed
 		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
@@ -778,6 +778,34 @@ 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
@@ -923,25 +951,23 @@ void Ekf::controlHeightFusion()
 	updateBaroHgtOffset();
 	updateGroundEffect();
 
-	if (_control_status.flags.baro_hgt) {
+	if (_baro_data_ready) {
+		updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
 
-		if (_baro_data_ready && !_baro_hgt_faulty) {
-			fuseBaroHgt();
+		if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
+			fuseBaroHgt(_aid_src_baro_hgt);
 		}
+	}
 
-	} else if (_control_status.flags.gps_hgt) {
+	if (_rng_data_ready) {
+		updateRngHgt(_aid_src_rng_hgt);
 
-		if (_gps_data_ready) {
-			fuseGpsHgt();
+		if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
+			fuseRngHgt(_aid_src_rng_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) {
 
 		if (_control_status.flags.ev_hgt && _ev_data_ready) {
 			fuseEvHgt();

src/modules/ekf2/EKF/covariance.cpp

@@ -984,13 +984,24 @@ 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 = (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)));
+		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;
+				}
+			}
+		}
 
 		// record the pass/fail
 		if (!bad_acc_bias) {

src/modules/ekf2/EKF/ekf.h

@@ -48,6 +48,10 @@
 #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:
@@ -82,13 +86,13 @@ public:
 	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
 
-	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov; }
-	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var; }
-	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio; }
+	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
+	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
+	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
 
-	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov; }
-	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var; }
-	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio; }
+	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
+	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
+	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
 
 	void getAuxVelInnov(float aux_vel_innov[2]) const;
 	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
@@ -336,6 +340,14 @@ 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
@@ -377,6 +389,7 @@ private:
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 	bool _baro_data_ready{false};	///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
+	bool _rng_data_ready{false};
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
@@ -394,7 +407,6 @@ 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)
@@ -438,24 +450,12 @@ 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,6 +491,14 @@ private:
 	bool _inhibit_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
 	Vector2f _flow_compensated_XY_rad{};	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
+	estimator_aid_source_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)
@@ -521,8 +529,12 @@ 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};
 
@@ -633,11 +645,13 @@ private:
 	// fuse body frame drag specific forces for multi-rotor wind estimation
 	void fuseDrag(const dragSample &drag_sample);
 
-	void fuseBaroHgt();
-	void fuseGpsHgt();
-	void fuseRngHgt();
+	void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
+	void fuseRngHgt(estimator_aid_source_1d_s &range_hgt);
 	void fuseEvHgt();
 
+	void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt);
+	void updateRngHgt(estimator_aid_source_1d_s &rng_hgt);
+
 	// fuse single velocity and position measurement
 	bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
 
@@ -676,12 +690,16 @@ 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, bool inhibit_gate = false);
+				    Vector3f &innov_var, Vector2f &test_ratiov);
 
 	bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
 				  float &innov_var, float &test_ratio);
 
-	void fuseGpsVelPos();
+	void updateGpsVel(const gpsSample &gps_sample);
+	void fuseGpsVel();
+
+	void updateGpsPos(const gpsSample &gps_sample);
+	void fuseGpsPos();
 
 	// calculate optical flow body angular rate compensation
 	// returns false if bias corrected body rate data is unavailable
@@ -1048,6 +1066,80 @@ 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

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -241,7 +241,10 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos)
 
 void Ekf::resetHeightToBaro()
 {
-	resetVerticalPositionTo(-_baro_sample_delayed.hgt + _baro_hgt_offset);
+	ECL_INFO("reset height to baro");
+	_information_events.flags.reset_hgt_to_baro = true;
+
+	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));
@@ -249,6 +252,9 @@ 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);
 
@@ -261,6 +267,9 @@ 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) {
@@ -273,7 +282,7 @@ void Ekf::resetHeightToRng()
 
 	// update the state and associated variance
 	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset);
+	resetVerticalPositionTo(-dist_bottom + _rng_hgt_offset);
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
@@ -284,6 +293,9 @@ 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));
 
@@ -352,7 +364,8 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 	}
 
 	// check for excessive horizontal GPS velocity innovations
-	const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;
+	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;
 
 	// calculate GPS course over ground angle
 	const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
@@ -593,30 +606,33 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 
 void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2],  float &vpos) const
 {
-	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);
+	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];
 }
 
 void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)  const
 {
-	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);
+	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];
 }
 
 void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
 {
-	hvel = _gps_vel_test_ratio(0);
-	vvel = _gps_vel_test_ratio(1);
-	hpos = _gps_pos_test_ratio(0);
-	vpos = _gps_pos_test_ratio(1);
+	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];
 }
 
 void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
@@ -728,7 +744,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, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
+			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
 
 		if (_control_status.flags.ev_pos) {
@@ -750,7 +766,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, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
+		hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 	}
 
 	*ekf_eph = hpos_err;
@@ -774,7 +790,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, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
+			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
 
 		} 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))));
@@ -901,10 +917,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(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1)));
+		float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max());
 		vel = math::max(gps_vel, FLT_MIN);
 
-		float gps_pos = sqrtf(_gps_pos_test_ratio(0));
+		float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max());
 		pos = math::max(gps_pos, FLT_MIN);
 	}
 
@@ -925,13 +941,13 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 
 	// return the vertical position innovation test ratio
 	if (_control_status.flags.baro_hgt) {
-		hgt = math::max(sqrtf(_baro_hgt_test_ratio), FLT_MIN);
+		hgt = math::max(sqrtf(_aid_src_baro_hgt.test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.gps_hgt) {
-		hgt = math::max(sqrtf(_gps_pos_test_ratio(1)), FLT_MIN);
+		hgt = math::max(sqrtf(_aid_src_gnss_pos.test_ratio[2]), FLT_MIN);
 
 	} else if (_control_status.flags.rng_hgt) {
-		hgt = math::max(sqrtf(_rng_hgt_test_ratio), FLT_MIN);
+		hgt = math::max(sqrtf(_aid_src_rng_hgt.test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.ev_hgt) {
 		hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);
@@ -965,8 +981,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 = (_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 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 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;
@@ -1000,6 +1016,7 @@ 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);
@@ -1254,7 +1271,8 @@ void Ekf::startBaroHgtFusion()
 
 		// We don't need to set a height sensor offset
 		// since we track a separate _baro_hgt_offset
-		_hgt_sensor_offset = 0.0f;
+
+		ECL_INFO("starting baro height fusion");
 	}
 }
 
@@ -1262,17 +1280,19 @@ void Ekf::startGpsHgtFusion()
 {
 	if (!_control_status.flags.gps_hgt) {
 		if (_control_status.flags.rng_hgt) {
-			// swith out of range aid
+			// switch out of range aid
 			// calculate height sensor offset such that current
 			// measurement matches our current height estimate
-			_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
+			_gps_hgt_offset = (_gps_sample_delayed.hgt - _gps_alt_ref) + _state.pos(2);
 
 		} else {
-			_hgt_sensor_offset = 0.f;
+			_gps_hgt_offset = 0.f;
 			resetHeightToGps();
 		}
 
 		setControlGPSHeight();
+
+		ECL_INFO("starting GPS height fusion");
 	}
 }
 
@@ -1283,12 +1303,14 @@ void Ekf::startRngHgtFusion()
 
 		// Range finder is the primary height source, the ground is now the datum used
 		// to compute the local vertical position
-		_hgt_sensor_offset = 0.f;
+		_rng_hgt_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");
 	}
 }
 
@@ -1299,7 +1321,9 @@ void Ekf::startRngAidHgtFusion()
 
 		// calculate height sensor offset such that current
 		// measurement matches our current height estimate
-		_hgt_sensor_offset = _terrain_vpos;
+		_rng_hgt_offset = _terrain_vpos;
+
+		ECL_INFO("starting RNG aid height fusion");
 	}
 }
 
@@ -1312,6 +1336,8 @@ void Ekf::startEvHgtFusion()
 			// EV and range finders are using the same height datum
 			resetHeightToEv();
 		}
+
+		ECL_INFO("starting EV height fusion");
 	}
 }
 
@@ -1557,6 +1583,8 @@ void Ekf::stopGpsFusion()
 	if (_control_status.flags.gps) {
 		stopGpsPosFusion();
 		stopGpsVelFusion();
+
+		_control_status.flags.gps = false;
 	}
 
 	if (_control_status.flags.gps_yaw) {
@@ -1570,27 +1598,27 @@ void Ekf::stopGpsFusion()
 
 void Ekf::stopGpsPosFusion()
 {
-	_control_status.flags.gps = false;
+	ECL_INFO("stopping GPS position fusion");
 
 	if (_control_status.flags.gps_hgt) {
+		ECL_INFO("stopping GPS height fusion");
 		startBaroHgtFusion();
 	}
 
-	_gps_pos_innov.setZero();
-	_gps_pos_innov_var.setZero();
-	_gps_pos_test_ratio.setZero();
+	resetEstimatorAidStatus(_aid_src_gnss_pos);
 }
 
 void Ekf::stopGpsVelFusion()
 {
-	_gps_vel_innov.setZero();
-	_gps_vel_innov_var.setZero();
-	_gps_vel_test_ratio.setZero();
+	ECL_INFO("stopping GPS velocity fusion");
+
+	resetEstimatorAidStatus(_aid_src_gnss_vel);
 }
 
 void Ekf::startGpsYawFusion()
 {
-	if (resetYawToGps()) {
+	if (!_control_status.flags.gps_yaw && resetYawToGps()) {
+		ECL_INFO("starting GPS yaw fusion");
 		_control_status.flags.yaw_align = true;
 		_control_status.flags.mag_dec = false;
 		stopEvYawFusion();
@@ -1598,12 +1626,14 @@ void Ekf::startGpsYawFusion()
 		stopMag3DFusion();
 		_control_status.flags.gps_yaw = true;
 	}
-
 }
 
 void Ekf::stopGpsYawFusion()
 {
-	_control_status.flags.gps_yaw = false;
+	if (_control_status.flags.gps_yaw) {
+		ECL_INFO("stopping GPS yaw fusion");
+		_control_status.flags.gps_yaw = false;
+	}
 }
 
 void Ekf::startEvPosFusion()

src/modules/ekf2/EKF/estimator_interface.h

@@ -329,15 +329,11 @@ 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

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -40,9 +40,14 @@
 
 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(_time_last_fake_pos_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate
+	const bool fake_pos_data_ready = isTimedOut(fake_pos.time_last_fuse[0], (uint64_t)2e5); // Fuse fake position at a limited rate
 
 	if (fake_pos_data_ready) {
 		const bool continuing_conditions_passing = !isHorizontalAidingActive();
@@ -52,7 +57,7 @@ void Ekf::controlFakePosFusion()
 			if (continuing_conditions_passing) {
 				fuseFakePosition();
 
-				const bool is_fusion_failing = isTimedOut(_time_last_fake_pos_fuse, (uint64_t)4e5);
+				const bool is_fusion_failing = isTimedOut(fake_pos.time_last_fuse[0], (uint64_t)4e5);
 
 				if (is_fusion_failing) {
 					resetFakePosFusion();
@@ -79,6 +84,7 @@ 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();
@@ -87,39 +93,68 @@ void Ekf::startFakePosFusion()
 
 void Ekf::resetFakePosFusion()
 {
+	ECL_INFO("reset fake position fusion");
 	_last_known_posNE = _state.pos.xy();
+
 	resetHorizontalPositionToLastKnown();
 	resetHorizontalVelocityToZero();
-	_time_last_fake_pos_fuse = _time_last_imu;
+
+	_aid_src_fake_pos.time_last_fuse[0] = _time_last_imu;
+	_aid_src_fake_pos.time_last_fuse[1] = _time_last_imu;
 }
 
 void Ekf::stopFakePosFusion()
 {
-	_using_synthetic_position = false;
+	if (_using_synthetic_position) {
+		ECL_INFO("stop fake position fusion");
+		_using_synthetic_position = false;
+
+		resetEstimatorAidStatus(_aid_src_fake_pos);
+	}
 }
 
 void Ekf::fuseFakePosition()
 {
-	Vector3f fake_pos_obs_var;
+	Vector2f obs_var;
 
 	if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
+		obs_var(0) = 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
-		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.01f);
+		obs_var(0) = obs_var(1) = sq(0.01f);
 
 	} else {
-		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f);
+		obs_var(0) = obs_var(1) = sq(0.5f);
 	}
 
-	_gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE;
+	const float innov_gate = 3.f;
 
-	const float fake_pos_innov_gate = 3.f;
+	auto &fake_pos = _aid_src_fake_pos;
 
-	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;
+	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);
 	}
+
+	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;
 }

src/modules/ekf2/EKF/gps_control.cpp

@@ -48,6 +48,14 @@ 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);
 
@@ -67,7 +75,8 @@ void Ekf::controlGpsFusion()
 				if (continuing_conditions_passing
 				    || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 
-					fuseGpsVelPos();
+					fuseGpsVel();
+					fuseGpsPos();
 
 					if (shouldResetGpsFusion()) {
 						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -39,39 +39,143 @@
 /* #include <mathlib/mathlib.h> */
 #include "ekf.h"
 
-void Ekf::fuseGpsVelPos()
+void Ekf::updateGpsVel(const gpsSample &gps_sample)
 {
-	Vector3f gps_pos_obs_var;
+	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);
 
 	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
-		gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit));
+		obs_var(0) = obs_var(1) = sq(fmaxf(gps_sample.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);
-		gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
+		obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit));
 	}
 
+	obs_var(2) = getGpsHeightVariance();
 
+	// innovation gate size
+	float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
 
-	const float vel_var = sq(_gps_sample_delayed.sacc);
-	const Vector3f gps_vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
+	auto &gps_pos = _aid_src_gnss_pos;
 
-	// calculate innovations
-	_gps_vel_innov = _state.vel - _gps_sample_delayed.vel;
-	_gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos;
+	for (int i = 0; i < 3; i++) {
+		gps_pos.observation[i] = position(i);
+		gps_pos.observation_variance[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);
+		gps_pos.innovation[i] = _state.pos(i) - position(i);
+		gps_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
+	}
 
-	// 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);
+	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;
+		}
+	}
 }

src/modules/ekf2/EKF/height_fusion.cpp

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

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -174,12 +174,20 @@ 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));
 
-		if (_mag_test_ratio(index) > 1.0f) {
+		const bool innov_check_fail = (_mag_test_ratio(index) > 1.0f);
+
+		if (innov_check_fail) {
 			all_innovation_checks_passed = false;
-			_innov_check_fail_status.value |= (1 << (index + 3));
+		}
+
+		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;
 
 		} else {
-			_innov_check_fail_status.value &= ~(1 << (index + 3));
+			_innov_check_fail_status.flags.reject_mag_z = innov_check_fail;
 		}
 	}
 

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -236,25 +236,29 @@ void Ekf::fuseOptFlow()
 
 
 	// run the innovation consistency check and record result
-	bool flow_fail = false;
+	bool all_innovation_checks_passed = true;
 	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++) {
-		if (test_ratio[obs_index] > 1.0f) {
-			flow_fail = true;
-			_innov_check_fail_status.value |= (1 << (obs_index + 10));
+		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;
 
 		} else {
-			_innov_check_fail_status.value &= ~(1 << (obs_index + 10));
-
+			_innov_check_fail_status.flags.reject_optflow_Y = innov_check_fail;
 		}
 	}
 
 	// if either axis fails we abort the fusion
-	if (flow_fail) {
+	if (!all_innovation_checks_passed) {
 		return;
 
 	}

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -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, bool inhibit_gate)
+				 Vector3f &innov_var, Vector2f &test_ratio)
 {
 
 	innov_var(0) = P(7, 7) + obs_var(0);
@@ -112,12 +112,7 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate,
 
 	const bool innov_check_pass = test_ratio(0) <= 1.0f;
 
-	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;
-		}
-
+	if (innov_check_pass) {
 		_innov_check_fail_status.flags.reject_hor_pos = false;
 
 		bool fuse_x = fuseVelPosHeight(innov(0), innov_var(0), 3);

src/modules/ekf2/EKF2.cpp

@@ -614,6 +614,8 @@ 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));
@@ -632,6 +634,23 @@ 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()) {
@@ -713,6 +732,10 @@ 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;