Fix heightrate feedforard for fixdwings

2026-06-11 00:00:05 +08:00 · 2023-03-27 13:13:24 +02:00
231 changed files with 2054 additions and 7227 deletions
@@ -105,6 +105,3 @@ src/modules/simulator/simulator_config.h
 src/systemcmds/topic_listener/listener_generated.cpp

 !src/drivers/distance_sensor/broadcom/afbrs50/Lib/*
-
-# colcon
-log/
@@ -137,6 +137,11 @@ add_custom_command(
 	COMMENT "ROMFS: copying, generating airframes"
 	)

+if(EXISTS ${PX4_BOARD_DIR}/extras/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin)
+	set(BOARD_FIRMWARE_BIN "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")
+	configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade @ONLY)
+endif()
+
 # copy extras into ROMFS
 set(extras_dependencies)

@@ -203,24 +208,6 @@ endforeach()
 set(OPTIONAL_BOARD_EXTRAS)
 file(GLOB OPTIONAL_BOARD_EXTRAS ${PX4_BOARD_DIR}/extras/*)

-# bootloader (optional)
-#  - if systemcmds/bl_update included and board bootloader available then generate rc.board_bootloader_upgrade and copy bootloader binary
-#  - otherwise remove bootloader binary from extras in final ROMFS
-foreach(board_extra_file ${OPTIONAL_BOARD_EXTRAS})
-	file(RELATIVE_PATH extra_file_base_name ${PX4_BOARD_DIR}/extras/ ${board_extra_file})
-	if(${extra_file_base_name} MATCHES "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")
-		if(CONFIG_SYSTEMCMDS_BL_UPDATE)
-			# generate rc.board_bootloader_upgrade
-			set(BOARD_FIRMWARE_BIN "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")
-			configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade @ONLY)
-		else()
-			# remove bootloader from extras
-			list(REMOVE_ITEM OPTIONAL_BOARD_EXTRAS ${board_extra_file})
-		endif()
-
-	endif()
-endforeach()
-
 foreach(board_extra_file ${OPTIONAL_BOARD_EXTRAS})

 	if(EXISTS "${board_extra_file}")
@@ -11,8 +11,6 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}

-param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
-
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
@@ -9,8 +9,6 @@

 param set-default MAV_TYPE 20

-param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
-
 param set-default CA_AIRFRAME 4

 param set-default CA_ROTOR_COUNT 4
@@ -17,8 +17,6 @@

 . ${R}etc/init.d/rc.vtol_defaults

-param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
-
 param set UAVCAN_ENABLE 0
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
@@ -12,8 +12,6 @@

 . ${R}etc/init.d/rc.vtol_defaults

-param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
-
 param set-default CA_AIRFRAME 4
 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_KM -0.05
@@ -7,7 +7,7 @@ import os
 class JsonOutput():
    def __init__(self, groups):
        all_json = {}
-        all_json['version'] = 2
+        all_json['version'] = 1
        component = {}
        all_json['components'] = {1: component} #1: autopilot component

@@ -35,20 +35,7 @@ function spawn_model() {
 	pushd "$working_dir" &>/dev/null
 	echo "starting instance $N in $(pwd)"
 	$build_path/bin/px4 -i $N -d "$build_path/etc" >out.log 2>err.log &
-
-	set --
-	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/scripts/jinja_gen.py
-	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/${MODEL}/${MODEL}.sdf.jinja
-	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic
-	set -- ${@} --mavlink_tcp_port $((4560+${N}))
-	set -- ${@} --mavlink_udp_port $((14560+${N}))
-	set -- ${@} --mavlink_id $((1+${N}))
-	set -- ${@} --gst_udp_port $((5600+${N}))
-	set -- ${@} --video_uri $((5600+${N}))
-	set -- ${@} --mavlink_cam_udp_port $((14530+${N}))
-	set -- ${@} --output-file /tmp/${MODEL}_${N}.sdf
-
-	python3 ${@}
+	python3 ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/scripts/jinja_gen.py ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_port $((5600+${N})) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --output-file /tmp/${MODEL}_${N}.sdf

 	echo "Spawning ${MODEL}_${N} at ${X} ${Y}"

@@ -110,6 +110,10 @@ class PubSub(object):

            log.debug("          ####:{}:  {}, {}".format( self._name, route_group, topic_group))

+            # # TODO: handle this case... but not sure where, yet
+            # if match == 'ORB_ID_VEHICLE_ATTITUDE_CONTROLS': # special case
+            #     match = orb_id+orb_id_vehicle_attitude_controls_topic
+
            # match has the form: '[ORB_ID(]<topic_name>'
            if route_group:
                if route_group == 'ORB_ID':
@@ -228,6 +232,9 @@ class Graph(object):

        self._topic_blacklist = set(kwargs.get('topic_blacklist',set()))

+        self._orb_id_vehicle_attitude_controls_topic = 'actuator_controls_0'
+        self._orb_id_vehicle_attitude_controls_re = re.compile(r'\#define\s+ORB_ID_VEHICLE_ATTITUDE_CONTROLS\s+([^,)]+)')
+
        self._warnings = [] # list of all ambiguous scan sites

        self._current_scope = [] # stack with current module (they can be nested)
@@ -503,7 +510,18 @@ class Graph(object):
                elif current_scope.name == 'uorb_tests': # skip this
                    return
                elif current_scope.name == 'uorb':
-                    return # skip this
+
+                    # search and validate the ORB_ID_VEHICLE_ATTITUDE_CONTROLS define
+                    matches = self._orb_id_vehicle_attitude_controls_re.findall(content)
+                    for match in matches:
+                        if match != 'ORB_ID('+self._orb_id_vehicle_attitude_controls_topic:
+                            # if we land here, you need to change _orb_id_vehicle_attitude_controls_topic
+                            raise Exception(
+                                'The extracted define for ORB_ID_VEHICLE_ATTITUDE_CONTROLS '
+                                'is '+match+' but expected ORB_ID('+
+                                self._orb_id_vehicle_attitude_controls_topic)
+
+                    return # skip uorb module for the rest

            line_number = 0
            for full_line in content.splitlines():
@@ -13,8 +13,6 @@ CONFIG_DRIVERS_OPTICAL_FLOW_PAA3905=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
-CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
@@ -13,13 +13,6 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_BEEP_COMMAND=y
-CONFIG_UAVCANNODE_GNSS_FIX=y
-CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
-CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
-CONFIG_UAVCANNODE_SAFETY_BUTTON=y
-CONFIG_UAVCANNODE_STATIC_PRESSURE=y
-CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -13,14 +13,6 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_BEEP_COMMAND=y
-CONFIG_UAVCANNODE_GNSS_FIX=y
-CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
-CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
-CONFIG_UAVCANNODE_RTK_DATA=y
-CONFIG_UAVCANNODE_SAFETY_BUTTON=y
-CONFIG_UAVCANNODE_STATIC_PRESSURE=y
-CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -9,7 +9,6 @@ CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
-CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_COMMON_HYGROMETERS=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
@@ -18,28 +17,10 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
-CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_ARMING_STATUS=y
-CONFIG_UAVCANNODE_BEEP_COMMAND=y
-CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y
-CONFIG_UAVCANNODE_ESC_STATUS=y
-CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
-CONFIG_UAVCANNODE_GNSS_FIX=y
-CONFIG_UAVCANNODE_HYGROMETER_MEASUREMENT=y
-CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
-CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
-CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
-CONFIG_UAVCANNODE_RAW_AIR_DATA=y
-CONFIG_UAVCANNODE_RTK_DATA=y
-CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y
-CONFIG_UAVCANNODE_STATIC_PRESSURE=y
-CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
-CONFIG_MODULES_SENSORS=y
-CONFIG_NSH_BUILTIN_APPS=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -2,7 +2,6 @@
 #
 # board specific defaults
 #------------------------------------------------------------------------------
-
 pwm_out start

-dshot start
+control_allocator start
@@ -67,6 +67,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -15,8 +15,6 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_GNSS_YAW is not set
-# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -14,8 +14,6 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_GNSS_YAW is not set
-# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -14,13 +14,6 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_BEEP_COMMAND=y
-CONFIG_UAVCANNODE_GNSS_FIX=y
-CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
-CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
-CONFIG_UAVCANNODE_SAFETY_BUTTON=y
-CONFIG_UAVCANNODE_STATIC_PRESSURE=y
-CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
@@ -30,6 +30,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -75,6 +76,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -31,6 +31,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -76,6 +77,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -9,5 +9,4 @@ CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
@@ -69,6 +69,7 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -6,7 +6,6 @@ CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
@@ -69,6 +69,7 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -21,13 +21,9 @@ then
 fi

 # SPI1, body-fixed
-if icm45686 -s -b 1 -R 3 -q start
+if ! icm45686 -s -b 1 -R 3 -q start
 then
-	# if we detected the ICM45686 we also ought to have an AK09918
-	ak09916 start -I -R 13
-else
 	icm20649 -s -b 1 start
-
 fi

 ms5611 -s -b 1 start
@@ -187,7 +187,6 @@
 #define STM32_RCC_D1CCIPR_SDMMCSEL   RCC_D1CCIPR_SDMMC_PLL1

 #define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI  /* I2C123 clock source */
-#define STM32_RCC_D3CCIPR_I2C4SRC    RCC_D3CCIPR_I2C4SEL_HSI
 #define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */
 #define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2  /* SPI45 clock source */
 #define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3    /* USB 1 and 2 clock source */
@@ -265,6 +264,3 @@

 #define GPIO_I2C2_SCL GPIO_I2C2_SCL_1       /* PB10 */
 #define GPIO_I2C2_SDA GPIO_I2C2_SDA_1       /* PB11 */
-
-#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2        /* PF14 */
-#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2        /* PF15 */
@@ -189,7 +189,6 @@ CONFIG_STM32H7_DMACAPABLE=y
 CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
 CONFIG_STM32H7_I2C1=y
 CONFIG_STM32H7_I2C2=y
-CONFIG_STM32H7_I2C4=y
 CONFIG_STM32H7_I2C_DYNTIMEO=y
 CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
 CONFIG_STM32H7_OTGFS=y
@@ -189,7 +189,6 @@ CONFIG_STM32H7_DMACAPABLE=y
 CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
 CONFIG_STM32H7_I2C1=y
 CONFIG_STM32H7_I2C2=y
-CONFIG_STM32H7_I2C4=y
 CONFIG_STM32H7_I2C_DYNTIMEO=y
 CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
 CONFIG_STM32H7_OTGFS=y
@@ -180,8 +180,6 @@
 		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_I2C4_SCL), \
-		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SDA), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D0), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D1), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D2), \
@@ -36,5 +36,4 @@
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(2),
 	initI2CBusExternal(1),
-	initI2CBusInternal(4),
 };
@@ -3,9 +3,9 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_UAVCAN=n
+CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
@@ -11,14 +11,6 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_BEEP_COMMAND=y
-CONFIG_UAVCANNODE_GNSS_FIX=y
-CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
-CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
-CONFIG_UAVCANNODE_RTK_DATA=y
-CONFIG_UAVCANNODE_SAFETY_BUTTON=y
-CONFIG_UAVCANNODE_STATIC_PRESSURE=y
-CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -13,13 +13,6 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_BEEP_COMMAND=y
-CONFIG_UAVCANNODE_GNSS_FIX=y
-CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
-CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
-CONFIG_UAVCANNODE_SAFETY_BUTTON=y
-CONFIG_UAVCANNODE_STATIC_PRESSURE=y
-CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -27,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -68,6 +69,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -22,6 +22,7 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -65,6 +66,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -21,6 +21,7 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -63,6 +64,7 @@ CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -22,6 +22,7 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -65,6 +66,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -12,14 +12,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_BEEP_COMMAND=y
-CONFIG_UAVCANNODE_GNSS_FIX=y
-CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
-CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
-CONFIG_UAVCANNODE_RTK_DATA=y
-CONFIG_UAVCANNODE_SAFETY_BUTTON=y
-CONFIG_UAVCANNODE_STATIC_PRESSURE=y
-CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -29,6 +29,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
@@ -70,6 +71,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -25,6 +25,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -71,6 +72,7 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -25,6 +25,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -69,6 +70,7 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -25,6 +25,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -70,6 +71,7 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -25,6 +25,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -69,6 +70,7 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -13,21 +13,6 @@ CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_UAVCANNODE_ARMING_STATUS=y
-CONFIG_UAVCANNODE_BEEP_COMMAND=y
-CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y
-CONFIG_UAVCANNODE_ESC_STATUS=y
-CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
-CONFIG_UAVCANNODE_GNSS_FIX=y
-CONFIG_UAVCANNODE_HYGROMETER_MEASUREMENT=y
-CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
-CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
-CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
-CONFIG_UAVCANNODE_RAW_AIR_DATA=y
-CONFIG_UAVCANNODE_RTK_DATA=y
-CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y
-CONFIG_UAVCANNODE_STATIC_PRESSURE=y
-CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -23,18 +23,13 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_AUXVEL is not set
-# CONFIG_EKF2_BARO_COMPENSATION is not set
-# CONFIG_EKF2_DRAG_FUSION is not set
-# CONFIG_EKF2_EXTERNAL_VISION is not set
-# CONFIG_EKF2_GNSS_YAW is not set
-# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
@@ -39,6 +39,7 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -68,6 +68,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -141,13 +141,12 @@
 #define GPIO_HW_VER_SENSE      /* PC1 */  GPIO_ADC123_INP11
 #define HW_INFO_INIT_PREFIX    "V6C"

