make demo work with pca.

ucan pwm enablement.
S32K1 fixes.
2026-06-05 21:30:04 +08:00 · 2022-06-01 18:22:23 -04:00 · 2022-06-01 13:36:40 -04:00 · 2022-05-31 16:56:49 -04:00 · 2022-05-26 10:52:08 -04:00 · 2022-05-25 17:44:54 -04:00
134 changed files with 4121 additions and 1722 deletions
@@ -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'
@@ -32,3 +32,4 @@
 ############################################################################

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

 px4_add_romfs_files(
 	rcS
+	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
@@ -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
@@ -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
+)
@@ -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
@@ -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

@@ -55,4 +55,5 @@ px4_add_romfs_files(
 	rc.vehicle_setup
 	rc.vtol_apps
 	rc.vtol_defaults
+	rc.output_defaults
 )
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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')

@@ -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'),
@@ -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')]:

@@ -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:
@@ -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:
@@ -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
@@ -0,0 +1,3 @@
+#!/bin/zsh
+
+JLinkExe -device S32K146 -CommandFile /Users/landon/git/px4/Tools/flash_nxp/flash_ucan.jlink
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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 =
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -137,7 +141,6 @@ set(msg_files
 	rc_parameter_map.msg
 	rpm.msg
 	rtl_time_estimate.msg
-	safety.msg
 	satellite_info.msg
 	sensor_accel.msg
 	sensor_accel_fifo.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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -19,7 +19,7 @@ bool status_rc_ppm
 bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
-bool status_safety_off
+bool status_safety_button_event # px4io safety button was pressed for longer than 1 second

 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
 bool alarm_pwm_error
@@ -1,3 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-bool safety_switch_available		# Set to true if a safety switch is connected
-bool safety_off			# Set to true if safety is off
@@ -116,3 +116,6 @@ uint8 latest_disarming_reason
 uint64 armed_time       # Arming timestamp (microseconds)

 uint64 takeoff_time     # Takeoff timestamp (microseconds)
+
+bool safety_button_available	# Set to true if a safety button is connected
+bool safety_off					# Set to true if safety is off
@@ -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
@@ -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"},
 	};
@@ -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
 	};
 };
@@ -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
 	};
 };
@@ -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);
@@ -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];};
+};
+};
@@ -188,12 +188,6 @@ typedef uint16_t	servo_position_t;
 /** get the maximum PWM value the output will send */
 #define PWM_SERVO_GET_MAX_PWM	_PX4_IOC(_PWM_SERVO_BASE, 19)

-/** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_PX4_IOC(_PWM_SERVO_BASE, 25)
-
-/** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)
-
 /*
 *
 *
@@ -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
+	)
@@ -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);
+}
@@ -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};
+};
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_EXTREME_SWITCH
+	bool "extreme_switch"
+	default n
+	---help---
+		Enable support for extreme_switch
@@ -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();
+				}
 			}
 		}

@@ -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
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_LIGHTS_APA102
+	bool "apa102"
+	default n
+	---help---
+		Enable support for apa102
@@ -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);
+}
@@ -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;
+};
@@ -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;
@@ -492,9 +492,7 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 		break;

 	case PWM_SERVO_SET_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 	case PWM_SERVO_ARM:
 	case PWM_SERVO_DISARM:
 		break;
@@ -717,8 +717,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)

 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 		break;

 	case PWM_SERVO_DISARM:
@@ -74,6 +74,7 @@
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/px4io_status.h>
 #include <uORB/topics/parameter_update.h>

@@ -196,7 +197,8 @@ private:
 	uint16_t		_last_written_arming_c{0};	///< the last written arming state reg

 	uORB::Subscription	_t_actuator_armed{ORB_ID(actuator_armed)};		///< system armed control topic
-	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};		///< vehicle command topic
+	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};	///< vehicle command topic
+	uORB::Subscription	_t_vehicle_status{ORB_ID(vehicle_status)};		///< vehicle status topic

 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

@@ -443,15 +445,14 @@ int PX4IO::init()
 		// the startup script to be able to load a new IO
 		// firmware

-		// If IO has already safety off it won't accept going into bootloader mode,
-		// therefore we need to set safety on first.
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-
 		// Now the reboot into bootloader mode should succeed.
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, PX4IO_REBOOT_BL_MAGIC);
 		return -1;
 	}

+	/* Set safety_off to false when FMU boot*/
+	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, 0);
+
 	if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) {
 		_max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS;
 	}
