Log camera topics

Eliminate intermediate variable
Add fw state for path following
2026-06-12 07:40:05 +08:00 · 2023-03-30 19:05:50 +02:00 · 2023-03-28 18:48:26 +02:00 · 2023-03-28 17:13:07 +02:00 · 2023-03-27 10:28:43 -07:00 · 2023-03-27 10:28:43 -07:00
260 changed files with 11776 additions and 7491 deletions
@@ -105,3 +105,6 @@ src/modules/simulator/simulator_config.h
 src/systemcmds/topic_listener/listener_generated.cpp

 !src/drivers/distance_sensor/broadcom/afbrs50/Lib/*
+
+# colcon
+log/
@@ -104,8 +104,9 @@ These boards fully comply with Pixhawk Standard, and are maintained by the PX4-A
 These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers.

 * [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html)
-* [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
-* [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
+* [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html)
+* [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
+* [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
 * [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
 * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
 * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
@@ -137,11 +137,6 @@ 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)

@@ -208,6 +203,24 @@ 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,6 +11,8 @@
 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
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -27,7 +27,6 @@ param set-default FW_W_EN 1
 param set-default MIS_LTRMIN_ALT 30
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -9,6 +9,8 @@

 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
@@ -277,7 +277,7 @@ then
 	# Override namespace if environment variable is defined
 	microdds_ns="-n $PX4_MICRODDS_NS"
 fi
-microdds_client start -t udp -p 8888 $microdds_ns
+microdds_client start -t udp -h 127.0.0.1 -p 8888 $microdds_ns

 if param greater -s MNT_MODE_IN -1
 then
@@ -17,6 +17,8 @@

 . ${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
@@ -79,7 +79,6 @@ param set-default MC_YAWRATE_MAX 20
 param set-default MC_AIRMODE 1

 param set-default MIS_DIST_1WP 100
-param set-default MIS_DIST_WPS 100000
 param set-default MIS_TAKEOFF_ALT 15

 param set-default MPC_XY_P 0.8
@@ -12,6 +12,8 @@

 . ${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
@@ -46,7 +46,6 @@ param set-default RTL_LAND_DELAY -1
 #
 param set-default NAV_ACC_RAD 10

-param set-default MIS_DIST_WPS 5000
 param set-default MIS_TAKEOFF_ALT 25
 param set-default MIS_TKO_LAND_REQ 2

@@ -23,7 +23,6 @@ param set-default EKF2_FUSE_BETA 1

 param set-default HTE_VXY_THR 2.0

-param set-default MIS_DIST_WPS 5000
 param set-default MIS_TKO_LAND_REQ 2

 param set-default MPC_ACC_HOR_MAX 2
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+
+"""
+Converts IP address given in decimal dot notation to int32 to be used in XRCE_DDS_0_CFG parameter
+and vice-versa
+
+@author: Beniamino Pozzan (beniamino.pozzan@phd.unipd.it)
+"""
+
+
+import argparse
+
+parser = argparse.ArgumentParser(
+    prog = 'convert_ip',
+    description = 'converts IP to int32 and viceversa'
+    )
+parser.add_argument('input',
+                    type=str,
+                    help='IP address to convert')
+parser.add_argument('-r','--reverse',
+                    action='store_true',
+                    help='converts from int32 to dot decimal')
+
+args = parser.parse_args()
+
+if( args.reverse == False ):
+
+    try:
+        ip_parts = [int(x) for x in args.input.split('.')]
+    except:
+        raise ValueError("Not a valid IP")
+    if( len(ip_parts)!=4 ):
+        raise ValueError("Not a valid IP")
+    if( not all(x>=0 and x<255 for x in ip_parts) ):
+        raise ValueError("Not a valid IP")
+
+    ip = (ip_parts[0]<<24) + (ip_parts[1]<<16) + (ip_parts[2]<<8) + ip_parts[3]
+
+    if(ip & 0x80000000):
+        ip = -0x100000000 + ip
+
+    print(ip)
+
+else:
+    try:
+        ip = int(args.input)
+    except:
+        raise ValueError("Not a valid IP")
+    if(ip < 0):
+        ip = ip + 0x100000000
+    print('{}.{}.{}.{}'.format(ip>>24, (ip>>16)&0xff, (ip>>8)&0xff, ip&0xff))
@@ -7,7 +7,7 @@ import os
 class JsonOutput():
    def __init__(self, groups):
        all_json = {}
-        all_json['version'] = 1
+        all_json['version'] = 2
        component = {}
        all_json['components'] = {1: component} #1: autopilot component

@@ -35,7 +35,20 @@ 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 &
-	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
+
+	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 ${@}

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

@@ -106,7 +119,7 @@ if [ -z ${SCRIPT} ]; then
 	fi

 	while [ $n -lt $num_vehicles ]; do
-		spawn_model ${vehicle_model} $n
+		spawn_model ${vehicle_model} $(($n + 1))
 		n=$(($n + 1))
 	done
 else
@@ -127,7 +140,7 @@ else
 		m=0
 		while [ $m -lt ${target_number} ]; do
 			export PX4_SIM_MODEL=gazebo-classic_${target_vehicle}
-			spawn_model ${target_vehicle}${LABEL} $n $target_x $target_y
+			spawn_model ${target_vehicle}${LABEL} $(($n + 1)) $target_x $target_y
 			m=$(($m + 1))
 			n=$(($n + 1))
 		done
@@ -110,10 +110,6 @@ 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':
@@ -232,9 +228,6 @@ 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)
@@ -510,18 +503,7 @@ class Graph(object):
                elif current_scope.name == 'uorb_tests': # skip this
                    return
                elif current_scope.name == 'uorb':
-
-                    # 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
+                    return # skip this

            line_number = 0
            for full_line in content.splitlines():
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=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
@@ -39,7 +39,6 @@ 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
@@ -3,10 +3,6 @@
 # board specific defaults
 #------------------------------------------------------------------------------

-param set-default CANNODE_ARM_STAT 1
-param set-default CANNODE_SUB_ESC 1
-param set-default CANNODE_SUB_SERV 1
-
 pwm_out start

 dshot start
@@ -67,7 +67,6 @@ 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
@@ -26,7 +26,6 @@ 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_UAVCAN=y
@@ -15,6 +15,8 @@ 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,6 +14,8 @@ 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
@@ -30,7 +30,6 @@ 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,7 +75,6 @@ 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,7 +31,6 @@ 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
@@ -77,7 +76,6 @@ 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,4 +9,5 @@ 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,7 +69,6 @@ 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,6 +6,7 @@ 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,7 +69,6 @@ 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
@@ -209,7 +209,6 @@ CONFIG_STM32H7_SPI2=y
 CONFIG_STM32H7_SPI4=y
 CONFIG_STM32H7_SPI4_DMA=y
 CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
-CONFIG_STM32H7_SPI_DMA=y
 CONFIG_STM32H7_TIM1=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
@@ -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
@@ -28,7 +28,6 @@ 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
@@ -27,7 +27,6 @@ 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
@@ -69,7 +68,6 @@ 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,7 +22,6 @@ 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
@@ -66,7 +65,6 @@ 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,7 +21,6 @@ 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
@@ -64,7 +63,6 @@ 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,7 +22,6 @@ 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
@@ -66,7 +65,6 @@ 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
@@ -31,7 +31,6 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=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
@@ -30,7 +30,6 @@ 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_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -29,7 +29,6 @@ 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
@@ -71,7 +70,6 @@ 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
@@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
+CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -12,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_MUORB_SLPI=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_COMMANDER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
@@ -1,6 +1,7 @@
 CONFIG_PLATFORM_POSIX=y
 CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
+CONFIG_BOARD_ROOTFSDIR="/data/px4"
 CONFIG_DRIVERS_QSHELL_POSIX=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_DATAMAN=y
@@ -17,4 +18,3 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_ORB_COMMUNICATOR=y
-CONFIG_BOARD_ROOTFSDIR="/data/px4"
@@ -25,7 +25,6 @@ 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
@@ -72,7 +71,6 @@ 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,7 +25,6 @@ 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
@@ -24,7 +24,6 @@ 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
@@ -25,7 +25,6 @@ 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,7 +69,6 @@ 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,7 +25,6 @@ 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,7 +70,6 @@ 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,7 +25,6 @@ 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,7 +69,6 @@ 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
@@ -26,7 +26,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=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
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=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
@@ -27,7 +27,6 @@ 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
@@ -28,7 +28,6 @@ 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
@@ -23,13 +23,18 @@ 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
@@ -1,6 +1,5 @@
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
@@ -1,6 +1,5 @@
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
@@ -31,7 +31,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=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
@@ -30,7 +30,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=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
@@ -29,7 +29,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=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
@@ -2,6 +2,6 @@
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
+CONFIG_CYPHAL_ESC_CONTROLLER=y
 CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
 CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_ESC_CONTROLLER=y
@@ -14,7 +14,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
@@ -35,7 +35,6 @@ 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
@@ -9,7 +9,6 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_AIRSPEED_SELECTOR=n
@@ -17,7 +17,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_EXAMPLES_FAKE_GPS=n
@@ -11,7 +11,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
@@ -68,7 +68,6 @@ 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,12 +141,13 @@
 #define GPIO_HW_VER_SENSE      /* PC1 */  GPIO_ADC123_INP11
 #define HW_INFO_INIT_PREFIX    "V6C"