-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0, 10 and Mini Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
 //                 Base/FMUM
 #define V6C00   HW_VER_REV(0x0,0x0) // FMUV6C,                 Rev 0  I2C4 External but with Internal devices
 #define V6C01   HW_VER_REV(0x0,0x1) // FMUV6C,                 Rev 1  I2C4 Internal I2C2 External
 #define V6C10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0  I2C4 External but with Internal devices
 #define V6C11   HW_VER_REV(0x1,0x1) // NO PX4IO,               Rev 1  I2C4 Internal I2C2 External
-#define V6C21   HW_VER_REV(0x2,0x1) // FMUV6CMini,             Rev 1  I2C4 Internal I2C2 External


 /* HEATER
@@ -79,11 +79,6 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
 		.mandatory   = 1,
 		.connection  = px4_hw_con_onboard,
 	},
-	{
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
 };

 static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
@@ -106,7 +101,6 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6C01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1
 	{V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 0 No PX4IO
 	{V6C11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 1 No PX4IO
-	{V6C21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1 MINI
 };

 /************************************************************************************
@@ -46,17 +46,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
 		}),
 	}),
-	initSPIHWVersion(V6C01, {
-		initSPIBus(SPI::Bus::SPI1, {
-			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
-			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
-			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
-		}, {GPIO::PortB, GPIO::Pin2}),
-		initSPIBus(SPI::Bus::SPI2, {
-			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
-		}),
-	}),
-	initSPIHWVersion(V6C21, {
+	initSPIHWVersion(V6C10, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
 			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
@@ -28,6 +28,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -72,6 +73,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -73,6 +73,7 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -4,7 +4,7 @@ project(googletest-download NONE)

 include(ExternalProject)
 ExternalProject_Add(googletest
-	URL https://github.com/google/googletest/archive/b796f7d44681514f58a683a3a71ff17c94edb0c1.zip # 1.13
+	URL https://github.com/google/googletest/archive/58d77fa8070e8cec2dc1ed015d66b454c8d78850.zip # 1.12.1
 	SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
 	BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
 	CONFIGURE_COMMAND ""
@@ -46,7 +46,4 @@ add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src ${CMAKE_CURRENT_BINA
 get_target_property(GTEST_COMPILE_FLAGS gtest COMPILE_OPTIONS)
 list(REMOVE_ITEM GTEST_COMPILE_FLAGS "-include")
 list(REMOVE_ITEM GTEST_COMPILE_FLAGS "visibility.h")
-# Remove float warnings added by PX4 which trigger in gtest-printers.h
-list(REMOVE_ITEM GTEST_COMPILE_FLAGS "-Wdouble-promotion")
-list(REMOVE_ITEM GTEST_COMPILE_FLAGS "-Wfloat-equal")
 set_target_properties(gtest PROPERTIES COMPILE_OPTIONS "${GTEST_COMPILE_FLAGS}")
@@ -0,0 +1,19 @@
+uint64 timestamp			# time since system start (microseconds)
+uint8 NUM_ACTUATOR_CONTROLS = 9
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint8 INDEX_ROLL = 0
+uint8 INDEX_PITCH = 1
+uint8 INDEX_YAW = 2
+uint8 INDEX_THROTTLE = 3
+uint8 INDEX_GIMBAL_SHUTTER = 3
+uint8 INDEX_CAMERA_ZOOM = 4
+
+uint8 GROUP_INDEX_ATTITUDE = 0
+uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
+uint8 GROUP_INDEX_GIMBAL = 2
+
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[9] control
+
+# TOPICS actuator_controls_0 actuator_controls_1 actuator_controls_2
+# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc
@@ -1,5 +1,10 @@
 uint64 timestamp			# time since system start (microseconds)

-float32[3] control_power
+uint8 INDEX_ROLL = 0
+uint8 INDEX_PITCH = 1
+uint8 INDEX_YAW = 2
+uint8 INDEX_THROTTLE = 3
+
+float32[4] control_power

 # TOPICS actuator_controls_status_0 actuator_controls_status_1
@@ -39,6 +39,7 @@ include(px4_list_make_absolute)
 set(msg_files
 	ActionRequest.msg
 	ActuatorArmed.msg
+	ActuatorControls.msg
 	ActuatorControlsStatus.msg
 	ActuatorMotors.msg
 	ActuatorOutputs.msg
@@ -99,7 +100,6 @@ set(msg_files
 	GimbalManagerStatus.msg
 	GpsDump.msg
 	GpsInjectData.msg
-	GimbalControls.msg
 	Gripper.msg
 	HealthReport.msg
 	HeaterStatus.msg
@@ -1,9 +0,0 @@
-uint64 timestamp			# time since system start (microseconds)
-uint8 INDEX_ROLL = 0
-uint8 INDEX_PITCH = 1
-uint8 INDEX_YAW = 2
-
-uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[3] control
-
-float32 retract_gimbal
@@ -30,12 +30,6 @@ uint8 JAMMING_STATE_CRITICAL = 3
 uint8 jamming_state		# indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
 int32 jamming_indicator		# indicates jamming is occurring

-uint8 SPOOFING_STATE_UNKNOWN   = 0
-uint8 SPOOFING_STATE_NONE      = 1
-uint8 SPOOFING_STATE_INDICATED = 2
-uint8 SPOOFING_STATE_MULTIPLE  = 3
-uint8 spoofing_state		# indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
-
 float32 vel_m_s			# GPS ground speed, (metres/sec)
 float32 vel_n_m_s		# GPS North velocity, (metres/sec)
 float32 vel_e_m_s		# GPS East velocity, (metres/sec)
@@ -36,6 +36,7 @@ uint16 VEHICLE_CMD_DO_SET_HOME = 179			# Changes the home location either to the
 uint16 VEHICLE_CMD_DO_SET_PARAMETER = 180		# Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_SET_RELAY = 181			# Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_REPEAT_RELAY = 182		# Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|
+uint16 VEHICLE_CMD_DO_SET_SERVO = 183			# Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184		# Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185		# Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_CHANGE_ALTITUDE = 186		# Set the vehicle to Loiter mode and change the altitude to specified value |Altitude| Frame of new altitude | Empty| Empty| Empty| Empty| Empty|
@@ -117,6 +118,11 @@ uint8 VEHICLE_ROI_LOCATION = 3                    	# Point toward fixed location
 uint8 VEHICLE_ROI_TARGET = 4                       	# Point toward target
 uint8 VEHICLE_ROI_ENUM_END = 5

+uint8 VEHICLE_CAMERA_ZOOM_TYPE_STEP = 0            	# Zoom one step increment
+uint8 VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS = 1      	# Continuous zoom up/down until stopped
+uint8 VEHICLE_CAMERA_ZOOM_TYPE_RANGE = 2           	# Zoom value as proportion of full camera range
+uint8 VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH = 3    	# Zoom to a focal length
+
 uint8 PARACHUTE_ACTION_DISABLE = 0
 uint8 PARACHUTE_ACTION_ENABLE = 1
 uint8 PARACHUTE_ACTION_RELEASE = 2
@@ -3,6 +3,3 @@ uint64 timestamp        # time since system start (microseconds)
 uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)

 float32[3] xyz          # thrust setpoint along X, Y, Z body axis [-1, 1]
-
-# TOPICS vehicle_thrust_setpoint
-# TOPICS vehicle_thrust_setpoint_virtual_fw vehicle_thrust_setpoint_virtual_mc
@@ -3,6 +3,3 @@ uint64 timestamp        # time since system start (microseconds)
 uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)

 float32[3] xyz          # torque setpoint about X, Y, Z body axis (normalized)
-
-# TOPICS vehicle_torque_setpoint
-# TOPICS vehicle_torque_setpoint_virtual_fw vehicle_torque_setpoint_virtual_mc
@@ -93,13 +93,6 @@ bool keystore_put_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *ke

 void crypto_init(void);

-/*
- * De-initialize hw level crypto
- * This may be called to shut down hw level crypto
- */
-
-void crypto_deinit(void);
-
 /*
 * Open a session for performing crypto functions
 * algorithm: The crypto algorithm to be used in this session
@@ -251,6 +251,7 @@ void orb_print_message_internal(const struct orb_metadata *meta, const void *dat
 __END_DECLS

 /* Diverse uORB header defines */ //XXX: move to better location
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
 typedef uint8_t arming_state_t;
 typedef uint8_t main_state_t;
 typedef uint8_t hil_state_t;
@@ -392,10 +392,6 @@ jump_to_app()

 #endif

-#ifdef BOOTLOADER_USE_SECURITY
-	crypto_deinit();
-#endif
-
 	/* just for paranoia's sake */
 	arch_flash_lock();

@@ -236,7 +236,7 @@ LeddarOne::open_serial_port(const speed_t speed)
 	uart_config.c_cflag |= (CS8 | CREAD | CLOCAL);

 	// Clear: echo, echo new line, canonical input and extended input.
-	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN);
+	uart_config.c_lflag &= (ECHO | ECHONL | ICANON | IEXTEN);

 	// Clear ONLCR flag (which appends a CR for every LF).
 	uart_config.c_oflag &= ~ONLCR;
@@ -37,7 +37,7 @@
 #include <inttypes.h>
 #include <fcntl.h>
 #include <termios.h>
-#include <lib/crc/crc.h>
+#include <lib/systemlib/crc.h>

 #include <float.h>
 #include <mathlib/mathlib.h>
@@ -40,7 +40,6 @@ px4_add_module(
 		-Wno-stringop-overflow # due to https://gcc.gnu.org/bugzilla/show_bug.cgi?id=91707
 	SRCS
 		gps.cpp
-		devices/src/crc.cpp
 		devices/src/gps_helper.cpp
 		devices/src/mtk.cpp
 		devices/src/ashtech.cpp
@@ -49,8 +48,6 @@ px4_add_module(
 		devices/src/emlid_reach.cpp
 		devices/src/femtomes.cpp
 		devices/src/nmea.cpp
-		devices/src/unicore.cpp
-		devices/src/crc.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
@@ -182,8 +182,6 @@ private:

 	sensor_gps_s			_report_gps_pos{};				///< uORB topic for gps position
 	satellite_info_s		*_p_report_sat_info{nullptr};			///< pointer to uORB topic for satellite info
-	uint8_t                         _spoofing_state{0};                             ///< spoofing state
-	uint8_t                         _jamming_state{0};                              ///< jamming state

 	uORB::PublicationMulti<sensor_gps_s>	_report_gps_pos_pub{ORB_ID(sensor_gps)};	///< uORB pub for gps position
 	uORB::PublicationMulti<sensor_gnss_relative_s> _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)};
@@ -1196,25 +1194,6 @@ GPS::publish()
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
 		_report_gps_pos.heading = NAN;
 		_is_gps_main_advertised.store(true);
-
-		if (_report_gps_pos.spoofing_state != _spoofing_state) {
-
-			if (_report_gps_pos.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) {
-				PX4_WARN("GPS spoofing detected! (state: %d)", _report_gps_pos.spoofing_state);
-			}
-
-			_spoofing_state = _report_gps_pos.spoofing_state;
-		}
-
-		if (_report_gps_pos.jamming_state != _jamming_state) {
-
-			if (_report_gps_pos.jamming_state > sensor_gps_s::JAMMING_STATE_WARNING) {
-				PX4_WARN("GPS jamming detected! (state: %d) (indicator: %d)", _report_gps_pos.jamming_state,
-					 (uint8_t)_report_gps_pos.jamming_indicator);
-			}
-
-			_jamming_state = _report_gps_pos.jamming_state;
-		}
 	}
 }