@@ -476,14 +477,6 @@ int PX4IO::init()
 		return ret;
 	}

-	/* set safety to off if circuit breaker enabled */
-	if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
-
-	} else {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-	}
-
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
 		_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
@@ -621,11 +614,6 @@ void PX4IO::Run()

 			update_params();

-			/* Check if the IO safety circuit breaker has been updated */
-			bool circuit_breaker_io_safety_enabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
-			/* Bypass IO safety switch logic by setting FORCE_SAFETY_OFF */
-			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, circuit_breaker_io_safety_enabled);
-
 			/* Check if the flight termination circuit breaker has been updated */
 			bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
 			/* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */
@@ -974,15 +962,12 @@ int PX4IO::io_handle_status(uint16_t status)
 	 */

 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
-	    && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+	if (!(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
-				    PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);

 		/* set new status */
 		_status = status;
-		_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;

 	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {

@@ -999,18 +984,26 @@ int PX4IO::io_handle_status(uint16_t status)
 	}

 	/**
-	 * Get and handle the safety status
+	 * Get and handle the safety button status
 	 */
-	const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+	const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;

-	/* px4io board will change safety_off from false to true and stay like that until the vehicle is rebooted
-	 * where safety will change back to false. Here we are triggering the safety button event once.
-	 * TODO: change px4io firmware to act on the event. This will enable the "force safety on disarming" feature. */
-	if (_previous_safety_off != safety_off) {
+	if (safety_button_pressed) {
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
 		_button_publisher.safetyButtonTriggerEvent();
 	}

-	_previous_safety_off = safety_off;
+	/**
+	 * Inform PX4IO board about safety_off state for LED control
+	 */
+	vehicle_status_s vehicle_status;
+
+	if (_t_vehicle_status.update(&vehicle_status)) {
+		if (_previous_safety_off != vehicle_status.safety_off) {
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off);
+			_previous_safety_off = vehicle_status.safety_off;
+		}
+	}

 	return ret;
 }
@@ -1033,37 +1026,17 @@ int PX4IO::dsm_bind_ioctl(int dsmMode)
 		return -EINVAL;
 	}

-	// Check if safety was off
-	bool safety_off = (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-	int ret = -1;
-
-	// Put safety on
-	if (safety_off) {
-		// re-enable safety
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, 0);
-
-		// set new status
-		_status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-	}
-
 	PX4_INFO("Binding DSM%s RX", (dsmMode == DSM2_BIND_PULSES) ? "2" : ((dsmMode == DSMX_BIND_PULSES) ? "-X" : "-X8"));

-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+	int ret = OK;
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
 	px4_usleep(500000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
 	px4_usleep(72000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
 	px4_usleep(50000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
-	ret = OK;
-
-	// Put safety back off
-	if (safety_off) {
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
-				    PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-		_status |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
-	}
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);

 	if (ret != OK) {
 		PX4_INFO("Binding DSM failed");
@@ -1133,7 +1106,7 @@ int PX4IO::io_get_status()
 		status.status_arm_sync        = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_ARM_SYNC;
 		status.status_init_ok         = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_INIT_OK;
 		status.status_failsafe        = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FAILSAFE;
-		status.status_safety_off      = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+		status.status_safety_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;

 		// PX4IO_P_STATUS_ALARMS
 		status.alarm_rc_lost       = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
@@ -1644,18 +1617,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;

-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-		PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_OFF");
-		/* force safety swith off */
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
-		break;
-
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
-		PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_ON");
-		/* force safety switch on */
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-		break;
-
 	case DSM_BIND_START:
 		/* bind a DSM receiver */
 		ret = dsm_bind_ioctl(arg);
@@ -1725,16 +1686,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)

 		}

-		// re-enable safety
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-
-		if (ret != PX4_OK) {
-			PX4_WARN("IO refused to re-enable safety");
-		}
-
-		// set new status
-		_status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-
 		/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
 		usleep(1);
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
@@ -2058,29 +2009,6 @@ int PX4IO::custom_command(int argc, char *argv[])
 		return 1;
 	}