-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0, 10 and Mini 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,6 +79,11 @@ 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[] = {
@@ -101,6 +106,7 @@ 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,7 +46,17 @@ 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(V6C10, {
+	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, {
 		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,7 +28,6 @@ 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
@@ -73,7 +72,6 @@ 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,7 +73,6 @@ 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
@@ -28,7 +28,6 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=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
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=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
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=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
@@ -4,7 +4,7 @@ project(googletest-download NONE)

 include(ExternalProject)
 ExternalProject_Add(googletest
-	URL https://github.com/google/googletest/archive/58d77fa8070e8cec2dc1ed015d66b454c8d78850.zip # 1.12.1
+	URL https://github.com/google/googletest/archive/b796f7d44681514f58a683a3a71ff17c94edb0c1.zip # 1.13
 	SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
 	BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
 	CONFIGURE_COMMAND ""
@@ -46,4 +46,7 @@ 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}")
@@ -1,19 +0,0 @@
-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,10 +1,5 @@
 uint64 timestamp			# time since system start (microseconds)

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

 # TOPICS actuator_controls_status_0 actuator_controls_status_1
@@ -39,7 +39,6 @@ include(px4_list_make_absolute)
 set(msg_files
 	ActionRequest.msg
 	ActuatorArmed.msg
-	ActuatorControls.msg
 	ActuatorControlsStatus.msg
 	ActuatorMotors.msg
 	ActuatorOutputs.msg
@@ -100,6 +99,7 @@ set(msg_files
 	GimbalManagerStatus.msg
 	GpsDump.msg
 	GpsInjectData.msg
+	GimbalControls.msg
 	Gripper.msg
 	HealthReport.msg
 	HeaterStatus.msg
@@ -0,0 +1,7 @@
+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
@@ -36,7 +36,6 @@ 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|
@@ -118,11 +117,6 @@ 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,3 +3,6 @@ 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,3 +3,6 @@ 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,6 +93,13 @@ 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,7 +251,6 @@ 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,6 +392,10 @@ 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/systemlib/crc.h>
+#include <lib/crc/crc.h>

 #include <float.h>
 #include <mathlib/mathlib.h>
@@ -50,6 +50,7 @@ px4_add_module(
 		devices/src/femtomes.cpp
 		devices/src/nmea.cpp
 		devices/src/unicore.cpp
+		devices/src/crc.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
@@ -1208,7 +1208,7 @@ GPS::publish()

 		if (_report_gps_pos.jamming_state != _jamming_state) {

-			if (_report_gps_pos.jamming_state > sensor_gps_s::JAMMING_STATE_OK) {
+			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);
 			}
@@ -147,10 +147,16 @@ 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 antenna is in
- * front. The offset angle increases clockwise.
+ * 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 90 if the rover antenna is placed on the right side of the vehicle and the moving base antenna is on the left side.
+ * 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).
 *
 * @min 0
 * @max 360
@@ -68,7 +68,6 @@
 #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,6 +59,10 @@ 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
 */
@@ -151,6 +155,10 @@ 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;
@@ -378,12 +386,22 @@ bool crypto_get_encrypted_key(crypto_session_handle_t handle,
 					  max_len);

 	} else {
-		// 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;
+		switch (handle.algorithm) {

-		if (key_sz % min_block) {
-			*max_len += 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;
 		}
 	}

@@ -423,24 +441,6 @@ 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;
 	}
@@ -33,8 +33,8 @@ if DRIVERS_UAVCANNODE
        bool "Include GNSS fix"
        default n

-    config UAVCANNODE_HYDROMETER_MEASUREMENT
-        bool "Include hydrometer measurement"
+    config UAVCANNODE_HYGROMETER_MEASUREMENT
+        bool "Include hygrometer measurement"
        default n

    config UAVCANNODE_LIGHTS_COMMAND
@@ -52,9 +52,9 @@
 #include "Publishers/FlowMeasurement.hpp"
 #endif // CONFIG_UAVCANNODE_FLOW_MEASUREMENT

-#if defined(CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT)
+#if defined(UAVCANNODE_HYGROMETER_MEASUREMENT)
 #include "Publishers/HygrometerMeasurement.hpp"
-#endif // CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT
+#endif // UAVCANNODE_HYGROMETER_MEASUREMENT

 #if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 #include "Publishers/GnssFix2.hpp"
@@ -360,9 +360,9 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_publisher_list.add(new FlowMeasurement(this, _node));
 #endif // CONFIG_UAVCANNODE_FLOW_MEASUREMENT

-#if defined(CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT)
+#if defined(UAVCANNODE_HYGROMETER_MEASUREMENT)
 	_publisher_list.add(new HygrometerMeasurement(this, _node));
-#endif // CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT
+#endif // UAVCANNODE_HYGROMETER_MEASUREMENT

 #if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 	_publisher_list.add(new GnssFix2(this, _node));
@@ -438,13 +438,7 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 #endif // CONFIG_UAVCANNODE_RTK_DATA

 #if defined(CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND)
-	int32_t enable_servo_array_command_sub = 0;
-	param_get(param_find("CANNODE_SUB_SERV"), &enable_servo_array_command_sub);
-
-	if (enable_servo_array_command_sub != 0) {
-		_subscriber_list.add(new ServoArrayCommand(_node));
-	}
-
+	_subscriber_list.add(new ServoArrayCommand(_node));
 #endif // CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND

 	for (auto &subscriber : _subscriber_list) {
@@ -0,0 +1,503 @@
+/****************************************************************************
+ *
+ *   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]));
+	}
+	*/
+}
@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   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};
+
+};
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
JaeyoungLim	21b4052d82	Log camera topics	2023-03-30 19:05:50 +02:00
Jaeyoung Lim	8296d02b1e	Eliminate intermediate variable	2023-03-28 18:48:26 +02:00
Jaeyoung Lim	e3db31f5ab	Add fw state for path following	2023-03-28 17:13:07 +02:00
Ramon Roche	319f3db5e1	tools: ip converter remove verbose output	2023-03-27 10:28:43 -07:00
Beniamino Pozzan	51dd6b783c	microdds_client: Mark parameter as used only if truly used Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-27 10:28:43 -07:00
Beniamino Pozzan	c7f67a3328	microdds_client: remove multi-instances supports, add agent IP as PX4 parameter - multi-instances support is removed from the parameter definitions. - XRCE_DDS_AG_IP allows to define the agent IP when udp transport is used. The parameter is used by default if the module is started without the -h argument. - XRCE_DDS_PRT allows to define the agent listning udp port when the udp transport is uded. The parameter is used by default if the module is started without the -p argument. - Tools/convert_ip.py assists in converting ips in decimal dot notation into int32 notation. Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-27 10:28:43 -07:00
Daniel Mesham	6fddbea3e4	microdds_client: fix check for length of participant XML (#21374 )	2023-03-27 10:03:09 -07:00
Silvan Fuhrer	ba17a137e1	Navigator: MissionFeasibiltyCheck first WP: check only to current vehicle position Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-27 17:43:33 +02:00
Silvan Fuhrer	465ddddbfd	Navigator: take min of vehicle position or Home distance for first WP check To prevent mission rejections when vehicle is currently far away from Mission start, but planned home is close to it. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-27 17:43:33 +02:00
Silvan Fuhrer	d8ed35c422	Navigator: take vehicle position for first-WP-distance-check And only use Home position if current vehicle position is unknown. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-27 17:43:33 +02:00
Silvan Fuhrer	4d2a31afe3	Navigator: remove max distance between WP mission feasibility check (MIS_DIST_WPS) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-27 17:43:33 +02:00
Junwoo Hwang	9c001f2e40	Fix SITL Test failure: Place PX4 instance runner after Gazebo server runner (#21230 ) * MAVSDK_Test_Runner: Place PX4 instance runner after Gazebo server runner - This was a nasty bug where starting PX4 instance first, then starting Gazebo server was causing PX4 instance' EKF to freak out, probably because it doesn't like getting data a while after it is started - Detailed breakdown is given here: https://github.com/PX4/PX4-Autopilot/issues/21229#issuecomment-1450761542 - This now guarantees that such edge case won't occur and MAVSDK test will run as it should * MAVSDK Test Runner: Retain comment within 79 character limit - To pass flake8 python style check * Update test/mavsdk_tests/mavsdk_test_runner.py Co-authored-by: Julian Oes <julian@oes.ch> --------- Co-authored-by: Julian Oes <julian@oes.ch>	2023-03-26 11:27:01 +09:00
PX4 BuildBot	58a5aa26a0	Update submodule mavlink to latest Sat Mar 25 00:39:09 UTC 2023 - mavlink in PX4/Firmware (73104cab2e49118903cf32bdf11f6ea917fa394e): https://github.com/mavlink/mavlink/commit/2bdcab78b53d1e349079b43c9d726036abe0617c - mavlink current upstream: https://github.com/mavlink/mavlink/commit/e1e68da5e6f2e44c3126cafc0780f6fee2c2b4f2 - Changes: https://github.com/mavlink/mavlink/compare/2bdcab78b53d1e349079b43c9d726036abe0617c...e1e68da5e6f2e44c3126cafc0780f6fee2c2b4f2 e1e68da5 2023-03-08 Hamish Willee - Add quickstart to the README (#1960) 9b92bf2e 2023-03-08 Hamish Willee - Fix enum value for payload battery (#1959) 81e9fcd5 2023-03-08 Hamish Willee - Allocate range for ras-a and indicate next free range (#1957) abbda253 2023-03-02 Hamish Willee - Indicate preference for COMMAND_INT (#1955) d3a8e4b8 2023-02-08 Julian Oes - development: add command to configure SiK radio	2023-03-24 21:43:59 -04:00
PX4 BuildBot	8306fb96ea	boards: update all NuttX defconfigs	2023-03-24 10:03:14 -04:00
PX4 BuildBot	efab9be488	update all px4board kconfig	2023-03-24 10:01:58 -04:00
Daniel Agar	4d9369dc67	Update world_magnetic_model to latest Fri Mar 24 11:14:06 UTC 2023	2023-03-24 09:39:26 -04:00
Silvan Fuhrer	e8a8e7c677	VTOL: increase deafult of VT_QC_T_ALT_LOSS from 10 to 20m Testing has shown that 10m is a bit too tight, most of all as this check is also run in not height-controlled flight modes (eg Stabilized), and there 10m altitude loss during transitions can happen easily. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-24 13:33:52 +01:00
Silvan Fuhrer	e58d33284a	vtol: correct/improve quad-chute param description Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-24 13:33:52 +01:00
Matthias Grob	937d27f8ee	Helicopter: add offset for yaw compensation based on collective pitch	2023-03-24 07:16:36 +01:00
Beniamino Pozzan	eeb90ac70a	gazebo-classic/sitl_mutliple_run.sh: start spawning drones from index 1 (#21183 ) Fix microdds key session collision when drones with index 0 and 1 are used together Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-24 05:58:26 +01:00
Antonio Sanjurjo C	1ea993880c	Fix typo in WORK_ITEM_TYPE (#21311 ) Signed-off-by: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com>	2023-03-24 05:49:14 +01:00
Julian Oes	09a0089c80	README: add Cube Orange+ (#21361 ) This adds the Cube Orange+ to the list, and also changes the Hex naming to CubePilot as that is how it is sold/marketed now. Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-24 05:21:43 +01:00
Daniel Agar	cc2b367c0b	drivers/uavcannode: fix typo (hydrometer->hygrometer)	2023-03-23 22:06:55 -04:00
Silvan Fuhrer	5b6d1e9290	TECS: change default for FW_T_I_GAIN_THR from 0.3 to more appropriate 0.1 Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-23 15:00:36 +01:00
Silvan Fuhrer	ac6c9c857a	Gimbal: remove shutter and retraction handling Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-23 06:59:02 +01:00
Frederic Taillandier	18898f1876	updating sitl-gazebo classic (#21352 ) Co-authored-by: root <root@DESKTOP-OINCOKE>	2023-03-22 14:04:02 +01:00
Silvan Fuhrer	d532578ecc	EKF2: only check for EKF2_BETA_FUSION when corresponding kconfig option is set Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-22 10:05:32 +01:00
Silvan Fuhrer	98d07ad1f3	disable side slip fusion for tailsitters The attitude frame is wrong for tailsitters doing side slip fusion for wind estimation. It doesn't take into account that the frames is 90deg tilted in FW flight. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-22 10:05:32 +01:00
Silvan Fuhrer	07531d29b7	FailureDetector: fix attitude check for tailsitter -set roll/pitch used for failure detection during transition to 0 -rotate estimated attitude 90° in FW flight Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-22 10:05:32 +01:00
Silvan Fuhrer	edc570adff	AirspeedSelector: fix attitude rotation for tailsitter Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-22 10:05:32 +01:00
bresch	6bee0893de	add circle intersection unit tests	2023-03-22 10:03:48 +01:00
bresch	af345c88e9	TrajMath: use fmaxf instead of max	2023-03-22 10:03:48 +01:00
bresch	c7bddda1db	MC auto: add maximum RC assist distance during landing	2023-03-22 10:03:48 +01:00
bresch	d6fa42fefd	mc att: add unit tests for AttitudeControlMath helper	2023-03-22 10:01:27 +01:00
bresch	edaf5bf0cd	mc att: extract tilt yaw error compensation to lib	2023-03-22 10:01:27 +01:00
bresch	f65daf7259	mc att: do not pass by euler angles to generate att sp from sticks	2023-03-22 10:01:27 +01:00
bresch	3e3307c0d0	ekf2: fix mag declination innovation angle wrapping	2023-03-21 21:07:28 -04:00
David Sidrane	64e90b91aa	px4_fmu-v6c:Add Mini & fix Rev 1 (#21226 )	2023-03-21 09:16:35 -07:00
Daniel Agar	32a5bd32ad	ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)	2023-03-21 11:25:34 -04:00
Daniel Agar	0784901a66	ekf2: add kconfig option to enable/disable optical flow fusion	2023-03-21 11:25:34 -04:00
Matthias Grob	9f8fa99d70	matrix: use stack allocation for debug output string	2023-03-21 15:01:26 +01:00
Matthias Grob	b1435c6e34	matrix: use output stream function template to enable proper automatic output in gtest unit tests that compare two `Matrix`, `Vector` or two `Dual` objects. Credits to @jwidauer for showing me this trick.	2023-03-21 15:01:26 +01:00
Matthias Grob	b0189d95af	Use new Vector4 class	2023-03-21 15:01:26 +01:00
Matthias Grob	08d6465ce5	matrix: add explicit Vector4 type	2023-03-21 15:01:26 +01:00
Matthias Grob	da0e40c72b	googletest: switch to latest version 1.12.1 -> 1.13	2023-03-21 15:01:26 +01:00
bresch	91364ba901	mc land detect: use z_deriv and vz for vert movement check z_deriv is less prone to bias errors	2023-03-21 09:22:58 -04:00
Beat Küng	22c94f805a	component information: add translation support	2023-03-20 21:57:25 -04:00
Jukka Laitinen	631046b962	src/drivers/sw_crypto: Correct RSA lengths in the example sw crypto backend driver Maximum plaintext length was wrong, it is just k - 2 * hLen -2, where k is RSA key modulus length and hLen is hash length Also minimum block that can be encrypted didn't make sense for RSA-OAEP, it is just 1 byte, the rest will be padded. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2023-03-20 21:54:40 -04:00
Jukka Laitinen	b26669b085	Add crypto_deinit function for crypto_backend module Bootloader needs to have a mechanism to de-initialize crypto, in case some HW accelerator is being used. This adds the needed function for it Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2023-03-20 21:54:40 -04:00
Beat Küng	8497d3388f	fix mavlink_ulog: use hrt_abstime instead of float The float inaccuracy was leading to problems on SITL with large timestamps.	2023-03-20 21:47:41 -04:00
Daniel Agar	1524bd39b5	.gitignore: ignore generate top level log/ folder (colcon)	2023-03-20 11:06:12 -04:00
Daniel Agar	4363b09421	ekf2: add kconfig option to enable/disable external vision fusion	2023-03-20 10:12:17 -04:00
Daniel Agar	d47f96f1a5	ekf2: add kconfig option to enable/disable AUX velocity fusion	2023-03-20 10:12:17 -04:00
Daniel Agar	4270a303ab	ekf2: add kconfig option to enable/disable airspeed and sideslip fusion	2023-03-20 10:12:17 -04:00
Daniel Agar	98ff1afc19	ekf2: add kconfig option to enable/disable GNSS yaw	2023-03-20 10:12:17 -04:00
Daniel Agar	8b2205810b	ekf2: add kconfig option to enable/disable baro compensation	2023-03-20 10:12:17 -04:00
Daniel Agar	fe0e3acf09	ekf2: add kconfig option to enable/disable drag fusion	2023-03-20 10:12:17 -04:00
frederic@auterion.com	9a56b0a70d	refactoring jinja-gen call in a cleaner way	2023-03-20 14:50:56 +01:00
numan	ed174d54e9	fix typo in LeddarOne.cpp added missing "~"	2023-03-20 07:51:00 +01:00
alexklimaj	e2de62e38b	GPS only warn if jamming state is critical	2023-03-17 21:38:28 -04:00
Silvan Fuhrer	0afda910d1	VTOL: make sure that all virtual topics are updated Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-17 19:43:01 +01:00
Andrew Brahim	71c746b9bf	drivers/distance_sensor/lightware_sf45_serial: fix path to crc lib (no longer in systemlib) Signed-off-by: dirksavage88 <dirksavage88@gmail.com>	2023-03-17 09:35:46 -04:00
alexklimaj	0ae296bfe2	ARK CANnode fix params	2023-03-16 14:32:36 -04:00
Silvan Fuhrer	c7d4d1f45c	FWPositionControl: HOTFIX: zero airspeed rate estimation input to TECS Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-16 16:58:08 +01:00
Silvan Fuhrer	b33a8686f7	FWPositionController: only store acceleration and velocity in x Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-16 16:58:08 +01:00
Silvan Fuhrer	28db47480d	TECS: remove ununsed getSteRate() Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-16 16:58:08 +01:00
Daniel Agar	6ec2b902cc	ROMFS: only copy bootloader binary if systemcmds/bl_update included	2023-03-16 09:43:56 -04:00
Silvan Fuhrer	d5e1f0fdaa	uorb_graph: remove handling for actuator_controls topic Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-16 11:55:45 +01:00
Silvan Fuhrer	17cd65a239	Navigator: remove support for DO_SET_SERVO Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-16 11:55:45 +01:00
Silvan Fuhrer	a1812dbde0	gimbal: move gimbal controls to new dedicated topic Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-16 11:55:45 +01:00
Silvan Fuhrer	1218d9b2fc	mavlink_mission: remove support for DO_SET_CAMERA_ZOOM Camera controls should not happen through the flight controller, and the control allocation has no means of controlling the camera zoom. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-16 11:55:45 +01:00
Silvan Fuhrer	27658354da	MavlinkReceiver: remove support for SET_ACTUATOR_CONTROL_TARGET Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-16 11:55:45 +01:00
Silvan Fuhrer	b16f16598b	VTOL: remove virtual actuator_controls and instead use virtual torque/thrust topics MC/FW rate controller and auto tuner: remove actuator_controls AirshipAttControl: remove actuator_controls MulticopterLandDetector: remove actuator_controls mavlink streams vfr_hud and high_latency2: remove actuator_controls RoverPositionController: remove actuator_controls UUVAttitudeController: remove actuator_controls battery: use length of thrust_setpoint for throttle compensation VehicleMagnetometer: use length of thrust_setpoint for throttle compensation Signed-off-by: Silvan Fuhrer	2023-03-16 11:55:45 +01:00
akkawimo	bcd6e7adee	comment out fake_traffic to prevent flash overflow	2023-03-16 07:32:43 +01:00
Mohamad Akkawi	9de52bb5ec	[navigator] Tone Down traffic warnings - add unit tests for adsb conflict detection - move adsb conflict detection to lib/adsb and adsb conflict class - use containers/Array.hpp for buffer array - expand fake_traffic	2023-03-16 07:32:43 +01:00
Julian Oes	89548e4f9e	gps: Support for Unicore HEADINGA sentence in NMEA This adds a parser for Unicore sentences sent in-between NMEA sentences. This brings support for the Unicore UM982 module which publishes heading information based on the two antennas. Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-15 21:55:15 -04:00