@@ -147,16 +147,10 @@ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0);
 *
 * Heading offset angle for dual antenna GPS setups that support heading estimation.
 *
- * Set this to 0 if the antennas are parallel to the forward-facing direction
- * of the vehicle and the rover (or Unicore primary) antenna is in front.
+ * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the rover antenna is in
+ * front. The offset angle increases clockwise.
 *
- * The offset angle increases clockwise.
- *
- * Set this to 90 if the rover (or Unicore primary) antenna is placed on the
- * right side of the vehicle and the moving base antenna is on the left side.
- *
- * (Note: the Unicore primary antenna is the one connected on the right as seen
- *        from the top).
+ * Set this to 90 if the rover antenna is placed on the right side of the vehicle and the moving base antenna is on the left side.
 *
 * @min 0
 * @max 360
@@ -78,7 +78,6 @@ bool AK09916::Reset()
 void AK09916::print_status()
 {
 	I2CSPIDriverBase::print_status();
-	PX4_INFO("Variant: %s", _is_ak09918 ? "AK09918" : "AK09916");

 	perf_print_counter(_reset_perf);
 	perf_print_counter(_bad_register_perf);
@@ -95,21 +94,15 @@ int AK09916::probe()
 		const uint8_t WIA2 = RegisterRead(Register::WIA2);

 		if ((WIA1 == Company_ID) && (WIA2 == Device_ID)) {
-			_is_ak09918 = false;
-			return PX4_OK;
-		}
-
-		if ((WIA1 == Company_ID) && (WIA2 == Device_ID_AK09918)) {
-			_is_ak09918 = true;
 			return PX4_OK;
 		}

 		if (WIA1 != Company_ID) {
-			PX4_DEBUG("unexpected WIA1 0x%02x", WIA1);
+			DEVICE_DEBUG("unexpected WIA1 0x%02x", WIA1);
 		}

-		if (WIA2 != Device_ID && WIA2 != Device_ID_AK09918) {
-			PX4_DEBUG("unexpected WIA2 0x%02x", WIA2);
+		if (WIA2 != Device_ID) {
+			DEVICE_DEBUG("unexpected WIA2 0x%02x", WIA2);
 		}
 	}

@@ -131,29 +124,26 @@ void AK09916::RunImpl()
 		ScheduleDelayed(100_ms);
 		break;

-	case STATE::WAIT_FOR_RESET: {
-			uint8_t device_id = _is_ak09918 ? Device_ID_AK09918 : Device_ID;
+	case STATE::WAIT_FOR_RESET:
+		if ((RegisterRead(Register::WIA1) == Company_ID) && (RegisterRead(Register::WIA2) == Device_ID)) {
+			// if reset succeeded then configure
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(100_ms);

-			if ((RegisterRead(Register::WIA1) == Company_ID) && (RegisterRead(Register::WIA2) == device_id)) {
-				// if reset succeeded then configure
-				_state = STATE::CONFIGURE;
-				ScheduleDelayed(100_ms);
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 30_s) {
+				PX4_ERR("Reset failed, retrying");
+				Reset();

 			} else {
-				// RESET not complete
-				if (hrt_elapsed_time(&_reset_timestamp) > 30_s) {
-					PX4_ERR("Reset failed, retrying");
-					Reset();
-
-				} else {
-					PX4_DEBUG("Reset not complete, check again in 100 ms");
-					ScheduleDelayed(100_ms);
-				}
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
-
-			break;
 		}

+		break;
+
 	case STATE::CONFIGURE:
 		if (Configure()) {
 			// if configure succeeded then start reading
@@ -106,6 +106,4 @@ private:
 		// Register          | Set bits, Clear bits
 		{ Register::CNTL2,   CNTL2_BIT::MODE3_SET, CNTL2_BIT::MODE3_CLEAR },
 	};
-
-	bool _is_ak09918 {false};
 };
@@ -60,7 +60,6 @@ static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001100;

 static constexpr uint8_t Company_ID = 0x48;
 static constexpr uint8_t Device_ID = 0x09;
-static constexpr uint8_t Device_ID_AK09918 = 0x0C;

 enum class Register : uint8_t {
 	WIA1  = 0x00, // Company ID of AKM
@@ -68,6 +68,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/vehicle_command.h>
@@ -59,10 +59,6 @@ extern void libtomcrypt_init(void);
 #define SECMEM_FREE XFREE
 #endif

-#define SHA256_HASHLEN 32
-#define OAEP_MAX_RSA_MODLEN 256  /* RSA2048 */
-#define OAEP_MAX_MSGLEN (OAEP_MAX_RSA_MODLEN - 2 * SHA256_HASHLEN - 2)
-
 /*
 * For now, this is just a dummy up/down counter for tracking open/close calls
 */
@@ -155,10 +151,6 @@ void crypto_init()
 	clear_key_cache();
 }

-void crypto_deinit()
-{
-}
-
 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
 {
 	crypto_session_handle_t ret;
@@ -386,22 +378,12 @@ bool crypto_get_encrypted_key(crypto_session_handle_t handle,
 					  max_len);

 	} else {
-		switch (handle.algorithm) {
+		// The key size, encrypted, is a multiple of minimum block size for the algorithm+key
+		size_t min_block = crypto_get_min_blocksize(handle, encryption_key_idx);
+		*max_len = key_sz / min_block * min_block;

-		case CRYPTO_RSA_OAEP:
-			/* The length is the RSA key modulus length, and the maximum plaintext
-			 * length is calculated from that. This is now just fixed for RSA2048,
-			 * but one could also parse the RSA key
-			 * (encryption_key_idx) here and calculate the lengths.
-			 */
-
-			*max_len = key_sz <= OAEP_MAX_MSGLEN ?  OAEP_MAX_RSA_MODLEN : 0;
-			ret = true;
-			break;
-
-		default:
-			*max_len = 0;
-			break;
+		if (key_sz % min_block) {
+			*max_len += min_block;
 		}
 	}

@@ -441,6 +423,24 @@ size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx)
 		ret = 64;
 		break;

+	case CRYPTO_RSA_OAEP: {
+			rsa_key enc_key;
+			size_t pub_key_sz;
+			uint8_t *pub_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &pub_key_sz);
+
+			initialize_tomcrypt();
+
+			if (pub_key &&
+			    rsa_import(pub_key, pub_key_sz, &enc_key) == CRYPT_OK) {
+				ret = ltc_mp.unsigned_size(enc_key.N);
+				rsa_free(&enc_key);
+
+			} else {
+				ret = 0;
+			}
+		}
+		break;
+
 	default:
 		ret = 1;
 	}
@@ -142,8 +142,6 @@ px4_add_module(
 		${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include
 		${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include
 	SRCS
-		arming_status.cpp
-		arming_status.hpp
 		beep.cpp
 		beep.hpp
 		rgbled.cpp
@@ -50,7 +50,7 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_uavcan_pub_raw_cmd(node),
 	_uavcan_sub_status(node)
 {
-	_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin); // Highest priority
+	_uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
 }

 int
@@ -91,6 +91,8 @@ private:
 	 */
 	uint8_t check_escs_status();

+	static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5;	///< 0..31, inclusive, 0 - highest, 31 - lowest
+
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;

@@ -1,81 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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
- * 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 arming_status.cpp
- *
- * @author Alex Klimaj <alex@arkelectron.com>
- */
-
-#include "arming_status.hpp"
-
-UavcanArmingStatus::UavcanArmingStatus(uavcan::INode &node) :
-	_arming_status_pub(node),
-	_timer(node)
-{
-	_arming_status_pub.setPriority(uavcan::TransferPriority::Default);
-}
-
-int UavcanArmingStatus::init()
-{
-	/*
-	 * Setup timer and call back function for periodic updates
-	 */
-	if (!_timer.isRunning()) {
-		_timer.setCallback(TimerCbBinder(this, &UavcanArmingStatus::periodic_update));
-		_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
-	}
-
-	return 0;
-}
-
-void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &)
-{
-	actuator_armed_s actuator_armed;
-
-	if (_actuator_armed_sub.update(&actuator_armed)) {
-		uavcan::equipment::safety::ArmingStatus cmd;
-
-		if (actuator_armed.lockdown || actuator_armed.manual_lockdown) {
-			cmd.status = cmd.STATUS_DISARMED;
-
-		} else if (actuator_armed.armed || actuator_armed.prearmed) {
-			cmd.status = cmd.STATUS_FULLY_ARMED;
-
-		} else {
-			cmd.status = cmd.STATUS_DISARMED;
-		}
-
-		(void)_arming_status_pub.broadcast(cmd);
-	}
-}
@@ -1,83 +0,0 @@
-/****************************************************************************
-*
-*   Copyright (c) 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
-* 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 arming_status.hpp
- *
- * @author Alex Klimaj <alex@arkelectron.com>
- *
- * @brief Publish actuator_armed to CAN ArmingStatus message
- */
-
-#pragma once
-
-#include <uavcan/uavcan.hpp>
-#include <uavcan/equipment/safety/ArmingStatus.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/actuator_armed.h>
-
-
-class UavcanArmingStatus
-{
-public:
-	UavcanArmingStatus(uavcan::INode &node);
-
-	/*
-	 * setup periodic updater
-	 */
-	int init();
-
-private:
-	/*
-	 * Max update rate to avoid exessive bus traffic
-	 */
-	static constexpr unsigned MAX_RATE_HZ = 10;
-
-	/*
-	 * Setup timer and call back function for periodic updates
-	 */
-	void periodic_update(const uavcan::TimerEvent &);
-
-	typedef uavcan::MethodBinder<UavcanArmingStatus *, void (UavcanArmingStatus::*)(const uavcan::TimerEvent &)>
-	TimerCbBinder;
-
-	/*
-	 * Publish CAN ArmingState
-	 */
-	uavcan::Publisher<uavcan::equipment::safety::ArmingStatus> _arming_status_pub;
-
-	uavcan::TimerEventForwarder<TimerCbBinder> _timer;
-
-	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
-
-};
@@ -80,7 +80,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
 	ModuleParams(nullptr),
 	_node(can_driver, system_clock, _pool_allocator),
-	_arming_status_controller(_node),
 	_beep_controller(_node),
 	_esc_controller(_node),
 	_servo_controller(_node),
@@ -479,21 +478,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)

 	fill_node_info();