-
-	if (!strcmp(verb, "safety_off")) {
-		int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
-
-		if (ret != OK) {
-			PX4_ERR("failed to disable safety (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(verb, "safety_on")) {
-		int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
-
-		if (ret != OK) {
-			PX4_ERR("failed to enable safety (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
 	if (!strcmp(verb, "debug")) {
 		if (argc <= 1) {
 			PX4_ERR("usage: px4io debug LEVEL");
@@ -2162,8 +2090,6 @@ Output driver communicating with the IO co-processor.
 	PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("update", "Update IO firmware");
 	PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", true);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("safety_off", "Turn off safety (force)");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("safety_on", "Turn on safety (force)");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("debug", "set IO debug level");
 	PRINT_MODULE_USAGE_ARG("<debug_level>", "0=disabled, 9=max verbosity", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind");
@@ -951,8 +951,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-		// these are no-ops, as no safety switch
 		break;

 	case MIXERIOCRESET:
@@ -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);
+}
@@ -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};
+};
@@ -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
+	)
@@ -0,0 +1,5 @@
+menuconfig EXAMPLES_ACTUATOR_CONTROLS_DRIVER
+	bool "actuator_controls_driver"
+	default n
+	---help---
+		Enable support for actuator_controls_driver
@@ -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
+	)
@@ -0,0 +1,5 @@
+menuconfig EXAMPLES_ELM_LIGHTING_MODULE
+	bool "elm_lighting_module"
+	default n
+	---help---
+		Enable support for elm_lighting_module
@@ -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;
+}
@@ -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 {
@@ -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.
@@ -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:
@@ -146,38 +146,34 @@
                            "description": "mission start"
                        },
                        "6": {
-                            "name": "safety_button",
-                            "description": "safety button"
-                        },
-                        "7": {
                            "name": "auto_disarm_land",
                            "description": "landing"
                        },
-                        "8": {
+                        "7": {
                            "name": "auto_disarm_preflight",
                            "description": "auto preflight disarming"
                        },
-                        "9": {
+                        "8": {
                            "name": "kill_switch",
                            "description": "kill switch"
                        },
-                        "10": {
+                        "9": {
                            "name": "lockdown",
                            "description": "lockdown"
                        },
-                        "11": {
+                        "10": {
                            "name": "failure_detector",
                            "description": "failure detector"
                        },
-                        "12": {
+                        "11": {
                            "name": "shutdown",
                            "description": "shutdown request"
                        },
-                        "13": {
+                        "12": {
                            "name": "unit_test",
                            "description": "unit tests"
                        },
-                        "14": {
+                        "13": {
                            "name": "rc_button",
                            "description": "RC (button)"
                        }
@@ -39,7 +39,7 @@ constexpr bool
 ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX];

 transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
-		const vehicle_control_mode_s &control_mode, const safety_s &safety,
+		const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
 		const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
 		orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
 		const PreFlightCheck::arm_requirements_t &arm_requirements,
@@ -112,8 +112,9 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s

 					if (fRunPreArmChecks && preflight_check_ret) {
 						// only bother running prearm if preflight was successful
-						prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements,
-								   status);
+						prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode,
+								   safety_button_available, safety_off,
+								   arm_requirements, status);
 					}

 					if (!preflight_check_ret || !prearm_check_ret) {
@@ -37,7 +37,6 @@
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/events.h>
 #include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/safety.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
@@ -55,8 +54,8 @@ public:
 	void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; }

 	transition_result_t
-	arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety,
-				const arming_state_t new_arming_state,
+	arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode,
+				const bool safety_button_available, const bool safety_off, const arming_state_t new_arming_state,
 				actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
 				vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
 				const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
@@ -55,7 +55,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		hil_state_t                     hil_state;                              // Current vehicle_status_s.hil_state
 		bool
 		system_sensors_initialized;   // Current vehicle_status_s.system_sensors_initialized
-		bool                            safety_switch_available;                // Current safety_s.safety_switch_available
+		bool                            safety_button_available;                // Current safety_s.safety_button_available
 		bool                            safety_off;                             // Current safety_s.safety_off
 		arming_state_t                  requested_state;                        // Requested arming state to transition to
 		ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
@@ -166,17 +166,17 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
 		},

-		// Safety switch arming tests
+		// Safety button arming tests

 		{
-			"transition: standby to armed, no safety switch",
+			"transition: standby to armed, no safety button",
 			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
 			vehicle_status_s::ARMING_STATE_ARMED,
 			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
 		},

 		{
-			"transition: standby to armed, safety switch off",
+			"transition: standby to armed, safety button off",
 			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
 			vehicle_status_s::ARMING_STATE_ARMED,
 			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
@@ -242,10 +242,10 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		//    vehicle_status_s::ARMING_STATE_STANDBY,
 		//    { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },

-		// Safety switch arming tests
+		// safety button arming tests

 		{
-			"no transition: init to armed, safety switch on",
+			"no transition: init to armed, safety button on",
 			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_ARMED,
 			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED
@@ -254,7 +254,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)

 	struct vehicle_status_s status {};
 	struct vehicle_status_flags_s status_flags {};
-	struct safety_s safety {};
 	struct actuator_armed_s armed {};

 	size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
@@ -270,8 +269,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		status.hil_state = test->hil_state;
 		// The power status of the test unit is not relevant for the unit test
 		status_flags.circuit_breaker_engaged_power_check = true;
-		safety.safety_switch_available = test->safety_switch_available;
-		safety.safety_off = test->safety_off;

 		vehicle_control_mode_s control_mode{};

@@ -279,7 +276,8 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		transition_result_t result = arm_state_machine.arming_state_transition(
 						     status,
 						     control_mode,
-						     safety,
+						     test->safety_button_available,
+						     test->safety_off,
 						     test->requested_state,
 						     armed,
 						     true /* enable pre-arm checks */,
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 - 2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -41,11 +41,11 @@
 #pragma once

 #include <uORB/uORB.h>
-#include <uORB/topics/safety.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status_flags.h>
 #include <uORB/topics/vehicle_status.h>
 #include <drivers/drv_hrt.h>
+#include "../../Safety.hpp"

 typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 				  const bool is_mandatory, bool &report_fail);
@@ -94,8 +94,8 @@ public:
 	};

 	static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
-				const vehicle_control_mode_s &control_mode,
-				const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status,
+				const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
+				const arm_requirements_t &arm_requirements, vehicle_status_s &status,
 				bool report_fail = true);

 private:
@@ -39,8 +39,8 @@
 #include <HealthFlags.h>

 bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
-				 const vehicle_control_mode_s &control_mode,
-				 const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
+				 const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
+				 const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
 {
 	bool prearm_ok = true;

@@ -156,10 +156,10 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 	}

 	// safety button
-	if (safety.safety_switch_available && !safety.safety_off) {
-		// Fail transition if we need safety switch press
+	if (safety_button_available && !safety_off) {
+		// Fail transition if we need safety button press
 		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first"); }
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety button first"); }
 		}

 		prearm_ok = false;
@@ -80,7 +80,6 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/power_button_state.h>
 #include <uORB/topics/rtl_time_estimate.h>
-#include <uORB/topics/safety.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -392,19 +391,18 @@ private:

 	geofence_result_s	_geofence_result{};
 	vehicle_land_detected_s	_vehicle_land_detected{};
-	safety_s		_safety{};
-	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_handler{};
+	Safety _safety{};

 	WorkerThread _worker_thread;

@@ -419,7 +417,6 @@ private:
 	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
-	uORB::Subscription					_safety_sub{ORB_ID(safety)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription					_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -445,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};

@@ -48,36 +48,35 @@ Safety::Safety()
 	_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
 }

-void Safety::safetyButtonHandler()
+bool Safety::safetyButtonHandler()
 {
 	if (_safety_disabled) {
-		_safety.safety_switch_available = true;
-		_safety.safety_off = true;
+		_button_available = true;
+		_safety_off = true;

 	} else {

-		if (!_safety.safety_switch_available && _safety_button_sub.advertised()) {
-			_safety.safety_switch_available = true;
+		if (!_button_available && _safety_button_sub.advertised()) {
+			_button_available = true;
 		}

 		button_event_s button_event;

 		while (_safety_button_sub.update(&button_event)) {
-			_safety.safety_off |= button_event.triggered; // triggered safety button activates safety off
+			_safety_off |= button_event.triggered; // triggered safety button activates safety off
 		}
 	}

-	// publish immediately on change, otherwise at 1 Hz for logging
-	if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s) ||
-	    (_safety.safety_off != _previous_safety_off)) {
-		_safety.timestamp = hrt_absolute_time();
-		_safety_pub.publish(_safety);
-	}
+	bool safety_changed = _previous_safety_off != _safety_off;

-	_previous_safety_off = _safety.safety_off;
+	_previous_safety_off = _safety_off;
+
+	return safety_changed;
 }

-void Safety::enableSafety()
+void Safety::activateSafety()
 {
-	_safety.safety_off = false;
+	if (!_safety_disabled) {
+		_safety_off = false;
+	}
 }
@@ -40,7 +40,6 @@
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/button_event.h>
-#include <uORB/topics/safety.h>

 class Safety
 {
@@ -50,15 +49,17 @@ public:
 	Safety();
 	~Safety() = default;

-	void safetyButtonHandler();
-	void enableSafety();
+	bool safetyButtonHandler();
+	void activateSafety();
+	bool isButtonAvailable() { return _button_available;}
+	bool isSafetyOff() { return _safety_off;}

 private:

 	uORB::Subscription _safety_button_sub{ORB_ID::safety_button};
-	uORB::Publication<safety_s>	_safety_pub{ORB_ID(safety)};

-	safety_s _safety{};
-	bool _safety_disabled{false};
-	bool _previous_safety_off{false};
+	bool _button_available{false};		//<! Set to true if a safety button is connected
+	bool _safety_off{false};			//<! Set to true if safety is off
+	bool _previous_safety_off{false};	//<! Previous safety value
+	bool _safety_disabled{false};		//<! Set to true if safety is disabled
 };
@@ -927,7 +927,7 @@ PARAM_DEFINE_INT32(COM_PREARM_MODE, 0);
 /**
 * Enable force safety
 *
- * Force safety when the vehicle disarms. Not supported when safety button used over PX4IO board.
+ * Force safety when the vehicle disarms
 *
 * @boolean
 * @group Commander
@@ -52,7 +52,6 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/actuator_test.h>
-#include <uORB/topics/safety.h>
 #include <parameters/param.h>

 using namespace time_literals;
@@ -94,15 +93,6 @@ static void set_motor_actuators(uORB::Publication<actuator_test_s> &publisher, f

 int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
 {
-	// check safety
-	uORB::SubscriptionData<safety_s> safety_sub{ORB_ID(safety)};
-	safety_sub.update();
-
-	if (safety_sub.get().safety_switch_available && !safety_sub.get().safety_off) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disable safety first");
-		return PX4_ERROR;
-	}
-
 	int	return_code = PX4_OK;
 	uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)};
 	// since we publish multiple at once, make sure the output driver subscribes before we publish
@@ -197,13 +187,6 @@ static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub)
 		goto Out;
 	}

-	/* tell IO to switch off safety without using the safety switch */
-	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
-		return_code = PX4_ERROR;
-		goto Out;
-	}
-
 	calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");

 	timeout_start = hrt_absolute_time();
@@ -243,9 +226,6 @@ static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub)
 Out:

 	if (fd != -1) {
-		if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
-			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
-		}

 		if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
 			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
@@ -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;
 };
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Benjamin Perseghetti	5e5a1e1b1f	make demo work with pca.	2022-06-01 18:22:23 -04:00
Benjamin Perseghetti	b965b38730	ucan pwm enablement.	2022-06-01 13:36:40 -04:00
Benjamin Perseghetti	d688400dfa	S32K1 fixes.	2022-05-31 16:56:49 -04:00
Benjamin	f3839cb90c	Cyphal param updates.	2022-05-26 10:52:08 -04:00
Benjamin Perseghetti	925a6808e0	NXP demo board config. -LH	2022-05-25 17:44:54 -04:00
Matthias Grob	c03f5b9481	Commander: fix overlooked rebase naming error	2022-05-25 21:10:19 +02:00
alessandro	29c4119e24	Match commander uORB var names to message names (#19707 ) * match vehicle_status * match home_position * match vehicle_command_ack * match actuator_armed * match vehicle_control_mode * match commander_state	2022-05-25 17:08:48 +02:00
Daniel Agar	98354ba10a	Update submodule GPSDrivers to latest Wed May 25 12:38:58 UTC 2022 - devices in PX4/Firmware (7cc84e0f9e44697475b0f355df9a29de6ea6b9ff): https://github.com/PX4/PX4-GPSDrivers/commit/58968922b718176be8756f11113d16b2cfbc4022 - devices current upstream: https://github.com/PX4/PX4-GPSDrivers/commit/181fae1a4b5e33576d786755782adb2f195ecc48 - Changes: https://github.com/PX4/PX4-GPSDrivers/compare/58968922b718176be8756f11113d16b2cfbc4022...181fae1a4b5e33576d786755782adb2f195ecc48 181fae1 2022-05-23 numan - README: add information for definition.h	2022-05-25 09:56:40 -04:00
Daniel Agar	58ea6235fe	ekf2: estimator aid source status (range height)	2022-05-25 09:25:12 -04:00
Daniel Agar	1e25aee6fa	ekf2: estimator aid source status (baro height)	2022-05-25 09:25:12 -04:00
Silvan Fuhrer	4dbe6f0a1c	simulator_mavlink: fix MAV_TYPE_VTOL_TAILSITTER case This type (23) doesn't specify a motor number, so it can't be properly handled. There are duo (19) and quad (20) tailsitter types that still work in simulation. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-24 17:58:44 -04:00
Daniel Agar	376201e64d	boards: px4_fmu-v5/v5x init all I2C pins	2022-05-24 11:02:03 -04:00
bresch	d04a91a3ae	ekf2_post-processing: use estimator_status_flags instead of bitmasks	2022-05-24 16:15:42 +02:00
Beat Küng	016b8aeb35	MulticopterRateControl: fix thrust sign for acro Regression from `fbc109436f`	2022-05-24 15:03:17 +02:00
Matthias Grob	44be415e0e	mission_block: make "Delay until" item work while landed To enable landing, waiting and taking off again e.g. for delivery.	2022-05-24 13:42:03 +02:00
Matthias Grob	0be474430c	Commander: don't disarm on landing amid a mission	2022-05-24 13:42:03 +02:00
Matthias Grob	4f34207c4e	battery_params: increase default empty cellvoltage to 3.6v Based on feedback that very often the battery is used down too low. I observed this happens consistently when the cell voltage is properly load compensated. The default load compensation before #19429 was very inaccurate and resulted in unpredictable estimate. After that if there is a usable current measurement and the battery is within expected tolerances of the default internal resistance the compensation is pretty good and 3.5V is too low for an empty compensated cell voltage. That was seen in various logs where the compensated cell voltage was already dropping fast after 3.6V. In case the voltage is not load compensated the vehicle estimates the state of charge a bit too low which is safer than to high especially for a default configuration.	2022-05-24 11:26:58 +02:00
Mark Sauder	a1fb9fb7c6	rcS: Keep Mag Cal with AUTOCONFIG param reset in rcS, deprecate rcS AUTOCNF param (#19693 ) * Deprecate the rcS AUTOCNF script param and associated logic. * Update posix rcS to match previous commit.	2022-05-23 20:35:22 -04:00
Daniel Agar	639222dd65	ekf2: estimator aid source status (GPS pos, GPS vel)	2022-05-23 16:21:49 -04:00
Daniel Agar	1ae467e9cd	ekf2: estimator aid source status (starting with fake position)	2022-05-23 16:21:49 -04:00
Igor Misic	79a34b5aed	px4iofirmware: fix for bug introduced with #19558	2022-05-23 14:59:11 -04:00
Daniel Agar	d25a784a3a	icm42688p: only check configured registers periodically (as intended)	2022-05-23 14:58:23 -04:00
bresch	c57c575cfe	ekf2: use explicit flags instead of bitmask position This prevents bitmask mismatch when a new flag is inserted	2022-05-23 14:43:02 -04:00
Matthias Grob	62edcc7a57	battery: only compensate using sane current	2022-05-23 16:40:32 +02:00
Matthias Grob	2cbdcc9671	battery: default to reasonable internal resistance instead of disabling the load compensation using current.	2022-05-23 16:40:32 +02:00
Matthias Grob	58b1139a21	battery: allow setting precise internal resistance with parameter	2022-05-23 16:40:32 +02:00
Matthias Grob	2c5a7ea118	battery: rather undercompensate for load than over compensate which makes the estimate to high and breaks batteries beause they get flown for too long.	2022-05-23 16:40:32 +02:00
Serhat Aksun	bc220ddb82	sensors/vehicle_magnetometer: fix multi_mode check Signed-off-by: Serhat Aksun <serhat.aksun@maxwell-innovations.com>	2022-05-23 10:09:41 -04:00
Igor Misic	12a81979a8	px4io: replace safety_off state with safety button event (#19558 ) internal PX4IO safety_off state is removed and replaced with a normal safety button event. From this 'commit' commander is taking care of the PX4IO safety.	2022-05-23 06:54:37 +02:00
Igor Misic	08dcc72e1f	commander/safety: replace safety.msg with Safety class (#19558 )	2022-05-23 06:54:37 +02:00