-	int ret;
-
-	// UAVCAN_PUB_ARM
-	int32_t uavcan_pub_arm = 0;
-	param_get(param_find("UAVCAN_PUB_ARM"), &uavcan_pub_arm);
-
-	if (uavcan_pub_arm == 1) {
-		ret = _arming_status_controller.init();
-
-		if (ret < 0) {
-			return ret;
-		}
-	}
-
-	ret = _beep_controller.init();
+	int ret = _beep_controller.init();

 	if (ret < 0) {
 		return ret;
@@ -50,7 +50,6 @@
 #include "actuators/hardpoint.hpp"
 #include "actuators/servo.hpp"
 #include "allocator.hpp"
-#include "arming_status.hpp"
 #include "beep.hpp"
 #include "logmessage.hpp"
 #include "rgbled.hpp"
@@ -225,7 +224,6 @@ private:
 	uavcan::Node<>			_node;				///< library instance
 	pthread_mutex_t			_node_mutex;

-	UavcanArmingStatus		_arming_status_controller;
 	UavcanBeepController		_beep_controller;
 	UavcanEscController		_esc_controller;
 	UavcanServoController		_servo_controller;
@@ -185,18 +185,6 @@ PARAM_DEFINE_INT32(UAVCAN_LGT_NAV, 3);
 */
 PARAM_DEFINE_INT32(UAVCAN_LGT_LAND, 0);

-/**
- * publish Arming Status stream
- *
- * Enable UAVCAN Arming Status stream publication
- *  uavcan::equipment::safety::ArmingStatus
- *
- * @boolean
- * @reboot_required true
- * @group UAVCAN
- */
-PARAM_DEFINE_INT32(UAVCAN_PUB_ARM, 0);
-
 /**
 * publish RTCM stream
 *
@@ -2,75 +2,4 @@ menuconfig DRIVERS_UAVCANNODE
 	bool "uavcannode"
 	default n
 	---help---
-		Enable support for uavcannode
-
-if DRIVERS_UAVCANNODE
-    config UAVCANNODE_ARMING_STATUS
-        bool "Include arming status"
-        default n
-
-    config UAVCANNODE_BATTERY_INFO
-        bool "Include battery info"
-        default n
-
-    config UAVCANNODE_BEEP_COMMAND
-        bool "Include beep command"
-        default n
-
-    config UAVCANNODE_ESC_RAW_COMMAND
-        bool "Include ESC raw command"
-        default n
-
-    config UAVCANNODE_ESC_STATUS
-        bool "Include ESC status"
-        default n
-
-    config UAVCANNODE_FLOW_MEASUREMENT
-        bool "Include flow measurement"
-        default n
-
-    config UAVCANNODE_GNSS_FIX
-        bool "Include GNSS fix"
-        default n
-
-    config UAVCANNODE_HYDROMETER_MEASUREMENT
-        bool "Include hydrometer measurement"
-        default n
-
-    config UAVCANNODE_LIGHTS_COMMAND
-        bool "Include lights command"
-        default n
-
-    config UAVCANNODE_MAGNETIC_FIELD_STRENGTH
-        bool "Include magnetic field strength"
-        default n
-
-    config UAVCANNODE_RANGE_SENSOR_MEASUREMENT
-        bool "Include range sensor measurement"
-        default n
-
-    config UAVCANNODE_RAW_AIR_DATA
-        bool "Include raw air data"
-        default n
-
-    config UAVCANNODE_RTK_DATA
-        bool "Include RTK data"
-        default n
-
-    config UAVCANNODE_SAFETY_BUTTON
-        bool "Include safety button"
-        default n
-
-    config UAVCANNODE_SERVO_ARRAY_COMMAND
-        bool "Include servo array command"
-        default n
-
-    config UAVCANNODE_STATIC_PRESSURE
-        bool "Include static pressure"
-        default n
-
-    config UAVCANNODE_STATIC_TEMPERATURE
-        bool "Include static temperature"
-        default n
-
-endif #DRIVERS_UAVCANNODE
+		Enable support for uavcannode
@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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
- * 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 "UavcanPublisherBase.hpp"
-
-#include <uavcan/equipment/esc/Status.hpp>
-
-#include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/esc_status.h>
-
-namespace uavcannode
-{
-
-class ESCStatus :
-	public UavcanPublisherBase,
-	private uORB::SubscriptionCallbackWorkItem,
-	private uavcan::Publisher<uavcan::equipment::esc::Status>
-{
-public:
-	ESCStatus(px4::WorkItem *work_item, uavcan::INode &node) :
-		UavcanPublisherBase(uavcan::equipment::esc::Status::DefaultDataTypeID),
-		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(esc_status)),
-		uavcan::Publisher<uavcan::equipment::esc::Status>(node)
-	{
-		this->setPriority(uavcan::TransferPriority::MiddleLower);
-	}
-
-	void PrintInfo() override
-	{
-		if (uORB::SubscriptionCallbackWorkItem::advertised()) {
-			printf("\t%s -> %s:%d\n",
-			       uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
-			       uavcan::equipment::esc::Status::getDataTypeFullName(),
-			       uavcan::equipment::esc::Status::DefaultDataTypeID);
-		}
-	}
-
-	void BroadcastAnyUpdates() override
-	{
-		// esc_status -> uavcan::equipment::esc::Status
-		esc_status_s esc_status;
-
-		if (uORB::SubscriptionCallbackWorkItem::update(&esc_status)) {
-
-			for (size_t i = 0; i < esc_status.esc_count; i++) {
-				uavcan::equipment::esc::Status status{};
-
-				status.esc_index = i;
-				status.error_count = esc_status.esc[i].esc_errorcount;
-				status.voltage = esc_status.esc[i].esc_voltage;
-				status.current = esc_status.esc[i].esc_current;
-				status.temperature = esc_status.esc[i].esc_temperature;
-				status.rpm = esc_status.esc[i].esc_rpm;
-				//status.power_rating_pct = NAN;
-
-				uavcan::Publisher<uavcan::equipment::esc::Status>::broadcast(status);
-
-				// ensure callback is registered
-				uORB::SubscriptionCallbackWorkItem::registerCallback();
-			}
-		}
-	}
-};
-} // namespace uavcan
@@ -1,98 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include "UavcanSubscriberBase.hpp"
-
-#include <uavcan/equipment/safety/ArmingStatus.hpp>
-
-#include <uORB/Publication.hpp>
-#include <uORB/topics/actuator_armed.h>
-
-namespace uavcannode
-{
-
-class ArmingStatus;
-
-typedef uavcan::MethodBinder<ArmingStatus *,
-	void (ArmingStatus::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::safety::ArmingStatus>&)>
-	ArmingStatusBinder;
-
-class ArmingStatus :
-	public UavcanSubscriberBase,
-	private uavcan::Subscriber<uavcan::equipment::safety::ArmingStatus, ArmingStatusBinder>
-{
-public:
-	ArmingStatus(uavcan::INode &node) :
-		UavcanSubscriberBase(uavcan::equipment::safety::ArmingStatus::DefaultDataTypeID),
-		uavcan::Subscriber<uavcan::equipment::safety::ArmingStatus, ArmingStatusBinder>(node)
-	{}
-
-	bool init()
-	{
-		if (start(ArmingStatusBinder(this, &ArmingStatus::callback)) < 0) {
-			PX4_ERR("uavcan::equipment::safety::ArmingStatus subscription failed");
-			return false;
-		}
-
-		return true;
-	}
-
-	void PrintInfo() const override
-	{
-		printf("\t%s:%d -> %s\n",
-		       uavcan::equipment::safety::ArmingStatus::getDataTypeFullName(),
-		       uavcan::equipment::safety::ArmingStatus::DefaultDataTypeID,
-		       _actuator_armed_pub.get_topic()->o_name);
-	}
-
-private:
-	void callback(const uavcan::ReceivedDataStructure<uavcan::equipment::safety::ArmingStatus> &msg)
-	{
-		actuator_armed_s actuator_armed{};
-
-		if (msg.status) {
-			actuator_armed.armed = true;
-
-		} else {
-			actuator_armed.armed = false;
-		}
-
-		actuator_armed.timestamp = hrt_absolute_time();
-		_actuator_armed_pub.publish(actuator_armed);
-	}
-
-	uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
-};
-} // namespace uavcannode
@@ -1,105 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include "UavcanSubscriberBase.hpp"
-
-#include <uavcan/equipment/esc/RawCommand.hpp>
-
-#include <uORB/Publication.hpp>
-#include <uORB/topics/actuator_motors.h>
-
-namespace uavcannode
-{
-
-class ESCRawCommand;
-
-typedef uavcan::MethodBinder<ESCRawCommand *,
-	void (ESCRawCommand::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>&)>
-	ESCRawCommandBinder;
-
-class ESCRawCommand :
-	public UavcanSubscriberBase,
-	private uavcan::Subscriber<uavcan::equipment::esc::RawCommand, ESCRawCommandBinder>
-{
-public:
-	ESCRawCommand(uavcan::INode &node) :
-		UavcanSubscriberBase(uavcan::equipment::esc::RawCommand::DefaultDataTypeID),
-		uavcan::Subscriber<uavcan::equipment::esc::RawCommand, ESCRawCommandBinder>(node)
-	{}
-
-	bool init()
-	{
-		if (start(ESCRawCommandBinder(this, &ESCRawCommand::callback)) < 0) {
-			PX4_ERR("uavcan::equipment::esc::RawCommand subscription failed");
-			return false;
-		}
-
-		return true;
-	}
-
-	void PrintInfo() const override
-	{
-		printf("\t%s:%d -> %s\n",
-		       uavcan::equipment::esc::RawCommand::getDataTypeFullName(),
-		       uavcan::equipment::esc::RawCommand::DefaultDataTypeID,
-		       _actuator_motors_pub.get_topic()->o_name);
-	}
-
-private:
-	void callback(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand> &msg)
-	{
-
-		actuator_motors_s actuator_motors;
-
-		actuator_motors.timestamp = hrt_absolute_time();
-		actuator_motors.timestamp_sample = actuator_motors.timestamp;
-
-		for (unsigned i = 0; i < msg.cmd.size(); i++) {
-			if (i >= actuator_motors_s::NUM_CONTROLS) {
-				break;
-			}
-
-			// Normalized to -8192, 8191 in uavcan. actuator_motors is -1 to 1
-			actuator_motors.control[i] = msg.cmd[i] / 8192.f;
-
-		}
-
-		_actuator_motors_pub.publish(actuator_motors);
-
-	}
-
-	uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
-};
-} // namespace uavcannode
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include "UavcanSubscriberBase.hpp"
-
-#include <uavcan/equipment/actuator/ArrayCommand.hpp>
-
-#include <uORB/Publication.hpp>
-#include <uORB/topics/actuator_servos.h>
-
-namespace uavcannode
-{
-
-class ServoArrayCommand;
-
-typedef uavcan::MethodBinder<ServoArrayCommand *,
-	void (ServoArrayCommand::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::actuator::ArrayCommand>&)>
-	ServoArrayCommandBinder;
-
-class ServoArrayCommand :
-	public UavcanSubscriberBase,
-	private uavcan::Subscriber<uavcan::equipment::actuator::ArrayCommand, ServoArrayCommandBinder>
-{
-public:
-	ServoArrayCommand(uavcan::INode &node) :
-		UavcanSubscriberBase(uavcan::equipment::actuator::ArrayCommand::DefaultDataTypeID),
-		uavcan::Subscriber<uavcan::equipment::actuator::ArrayCommand, ServoArrayCommandBinder>(node)
-	{}
-
-	bool init()
-	{
-		if (start(ServoArrayCommandBinder(this, &ServoArrayCommand::callback)) < 0) {
-			PX4_ERR("uavcan::equipment::actuator::ArrayCommand subscription failed");
-			return false;
-		}
-
-		return true;
-	}
-
-	void PrintInfo() const override
-	{
-		printf("\t%s:%d -> %s\n",
-		       uavcan::equipment::actuator::ArrayCommand::getDataTypeFullName(),
-		       uavcan::equipment::actuator::ArrayCommand::DefaultDataTypeID,
-		       _actuator_servos_pub.get_topic()->o_name);
-	}
-
-private:
-	void callback(const uavcan::ReceivedDataStructure<uavcan::equipment::actuator::ArrayCommand> &msg)
-	{
-
-		actuator_servos_s actuator_servos;
-
-		actuator_servos.timestamp = hrt_absolute_time();
-		actuator_servos.timestamp_sample = actuator_servos.timestamp;
-
-		for (unsigned i = 0; i < msg.commands.size(); i++) {
-			if (i >= actuator_servos_s::NUM_CONTROLS) {
-				break;
-			}
-
-			if (msg.commands[i].command_type == uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS) {
-				actuator_servos.control[i] = msg.commands[i].command_value; // -1.0 to +1.0
-
-			} else {
-				actuator_servos.control[i] = NAN; // disarmed
-			}
-		}
-
-		_actuator_servos_pub.publish(actuator_servos);
-
-	}
-
-	uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
-};
-} // namespace uavcannode
@@ -40,77 +40,23 @@
 #include <lib/geo/geo.h>
 #include <lib/version/version.h>

-#if defined(CONFIG_UAVCANNODE_BATTERY_INFO)
 #include "Publishers/BatteryInfo.hpp"
-#endif // CONFIG_UAVCANNODE_BATTERY_INFO
-
-#if defined(CONFIG_UAVCANNODE_ESC_STATUS)
-#include "Publishers/ESCStatus.hpp"
-#endif // CONFIG_UAVCANNODE_ESC_STATUS
-
-#if defined(CONFIG_UAVCANNODE_FLOW_MEASUREMENT)
 #include "Publishers/FlowMeasurement.hpp"
-#endif // CONFIG_UAVCANNODE_FLOW_MEASUREMENT
-
-#if defined(CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT)
 #include "Publishers/HygrometerMeasurement.hpp"
-#endif // CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT
-
-#if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 #include "Publishers/GnssFix2.hpp"
-#endif // CONFIG_UAVCANNODE_GNSS_FIX
-
-#if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH)
 #include "Publishers/MagneticFieldStrength2.hpp"
-#endif // CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH
-
-#if defined(CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT)
-#include "Publishers/RangeSensorMeasurement.hpp"
-#endif // CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT
-
-#if defined(CONFIG_UAVCANNODE_RAW_AIR_DATA)
-#include "Publishers/RawAirData.hpp"
-#endif // CONFIG_UAVCANNODE_RAW_AIR_DATA
-
-#if defined(CONFIG_UAVCANNODE_SAFETY_BUTTON)
-#include "Publishers/SafetyButton.hpp"
-#endif // CONFIG_UAVCANNODE_SAFETY_BUTTON
-
-#if defined(CONFIG_UAVCANNODE_STATIC_PRESSURE)
-#include "Publishers/StaticPressure.hpp"
-#endif // CONFIG_UAVCANNODE_STATIC_PRESSURE
-
-#if defined(CONFIG_UAVCANNODE_STATIC_TEMPERATURE)
-#include "Publishers/StaticTemperature.hpp"
-#endif // CONFIG_UAVCANNODE_STATIC_TEMPERATURE
-
-#if defined(CONFIG_UAVCANNODE_ARMING_STATUS)
-#include "Subscribers/ArmingStatus.hpp"
-#endif // CONFIG_UAVCANNODE_ARMING_STATUS
-
-#if defined(CONFIG_UAVCANNODE_BEEP_COMMAND)
-#include "Subscribers/BeepCommand.hpp"
-#endif // CONFIG_UAVCANNODE_BEEP_COMMAND
-
-#if defined(CONFIG_UAVCANNODE_ESC_RAW_COMMAND)
-#include "Subscribers/ESCRawCommand.hpp"
-#endif // CONFIG_UAVCANNODE_ESC_RAW_COMMAND
-
-#if defined(CONFIG_UAVCANNODE_LIGHTS_COMMAND)
-#include "Subscribers/LightsCommand.hpp"
-#endif // CONFIG_UAVCANNODE_LIGHTS_COMMAND
-
-#if defined(CONFIG_UAVCANNODE_RTK_DATA)
-#include "Publishers/RelPosHeading.hpp"
 #include "Publishers/MovingBaselineData.hpp"
+#include "Publishers/RangeSensorMeasurement.hpp"
+#include "Publishers/RawAirData.hpp"
+#include "Publishers/RelPosHeading.hpp"
+#include "Publishers/SafetyButton.hpp"
+#include "Publishers/StaticPressure.hpp"
+#include "Publishers/StaticTemperature.hpp"

+#include "Subscribers/BeepCommand.hpp"
+#include "Subscribers/LightsCommand.hpp"
 #include "Subscribers/MovingBaselineData.hpp"
 #include "Subscribers/RTCMStream.hpp"
-#endif // CONFIG_UAVCANNODE_RTK_DATA
-
-#if defined(CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND)
-#include "Subscribers/ServoArrayCommand.hpp"
-#endif // CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND

 using namespace time_literals;

@@ -348,39 +294,14 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 		return PX4_ERROR;
 	}

-#if defined(CONFIG_UAVCANNODE_BATTERY_INFO)
+	// TODO: make runtime (and build time?) configurable
 	_publisher_list.add(new BatteryInfo(this, _node));
-#endif // CONFIG_UAVCANNODE_BATTERY_INFO
-
-#if defined(CONFIG_UAVCANNODE_ESC_STATUS)
-	_publisher_list.add(new ESCStatus(this, _node));
-#endif // CONFIG_UAVCANNODE_ESC_STATUS
-
-#if defined(CONFIG_UAVCANNODE_FLOW_MEASUREMENT)
 	_publisher_list.add(new FlowMeasurement(this, _node));
-#endif // CONFIG_UAVCANNODE_FLOW_MEASUREMENT
-
-#if defined(CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT)
 	_publisher_list.add(new HygrometerMeasurement(this, _node));
-#endif // CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT
-
-#if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 	_publisher_list.add(new GnssFix2(this, _node));
-#endif // CONFIG_UAVCANNODE_GNSS_FIX
-
-#if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH)
 	_publisher_list.add(new MagneticFieldStrength2(this, _node));
-#endif // CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH
-
-#if defined(CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT)
 	_publisher_list.add(new RangeSensorMeasurement(this, _node));
-#endif // CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT
-
-#if defined(CONFIG_UAVCANNODE_RAW_AIR_DATA)
 	_publisher_list.add(new RawAirData(this, _node));
-#endif // CONFIG_UAVCANNODE_RAW_AIR_DATA
-
-#if defined(CONFIG_UAVCANNODE_RTK_DATA)
 	_publisher_list.add(new RelPosHeadingPub(this, _node));

 	int32_t cannode_pub_mbd = 0;
@@ -390,37 +311,13 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 		_publisher_list.add(new MovingBaselineDataPub(this, _node));
 	}

-#endif // CONFIG_UAVCANNODE_RTK_DATA
-
-#if defined(CONFIG_UAVCANNODE_SAFETY_BUTTON)
 	_publisher_list.add(new SafetyButton(this, _node));
-#endif // CONFIG_UAVCANNODE_SAFETY_BUTTON
-
-#if defined(CONFIG_UAVCANNODE_STATIC_PRESSURE)
 	_publisher_list.add(new StaticPressure(this, _node));
-#endif // CONFIG_UAVCANNODE_STATIC_PRESSURE
-
-#if defined(CONFIG_UAVCANNODE_STATIC_TEMPERATURE)
 	_publisher_list.add(new StaticTemperature(this, _node));
-#endif // CONFIG_UAVCANNODE_STATIC_TEMPERATURE

-#if defined(CONFIG_UAVCANNODE_ARMING_STATUS)
-	_subscriber_list.add(new ArmingStatus(_node));
-#endif // CONFIG_UAVCANNODE_ARMING_STATUS
-
-#if defined(CONFIG_UAVCANNODE_BEEP_COMMAND)
 	_subscriber_list.add(new BeepCommand(_node));
-#endif // CONFIG_UAVCANNODE_BEEP_COMMAND
-
-#if defined(CONFIG_UAVCANNODE_ESC_RAW_COMMAND)
-	_subscriber_list.add(new ESCRawCommand(_node));
-#endif // CONFIG_UAVCANNODE_ESC_RAW_COMMAND
-
-#if defined(CONFIG_UAVCANNODE_LIGHTS_COMMAND)
 	_subscriber_list.add(new LightsCommand(_node));
-#endif // CONFIG_UAVCANNODE_LIGHTS_COMMAND

-#if defined(CONFIG_UAVCANNODE_RTK_DATA)
 	int32_t cannode_sub_mbd = 0;
 	param_get(param_find("CANNODE_SUB_MBD"), &cannode_sub_mbd);

@@ -435,12 +332,6 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 		_subscriber_list.add(new RTCMStream(_node));
 	}

-#endif // CONFIG_UAVCANNODE_RTK_DATA
-
-#if defined(CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND)
-	_subscriber_list.add(new ServoArrayCommand(_node));
-#endif // CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND
-
 	for (auto &subscriber : _subscriber_list) {
 		subscriber->init();
 	}
@@ -82,7 +82,6 @@ void FakeGps::Run()
 	sensor_gps.heading_offset = 0.0000;
 	sensor_gps.fix_type = 4;
 	sensor_gps.jamming_state = 0;
-	sensor_gps.spoofing_state = 0;
 	sensor_gps.vel_ned_valid = true;
 	sensor_gps.satellites_used = 14;
 	sensor_gps.timestamp = hrt_absolute_time();
@@ -1,503 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023 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 "AdsbConflict.h"
-#include "geo/geo.h"
-
-#include <uORB/topics/transponder_report.h>
-
-#include <float.h>
-
-
-void AdsbConflict::detect_traffic_conflict(double lat_now, double lon_now, float alt_now, float vx_now, float vy_now,
-		float vz_now)
-
-{
-
-	float d_hor, d_vert;
-	get_distance_to_point_global_wgs84(lat_now, lon_now, alt_now,
-					   _transponder_report.lat, _transponder_report.lon, _transponder_report.altitude, &d_hor, &d_vert);
-
-	const float xyz_traffic_speed = sqrtf(_transponder_report.hor_velocity * _transponder_report.hor_velocity +
-					      _transponder_report.ver_velocity * _transponder_report.ver_velocity);
-
-	const float hor_uav_speed = sqrtf(vx_now * vx_now + vy_now * vy_now);
-	const float xyz_uav_speed = sqrtf(hor_uav_speed * hor_uav_speed + vz_now * vz_now);
-
-	//assume always pointing at each other
-	const float relative_uav_traffic_speed = xyz_traffic_speed + xyz_uav_speed;
-
-
-	// Predict until the vehicle would have passed this system at its current speed
-	const float prediction_distance = d_hor + TRAFFIC_TO_UAV_DISTANCE_EXTENSION;
-
-	double end_lat, end_lon;
-	waypoint_from_heading_and_distance(_transponder_report.lat, _transponder_report.lon,
-					   _transponder_report.heading, prediction_distance, &end_lat, &end_lon);
-
-
-	const bool cs_distance_conflict_threshold = (!get_distance_to_line(_crosstrack_error, lat_now,
-			lon_now, _transponder_report.lat,
-			_transponder_report.lon, end_lat,
-			end_lon))
-			&& (!_crosstrack_error.past_end)
-			&& (fabsf(_crosstrack_error.distance) < _conflict_detection_params.crosstrack_separation);
-
-	const bool _crosstrack_separation_check = (fabsf(alt_now - _transponder_report.altitude) <
-			_conflict_detection_params.crosstrack_separation);
-
-	bool collision_time_check = false;
-
-	const float d_xyz = sqrtf(d_hor * d_hor + d_vert * d_vert);
-
-	if (relative_uav_traffic_speed > FLT_EPSILON) {
-		const float time_to_collsion =  d_xyz / relative_uav_traffic_speed;
-		collision_time_check = (time_to_collsion < _conflict_detection_params.collision_time_threshold);
-	}
-
-	_conflict_detected = (cs_distance_conflict_threshold && _crosstrack_separation_check
-			      && collision_time_check);
-}
-
-int AdsbConflict::find_icao_address_in_conflict_list(uint32_t icao_address)
-{
-
-	for (uint8_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
-		if (_traffic_buffer.icao_address[i] == icao_address) {
-			return i;
-		}
-	}
-
-	return -1;
-}
-
-void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index)
-{
-	_traffic_buffer.icao_address.remove(traffic_index);
-	_traffic_buffer.timestamp.remove(traffic_index);
-}
-
-void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address)
-{
-	_traffic_buffer.timestamp.push_back(hrt_absolute_time());
-	_traffic_buffer.icao_address.push_back(icao_address);
-}
-
-void AdsbConflict::get_traffic_state()
-{
-
-	const int traffic_index = find_icao_address_in_conflict_list(_transponder_report.icao_address);
-
-	const bool old_conflict = (traffic_index >= 0);
-	const bool new_traffic = (traffic_index < 0);
-	const bool _traffic_buffer_full = (_traffic_buffer.icao_address.size() >= NAVIGATOR_MAX_TRAFFIC);
-
-	bool old_conflict_warning_expired = false;
-
-	if (old_conflict && _conflict_detected) {
-		old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT);
-
-	}
-
-	if (new_traffic && _conflict_detected && !_traffic_buffer_full) {
-		add_icao_address_from_conflict_list(_transponder_report.icao_address);
-		_traffic_state = TRAFFIC_STATE::ADD_CONFLICT;
-
-	} else if (new_traffic && _conflict_detected && _traffic_buffer_full) {
-		_traffic_state = TRAFFIC_STATE::BUFFER_FULL;
-
-	} else if (old_conflict && _conflict_detected
-		   && old_conflict_warning_expired) {
-		_traffic_buffer.timestamp[traffic_index] = hrt_absolute_time();
-		_traffic_state = TRAFFIC_STATE::REMIND_CONFLICT;
-
-	} else if (old_conflict && !_conflict_detected) {
-		remove_icao_address_from_conflict_list(traffic_index);
-		_traffic_state = TRAFFIC_STATE::REMOVE_OLD_CONFLICT;
-
-	} else {
-		_traffic_state = TRAFFIC_STATE::NO_CONFLICT;
-	}
-
-}
-
-
-bool AdsbConflict::handle_traffic_conflict()
-{
-
-	get_traffic_state();
-
-	char uas_id[UTM_GUID_MSG_LENGTH]; //GUID of incoming UTM messages
-
-	//convert UAS_id byte array to char array for User Warning
-	for (int i = 0; i < 5; i++) {
-		snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", _transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]);
-	}
-
-	uint64_t uas_id_int = 0;
-
-	for (int i = 0; i < 8; i++) {
-		uas_id_int |= (uint64_t)(_transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8);
-	}
-
-
-	bool take_action = false;
-
-	switch (_traffic_state) {
-
-	case TRAFFIC_STATE::ADD_CONFLICT:
-	case TRAFFIC_STATE::REMIND_CONFLICT: {
-
-			take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
-							   (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
-							   _transponder_report.callsign,
-							   uas_id_int);
-
-		}
-		break;
-
-	case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: {
-
-			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC UPDATE: %s is no longer in conflict!",
-					     _transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ?
-					     _transponder_report.callsign : uas_id);
-
-			events::send<uint64_t>(events::ID("navigator_traffic_resolved"), events::Log::Critical, "Traffic Conflict Resolved",
-					       uas_id_int);
-
-		}
-		break;
-
-	case TRAFFIC_STATE::BUFFER_FULL: {
-
-			if (_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) {
-				PX4_WARN("Too much traffic! Showing all messages from now on");
-			}
-
-			//stop buffering incoming conflicts
-			take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
-							   (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
-							   _transponder_report.callsign,
-							   uas_id_int);
-
-		}
-		break;
-
-	case TRAFFIC_STATE::NO_CONFLICT: {
-
-		}
-		break;
-	}
-
-
-	_traffic_state_previous = _traffic_state;
-
-	return take_action;
-
-}
-
-void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, float vertical_separation,
-		int collision_time_threshold, uint8_t traffic_avoidance_mode)
-{
-
-	_conflict_detection_params.crosstrack_separation = crosstrack_separation;
-	_conflict_detection_params.vertical_separation = vertical_separation;
-	_conflict_detection_params.collision_time_threshold = collision_time_threshold;
-	_conflict_detection_params.traffic_avoidance_mode = traffic_avoidance_mode;
-
-}
-
-
-bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
-					char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int)
-{
-
-	switch (_conflict_detection_params.traffic_avoidance_mode) {
-
-	case 0: {
-			PX4_WARN("TRAFFIC %s! dst %d, hdg %d",
-				 tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
-				 traffic_seperation,
-				 traffic_direction);
-			break;
-		}
-
-	case 1: {
-			mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d\t",
-					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
-					     traffic_seperation,
-					     traffic_direction);
-			/* EVENT
-			 * @description
-			 * - ID: {1}
-			 * - Distance: {2m}
-			 * - Direction: {3} degrees
-			 */
-			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert",
-					uas_id_int, traffic_seperation, traffic_direction);
-			break;
-		}
-
-	case 2: {
-			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d\t",
-					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
-					     traffic_seperation,
-					     traffic_direction);
-			/* EVENT
-			 * @description
-			 * - ID: {1}
-			 * - Distance: {2m}
-			 * - Direction: {3} degrees
-			 */
-			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Critical,
-					"Traffic alert, returning home",
-					uas_id_int, traffic_seperation, traffic_direction);
-
-			return true;
-
-			break;
-		}
-
-	case 3: {
-			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d\t",
-					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
-					     traffic_seperation,
-					     traffic_direction);
-			/* EVENT
-			 * @description
-			 * - ID: {1}
-			 * - Distance: {2m}
-			 * - Direction: {3} degrees
-			 */
-			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Critical,
-					"Traffic alert, landing",
-					uas_id_int, traffic_seperation, traffic_direction);
-
-			return true;
-
-			break;
-
-		}
-
-	case 4: {
-			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d\t",
-					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
-					     traffic_seperation,
-					     traffic_direction);
-			/* EVENT
-			 * @description
-			 * - ID: {1}
-			 * - Distance: {2m}
-			 * - Direction: {3} degrees
-			 */
-			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Critical,
-					"Traffic alert, holding position",
-					uas_id_int, traffic_seperation, traffic_direction);
-
-			return true;
-
-			break;
-
-		}
-	}
-
-
-	return false;
-
-}
-
-void AdsbConflict::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
-				float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav,
-				double lon_uav,
-				float &alt_uav)
-{
-	double lat{0.0};
-	double lon{0.0};
-
-	waypoint_from_heading_and_distance(lat_uav, lon_uav, direction, distance, &lat,
-					   &lon);
-	float alt = alt_uav + altitude_diff;
-
-	tr.timestamp = hrt_absolute_time();
-	tr.icao_address = icao_address;
-	tr.lat = lat; // Latitude, expressed as degrees
-	tr.lon = lon; // Longitude, expressed as degrees
-	tr.altitude_type = 0;
-	tr.altitude = alt;
-	tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians
-	tr.hor_velocity	= hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s
-	tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up
-	strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign) - 1);
-	tr.callsign[sizeof(tr.callsign) - 1] = 0;
-	tr.emitter_type = emitter_type; // Type from ADSB_EMITTER_TYPE enum
-	tr.tslc = 2; // Time since last communication in seconds
-	tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
-		   transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
-		   transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
-		   (transponder_report_s::ADSB_EMITTER_TYPE_UAV & emitter_type ? 0 :
-		    transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN); // Flags to indicate various statuses including valid data fields
-	tr.squawk = 6667;
-
-#ifndef BOARD_HAS_NO_UUID
-	px4_guid_t px4_guid;
-	board_get_px4_guid(px4_guid);
-	memcpy(tr.uas_id, px4_guid, sizeof(px4_guid_t)); //simulate own GUID
-#else
-
-	for (int i = 0; i < PX4_GUID_BYTE_LENGTH ; i++) {
-		tr.uas_id[i] = 0xe0 + i; //simulate GUID
-	}
-
-#endif /* BOARD_HAS_NO_UUID */
-
-	orb_publish(ORB_ID(transponder_report), fake_traffic_report_publisher, &tr);
-
-}
-
-
-void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
-				    float &alt_uav)
-{
-
-	/*
-	//first conflict
-	fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
-		     alt_uav);
-
-	//spam
-	fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
-		     alt_uav);
-	fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
-		     alt_uav);
-	fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
-		     alt_uav);
-
-	//stop spamming
-
-	fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX005", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 5, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX006", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 6, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX007", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 7, lat_uav, lon_uav,
-		     alt_uav);
-
-
-	fake_traffic("LX008", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 8, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX009", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 9, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX010", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 10, lat_uav, lon_uav,
-		     alt_uav);
-
-	//buffer full
-	fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX012", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 12, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
-		     alt_uav);
-
-
-	//end conflict
-	fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
-		     alt_uav);
-	fake_traffic("LX002", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX003", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX004", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
-		     alt_uav);
-
-
-	//new conflicts with space in buffer
-	fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
-		     alt_uav);
-
-	fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
-		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav,
-		     alt_uav);
-
-	for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
-		PX4_INFO("%u ", static_cast<unsigned int>(_traffic_buffer.icao_address[i]));
-	}
-	*/
-}
@@ -1,158 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012-2023 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 <stdbool.h>
-#include <stdint.h>
-
-#include <lib/geo/geo.h>
-
-#include <drivers/drv_hrt.h>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/transponder_report.h>
-#include <uORB/topics/vehicle_command.h>
-
-#include <systemlib/mavlink_log.h>
-#include <px4_platform_common/events.h>
-
-#include <px4_platform_common/board_common.h>
-
-#include <containers/Array.hpp>
-
-using namespace time_literals;
-
-static constexpr uint8_t NAVIGATOR_MAX_TRAFFIC{10};
-
-static constexpr uint8_t UTM_GUID_MSG_LENGTH{11};
-
-static constexpr uint8_t UTM_CALLSIGN_LENGTH{9};
-
-static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s};
-
-static constexpr float TRAFFIC_TO_UAV_DISTANCE_EXTENSION{1000.0f};
-
-
-struct traffic_data_s {
-	double lat_traffic;
-	double lon_traffic;
-	float alt_traffic;
-	float heading_traffic;
-	float vxy_traffic;
-	float vz_traffic;
-	bool in_conflict;
-};
-
-struct traffic_buffer_s {
-	px4::Array<uint32_t, NAVIGATOR_MAX_TRAFFIC> icao_address {};
-	px4::Array<hrt_abstime, NAVIGATOR_MAX_TRAFFIC> timestamp {};
-};
-
-struct conflict_detection_params_s {
-	float crosstrack_separation;
-	float vertical_separation;
-	int collision_time_threshold;
-	uint8_t traffic_avoidance_mode;
-};
-
-enum class TRAFFIC_STATE {
-	NO_CONFLICT = 0,
-	ADD_CONFLICT = 1,
-	REMIND_CONFLICT = 2,
-	REMOVE_OLD_CONFLICT = 3,
-	BUFFER_FULL = 4
-};
-
-
-class AdsbConflict
-{
-public:
-	AdsbConflict() = default;
-	~AdsbConflict() = default;
-
-	void detect_traffic_conflict(double lat_now, double lon_now, float alt_now, float vx_now, float vy_now, float vz_now);
-
-	int find_icao_address_in_conflict_list(uint32_t icao_address);
-
-	void remove_icao_address_from_conflict_list(int traffic_index);
-
-	void add_icao_address_from_conflict_list(uint32_t icao_address);
-
-	void get_traffic_state();
-
-	void set_conflict_detection_params(float crosstrack_separation, float vertical_separation,
-					   int collision_time_threshold, uint8_t traffic_avoidance_mode);
-
-
-	bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
-				  char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int);
-
-	transponder_report_s _transponder_report{};
-
-	bool handle_traffic_conflict();
-
-	void fake_traffic(const char *const callsign, float distance, float direction, float traffic_heading,
-			  float altitude_diff,
-			  float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav, double lon_uav,
-			  float &alt_uav);
-
-	void run_fake_traffic(double &lat_uav, double &lon_uav,
-			      float &alt_uav);
-
-	bool _conflict_detected{false};
-
-	TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT};
-
-	conflict_detection_params_s _conflict_detection_params{};
-
-
-protected:
-	traffic_buffer_s _traffic_buffer;
-
-private:
-
-	orb_advert_t _mavlink_log_pub{nullptr};
-
-	crosstrack_error_s _crosstrack_error{};
-
-
-	transponder_report_s tr{};
-
-	orb_advert_t fake_traffic_report_publisher = orb_advertise_queue(ORB_ID(transponder_report), &tr, (unsigned int)20);
-
-	TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT};
-
-};
@@ -1,288 +0,0 @@
-#include <gtest/gtest.h>
-
-#include "AdsbConflict.h"
-
-#include "AdsbConflictTest.h"
-
-
-class TestAdsbConflict : public AdsbConflict
-{
-public:
-	TestAdsbConflict() : AdsbConflict() {}
-	~TestAdsbConflict() = default;
-
-	void set_traffic_buffer(const traffic_buffer_s &traffic_buffer)
-	{
-		_traffic_buffer = traffic_buffer;
-	}
-
-	void set_conflict(bool &conflict_detected)
-	{
-		_conflict_detected = conflict_detected;
-	}
-};
-
-
-TEST_F(AdsbConflictTest, detectTrafficConflict)
-{
-
-
-	int collision_time_threshold = 60;
-
-	float crosstrack_separation = 500.0f;
-	float vertical_separation = 500.0f;
-
-	double lat_now = 32.617013;
-	double lon_now = -96.490564;
-	float alt_now = 1000.0f;
-
-	float vx_now = 0.0f;
-	float vy_now = 0.0f;
-	float vz_now = 0.0f;
-
-	uint32_t traffic_dataset_size = sizeof(traffic_dataset) / sizeof(traffic_dataset[0]);
-
-
-	TestAdsbConflict 	adsb_conflict;
-
-	adsb_conflict.set_conflict_detection_params(crosstrack_separation, vertical_separation, collision_time_threshold, 1);
-
-	for (uint32_t i = 0; i < traffic_dataset_size; i++) {
-
-
-		printf("---------------%d--------------\n", i);
-
-		struct traffic_data_s traffic = traffic_dataset[i];
-
-
-		adsb_conflict._transponder_report.lat = traffic.lat_traffic;
-		adsb_conflict._transponder_report.lon = traffic.lon_traffic;
-		adsb_conflict._transponder_report.altitude = traffic.alt_traffic;
-		adsb_conflict._transponder_report.heading = traffic.heading_traffic;
-		adsb_conflict._transponder_report.hor_velocity = traffic.vxy_traffic;
-		adsb_conflict._transponder_report.ver_velocity = traffic.vz_traffic;
-
-		adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now);
-
-		printf("conflict_detected %d \n", adsb_conflict._conflict_detected);
-
-		printf("------------------------------\n");
-		printf("------------------------------\n \n");
-
-		EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict);
-	}
-
-}
-
-
-TEST_F(AdsbConflictTest, trafficAlerts)
-{
-
-	struct	traffic_buffer_s used_buffer;
-	used_buffer.icao_address.push_back(2345);
-	used_buffer.icao_address.push_back(1234);
-	used_buffer.icao_address.push_back(1897);
-	used_buffer.icao_address.push_back(0567);
-	used_buffer.icao_address.push_back(8685);
-	used_buffer.icao_address.push_back(5000);
-
-	used_buffer.timestamp.push_back(3_s);
-	used_buffer.timestamp.push_back(800_s);
-	used_buffer.timestamp.push_back(100_s);
-	used_buffer.timestamp.push_back(20000_s);
-	used_buffer.timestamp.push_back(6000_s);
-	used_buffer.timestamp.push_back(6587_s);
-
-	struct	traffic_buffer_s full_buffer;
-	full_buffer.icao_address.push_back(2345);
-	full_buffer.icao_address.push_back(1234);
-	full_buffer.icao_address.push_back(1897);
-	full_buffer.icao_address.push_back(0567);
-	full_buffer.icao_address.push_back(8685);
-	full_buffer.icao_address.push_back(5000);
-	full_buffer.icao_address.push_back(0000);
-	full_buffer.icao_address.push_back(2);
-	full_buffer.icao_address.push_back(589742397);
-	full_buffer.icao_address.push_back(99999);
-
-	full_buffer.timestamp.push_back(1_s);
-	full_buffer.timestamp.push_back(800_s);
-	full_buffer.timestamp.push_back(100_s);
-	full_buffer.timestamp.push_back(20000_s);
-	full_buffer.timestamp.push_back(6000_s);
-	full_buffer.timestamp.push_back(19000_s);
-	full_buffer.timestamp.push_back(5000_s);
-	full_buffer.timestamp.push_back(2_s);
-	full_buffer.timestamp.push_back(1000_s);
-	full_buffer.timestamp.push_back(58943_s);
-
-
-
-	struct traffic_buffer_s empty_buffer = {};
-
-
-	TestAdsbConflict 	adsb_conflict;
-
-	adsb_conflict.set_traffic_buffer(used_buffer);
-
-	bool conflict_detected  = false;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 00001;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
-
-	conflict_detected  = true;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 9876;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
-
-	adsb_conflict.set_traffic_buffer(empty_buffer);
-
-	conflict_detected  = true;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 9876;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
-
-	adsb_conflict.set_traffic_buffer(full_buffer);
-
-	conflict_detected  = true;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 7777;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::BUFFER_FULL);
-
-	conflict_detected  = false;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 7777;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
-
-	conflict_detected  = false;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 8685;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
-
-	adsb_conflict.set_traffic_buffer(used_buffer);
-
-	conflict_detected  = false;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 8685;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
-
-}
-
-TEST_F(AdsbConflictTest, trafficReminder)
-{
-
-	struct	traffic_buffer_s used_buffer;
-	used_buffer.icao_address.push_back(2345);
-	used_buffer.icao_address.push_back(1234);
-	used_buffer.icao_address.push_back(1897);
-	used_buffer.icao_address.push_back(0567);
-	used_buffer.icao_address.push_back(8685);
-	used_buffer.icao_address.push_back(5000);
-
-	used_buffer.timestamp.push_back(3_s);
-	used_buffer.timestamp.push_back(800_s);
-	used_buffer.timestamp.push_back(100_s);
-	used_buffer.timestamp.push_back(20000_s);
-	used_buffer.timestamp.push_back(6000_s);
-	used_buffer.timestamp.push_back(6587_s);
-
-	struct	traffic_buffer_s full_buffer;
-	full_buffer.icao_address.push_back(2345);
-	full_buffer.icao_address.push_back(1234);
-	full_buffer.icao_address.push_back(1897);
-	full_buffer.icao_address.push_back(0567);
-	full_buffer.icao_address.push_back(8685);
-	full_buffer.icao_address.push_back(5000);
-	full_buffer.icao_address.push_back(0000);
-	full_buffer.icao_address.push_back(2);
-	full_buffer.icao_address.push_back(589742397);
-	full_buffer.icao_address.push_back(99999);
-
-	full_buffer.timestamp.push_back(1_s);
-	full_buffer.timestamp.push_back(800_s);
-	full_buffer.timestamp.push_back(100_s);
-	full_buffer.timestamp.push_back(20000_s);
-	full_buffer.timestamp.push_back(6000_s);
-	full_buffer.timestamp.push_back(19000_s);
-	full_buffer.timestamp.push_back(5000_s);
-	full_buffer.timestamp.push_back(2_s);
-	full_buffer.timestamp.push_back(1000_s);
-	full_buffer.timestamp.push_back(58943_s);
-
-
-	TestAdsbConflict 	adsb_conflict;
-
-	adsb_conflict.set_traffic_buffer(used_buffer);
-
-	bool conflict_detected  = true;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 8685;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT);
-
-	conflict_detected  = true;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 8685;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
-
-	adsb_conflict.set_traffic_buffer(full_buffer);
-
-	conflict_detected  = true;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 8685;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT);
-
-	conflict_detected  = true;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 8685;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
-
-	conflict_detected  = false;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 8685;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
-
-	conflict_detected  = true;
-	adsb_conflict.set_conflict(conflict_detected);
-	adsb_conflict._transponder_report.icao_address = 7777;
-	adsb_conflict.get_traffic_state();
-
-	printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state);
-	EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
-
-}
@@ -31,8 +31,4 @@
 #
 ############################################################################

-px4_add_library(adsb AdsbConflict.cpp)
-
-target_link_libraries(adsb PUBLIC geo)
-
-px4_add_functional_gtest(SRC AdsbConflictTest.cpp LINKLIBS adsb)
+px4_add_library(adsb adsb.cpp)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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
@@ -30,17 +30,3 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
-
-#include <gtest/gtest.h>
-#include <matrix/math.hpp>
-
-using namespace matrix;
-
-TEST(MatrixVector4Test, Vector4)
-{
-	Vector4f a(1.f, 2.f, 3.f, 4.f);
-	EXPECT_EQ(a(0), 1.f);
-	EXPECT_EQ(a(1), 2.f);
-	EXPECT_EQ(a(2), 3.f);
-	EXPECT_EQ(a(3), 4.f);
-}
@@ -1,646 +0,0 @@
-{
- "cells": [
-  {
-   "cell_type": "code",
-   "execution_count": 2340,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "from plotly.offline import init_notebook_mode, iplot\n",
-    "\n",
-    "import pandas as pd\n",
-    "import plotly.graph_objects as go\n",
-    "import math\n",
-    "import numpy as np\n",
-    "np.set_printoptions(threshold=sys.maxsize)\n",
-    "import random\n",
-    "from decimal import Decimal"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2341,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "\n",
-    "def to_lonlat(point_xy, origin):\n",
-    "    r = 6371000\n",
-    "\n",
-    "    o_lat = math.radians(Decimal(origin[0]))\n",
-    "    o_lon = math.radians(Decimal(origin[1]))\n",
-    "    cos_phi = math.cos(o_lat)\n",
-    "    lat = Decimal(point_xy[1])/Decimal(r) + Decimal(o_lat)\n",
-    "    lon = Decimal(point_xy[0])/Decimal((r*cos_phi)) + Decimal(o_lon)\n",
-    "    return {'lon': math.degrees(lon), 'lat': math.degrees(lat)}\n",
-    "\n",
-    "\n",
-    "def seg_lonlat(seg):\n",
-    "    return [to_lonlat(i,origin) for i in seg]\n",
-    "\n",
-    "def plot_segment(fig,seg):\n",
-    "\n",
-    "    df = pd.DataFrame(seg)\n",
-    "    fig.add_trace(go.Scattermapbox(\n",
-    "        mode = \"markers+lines\",\n",
-    "        lon = df['lon'],\n",
-    "        lat = df['lat'],\n",
-    "        marker = {'size': 10}))\n",
-    "\n",
-    "def wrap(x, low, high):\n",
-    "    if (low <= x and x < high):\n",
-    "        return x\n",
-    "\n",
-    "    range = high - low\n",
-    "    inv_range = 1.0/range\n",
-    "    num_wraps = math.floor((x - low) * inv_range)\n",
-    "    return x - (range * num_wraps)\n",
-    "\n",
-    "\n",
-    "def waypoint_from_heading_and_distance(lat_traffic, lon_traffic, bearing, dist):\n",
-    "    r = 6371000\n",
-    "    bearing = wrap(bearing, 0, 2*math.pi)\n",
-    "    radius_ratio = dist / r\n",
-    "\n",
-    "    lat_traffic_rad = math.radians(lat_traffic)\n",
-    "    lon_traffic_rad = math.radians(lon_traffic)\n",
-    "\n",
-    "    lat_end = math.asin(math.sin(lat_traffic_rad) * math.cos(radius_ratio) +\n",
-    "                        math.cos(lat_traffic_rad) * math.sin(radius_ratio) * math.cos(bearing))\n",
-    "\n",
-    "    lon_end = lon_traffic_rad + math.atan2(math.sin(bearing) * math.sin(radius_ratio) * math.cos(\n",
-    "        lat_traffic_rad), math.cos(radius_ratio) - math.sin(lat_traffic_rad) * math.sin(lat_end))\n",
-    "\n",
-    "    lat_end = math.degrees(lat_end)\n",
-    "    lon_end = math.degrees(lon_end)\n",
-    "\n",
-    "    return lat_end, lon_end, bearing, radius_ratio\n",
-    "\n",
-    "\n",
-    "def get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end):\n",
-    "\n",
-    "    lat_start_rad = math.radians(lat_start)\n",
-    "    lat_end_rad = math.radians(lat_end)\n",
-    "\n",
-    "    cos_lat_end = math.cos(lat_end_rad)\n",
-    "    d_lon = math.radians(lon_end - lon_start)\n",
-    "\n",
-    "    y = (math.sin(d_lon) * cos_lat_end)\n",
-    "    x = (math.cos(lat_start_rad) * math.sin(lat_end_rad) -\n",
-    "         math.sin(lat_start_rad) * cos_lat_end * math.cos(d_lon))\n",
-    "\n",
-    "    return wrap(math.atan2(y, x), -math.pi, math.pi)\n",
-    "\n",
-    "\n",
-    "def get_distance_to_next_waypoint(lat_uav, lon_uav, lat_end, lon_end):\n",
-    "\n",
-    "    r = 6371000\n",
-    "\n",
-    "    lat_uav_rad = math.radians(lat_uav)\n",
-    "    lat_end_rad = math.radians(lat_end)\n",
-    "\n",
-    "    d_lat = lat_end_rad - lat_uav_rad\n",
-    "    d_lon = math.radians(lon_end) - math.radians(lon_uav)\n",
-    "\n",
-    "    a = math.sin(d_lat / 2.0) * math.sin(d_lat / 2.0) + math.sin(d_lon / 2.0) * math.sin(d_lon / 2.0) * math.cos(lat_uav_rad) * math.cos(\n",
-    "        lat_end_rad)\n",
-    "\n",
-    "    c = math.atan2(math.sqrt(a), math.sqrt(1.0 - a))\n",
-    "\n",
-    "    return r * 2.0 * c\n",
-    "\n",
-    "\n",
-    "def get_distance_to_line(lat_uav, lon_uav, lat_traffic, lon_traffic, lat_end, lon_end):\n",
-    "\n",
-    "    return_value = 1\n",
-    "    crosstrack_distance = 0.0\n",
-    "\n",
-    "    dist_to_end = get_distance_to_next_waypoint(\n",
-    "        lat_uav, lon_uav, lat_end, lon_end)\n",
-    "\n",
-    "    if (dist_to_end < 0.1):\n",
-    "        return_value = 0 \n",
-    "\n",
-    "    bearing_end = get_bearing_to_next_waypoint(\n",
-    "        lat_uav, lon_uav, lat_end, lon_end)\n",
-    "    bearing_track = get_bearing_to_next_waypoint(\n",
-    "        lat_traffic, lon_traffic, lat_end, lon_end)\n",
-    "    bearing_diff = wrap(bearing_track - bearing_end, -math.pi, math.pi)\n",
-    "\n",
-    "    if (bearing_diff > math.pi/2 or bearing_diff < -math.pi/2):\n",
-    "        return_value = 0\n",
-    "\n",
-    "    crosstrack_distance = (dist_to_end) * math.sin(bearing_diff)\n",
-    "\n",
-    "    return return_value, crosstrack_distance \n"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2342,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "#uav location \n",
-    "origin=[32.617013,-96.490564]\n",
-    "h = 0 #x\n",
-    "k = 0 #y\n",
-    "uav_z = 1000\n",
-    "\n",
-    "vx_now = 0\n",
-    "vy_now = 0\n",
-    "vz_now = 0\n",
-    "\n",
-    "crosstrack_separation = 500\n",
-    "vertical_separation = 500\n",
-    "collision_time_threshold = 60.0\n",
-    "\n",
-    "rot_step = 1.86\n",
-    "\n"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2343,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "lines=[\n",
-    "    [[10000,10000],[h,k]],\n",
-    "    [[1000,-1000],[h,k]],\n",
-    "    [[-5000,-4000],[h,k]],\n",
-    "    [[-3000,3000],[h,k]],\n",
-    "    [[-200,200],[h,k]],\n",
-    "    [[0,0],[h,k]],\n",
-    "    [[50,100],[h,k]],\n",
-    "    [[1000000,9000000],[h,k]],\n",
-    "    [[800,30],[h,k]],\n",
-    "    [[7000,3],[h,k]],\n",
-    "]\n",
-    "\n",
-    "fig = go.Figure()\n",
-    "    \n",
-    "[plot_segment(fig,seg_lonlat(i)) for i in lines]\n",
-    "\n",
-    "fig.update_layout(\n",
-    "    margin ={'l':0,'t':0,'b':0,'r':0},\n",
-    "    mapbox = {\n",
-    "        'center': {'lon': origin[1], 'lat': origin[0]},\n",
-    "        'style': \"stamen-terrain\",\n",
-    "        'zoom': 13})\n",
-    "\n",
-    "fig.write_html('traffic.html', auto_open=True)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2344,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "\n",
-    "dist_xy = []\n",
-    "\n",
-    "for i in range(len(lines)):\n",
-    "    r = math.sqrt(lines[i][0][0]*lines[i][0][0] + lines[i][0][1]*lines[i][0][1])\n",
-    "    dist_xy.append(r)  "
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2345,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "a = [seg_lonlat(i) for i in lines]\n",
-    "\n",
-    "lat = []\n",
-    "lon = []\n",
-    "address = []\n",
-    "\n",
-    "for i in range(0,len(a)):\n",
-    "    #print(str(a[i][0]['lat'])+\",\"+str(a[i][0]['lon']))\n",
-    "    lat.append(a[i][0]['lat'])\n",
-    "    lon.append(a[i][0]['lon'])\n",
-    "    address.append(random.randint(1000,9999))\n"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2364,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "[6751, 2839, 2431, 3222, 2171, 7666, 4249, 8438, 4110, 5798]\n"
-     ]
-    }
-   ],
-   "source": [
-    "print(address)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2346,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "heading = np.arange(-math.pi, math.pi, math.radians(rot_step)) "
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2347,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "headings = []\n",
-    "\n",
-    "ct_dist = []\n",
-    "ct_distance_conflict_threshold = []\n",
-    "\n",
-    "vertical_arr = []\n",
-    "vertical_check = []\n",
-    "\n",
-    "dist_xyz = []\n",
-    "traffic_vel = []\n",
-    "time_check = []\n",
-    "collision_times = []"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2348,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "for j in range(len(dist_xy)):\n",
-    "\n",
-    "    prediction_dist = dist_xy[j] + 1000     \n",
-    "\n",
-    "    for i in heading:\n",
-    "\n",
-    "        headings.append(i)       \n",
-    "\n",
-    "        lat_end, lon_end, bearing, radius_ratio = waypoint_from_heading_and_distance(lat[j],lon[j],i,prediction_dist)\n",
-    "        \n",
-    "\n",
-    "        return_value, ct  = get_distance_to_line(origin[0],origin[1],lat[j],lon[j],lat_end, lon_end)\n",
-    "\n",
-    "        ct = abs(ct)\n",
-    "\n",
-    "        #print(lat_end,lon_end, bearing, radius_ratio, return_value, ct)\n",
-    "\n",
-    "        ct_dist.append(ct)\n",
-    "        if(ct < crosstrack_separation and return_value):\n",
-    "            ct_distance_conflict_threshold.append(True)\n",
-    "        else:\n",
-    "            ct_distance_conflict_threshold.append(False)\n",
-    "\n",
-    "        vertical_separation_check = random.choice([True, False])\n",
-    "        if (vertical_separation_check):\n",
-    "            z_diff = random.randrange(0, vertical_separation-1)\n",
-    "            vertical_check.append(True)\n",
-    "        else:\n",
-    "            z_diff = random.randrange(vertical_separation+1, 1000)\n",
-    "            vertical_check.append(False)\n",
-    "            \n",
-    "        sign = random.choice([-1, 1])\n",
-    "        vertical_arr.append(sign*z_diff+uav_z)\n",
-    "\n",
-    "        xyz = math.sqrt(z_diff*z_diff + dist_xy[j]*dist_xy[j])\n",
-    "        dist_xyz.append(xyz)\n",
-    "\n",
-    "        collision_time_check = random.choice([True, False])\n",
-    "        if (collision_time_check):\n",
-    "            time_to_collision = random.randrange(1, collision_time_threshold)\n",
-    "            time_check.append(True)\n",
-    "            collision_times.append(time_to_collision)\n",
-    "        else:\n",
-    "            time_to_collision = random.randrange(collision_time_threshold+1, 10*collision_time_threshold)\n",
-    "            time_check.append(False)\n",
-    "            collision_times.append(time_to_collision)\n",
-    "\n",
-    "        vel = xyz/time_to_collision\n",
-    "        traffic_vel.append(math.sqrt((vel*vel)/2))\n",
-    "\n",
-    "        "
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2349,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "lat_traffic = np.array([])\n",
-    "\n",
-    "for i in lat:\n",
-    "    for j in range(len(heading)):\n",
-    "        lat_now = np.array([i])\n",
-    "        lat_traffic = np.concatenate([lat_traffic, lat_now])\n",
-    "\n",
-    "\n",
-    "lon_traffic = np.array([])\n",
-    "\n",
-    "for i in lon:\n",
-    "    for j in range(len(heading)):\n",
-    "        lon_now = np.array([i])\n",
-    "        lon_traffic = np.concatenate([lon_traffic, lon_now])\n"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2350,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "in_conflict = []\n",
-    "\n",
-    "for i in range(len(time_check)):\n",
-    "    if((time_check[i]== True) and (vertical_check[i] == True) and (ct_distance_conflict_threshold[i] == True)):\n",
-    "        in_conflict.append(True)\n",
-    "    else:\n",
-    "        in_conflict.append(False)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2351,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "#print(vertical_check)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2352,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "946\n"
-     ]
-    }
-   ],
-   "source": [
-    "print(vertical_arr[78])"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2353,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "#print(headings)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2354,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "13903.21572374496\n"
-     ]
-    }
-   ],
-   "source": [
-    "print(ct_dist[78])"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2355,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "#print(ct_distance_conflict_threshold)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2356,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "False\n"
-     ]
-    }
-   ],
-   "source": [
-    "print(time_check[78])"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2357,
-   "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "456"
-      ]
-     },
-     "execution_count": 2357,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "collision_times[78]"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2358,
-   "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "21.929984429241845"
-      ]
-     },
-     "execution_count": 2358,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "traffic_vel[78]"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2359,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "False\n"
-     ]
-    }
-   ],
-   "source": [
-    "print(in_conflict[78])"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2360,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "158\n"
-     ]
-    }
-   ],
-   "source": [
-    "print(np.count_nonzero(in_conflict))"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2361,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "dist_xy 10\n",
-      "lat_traffic 1940\n",
-      "lon_traffic 1940\n"
-     ]
-    }
-   ],
-   "source": [
-    "print(\"dist_xy\", len(dist_xy))\n",
-    "print(\"lat_traffic\", len(lat_traffic))\n",
-    "print(\"lon_traffic\", len(lon_traffic))\n",
-    "\n"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2362,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "\"\"\"      \t\n",
-    "\tstruct traffic_data_s {\n",
-    "\tdouble lat_traffic\n",
-    "\tdouble lon_traffic\n",
-    "\tfloat alt_traffic\n",
-    "\tfloat heading_traffic\n",
-    "\tfloat vxy_traffic\n",
-    "\tfloat vz_traffic\n",
-    "\tbool in_conflict\n",
-    "\"\"\"\n",
-    "\n",
-    "traffic_data = np.zeros(shape=(7, len(lat_traffic)))\n",
-    "\n",
-    "traffic_data = np.concatenate([traffic_data,[lat_traffic]])\n",
-    "\n",
-    "traffic_data = np.concatenate([traffic_data,[lon_traffic]])\n",
-    "\n",
-    "\n",
-    "alt_traffic = np.asarray(vertical_arr)\n",
-    "traffic_data = np.concatenate([traffic_data,[alt_traffic]])\n",
-    "\n",
-    "headings_arr = np.asarray(headings)\n",
-    "traffic_data = np.concatenate([traffic_data,[headings_arr]])\n",
-    "\n",
-    "vxy_traffic = np.asarray(traffic_vel)\n",
-    "traffic_data = np.concatenate([traffic_data,[vxy_traffic]])\n",
-    "\n",
-    "vz_traffic = np.asarray(traffic_vel)\n",
-    "traffic_data = np.concatenate([traffic_data,[vz_traffic]])\n",
-    "\n",
-    "in_conflict_arr = np.asarray(in_conflict)\n",
-    "traffic_data = np.concatenate([traffic_data,[in_conflict_arr]])\n",
-    "\n",
-    "for i in range(7):\n",
-    "\ttraffic_data = np.delete(traffic_data,0,0)\n",
-    "\n"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2363,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "(1940, 7)\n"
-     ]
-    }
-   ],
-   "source": [
-    "np.savetxt(\"foo.csv\", traffic_data.transpose(), delimiter=\",\",newline='}, \\n {')\n",
-    "print(traffic_data.transpose().shape)"
-   ]
-  }
- ],
- "metadata": {
-  "kernelspec": {
-   "display_name": "Python 3.8.10 64-bit",
-   "language": "python",
-   "name": "python3"
-  },
-  "language_info": {
-   "codemirror_mode": {
-    "name": "ipython",
-    "version": 3
-   },
-   "file_extension": ".py",
-   "mimetype": "text/x-python",
-   "name": "python",
-   "nbconvert_exporter": "python",
-   "pygments_lexer": "ipython3",
-   "version": "3.8.10"
-  },
-  "toc": {
-   "base_numbering": 1,
-   "nav_menu": {},
-   "number_sections": true,
-   "sideBar": true,
-   "skip_h1_title": false,
-   "title_cell": "Table of Contents",
-   "title_sidebar": "Contents",
-   "toc_cell": false,
-   "toc_position": {},
-   "toc_section_display": true,
-   "toc_window_display": false
-  },
-  "vscode": {
-   "interpreter": {
-    "hash": "e7370f93d1d0cde622a1f8e1c04877d8463912d04d973331ad4851f04de6915a"
-   }
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 4
-}
@@ -213,11 +213,9 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f
 		cell_voltage += _params.r_internal * current_a;

 	} else {
-		vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-		_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint);
-		const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
-		const float throttle = thrust_setpoint.length();
-
+		actuator_controls_s actuator_controls{};
+		_actuator_controls_0_sub.copy(&actuator_controls);
+		const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
 		_throttle_filter.update(throttle);

 		if (!_battery_initialized) {
--- a/Show More
+++ b/Show More