* parameter renaming

* move hor-motion check to method
set state to unknown during init (re-init)
2026-06-06 16:20:05 +08:00 · 2025-08-26 15:08:55 +02:00 · 2025-08-26 15:08:55 +02:00 · 2025-08-26 15:08:55 +02:00 · 2025-08-26 15:08:55 +02:00 · 2025-08-26 15:08:53 +02:00
100 changed files with 2184 additions and 536 deletions
@@ -36,6 +36,11 @@ CONFIG:
      buildType: RelWithDebInfo
      settings:
        CONFIG: px4_sitl_test
+    px4_sitl_zenoh:
+      short: px4_sitl_zenoh
+      buildType: RelWithDebInfo
+      settings:
+        CONFIG: px4_sitl_test
    px4_io-v2_default:
      short: px4_io-v2
      buildType: MinSizeRel
@@ -73,6 +73,11 @@ menu "Toolchain"
        help
            relative path to the ROMFS root directory

+    config BOARD_ADDITIONAL_INIT
+        string "Additional init file"
+        help
+            additional configurable init file to include in the ROMFS
+
    config BOARD_IO
        string "IO board name"
        default "px4_io-v2_default"
@@ -511,6 +511,7 @@ validate_module_configs:
 	@find "$(SRC_DIR)"/src/modules "$(SRC_DIR)"/src/drivers "$(SRC_DIR)"/src/lib -name *.yaml -type f \
 	-not -path "$(SRC_DIR)/src/lib/mixer_module/*" \
 	-not -path "$(SRC_DIR)/src/modules/uxrce_dds_client/dds_topics.yaml" \
+	-not -path "$(SRC_DIR)/src/modules/zenoh/dds_topics.yaml" \
 	-not -path "$(SRC_DIR)/src/modules/zenoh/zenoh-pico/*" \
 	-not -path "$(SRC_DIR)/src/lib/events/libevents/*" \
 	-not -path "$(SRC_DIR)/src/lib/cdrstream/*" \
@@ -202,6 +202,31 @@ foreach(board_rc_file ${OPTIONAL_BOARD_RC})

 endforeach()

+if(config_additional_init)
+	if(EXISTS "${PX4_BOARD_DIR}/init/${config_additional_init}")
+		file(RELATIVE_PATH rc_file_relative ${PX4_SOURCE_DIR} ${PX4_BOARD_DIR}/init/${config_additional_init})
+		message(STATUS "ROMFS:  Adding ${rc_file_relative} -> /etc/init.d/rc.additional_init")
+
+		add_custom_command(
+			OUTPUT
+				${romfs_gen_root_dir}/init.d/rc.additional_init
+				${config_additional_init}.stamp
+			COMMAND ${CMAKE_COMMAND} -E copy_if_different ${PX4_BOARD_DIR}/init/${config_additional_init} ${romfs_gen_root_dir}/init.d/rc.additional_init
+			COMMAND ${CMAKE_COMMAND} -E touch ${config_additional_init}.stamp
+			DEPENDS
+				${PX4_BOARD_DIR}/init/${config_additional_init}
+				romfs_copy.stamp
+			COMMENT "ROMFS: copying ${config_additional_init}"
+		)
+
+		list(APPEND extras_dependencies
+			${config_additional_init}.stamp
+			)
+	else()
+		message(FATAL_ERROR "BOARD_ADDITIONAL_INIT file not found at: ${PX4_BOARD_DIR}/init/${config_additional_init}")
+	endif()
+endif()
+

 # board extras
 set(OPTIONAL_BOARD_EXTRAS)
@@ -56,6 +56,17 @@ then
 fi
 unset BOARD_RC_DEFAULTS

+#
+# Optional additional init file: rc.additional_init
+#
+set BOARD_RC_ADDITIONAL_INIT ${R}etc/init.d/rc.additional_init
+if [ -f $BOARD_RC_ADDITIONAL_INIT ]
+then
+	echo "Board additional init: ${BOARD_RC_ADDITIONAL_INIT}"
+	. $BOARD_RC_ADDITIONAL_INIT
+fi
+unset BOARD_RC_ADDITIONAL_INIT
+
 #
 # Start system state indicator.
 #
@@ -324,6 +324,11 @@ fi

 uxrce_dds_client start -t udp -p $uxrce_dds_port $uxrce_dds_ns

+if param greater -s ZENOH_ENABLE 0
+then
+	zenoh start
+fi
+
 if param greater -s MNT_MODE_IN -1
 then
 	gimbal start
@@ -217,6 +217,17 @@ else
 	fi
 	unset BOARD_RC_DEFAULTS

+	#
+	# Optional additional init file: rc.additional_init
+	#
+	set BOARD_RC_ADDITIONAL_INIT ${R}etc/init.d/rc.additional_init
+	if [ -f $BOARD_RC_ADDITIONAL_INIT ]
+	then
+		echo "Board additional init: ${BOARD_RC_ADDITIONAL_INIT}"
+		. $BOARD_RC_ADDITIONAL_INIT
+	fi
+	unset BOARD_RC_ADDITIONAL_INIT
+
 	# Load airframe configuration based on SYS_AUTOSTART parameter
 	if ! param compare SYS_AUTOSTART 0
 	then
@@ -49,14 +49,14 @@ for field in spec.parsed_fields():
            (package, name) = genmsg.names.package_resource_name(field.base_type)
            package = package or spec.package # convert '' to package

-            print('typedef px4_msg_%s px4_msg_px4__msg__%s;' % (name,name))
+            print('typedef px4_msgs_msg_%s px4_msgs_msg_px4_msgs__msg__%s;' % (name,name))
 }@



-typedef struct @uorb_struct px4_msg_@(file_base_name);
+typedef struct @uorb_struct px4_msgs_msg_@(file_base_name);

-extern const struct dds_cdrstream_desc px4_msg_@(file_base_name)_cdrstream_desc;
+extern const struct dds_cdrstream_desc px4_msgs_msg_@(file_base_name)_cdrstream_desc;

 #ifdef __cplusplus
 }
@@ -42,6 +42,7 @@ import os
 import argparse
 import re
 import sys
+import json

 try:
    import em
@@ -124,7 +125,7 @@ def generate_by_template(output_file, template_file, em_globals):
    return True


-def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir):
+def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir, rihs_path):
    # generate cpp file with topics list
    filenames = []
    for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
@@ -138,11 +139,27 @@ def generate_topics_list_file_from_files(files, outputdir, template_filename, te
    for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
        full_base_names.append(filename.replace(".msg",""))

-    topics = []
-    for msg_filename in files:
-        topics.extend(get_topics(msg_filename))
+    rihs01_hashes = dict()
+    if rihs_path != '':
+        for topic in full_base_names:
+            with open(rihs_path + "/msg/" + topic + ".json") as f:
+                d = json.load(f)
+                assert d['type_hashes'][0]['hash_string'][:7] == 'RIHS01_'

-    tl_globals = {"msgs": filenames, "topics": topics, "datatypes": datatypes, "full_base_names": full_base_names}
+                rihs01_hash = d['type_hashes'][0]['hash_string'][7:]
+
+                byte_array = [f"0x{rihs01_hash[i:i+2]}" for i in range(0, len(rihs01_hash), 2)]
+                c_code = f"{{ {', '.join(byte_array)} }};"
+                rihs01_hashes[topic] = c_code
+
+    topics = []
+    datatypes_with_topics = dict()
+    for msg_filename in files:
+        datatype = re.sub(r'(?<!^)(?=[A-Z])', '_', os.path.basename(msg_filename)).lower().replace(".msg","")
+        datatypes_with_topics[datatype] = get_topics(msg_filename)
+        topics.extend(datatypes_with_topics[datatype])
+
+    tl_globals = {"msgs": filenames, "topics": topics, "datatypes": datatypes, "full_base_names": full_base_names, "rihs01_hashes": rihs01_hashes, "datatypes_with_topics": datatypes_with_topics}
    tl_template_file = os.path.join(templatedir, template_filename)
    tl_out_file = os.path.join(outputdir, template_filename.replace(".em", ""))

@@ -162,13 +179,15 @@ if __name__ == "__main__":
    parser.add_argument('-p', dest='prefix', default='',
                        help='string added as prefix to the output file '
                        ' name when converting directories')
+    parser.add_argument('--rihs', dest='rihs', default='',
+                        help='path where rihs01 json files located')
    args = parser.parse_args()

    if args.zenoh_config:
-        generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[0], args.templatedir)
+        generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[0], args.templatedir, args.rihs)
        exit(0)
    elif args.zenoh_pub_sub:
-        generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[1], args.templatedir)
+        generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[1], args.templatedir, args.rihs)
        exit(0)
    else:
        print('Error: either --headers or --sources must be specified')
@@ -74,7 +74,7 @@ full_base_names.sort()

@[for idx, topic_name in enumerate(datatypes)]@
@{
-type_topic_count = len([e for e in topic_names_all if e.startswith(topic_name)])
+type_topic_count = len(datatypes_with_topics[topic_name])
 }@
 #ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper())
 #  define CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT @(type_topic_count)
@@ -88,9 +88,28 @@ type_topic_count = len([e for e in topic_names_all if e.startswith(topic_name)])
        CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT + \
@[end for]        0

+@[for topic_name, rihs01_hash in rihs01_hashes.items()]@
+const uint8_t @(topic_name)_hash[32] = @(rihs01_hash)
+@[end for]
+
+@[for idx, topic_name in enumerate(datatypes)]@
+#ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper())
+@{
+topic_names = datatypes_with_topics[topic_name]
+}@
+const orb_metadata* @(topic_name)_topic_meta[@(len(topic_names))] = {
+@[for topic_name_inst in topic_names]@
+	ORB_ID(@(topic_name_inst)),
+@[end for]};
+#endif
+@[end for]
+
 typedef struct {
+	const char *data_type_name;
 	const uint32_t *ops;
-	const orb_metadata* orb_meta;
+	const uint8_t *hash;
+	const orb_metadata** orb_topic;
+	const uint8_t orb_topics_size;
 } UorbPubSubTopicBinder;

 const UorbPubSubTopicBinder _topics[ZENOH_PUBSUB_COUNT] {
@@ -100,54 +119,95 @@ uorb_id_idx = 0
@[for idx, topic_name in enumerate(datatypes)]@
 #ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper())
@{
-topic_names = [e for e in topic_names_all if e.startswith(topic_name)]
+topic_names = datatypes_with_topics[topic_name]
 }@
-@[for topic_name_inst in topic_names]@
 		{
-		  px4_msg_@(topic_dict[topic_name])_cdrstream_desc.ops.ops,
-		  ORB_ID(@(topic_name_inst))
+		  "@(topic_name)",
+		  px4_msgs_msg_@(topic_dict[topic_name])_cdrstream_desc.ops.ops,
+		  @(topic_dict[topic_name])_hash,
+		  @(topic_name)_topic_meta,
+		  @(len(topic_names)),
 		},
-@{
-uorb_id_idx += 1
-}@
-@[end for]#endif
+#endif
@[end for]
 };

-uORB_Zenoh_Publisher* genPublisher(const orb_metadata *meta) {
+uORB_Zenoh_Publisher* genPublisher(const orb_metadata *meta, int instance) {
    for (auto &pub : _topics) {
-        if(pub.orb_meta->o_id == meta->o_id) {
-            return new uORB_Zenoh_Publisher(meta, pub.ops);
+        for(int i = 0; i < pub.orb_topics_size; i++) {
+            if(pub.orb_topic[i]->o_id == meta->o_id) {
+                return new uORB_Zenoh_Publisher(meta, pub.ops, instance);
+            }
        }
    }
    return NULL;
 }


-uORB_Zenoh_Publisher* genPublisher(const char *name) {
+uORB_Zenoh_Publisher* genPublisher(const char *name, int instance) {
    for (auto &pub : _topics) {
-        if(strcmp(pub.orb_meta->o_name, name) == 0) {
-            return new uORB_Zenoh_Publisher(pub.orb_meta, pub.ops);
+        for(int i = 0; i < pub.orb_topics_size; i++) {
+            if(strcmp(pub.orb_topic[i]->o_name, name) == 0) {
+                return new uORB_Zenoh_Publisher(pub.orb_topic[i], pub.ops, instance);
+            }
        }
    }
    return NULL;
 }


-Zenoh_Subscriber* genSubscriber(const orb_metadata *meta) {
+Zenoh_Subscriber* genSubscriber(const orb_metadata *meta, int instance) {
    for (auto &sub : _topics) {
-        if(sub.orb_meta->o_id == meta->o_id) {
-            return new uORB_Zenoh_Subscriber(meta, sub.ops);
+        for(int i = 0; i < sub.orb_topics_size; i++) {
+            if(sub.orb_topic[i]->o_id == meta->o_id) {
+                return new uORB_Zenoh_Subscriber(meta, sub.ops, instance);
+            }
        }
    }
    return NULL;
 }


-Zenoh_Subscriber* genSubscriber(const char *name) {
+Zenoh_Subscriber* genSubscriber(const char *name, int instance) {
    for (auto &sub : _topics) {
-        if(strcmp(sub.orb_meta->o_name, name) == 0) {
-            return new uORB_Zenoh_Subscriber(sub.orb_meta, sub.ops);
+        for(int i = 0; i < sub.orb_topics_size; i++) {
+            if(strcmp(sub.orb_topic[i]->o_name, name) == 0) {
+                return new uORB_Zenoh_Subscriber(sub.orb_topic[i], sub.ops, instance);
+            }
+        }
+    }
+    return NULL;
+}
+
+const char* getTypeName(const char *name) {
+    for (auto &sub : _topics) {
+        for(int i = 0; i < sub.orb_topics_size; i++) {
+            if(strcmp(sub.orb_topic[i]->o_name, name) == 0) {
+                return sub.data_type_name;
+            }
+        }
+    }
+    return NULL;
+}
+
+
+const uint8_t* getRIHS01_Hash(const orb_metadata *meta) {
+    for (auto &sub : _topics) {
+        for(int i = 0; i < sub.orb_topics_size; i++) {
+            if(sub.orb_topic[i]->o_id == meta->o_id) {
+                return sub.hash;
+            }
+        }
+    }
+    return NULL;
+}
+
+const uint8_t* getRIHS01_Hash(const char *name) {
+    for (auto &sub : _topics) {
+        for(int i = 0; i < sub.orb_topics_size; i++) {
+            if(strcmp(sub.orb_topic[i]->o_name, name) == 0) {
+                return sub.hash;
+            }
        }
    }
    return NULL;
@@ -14,6 +14,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_ZENOH=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
@@ -182,6 +182,7 @@ CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
 CONFIG_NET_TCP_DELAYED_ACK=y
+CONFIG_NET_TCP_KEEPALIVE=y
 CONFIG_NET_TCP_WRITE_BUFFERS=y
 CONFIG_NET_TIMESTAMP=y
 CONFIG_NET_UDP=y
@@ -207,6 +208,7 @@ CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_MUTEX_TYPES=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=272000
 CONFIG_RAM_START=0x20400000
@@ -1,57 +0,0 @@
-# CONFIG_BOARD_ROMFSROOT is not set
-CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
-CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
-CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
-CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
-CONFIG_BOARD_UAVCAN_INTERFACES=1
-CONFIG_COMMON_LIGHT=y
-CONFIG_DRIVERS_ADC_BOARD_ADC=y
-CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_IRLOCK=y
-CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
-CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
-CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_SAFETY_BUTTON=y
-CONFIG_DRIVERS_UAVCAN=y
-CONFIG_EXAMPLES_FAKE_GPS=y
-CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
-CONFIG_MODULES_BATTERY_STATUS=y
-CONFIG_MODULES_CAMERA_FEEDBACK=y
-CONFIG_MODULES_COMMANDER=y
-CONFIG_MODULES_ESC_BATTERY=y
-CONFIG_MODULES_EVENTS=y
-CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_MODE_MANAGER=y
-CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
-CONFIG_MODULES_FW_RATE_CONTROL=y
-CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
-CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
-CONFIG_MODULES_LAND_DETECTOR=y
-CONFIG_MODULES_LOAD_MON=y
-CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
-CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
-CONFIG_MODULES_MANUAL_CONTROL=y
-CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
-CONFIG_MODULES_MC_POS_CONTROL=y
-CONFIG_MODULES_MC_RATE_CONTROL=y
-CONFIG_MODULES_NAVIGATOR=y
-CONFIG_MODULES_RC_UPDATE=y
-CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_MODULES_ZENOH=y
-CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_DMESG=y
-CONFIG_SYSTEMCMDS_DUMPFILE=y
-CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
-CONFIG_SYSTEMCMDS_NSHTERM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_REFLECT=y
-CONFIG_SYSTEMCMDS_SERIAL_TEST=y
-CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
@@ -75,6 +75,7 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_MODULES_ZENOH=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -49,6 +49,7 @@ CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DEV_URANDOM=y
 CONFIG_ETH0_PHY_DP83825I=y
 CONFIG_FAT_DMAMEMORY=y
 CONFIG_FAT_LCNAMES=y
@@ -155,8 +156,8 @@ CONFIG_NETDEV_LATEINIT=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_DHCPC=y
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_DRIPADDR=0xA290AFE
 CONFIG_NETINIT_RETRY_MOUNTPATH=10
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
@@ -167,15 +168,18 @@ CONFIG_NET_BROADCAST=y
 CONFIG_NET_CAN=y
 CONFIG_NET_CAN_EXTID=y
 CONFIG_NET_CAN_NOTIFIER=y
+CONFIG_NET_CAN_RAW_FILTER_MAX=1
 CONFIG_NET_CAN_RAW_TX_DEADLINE=y
 CONFIG_NET_CAN_SOCK_OPTS=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_IGMP=y
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
 CONFIG_NET_TCP_DELAYED_ACK=y
+CONFIG_NET_TCP_KEEPALIVE=y
 CONFIG_NET_TCP_WRITE_BUFFERS=y
 CONFIG_NET_TIMESTAMP=y
 CONFIG_NET_UDP=y
@@ -196,6 +200,7 @@ CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_TYPES=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=1048576
 CONFIG_RAM_START=0x20200000
@@ -89,6 +89,7 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_MODULES_ZENOH=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -55,6 +55,7 @@ CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DEV_URANDOM=y
 CONFIG_ETH0_PHY_MULTI=y
 CONFIG_FAT_DMAMEMORY=y
 CONFIG_FAT_LCNAMES=y
@@ -214,15 +215,18 @@ CONFIG_NET_BROADCAST=y
 CONFIG_NET_CAN=y
 CONFIG_NET_CAN_EXTID=y
 CONFIG_NET_CAN_NOTIFIER=y
+CONFIG_NET_CAN_RAW_FILTER_MAX=1
 CONFIG_NET_CAN_RAW_TX_DEADLINE=y
 CONFIG_NET_CAN_SOCK_OPTS=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_IGMP=y
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
 CONFIG_NET_TCP_DELAYED_ACK=y
+CONFIG_NET_TCP_KEEPALIVE=y
 CONFIG_NET_TCP_WRITE_BUFFERS=y
 CONFIG_NET_TIMESTAMP=y
 CONFIG_NET_UDP=y
@@ -243,6 +247,7 @@ CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_TYPES=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=1835008
@@ -339,6 +339,11 @@ if(EXISTS ${BOARD_DEFCONFIG})
 		endif()
 	endif()

+	# ADDITIONAL INIT
+	if(ADDITIONAL_INIT)
+		set(config_additional_init ${ADDITIONAL_INIT} CACHE INTERNAL "additional init" FORCE)
+	endif()
+
 	if(UAVCAN_INTERFACES)
 		set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE)
 	endif()
@@ -90,6 +90,8 @@ This is documented below.
 The best way to customize the system startup is to introduce a [new frame configuration](../dev_airframes/adding_a_new_frame.md).
 The frame configuration file can be included in the firmware or on an SD Card.

+#### Dynamic customization
+
 If you only need to "tweak" the existing configuration, such as starting one more application or setting the value of a few parameters, you can specify these by creating two files in the `/etc/` directory of the SD Card:

 - [/etc/config.txt](#customizing-the-configuration-config-txt): modify parameter values
@@ -106,7 +108,7 @@ If editing on Windows use a suitable editor.
 These files are referenced in PX4 code as `/fs/microsd/etc/config.txt` and `/fs/microsd/etc/extras.txt`, where the root folder of the microsd card is identified by the path `/fs/microsd`.
 :::

-#### Customizing the Configuration (config.txt)
+##### Customizing the Configuration (config.txt)

 The `config.txt` file can be used to modify parameters.
 It is loaded after the main system has been configured and _before_ it is booted.
@@ -118,7 +120,7 @@ param set-default PWM_MAIN_DIS3 1000
 param set-default PWM_MAIN_MIN3 1120
 ```

-#### Starting Additional Applications (extras.txt)
+##### Starting Additional Applications (extras.txt)

 The `extras.txt` can be used to start additional applications after the main system boot.
 Typically these would be payload controllers or similar optional custom components.
@@ -145,3 +147,28 @@ The following example shows how to start custom applications:

  mandatory_app start     # Will abort boot if mandatory_app is unknown or fails
  ```
+
+#### Additional customization
+
+In rare cases where the desired setup cannot be achieved through frame configuration or dynamic customization,
+you can add a script that will be contained in the binary.
+
+**Note**: In almost all cases, you should use a frame configuration. This method should only be used for
+edge-cases such as customizing `cannode` based boards.
+
+- Add a new init script in `boards/<vendor>/<board>/init` that will run during board startup. For example:
+  ```sh
+  # File: boards/<vendor>/<board>/init/rc.additional
+  param set-default <param> <value>
+  ```
+
+- Add a new board variant in `boards/<vendor>/<board>/<variant>.px4board` that includes the additional script. For example:
+  ```sh
+  # File: boards/<vendor>/<board>/var.px4board
+  CONFIG_BOARD_ADDITIONAL_INIT="rc.additional"
+  ```
+
+- Compile the firmware with your new variant by appending the variant name to the compile target. For example:
+  ```sh
+  make <target>_var
+  ```
@@ -316,6 +316,43 @@ For SIH as SITL (no FC):

 For specific examples see the `_sihsim_` airframes in [ROMFS/px4fmu_common/init.d-posix/airframes](https://github.com/PX4/PX4-Autopilot/blob/main/ROMFS/px4fmu_common/init.d-posix/airframes/) (SIH as SITL) and [ROMFS/px4fmu_common/init.d/airframes](https://github.com/PX4/PX4-Autopilot/tree/main/ROMFS/px4fmu_common/init.d/airframes) (SIH on FC).

+## Controlling Actuators in SIH
+
+:::warning
+If you want to control throttling actuators in SIH, make sure to remove propellers for safety.
+:::
+
+In some scenarios, it may be useful to control an actuator while running SIH. For example, you might want to verify that winches or grippers are functioning correctly by checking the servo responses.
+
+To enable actuator control in SIH:
+
+1. Configure PWM parameters in the airframe file:
+
+Ensure your airframe file includes the necessary parameters to map PWM outputs to the correct channels.
+
+For example, if a servo is connected to MAIN 3 and you want to map it to AUX1 on your RC, use the following command:
+
+`param set-default PWM_MAIN_FUNC3 407`
+
+You can find a full list of available values for `PWM_MAIN_FUNCn` [here](../advanced_config/parameter_reference.md#PWM_MAIN_FUNC1). In this case, `407` maps the MAIN 3 output to AUX1 on the RC.
+
+Alternatively, you can use the [`PWM_AUX_FUNCn`](../advanced_config/parameter_reference.md#PWM_AUX_FUNC1) parameters.
+
+You may also configure the output as desired:
+- Disarmed PWM: ([`PWM_MAIN_DISn`](../advanced_config/parameter_reference.md#PWM_MAIN_DIS1) / [`PWM_AUX_DIS1`](../advanced_config/parameter_reference.md#PWM_AUX_DIS1))
+- Minimum PWM ([`PWM_MAIN_MINn`](../advanced_config/parameter_reference.md#PWM_MAIN_MIN1) / [`PWM_AUX_MINn`](../advanced_config/parameter_reference.md#PWM_AUX_MIN1))
+- Maximum PWM ([`PWM_MAIN_MAXn`](../advanced_config/parameter_reference.md#PWM_MAIN_MAX1) / [`PWM_AUX_MAXn`](../advanced_config/parameter_reference.md#PWM_AUX_MAX1))
+
+2. Manually start the PWM output driver
+
+For safety, the PWM driver is not started automatically in SIH. To enable it, run the following command in the MAVLink shell:
+
+`pwm_out start`
+
+And to disable it again:
+
+`pwm_out stop`
+
 ## Dynamic Models

 The dynamic models for the various vehicles are:
@@ -424,9 +424,11 @@ add_dependencies(uorb_msgs prebuild_targets uorb_headers)
 if(CONFIG_LIB_CDRSTREAM)
 	set(uorb_cdr_idl)
 	set(uorb_cdr_msg)
+	set(uorb_cdr_hash)
 	set(uorb_cdr_idl_uorb)
 	set(idl_include_path ${PX4_BINARY_DIR}/uORB/idl)
 	set(idl_out_path ${idl_include_path}/px4/msg)
+	set(idl_rihs01_out_path ${idl_include_path}/px4)
 	set(idl_uorb_path ${PX4_BINARY_DIR}/msg/px4/msg)

 	# Make sure that CycloneDDS has been checkout out
@@ -457,6 +459,7 @@ if(CONFIG_LIB_CDRSTREAM)
 		configure_file(${msg_file} ${idl_out_path}/${msg}.msg COPYONLY)
 		list(APPEND uorb_cdr_idl ${idl_out_path}/${msg}.idl)
 		list(APPEND uorb_cdr_msg ${idl_out_path}/${msg}.msg)
+		list(APPEND uorb_cdr_hash ${idl_out_path}/${msg}.json)
 		list(APPEND uorb_cdr_idl_uorb ${idl_uorb_path}/${msg}.h)
 	endforeach()

@@ -476,6 +479,25 @@ if(CONFIG_LIB_CDRSTREAM)
 		VERBATIM
 		)

+	file(CREATE_LINK ${idl_rihs01_out_path} ${idl_include_path}/px4_msgs SYMBOLIC)
+
+	# Generate IDL from .msg using rosidl_adapter
+	# Note this is a submodule inside PX4 hence no ROS2 installation required
+	add_custom_command(
+		OUTPUT ${uorb_cdr_hash}
+		COMMAND ${CMAKE_COMMAND}
+		        -E env "PYTHONPATH=${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_adapter:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_cli:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_parser:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_generator_type_description"
+			${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/idl2rihs01.py
+			--output-dir ${idl_rihs01_out_path}
+			${uorb_cdr_idl}
+		DEPENDS
+			${uorb_cdr_idl}
+			git_cyclonedds
+		COMMENT "Generating RIHS01 hashes from IDL"
+		WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+		VERBATIM
+		)
+
 	# Generate C definitions from IDL
 	set(CYCLONEDDS_DIR ${PX4_SOURCE_DIR}/src/lib/cdrstream/cyclonedds)
 	include("${CYCLONEDDS_DIR}/cmake/Modules/Generate.cmake")
@@ -505,6 +527,7 @@ if(CONFIG_LIB_CDRSTREAM)
 		DEPENDS
 			uorb_cdrstream
 			${msg_files}
+			${uorb_cdr_hash}
 			${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream/uorb_idl_header.h.em
 			${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
 			${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py
@@ -534,8 +557,10 @@ if(CONFIG_MODULES_ZENOH)
 			-f ${msg_files}
 			-o ${PX4_BINARY_DIR}/src/modules/zenoh/
 			-e ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh
+			--rihs ${idl_rihs01_out_path}
 		DEPENDS
 			${msg_files}
+			${uorb_cdr_hash}
 			${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em
 			${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
 		COMMENT "Generating Zenoh Topic Code"
@@ -49,6 +49,7 @@ bool cs_valid_fake_pos          # 41 - true if a valid constant position is bein
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
 bool cs_gnss_vel                # 44 - true if GNSS velocity measurement fusion is intended
+bool cs_rng_kin_unknown      	# 45 - true when the range finder kinematic consistency check is not running

 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes
@@ -0,0 +1,14 @@
+# Arming check request.
+#
+# Broadcast message to request arming checks be reported by all registered components, such as external ROS 2 navigation modes.
+# All registered components should respond with an ArmingCheckReply message that indicates their current mode requirements, and any arming failure information.
+# The request is sent regularly, even while armed, so that the FMU always knows the current arming state for external modes, and can forward it to ground stations.
+#
+# The reply will include the published request_id, allowing correlation of all arming check information for a particular request.
+# The reply will also include the registration_id for each external component, provided to it during the registration process (RegisterExtComponentReply).
+
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp  # [us] Time since system start.
+
+uint8 request_id  # Id of this request. Allows correlation with associated ArmingCheckReply messages.
@@ -8,6 +8,7 @@

 #include "translation_airspeed_validated_v1.h"
 #include "translation_arming_check_reply_v1.h"
+#include "translation_arming_check_request_v1.h"
 #include "translation_battery_status_v1.h"
 #include "translation_event_v1.h"
 #include "translation_home_position_v1.h"
@@ -0,0 +1,36 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+// Translate ArmingCheckRequest v0 <--> v1
+#include <px4_msgs_old/msg/arming_check_request_v0.hpp>
+#include <px4_msgs/msg/arming_check_request.hpp>
+
+class ArmingCheckRequestV1Translation {
+public:
+    using MessageOlder = px4_msgs_old::msg::ArmingCheckRequestV0;
+    static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+    using MessageNewer = px4_msgs::msg::ArmingCheckRequest;
+    static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+    static constexpr const char* kTopic = "/fmu/out/arming_check_request";
+
+    static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+	    msg_newer.timestamp = msg_older.timestamp;
+
+	    msg_newer.request_id = msg_older.request_id;
+
+	    msg_newer.valid_registrations_mask = 0xffffffff;
+    }
+
+    static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+	    msg_older.timestamp = msg_newer.timestamp;
+
+	    msg_older.request_id = msg_newer.request_id;
+    }
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(ArmingCheckRequestV1Translation);
@@ -7,8 +7,10 @@
 # The reply will include the published request_id, allowing correlation of all arming check information for a particular request.
 # The reply will also include the registration_id for each external component, provided to it during the registration process (RegisterExtComponentReply).

-uint32 MESSAGE_VERSION = 0
+uint32 MESSAGE_VERSION = 1

 uint64 timestamp  # [us] Time since system start.

 uint8 request_id  # Id of this request. Allows correlation with associated ArmingCheckReply messages.
+
+uint32 valid_registrations_mask # Bitmask of valid registration ID's (the bit is also cleared if flagged as unresponsive)
@@ -759,6 +759,10 @@ GPS::run()
 			ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2;
 			break;

+		case 6:
+			ubx_mode = GPSDriverUBX::UBXMode::GroundControlStation;
+			break;
+
 		default:
 			break;

@@ -94,6 +94,7 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
 * F9P units are connected to each other.
 * Modes 3 and 4 only require UART1 on each F9P connected to the Autopilot or Can Node. UART RX DMA is required.
 * RTK is still possible with this setup.
+ * Mode 6 is intended for use with a ground control station (not necessarily an RTK correction base).
 *
 * @min 0
 * @max 1
@@ -103,6 +104,7 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
 * @value 3 Heading (Rover With Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
 * @value 4 Moving Base (Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
 * @value 5 Rover with Static Base on UART2 (similar to Default, except coming in on UART2)
+ * @value 6 Ground Control Station (UART2 outputs NMEA)
 *
 * @reboot_required true
 * @group GPS
@@ -118,7 +118,7 @@ private:
 	static constexpr int BATTERY_INDEX_2 = 2;
 	static constexpr int BATTERY_INDEX_3 = 3;
 	static constexpr int BATTERY_INDEX_4 = 4;
-	static constexpr int SAMPLE_INTERVAL_US = 20_ms; // assume higher frequency UAVCAN feedback than 50Hz
+	static constexpr int SAMPLE_INTERVAL_US = 500_ms; // Typical message rate for a CAN battery monitor should be 2-5Hz.

 	static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_4, "Battery array too big");

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+import argparse
+import pathlib
+import sys
+import os
+import tempfile
+import json
+
+try:
+    from rosidl_generator_type_description import generate_type_hash
+except ImportError:
+    # modifying sys.path and importing the Python package with the same
+    # name as this script does not work on Windows
+    rosidl_generator_type_description_root = os.path.dirname(os.path.dirname(__file__))
+    rosidl_generator_type_description_module = os.path.join(
+        rosidl_generator_type_description_root, 'rosidl_generator_type_description', '__init__.py')
+    if not os.path.exists(rosidl_generator_type_description_module):
+        raise
+    from importlib.machinery import SourceFileLoader
+
+    loader = SourceFileLoader('rosidl_generator_type_description', rosidl_generator_type_description_module)
+    rosidl_generator_type_description = loader.load_module()
+    generate_type_hash = rosidl_generator_type_description.generate_type_hash
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(
+        description=f'Convert px4 .idl files to rihs01')
+    parser.add_argument(
+        'interface_files', nargs='+',
+        help='The interface files to convert')
+    parser.add_argument(
+        '--output-dir', '-o',
+         help='The directory to save converted files (default: current directory)')
+    args = parser.parse_args(sys.argv[1:])
+
+    # So for some odd reason rosidl doesn't do proper cli arguments but believes
+    # that some magically crafted json is better
+
+    idl_files = []
+
+    type_hash_arguments = {}
+    type_hash_arguments['package_name'] = "px4_msgs"
+    type_hash_arguments['output_dir'] = args.output_dir
+    type_hash_arguments["idl_tuples"] = idl_files
+
+    for interface_file in args.interface_files:
+        # So file path need to be magically encoded with a : to let the parser do its thing
+
+        interface_file = str(pathlib.Path(interface_file)).replace("px4/msg", "px4:msg")
+        idl_files.append(interface_file)
+
+    json_file = tempfile.NamedTemporaryFile(mode="w+")
+    json.dump(type_hash_arguments, json_file)
+    json_file.flush()
+
+    generate_type_hash(json_file.name)
@@ -52,6 +52,6 @@ if __name__ == '__main__':
        package_dir = interface_file.parent.absolute()

        convert_msg_to_idl(
-            package_dir, 'px4',
+            package_dir, 'px4_msgs',
            interface_file.absolute().relative_to(package_dir),
            interface_file.parent)
@@ -1895,8 +1895,7 @@ void Commander::run()
 		_actuator_armed.armed = isArmed();
 		_actuator_armed.prearmed = getPrearmState();
 		_actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed();
-		_actuator_armed.lockdown = ((_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)
-					    || _multicopter_throw_launch.isThrowLaunchInProgress());
+		_actuator_armed.lockdown = _multicopter_throw_launch.isThrowLaunchInProgress();
 		// _actuator_armed.kill // action_request_s::ACTION_KILL
 		_actuator_armed.termination = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION);
 		// _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION
@@ -229,8 +229,10 @@ void ExternalChecks::update()
 	int max_num_updates = arming_check_reply_s::ORB_QUEUE_LENGTH;

 	while (_arming_check_reply_sub.update(&reply) && --max_num_updates >= 0) {
-		if (reply.registration_id < MAX_NUM_REGISTRATIONS && registrationValid(reply.registration_id)
-		    && _current_request_id == reply.request_id) {
+		const bool valid = reply.registration_id < MAX_NUM_REGISTRATIONS && registrationValid(reply.registration_id);
+		const bool timed_out = now > reply.timestamp + 300_ms;
+
+		if (!timed_out && valid && _current_request_id == reply.request_id) {
 			_reply_received_mask |= 1u << reply.registration_id;
 			_registrations[reply.registration_id].num_no_response = 0;
 			_registrations[reply.registration_id].waiting_for_first_response = false;
@@ -291,6 +293,15 @@ void ExternalChecks::update()
 		arming_check_request_s request{};
 		request.request_id = ++_current_request_id;
 		request.timestamp = hrt_absolute_time();
+		request.valid_registrations_mask = _active_registrations_mask;
+
+		// Clear unresponsive ones
+		for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
+			if (_registrations[i].unresponsive) {
+				request.valid_registrations_mask &= ~(1u << i);
+			}
+		}
+
 		_arming_check_request_pub.publish(request);
 	}
 }
@@ -473,6 +473,29 @@ uint8_t ModeManagement::getReplacedModeIfAny(uint8_t nav_state)
 	return nav_state;
 }

+uint8_t ModeManagement::onDisarm(uint8_t stored_nav_state)
+{
+	// Switch to the owned mode if an executor is active
+	uint8_t returned_nav_state = stored_nav_state;
+
+	if (_mode_executors.valid(_mode_executor_in_charge)) {
+		returned_nav_state = _mode_executors.executor(_mode_executor_in_charge).owned_nav_state;
+	}
+
+	// Switch to Hold if the mode is unresponsive
+	if (_modes.valid(returned_nav_state)) {
+		if (_external_checks.isUnresponsive(_modes.mode(returned_nav_state).arming_check_registration_id)) {
+			returned_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+		}
+	}
+
+	// Update _mode_executor_in_charge if needed (in case stored_nav_state belongs to an executor
+	// that is currently not active)
+	onUserIntendedNavStateChange(ModeChangeSource::User, returned_nav_state);
+
+	return returned_nav_state;
+}
+
 void ModeManagement::removeModeExecutor(int mode_executor_id)
 {
 	if (mode_executor_id == -1) {
@@ -153,6 +153,8 @@ public:
 	void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override;
 	uint8_t getReplacedModeIfAny(uint8_t nav_state) override;

+	uint8_t onDisarm(uint8_t stored_nav_state) override;
+
 	uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true);

 	bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const;
@@ -209,6 +211,7 @@ public:

 	void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override {}
 	uint8_t getReplacedModeIfAny(uint8_t nav_state) override { return nav_state; }
+	uint8_t onDisarm(uint8_t stored_nav_state) override { return stored_nav_state; }

 	uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true) { return nav_state; }

@@ -92,5 +92,10 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource

 void UserModeIntention::onDisarm()
 {
-	_user_intented_nav_state = _nav_state_after_disarming;
+	if (_handler) {
+		_user_intented_nav_state = _handler->onDisarm(_nav_state_after_disarming);
+
+	} else {
+		_user_intented_nav_state = _nav_state_after_disarming;
+	}
 }
@@ -53,6 +53,8 @@ public:
 	 * @return nav_state or the mode that nav_state replaces
 	 */
 	virtual uint8_t getReplacedModeIfAny(uint8_t nav_state) = 0;
+
+	virtual uint8_t onDisarm(uint8_t stored_nav_state) = 0;
 };


@@ -76,18 +76,11 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 						measurement_var + bias_est.getBiasVar(),
 						math::max(_params.ekf2_gps_p_gate, 1.f));

-		// update the bias estimator before updating the main filter but after
-		// using its current state to compute the vertical position innovation
-		if (measurement_valid) {
-			bias_est.setMaxStateNoise(sqrtf(measurement_var));
-			bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
-		}
-
 		// determine if we should use height aiding
 		const bool common_conditions_passing = measurement_valid
 						       && _local_origin_lat_lon.isInitialized()
-						       && _gnss_checks.passed();
+						       && _gnss_checks.passed()
+						       && !_control_status.flags.gnss_fault;

 		const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
 				&& common_conditions_passing;
@@ -103,6 +96,12 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 		if (_control_status.flags.gps_hgt) {
 			if (continuing_conditions_passing) {

+				// update the bias estimator before updating the main filter but after
+				// using its current state to compute the vertical position innovation
+				bias_est.setMaxStateNoise(sqrtf(measurement_var));
+				bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
+				bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+
 				fuseVerticalPosition(aid_src);

 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
@@ -123,7 +123,8 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
 {
 	const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
 			&& _control_status.flags.tilt_align
-			&& _control_status.flags.yaw_align;
+			&& _control_status.flags.yaw_align
+			&& !_control_status.flags.gnss_fault;
 	const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed();

 	if (_control_status.flags.gnss_vel) {
@@ -36,56 +36,157 @@
 */

 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
+#include "ekf_derivation/generated/range_validation_filter_h.h"
+#include "ekf_derivation/generated/range_validation_filter_P_init.h"

-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
-		bool horizontal_motion, uint64_t time_us)
+using namespace matrix;
+
+void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom,
+				       const float dist_bottom_var)
 {
-	if (horizontal_motion) {
-		_time_last_horizontal_motion = time_us;
-	}
+	_P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
+	_Ht = sym::RangeValidationFilterH<float>();

+	_x(RangeFilter::z.idx) = z;
+	_x(RangeFilter::terrain.idx) = z - dist_bottom;
+	_initialized = true;
+	_test_ratio_lpf.reset(0.f);
+	_t_since_first_sample = 0.f;
+	_state = KinematicState::kUnknown;
+}
+
+void RangeFinderConsistencyCheck::update(const float z, const float z_var, const float vz, const float vz_var,
+		const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
+{
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;

-	if ((_time_last_update_us == 0)
-	    || (dt < 0.001f) || (dt > 0.5f)) {
+	if (dt > _kTestRatioLpfTimeConstant || _landed) {
 		_time_last_update_us = time_us;
-		_dist_bottom_prev = dist_bottom;
+		_initialized = false;
+		return;
+
+	} else if (!_initialized) {
+		init(z, z_var, dist_bottom, dist_bottom_var);
 		return;
 	}

-	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
-	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
-
-	// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
-	const float var = 2.f * dist_bottom_var / (dt * dt);
-	_innov_var = var + vz_var;
-
-	const float normalized_innov_sq = (_innov * _innov) / _innov_var;
-	_test_ratio = normalized_innov_sq / (_gate * _gate);
-	_signed_test_ratio_lpf.setParameters(dt, _signed_test_ratio_tau);
-	const float signed_test_ratio = matrix::sign(_innov) * _test_ratio;
-	_signed_test_ratio_lpf.update(signed_test_ratio);
-
-	updateConsistency(vz, time_us);
-
+	// prediction step
 	_time_last_update_us = time_us;
-	_dist_bottom_prev = dist_bottom;
+	_x(RangeFilter::z.idx) -= dt * vz;
+	_P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
+	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
+
+	// iterate through both measurements (z and dist_bottom)
+	const Vector2f measurements{z, dist_bottom};
+
+	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
+
+		// dist_bottom
+		Vector2f H = _Ht;
+		float R = dist_bottom_var;
+
+		// z, direct state measurement
+		if (measurement_idx == 0) {
+			H(RangeFilter::z.idx) = 1.f;
+			H(RangeFilter::terrain.idx) = 0.f;
+			R = z_var;
+
+		}
+
+		// residual
+		const float measurement_pred = H * _x;
+		const float y = measurements(measurement_idx) - measurement_pred;
+
+		// for H as col-vector:
+		// innovation variance S = H^T * P * H + R
+		// kalman gain K = P * H / S
+		const float S = (H.transpose() * _P * H + R)(0, 0);
+		Vector2f K = (_P * H / S);
+
+		if (measurement_idx == 0) {
+			K(RangeFilter::z.idx) = 1.f;
+
+		} else if (measurement_idx == 1) {
+			_innov = y;
+			const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
+			_test_ratio_lpf.update(test_ratio, dt);
+		}
+
+		// update step
+		_x(RangeFilter::z.idx)       += K(RangeFilter::z.idx) * y;
+		_x(RangeFilter::terrain.idx) += K(RangeFilter::terrain.idx) * y;
+
+		// covariance update with Joseph form:
+		// P = (I - K H) P (I - K H)^T + K R K^T
+		Matrix2f I;
+		I.setIdentity();
+		Matrix2f IKH = I - K.multiplyByTranspose(H);
+		_P = IKH * _P * IKH.transpose() + (K * R).multiplyByTranspose(K);
+	}
+
+	evaluateState(dt, vz, vz_var);
 }

-void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
+void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
 {
-	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
-		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
-			_is_kinematically_consistent = false;
-			_time_last_inconsistent_us = time_us;
+	// let the filter settle before starting the consistency check
+	if (_t_since_first_sample > _kTestRatioLpfTimeConstant) {
+		if (fabsf(_test_ratio_lpf.getState()) < 1.f) {
+			const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);
+
+			if (!_horizontal_motion && vertical_motion) {
+				_state = KinematicState::kConsistent;
+
+			} else if (_state == KinematicState::kConsistent || vertical_motion) {
+				_state = KinematicState::kUnknown;
+			}
+
+		} else {
+			_t_since_first_sample = 0.f;
+			_state = KinematicState::kInconsistent;
 		}

 	} else {
-		if ((fabsf(vz) > _min_vz_for_valid_consistency)
-		    && (_test_ratio < 1.f)
-		    && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
-		   ) {
-			_is_kinematically_consistent = true;
-		}
+		_t_since_first_sample += dt;
 	}
 }
+
+void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var,
+				      const float dist_bottom, const float dist_bottom_var, const uint64_t time_us,
+				      const uint8_t current_posD_reset_count
+				     )
+{
+	if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
+		_last_posD_reset_count = current_posD_reset_count;
+		_initialized = false;
+	}
+
+	update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
+}
+
+void RangeFinderConsistencyCheck::setIsLanded(const bool landed)
+{
+	if (landed) {
+		_state = KinematicState::kUnknown;
+	}
+
+	_landed = landed;
+}
+
+void RangeFinderConsistencyCheck::reset()
+{
+	if (_initialized && _state == KinematicState::kConsistent) {
+		_state = KinematicState::kUnknown;
+	}
+
+	_initialized = false;
+}
+
+void RangeFinderConsistencyCheck::setHorizontalMotion(const bool horizontal_motion)
+{
+	if (!horizontal_motion && getHorizontalMotion()) {
+		reset();
+	}
+
+	_horizontal_motion = horizontal_motion;
+}
@@ -48,36 +48,57 @@ public:
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;

-	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
+	enum class KinematicState : int {
+		kInconsistent = 0,
+		kConsistent = 1,
+		kUnknown = 2
+	};

-	void setGate(float gate) { _gate = gate; }
+	float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
+	float getInnov() const { return _initialized ? _innov : 0.f; }
+	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
+	bool isKinematicallyConsistent() const { return _state == KinematicState::kConsistent; }
+	bool isNotKinematicallyInconsistent() const { return _state != KinematicState::kInconsistent && (_t_since_first_sample > _kTestRatioLpfTimeConstant || _landed); }
+	void setGate(const float gate) { _gate = gate; }
+	void run(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
+		 uint64_t time_u, uint8_t current_posD_reset_coun);
+	void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; }
+	void reset();

-	float getTestRatio() const { return _test_ratio; }
-	float getSignedTestRatioLpf() const { return _signed_test_ratio_lpf.getState(); }
-	float getInnov() const { return _innov; }
-	float getInnovVar() const { return _innov_var; }
-	bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
+	void setHorizontalMotion(const bool horizontal_motion);
+	bool getHorizontalMotion() const { return _horizontal_motion; }
+	void setIsLanded(bool landed);

 private:
-	void updateConsistency(float vz, uint64_t time_us);

-	uint64_t _time_last_update_us{};
-	float _dist_bottom_prev{};
-
-	float _test_ratio{};
-	AlphaFilter<float> _signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
-	float _gate{.2f};
-	float _innov{};
-	float _innov_var{};
-
-	bool _is_kinematically_consistent{true};
-	uint64_t _time_last_inconsistent_us{};
-	uint64_t _time_last_horizontal_motion{};
-
-	static constexpr float _signed_test_ratio_tau = 2.f;
-
-	static constexpr float _min_vz_for_valid_consistency = .5f;
-	static constexpr uint64_t _consistency_hyst_time_us = 1e6;
+	void update(float z, float z_var, float vz, float vz_var, float dist_bottom,
+		    float dist_bottom_var, uint64_t time_us);
+	void init(float z, float z_var, float dist_bottom, float dist_bottom_var);
+	void evaluateState(float dt, float vz, float vz_var);
+	float _terrain_process_noise{0.0f};
+	matrix::SquareMatrix<float, 2> _P{};
+	matrix::Vector2f _Ht{};
+	matrix::Vector2f _x{};
+	bool _initialized{false};
+	float _innov{0.f};
+	float _innov_var{0.f};
+	uint64_t _time_last_update_us{0};
+	static constexpr float _kTestRatioLpfTimeConstant{1.f};
+	AlphaFilter<float> _test_ratio_lpf{_kTestRatioLpfTimeConstant};
+	float _gate{1.0f};
+	KinematicState _state{KinematicState::kUnknown};
+	float _t_since_first_sample{0.f};
+	uint8_t _last_posD_reset_count{0};
+	bool _horizontal_motion{false};
+	bool _landed{false};
 };

+namespace RangeFilter
+{
+struct IdxDof { unsigned idx; unsigned dof; };
+static constexpr IdxDof z{0, 1};
+static constexpr IdxDof terrain{1, 1};
+static constexpr uint8_t size{2};
+}
+
 #endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
@@ -51,45 +51,47 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
 		_range_sensor.setDataReadiness(rng_data_ready);

-		// update range sensor angle parameters in case they have changed
-		_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
-		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-		_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
-		_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
+		if (_range_sensor.isDataReady()) {

-		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
+			_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
+			_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+			_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
+			_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
+			_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);

-		if (_range_sensor.isDataHealthy()) {
-			// correct the range data for position offset relative to the IMU
-			const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
-			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
+			_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);

-			if (_control_status.flags.in_air) {
-				const bool horizontal_motion = _control_status.flags.fixed_wing
-							       || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
+			if (_range_sensor.isDataHealthy()) {
+				// correct the range data for position offset relative to the IMU
+				const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+				const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+				_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
+				const float dist_var = getRngVar();
+				const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.5f);

-				const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
-				const float var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
+				if (!updated_horizontal_motion && _rng_consistency_check.getHorizontalMotion()) {
+					_rng_consistency_check.reset();
+				}

-				_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
-							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
-			}
-
-		} else {
-			// If we are supposed to be using range finder data but have bad range measurements
-			// and are on the ground, then synthesise a measurement at the expected on ground value
-			if (!_control_status.flags.in_air
-			    && _range_sensor.isRegularlySendingData()
-			    && _range_sensor.isDataReady()) {
+				_rng_consistency_check.setIsLanded(!_control_status.flags.in_air);
+				_rng_consistency_check.setHorizontalMotion(updated_horizontal_motion);
+				const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
+				const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
+				_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
+							   dist_var, imu_sample.time_us, get_posD_reset_count());

+			} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
 				_range_sensor.setRange(_params.ekf2_min_rng);
-				_range_sensor.setValidity(true); // bypass the checks
+				_range_sensor.setValidity(true);
+
+			} else {
+				_rng_consistency_check.reset();
 			}
 		}

 		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+		_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
+							&& _rng_consistency_check.isNotKinematicallyInconsistent();

 	} else {
 		return;
@@ -97,7 +99,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)

 	auto &aid_src = _aid_src_rng_hgt;

-	if (rng_data_ready && _range_sensor.getSampleAddress()) {
+	if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) {

 		updateRangeHagl(aid_src);
 		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
@@ -107,13 +109,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				&& _control_status.flags.tilt_align
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy()
-				&& _rng_consistency_check.isKinematicallyConsistent();
+				&& _rng_consistency_check.isNotKinematicallyInconsistent();

 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();

-
 		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 						      && (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
 						      && isConditionalRangeAidSuitable();
@@ -137,7 +138,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();

-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
+					    && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -163,7 +165,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
 					_control_status.flags.rng_hgt = true;

-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
+					    && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -200,7 +203,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 						stopRngHgtFusion();
 						stopRngTerrFusion();

-					} else {
+					} else if (_rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -215,13 +218,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		} else {
 			if (starting_conditions_passing) {
 				if (_control_status.flags.opt_flow_terrain) {
-					if (!aid_src.innovation_rejected) {
+					if (!aid_src.innovation_rejected && _rng_consistency_check.isNotKinematicallyInconsistent()) {
 						_control_status.flags.rng_terrain = true;
 						fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
 					}

 				} else {
-					if (aid_src.innovation_rejected) {
+					if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -268,11 +271,9 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)

 float Ekf::getRngVar() const
 {
-	return fmaxf(
-		       P(State::pos.idx + 2, State::pos.idx + 2)
-		       + sq(_params.ekf2_rng_noise)
-		       + sq(_params.ekf2_rng_sfe * _range_sensor.getRange()),
-		       0.f);
+	const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
+	const float dist_var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
+	return dist_var;
 }

 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
@@ -164,6 +164,9 @@ void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t t
 		}

 		_prev_median_dist = median_dist;
+
+	} else {
+		_prev_median_dist = dist_bottom;
 	}
 }

@@ -425,7 +425,7 @@ struct parameters {
 	float ekf2_rng_a_vmax{1.0f};            ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
 	float ekf2_rng_qlty_t{1.0f};            ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
-	float ekf2_rng_k_gate{1.0f};            ///< gate size used by the range finder kinematic consistency check
+	float ekf2_rng_k_gate{0.5f};		///< gate size used by the range finder kinematic consistency check
 	float ekf2_rng_fog{0.f};                ///< max distance which a blocked range sensor measures (fog, dirt) [m]

 	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
@@ -608,6 +608,8 @@ uint64_t mag_heading_consistent  :
 		uint64_t gnss_vel                : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
 uint64_t gnss_fault              :
 		1; ///< 45 - true if GNSS measurements have been declared faulty and are no longer used
+		uint64_t rng_kin_unknown	 : 1; ///< 46 - true when the range finder kinematic consistency check is not running
+
 	} flags;
 	uint64_t value;
 };
@@ -227,6 +227,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.ekf2_terr_grad) * (sq(_state.vel(0)) + sq(_state.vel(
 						 1)));
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+		_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
+#endif // CONFIG_EKF2_RANGE_FINDER
 	}

 #endif // CONFIG_EKF2_TERRAIN
@@ -117,7 +117,7 @@ public:

 	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
 	float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); }
-	float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); }
+	float getHaglRateInnovRatio() const { return _rng_consistency_check.getTestRatioLpf(); }
 #endif // CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -721,6 +721,37 @@ def compute_gravity_z_innov_var_and_h(

    return (innov_var, H.T)

+def range_validation_filter_h() -> sf.Matrix:
+
+    state = Values(
+        z=sf.Symbol("z"),
+        terrain=sf.Symbol("terrain")
+    )
+    dist_bottom = state["z"] - state["terrain"]
+
+    H = sf.M21()
+    H[:, 0] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False).transpose()
+
+    return H
+
+def range_validation_filter_P_init(
+        z_var: sf.Scalar,
+        dist_bottom_var: sf.Scalar
+    ) -> sf.Matrix:
+
+    H = range_validation_filter_h().T
+    # enforce terrain to match the measurement
+    K = sf.V2(0, 1/H[1])
+    R = sf.V1(dist_bottom_var)
+
+    # dist_bottom = z - terrain
+    P = sf.M22.diag([z_var, z_var + dist_bottom_var])
+    I = sf.M22.eye()
+    # Joseph form
+    P = (I - K * H) * P * (I - K * H).T + K * R * K.T
+
+    return P
+
 print("Derive EKF2 equations...")
 generate_px4_function(predict_covariance, output_names=None)

@@ -752,5 +783,7 @@ generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_va
 generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
 generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
 generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
+generate_px4_function(range_validation_filter_h, output_names=None)
+generate_px4_function(range_validation_filter_P_init, output_names=None)

 generate_px4_state(State, tangent_idx)
@@ -0,0 +1,46 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: range_validation_filter_p_init
+ *
+ * Args:
+ *     z_var: Scalar
+ *     dist_bottom_var: Scalar
+ *
+ * Outputs:
+ *     res: Matrix22
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 2, 2> RangeValidationFilterPInit(const Scalar z_var,
+                                                       const Scalar dist_bottom_var) {
+  // Total ops: 1
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 2, 2> _res;
+
+  _res(0, 0) = z_var;
+  _res(1, 0) = z_var;
+  _res(0, 1) = z_var;
+  _res(1, 1) = dist_bottom_var + z_var;
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -0,0 +1,41 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: range_validation_filter_h
+ *
+ * Args:
+ *
+ * Outputs:
+ *     res: Matrix21
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 2, 1> RangeValidationFilterH() {
+  // Total ops: 0
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 2, 1> _res;
+
+  _res(0, 0) = 1;
+  _res(1, 0) = -1;
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -1935,6 +1935,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_constant_pos        = _ekf.control_status_flags().constant_pos;
 		status_flags.cs_baro_fault	    = _ekf.control_status_flags().baro_fault;
 		status_flags.cs_gnss_vel            = _ekf.control_status_flags().gnss_vel;
+		status_flags.cs_rng_kin_unknown     = _ekf.control_status_flags().rng_kin_unknown;

 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
@@ -103,7 +103,7 @@ parameters:
          ...). Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE)
          before tuning this gate.'
      type: float
-      default: 1.0
+      default: 0.5
      unit: SD
      min: 0.1
      max: 5.0
@@ -120,6 +120,16 @@ bool EkfWrapper::isIntendingGpsFusion() const
 	return _ekf->control_status_flags().gnss_vel || _ekf->control_status_flags().gnss_pos;
 }

+bool EkfWrapper::isGnssFaultDetected() const
+{
+	return _ekf->control_status_flags().gnss_fault;
+}
+
+void EkfWrapper::setGnssDeadReckonMode()
+{
+	_ekf_params->ekf2_gps_mode = static_cast<int32_t>(GnssMode::kDeadReckoning);
+}
+
 void EkfWrapper::enableGpsHeadingFusion()
 {
 	_ekf_params->ekf2_gps_ctrl |= static_cast<int32_t>(GnssCtrl::YAW);
@@ -78,6 +78,8 @@ public:
 	void enableGpsFusion();
 	void disableGpsFusion();
 	bool isIntendingGpsFusion() const;
+	bool isGnssFaultDetected() const;
+	void setGnssDeadReckonMode();

 	void enableGpsHeadingFusion();
 	void disableGpsHeadingFusion();
@@ -228,3 +228,54 @@ TEST_F(EkfGpsTest, altitudeDrift)
 	// THEN: the baro and local position should follow it
 	EXPECT_LT(fabsf(baro_innov), 0.1f);
 }
+
+TEST_F(EkfGpsTest, gnssJumpDetectionDRMode)
+{
+	// Dead-reckoning mode allows the EKF to reject GNSS data if another source
+	// of horizontal aiding is used (e.g.: airspped)
+	_ekf_wrapper.setGnssDeadReckonMode();
+	_ekf_wrapper.enableGpsHeightFusion();
+
+	// GIVEN:EKF that fuses GNSS and airspeed
+	const Vector3f previous_position = _ekf->getPosition();
+	const Vector3f simulated_velocity_earth(1.f, 0.5f, 0.0f);
+	const Vector2f airspeed_body(15.f, 0.0f);
+	_sensor_simulator._gps.setVelocity(simulated_velocity_earth);
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_is_fixed_wing(true);
+
+	_sensor_simulator.startAirspeedSensor();
+	_sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0));
+	_sensor_simulator.runSeconds(1);
+
+	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
+
+	// AND: simulate jump in position
+	const Vector3f simulated_position_change(500.0f, -1.0f, 0.f);
+	_sensor_simulator._gps.stepHorizontalPositionByMeters(
+		Vector2f(simulated_position_change));
+	_sensor_simulator.runSeconds(15);
+
+	// THEN: the GNSS jump should trigger the fault detection
+	// and stop the fusion (including height and velocity)
+	const Vector3f estimated_position = _ekf->getPosition();
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+	EXPECT_TRUE(_ekf_wrapper.isGnssFaultDetected());
+
+	// The position is obtained through dead-reckoning
+	EXPECT_TRUE(isEqual(estimated_position,
+			    previous_position, 25.f));
+
+	// BUT WHEN: the position data goes back to normal
+	_sensor_simulator._gps.stepHorizontalPositionByMeters(
+		-Vector2f(simulated_position_change) + Vector2f(20.f, 10.f));
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: the fault is cleared an dfusion restarts
+	EXPECT_FALSE(_ekf_wrapper.isGnssFaultDetected());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+}
@@ -162,7 +162,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(2);

 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -215,7 +215,7 @@ TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.disableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(2);

 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -251,7 +251,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);

 	_sensor_simulator.stopGps();
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(12);
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE);

 	_sensor_simulator.stopRangeFinder();
@@ -219,11 +219,36 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 {
 	// GIVEN: rng for terrain but not flow
 	_ekf_wrapper.disableFlowFusion();
-	_ekf_wrapper.enableRangeHeightFusion();
+	const float rng_height = 16.f;

-	const float rng_height = 18;
-	const float flow_height = 1.f;
-	runFlowAndRngScenario(rng_height, flow_height);
+	_sensor_simulator.startGps();
+	_ekf->set_min_required_gps_health_time(1e6);
+	_ekf->set_in_air_status(false);
+	_ekf->set_vehicle_at_rest(true);
+	_ekf_wrapper.enableGpsFusion();
+	_sensor_simulator.runSeconds(1.5); // Run to pass the GPS checks
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+
+	const Vector3f simulated_velocity(0.5f, -1.0f, 0.0f);
+
+	// Configure GPS simulator data
+	_sensor_simulator._gps.setVelocity(simulated_velocity);
+	_sensor_simulator._gps.setPositionRateNED(simulated_velocity);
+
+	// Simulate flight above max range finder distance
+	// run for a while to let terrain uncertainty increase
+	_sensor_simulator._rng.setData(30.f, 100);
+	_sensor_simulator._rng.setLimits(0.1f, 20.f);
+	_sensor_simulator.startRangeFinder();
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_sensor_simulator.runSeconds(40);
+
+	// quick range finder change to bypass stuck-check
+	_sensor_simulator._rng.setData(rng_height - 1.f, 100);
+	_sensor_simulator.runSeconds(1);
+	_sensor_simulator._rng.setData(rng_height, 100);
+	_sensor_simulator.runSeconds(10);

 	// THEN: the terrain should reset using rng
 	EXPECT_NEAR(rng_height, _ekf->getHagl(), 2e-2f);
@@ -234,13 +259,13 @@ TEST_F(EkfTerrainTest, testRngStartInAir)

 	const float corr_terrain_vz = P(State::vel.idx + 2,
 					State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain);
-	EXPECT_NEAR(corr_terrain_vz, 0.6f, 0.03f);
+	EXPECT_TRUE(fabsf(corr_terrain_vz) > 0.6f && fabsf(corr_terrain_vz) < 1.f);

 	const float corr_terrain_z = P(State::pos.idx + 2,
 				       State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain);
-	EXPECT_NEAR(corr_terrain_z, 0.8f, 0.03f);
+	EXPECT_TRUE(fabsf(corr_terrain_z) > 0.8f && fabsf(corr_terrain_z) < 1.f);

 	const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
 					     State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);
-	EXPECT_NEAR(corr_terrain_abias_z, -0.3f, 0.03f);
+	EXPECT_TRUE(corr_terrain_abias_z < -0.1f);
 }
@@ -1832,7 +1832,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", 2.0f);
-		configure_stream_local("COLLISION", 2.0f);
 		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 0.5f);
@@ -1852,7 +1851,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("SYS_STATUS", 0.5f);
 		configure_stream_local("SYSTEM_TIME", 2.0f);
 		configure_stream_local("TIME_ESTIMATE_TO_TARGET", 0.5f);
-		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 2.0f);
 		configure_stream_local("VFR_HUD", 1.0f);
 		configure_stream_local("VIBRATION", 0.1f);
 		configure_stream_local("WIND_COV", 0.1f);
@@ -57,7 +57,7 @@ void AckermannActControl::updateActControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	// Motor control
 	if (_rover_throttle_setpoint_sub.updated()) {
@@ -65,7 +65,7 @@ void AckermannAttControl::updateAttControl()

 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	if (PX4_ISFINITE(_yaw_setpoint)) {
 		// Calculate yaw rate limit for slew rate
@@ -58,10 +58,8 @@ void AckermannPosControl::updatePosControl()

 	hrt_abstime timestamp = hrt_absolute_time();

-	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
-
-	if (target_waypoint_ned.isAllFinite()) {
-		float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
+	if (_target_waypoint_ned.isAllFinite()) {
+		float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm();

 		if (_arrival_speed > FLT_EPSILON) {
 			distance_to_target -= _acceptance_radius; // shift target to the edge of the acceptance radius if arrival speed not zero
@@ -71,18 +69,13 @@ void AckermannPosControl::updatePosControl()

 			float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
 					       _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
-			speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
-
-			if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
-				speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
-						 fabsf(_rover_position_setpoint.cruising_speed));
-			}
+			speed_setpoint = math::min(speed_setpoint, _cruising_speed);

 			pure_pursuit_status_s pure_pursuit_status{};
 			pure_pursuit_status.timestamp = timestamp;

 			const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
-						       _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
+						       _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned,
 						       _curr_pos_ned, fabsf(speed_setpoint));

 			if (_param_ro_speed_red.get() > FLT_EPSILON) {
@@ -114,6 +107,16 @@ void AckermannPosControl::updatePosControl()
 			rover_attitude_setpoint.timestamp = timestamp;
 			rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
 			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+			if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
+				_stopped = true;
+				_target_waypoint_ned = _curr_pos_ned;
+			}
+
+			if (_stopped && _updated_reset_counter != _reset_counter) {
+				_target_waypoint_ned = _curr_pos_ned;
+				_reset_counter = _updated_reset_counter;
+			}
 		}
 	}
 }
@@ -136,21 +139,24 @@ void AckermannPosControl::updateSubscriptions()
 	if (_vehicle_local_position_sub.updated()) {
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
-
-		if (!_global_ned_proj_ref.isInitialized()
-		    || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
-			_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
-							   vehicle_local_position.ref_timestamp);
-		}
-
+		_updated_reset_counter = vehicle_local_position.xy_reset_counter;
 		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+		Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
+		Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1));
+		_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
 	}

 	if (_rover_position_setpoint_sub.updated()) {
-		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		rover_position_setpoint_s rover_position_setpoint;
+		_rover_position_setpoint_sub.copy(&rover_position_setpoint);
+		_start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]);
 		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
-		_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
+		_arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f;
+		_cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed :
+				  _param_ro_speed_limit.get();
+		_target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]);
+		_stopped = false;
 	}

 }
@@ -41,7 +41,6 @@
 #include <matrix/matrix/math.hpp>
 #include <lib/pure_pursuit/PurePursuit.hpp>
 #include <math.h>
-#include <lib/geo/geo.h>

 // uORB includes
 #include <uORB/Publication.hpp>
@@ -80,6 +79,11 @@ public:
 	 */
 	bool runSanityChecks();

+	/**
+	 * @brief Reset position controller.
+	 */
+	void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;};
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -97,7 +101,6 @@ private:
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
 	uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)};
-	rover_position_setpoint_s _rover_position_setpoint{};

 	// uORB publications
 	uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
@@ -108,14 +111,17 @@ private:
 	Quatf _vehicle_attitude_quaternion{};
 	Vector2f _curr_pos_ned{};
 	Vector2f _start_ned{};
+	Vector2f _target_waypoint_ned{};
 	float _arrival_speed{0.f};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
 	float _acceptance_radius{0.f}; // Acceptance radius for the waypoint.
 	float _min_speed{NAN};
-
-	// Class Instances
-	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
+	float _vehicle_speed{0.f};
+	float _cruising_speed{NAN};
+	bool _stopped{false};
+	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
+	uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
@@ -128,6 +134,7 @@ private:
 		(ParamFloat<px4::params::PP_LOOKAHD_MIN>)   _param_pp_lookahd_min,
 		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
 		(ParamFloat<px4::params::RA_WHEEL_BASE>)    _param_ra_wheel_base,
-		(ParamFloat<px4::params::RA_MAX_STR_ANG>)   _param_ra_max_str_ang
+		(ParamFloat<px4::params::RA_MAX_STR_ANG>)   _param_ra_max_str_ang,
+		(ParamFloat<px4::params::RO_SPEED_TH>)      _param_ro_speed_th
 	)
 };
@@ -62,7 +62,7 @@ void AckermannRateControl::updateRateControl()

 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	if (PX4_ISFINITE(_yaw_rate_setpoint)) {
 		if (fabsf(_estimated_speed) > FLT_EPSILON) {
@@ -63,7 +63,7 @@ void AckermannSpeedControl::updateSpeedControl()

 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	// Throttle Setpoint
 	if (PX4_ISFINITE(_speed_setpoint)) {
@@ -113,7 +113,7 @@ private:

 	// Controllers
 	PID _pid_speed;
-	SlewRate<float> _adjusted_speed_setpoint;
+	SlewRate<float> _adjusted_speed_setpoint{0.f};

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
@@ -97,7 +97,7 @@ void RoverAckermann::Run()
 void RoverAckermann::generateSetpoints()
 {
 	vehicle_status_s vehicle_status{};
-	_vehicle_status_sub.update(&vehicle_status);
+	_vehicle_status_sub.copy(&vehicle_status);

 	switch (vehicle_status.nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
@@ -182,6 +182,7 @@ void RoverAckermann::runSanityChecks()

 void RoverAckermann::reset()
 {
+	_ackermann_pos_control.reset();
 	_ackermann_speed_control.reset();
 	_ackermann_att_control.reset();
 	_ackermann_rate_control.reset();
@@ -53,7 +53,7 @@ void DifferentialActControl::updateActControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	// Motor control
 	if (_rover_throttle_setpoint_sub.updated()) {
@@ -63,7 +63,7 @@ void DifferentialAttControl::updateAttControl()
 {
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
@@ -55,10 +55,9 @@ void DifferentialPosControl::updatePosControl()

 	hrt_abstime timestamp = hrt_absolute_time();

-	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);

-	if (target_waypoint_ned.isAllFinite()) {
-		float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
+	if (_target_waypoint_ned.isAllFinite()) {
+		float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm();

 		if (_arrival_speed > FLT_EPSILON) {
 			distance_to_target -=
@@ -68,18 +67,13 @@ void DifferentialPosControl::updatePosControl()
 		if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) {
 			float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
 					       _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
-			speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
-
-			if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
-				speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
-						 fabsf(_rover_position_setpoint.cruising_speed));
-			}
+			speed_setpoint = math::min(speed_setpoint, _cruising_speed);

 			pure_pursuit_status_s pure_pursuit_status{};
 			pure_pursuit_status.timestamp = timestamp;

 			const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
-						   _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
+						   _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned,
 						   _curr_pos_ned, fabsf(speed_setpoint));
 			_pure_pursuit_status_pub.publish(pure_pursuit_status);

@@ -122,6 +116,16 @@ void DifferentialPosControl::updatePosControl()
 			rover_attitude_setpoint.timestamp = timestamp;
 			rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
 			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+			if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
+				_stopped = true;
+				_target_waypoint_ned = _curr_pos_ned;
+			}
+
+			if (_stopped && _updated_reset_counter != _reset_counter) {
+				_target_waypoint_ned = _curr_pos_ned;
+				_reset_counter = _updated_reset_counter;
+			}
 		}
 	}

@@ -132,24 +136,34 @@ void DifferentialPosControl::updateSubscriptions()
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
 		_vehicle_attitude_sub.copy(&vehicle_attitude);
-		matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
 	}

 	if (_vehicle_local_position_sub.updated()) {
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		_updated_reset_counter = vehicle_local_position.xy_reset_counter;
+
 		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+
+		Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
+		Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1));
+		_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
 	}

 	if (_rover_position_setpoint_sub.updated()) {
-		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		rover_position_setpoint_s rover_position_setpoint;
+		_rover_position_setpoint_sub.copy(&rover_position_setpoint);
+		_start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]);
 		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
-		_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
+		_arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f;
+		_cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed :
+				  _param_ro_speed_limit.get();
+		_target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]);
+		_stopped = false;
 	}
-
-
 }

 bool DifferentialPosControl::runSanityChecks()
@@ -38,7 +38,6 @@
 #include <px4_platform_common/events.h>

 // Libraries
-#include <lib/rover_control/RoverControl.hpp>
 #include <matrix/matrix/math.hpp>
 #include <lib/pure_pursuit/PurePursuit.hpp>
 #include <math.h>
@@ -87,6 +86,11 @@ public:
 	 */
 	bool runSanityChecks();

+	/**
+	 * @brief Reset position controller.
+	 */
+	void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;};
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -103,7 +107,6 @@ private:
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
-	rover_position_setpoint_s _rover_position_setpoint{};

 	// uORB publications
 	uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
@@ -111,10 +114,17 @@ private:
 	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};

 	// Variables
+	Quatf _vehicle_attitude_quaternion{};
 	Vector2f _curr_pos_ned{};
 	Vector2f _start_ned{};
+	Vector2f _target_waypoint_ned{};
 	float _arrival_speed{0.f};
 	float _vehicle_yaw{0.f};
+	float _vehicle_speed{0.f};
+	float _cruising_speed{NAN};
+	bool _stopped{false};
+	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
+	uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */
 	DrivingState _current_state{DrivingState::DRIVING};

 	DEFINE_PARAMETERS(
@@ -128,6 +138,7 @@ private:
 		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max,
 		(ParamFloat<px4::params::PP_LOOKAHD_MIN>)   _param_pp_lookahd_min,
 		(ParamFloat<px4::params::NAV_ACC_RAD>)      _param_nav_acc_rad,
-		(ParamFloat<px4::params::RO_SPEED_RED>)     _param_ro_speed_red
+		(ParamFloat<px4::params::RO_SPEED_RED>)     _param_ro_speed_red,
+		(ParamFloat<px4::params::RO_SPEED_TH>)      _param_ro_speed_th
 	)
 };
@@ -59,7 +59,7 @@ void DifferentialRateControl::updateRateControl()
 {
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	if (_vehicle_angular_velocity_sub.updated()) {
 		vehicle_angular_velocity_s vehicle_angular_velocity{};
@@ -63,7 +63,7 @@ void DifferentialSpeedControl::updateSpeedControl()

 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	// Throttle Setpoint
 	if (PX4_ISFINITE(_speed_setpoint)) {
@@ -122,7 +122,7 @@ private:

 	// Controllers
 	PID _pid_speed;
-	SlewRate<float> _adjusted_speed_setpoint;
+	SlewRate<float> _adjusted_speed_setpoint{0.f};

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
@@ -98,7 +98,7 @@ void RoverDifferential::Run()
 void RoverDifferential::generateSetpoints()
 {
 	vehicle_status_s vehicle_status{};
-	_vehicle_status_sub.update(&vehicle_status);
+	_vehicle_status_sub.copy(&vehicle_status);

 	switch (vehicle_status.nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
@@ -183,6 +183,7 @@ void RoverDifferential::runSanityChecks()

 void RoverDifferential::reset()
 {
+	_differential_pos_control.reset();
 	_differential_speed_control.reset();
 	_differential_att_control.reset();
 	_differential_rate_control.reset();
@@ -54,7 +54,7 @@ void MecanumActControl::updateActControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	// Motor control
 	if (_rover_throttle_setpoint_sub.updated()) {
@@ -63,7 +63,7 @@ void MecanumAttControl::updateAttControl()
 {
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
@@ -57,11 +57,9 @@ void MecanumPosControl::updatePosControl()

 	hrt_abstime timestamp = hrt_absolute_time();

-	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
+	if (_target_waypoint_ned.isAllFinite()) {

-	if (target_waypoint_ned.isAllFinite()) {
-
-		float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
+		float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm();

 		if (_arrival_speed > FLT_EPSILON) {
 			distance_to_target -=
@@ -72,18 +70,13 @@ void MecanumPosControl::updatePosControl()

 			float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
 					       _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
-			speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
-
-			if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
-				speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
-						 fabsf(_rover_position_setpoint.cruising_speed));
-			}
+			speed_setpoint = math::min(speed_setpoint, _cruising_speed);

 			pure_pursuit_status_s pure_pursuit_status{};
 			pure_pursuit_status.timestamp = timestamp;

 			const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
-						       _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
+						       _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned,
 						       _curr_pos_ned, fabsf(speed_setpoint));
 			_pure_pursuit_status_pub.publish(pure_pursuit_status);

@@ -111,6 +104,16 @@ void MecanumPosControl::updatePosControl()
 			rover_attitude_setpoint.timestamp = timestamp;
 			rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
 			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+			if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
+				_stopped = true;
+				_target_waypoint_ned = _curr_pos_ned;
+			}
+
+			if (_stopped && _updated_reset_counter != _reset_counter) {
+				_target_waypoint_ned = _curr_pos_ned;
+				_reset_counter = _updated_reset_counter;
+			}
 		}
 	}
 }
@@ -127,24 +130,25 @@ void MecanumPosControl::updateSubscriptions()
 	if (_vehicle_local_position_sub.updated()) {
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
-
-		if (!_global_ned_proj_ref.isInitialized()
-		    || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
-			_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
-							   vehicle_local_position.ref_timestamp);
-		}
-
+		_updated_reset_counter = vehicle_local_position.xy_reset_counter;
 		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+		Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
+		Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1));
+		_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
 	}

 	if (_rover_position_setpoint_sub.updated()) {
-		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		rover_position_setpoint_s rover_position_setpoint;
+		_rover_position_setpoint_sub.copy(&rover_position_setpoint);
+		_start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]);
 		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
-		_yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw;
-		_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
+		_arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f;
+		_cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed :
+				  _param_ro_speed_limit.get();
+		_target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]);
+		_stopped = false;
 	}
-
 }

 bool MecanumPosControl::runSanityChecks()
@@ -38,7 +38,6 @@
 #include <px4_platform_common/events.h>

 // Libraries
-#include <lib/rover_control/RoverControl.hpp>
 #include <matrix/matrix/math.hpp>
 #include <lib/pure_pursuit/PurePursuit.hpp>
 #include <lib/geo/geo.h>
@@ -80,6 +79,13 @@ public:
 	 */
 	bool runSanityChecks();

+	/**
+	 * @brief Reset position controller.
+	 */
+	void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;};
+
+protected:
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -96,7 +102,6 @@ private:
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
-	rover_position_setpoint_s _rover_position_setpoint{};

 	// uORB publications
 	uORB::Publication<rover_speed_setpoint_s>    _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
@@ -107,13 +112,16 @@ private:
 	Quatf _vehicle_attitude_quaternion{};
 	Vector2f _curr_pos_ned{};
 	Vector2f _start_ned{};
+	Vector2f _target_waypoint_ned{};
 	float _arrival_speed{0.f};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
 	float _yaw_setpoint{NAN};
-
-	// Class Instances
-	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
+	float _vehicle_speed{0.f};
+	float _cruising_speed{NAN};
+	bool _stopped{false};
+	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
+	uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th,
@@ -59,7 +59,7 @@ void MecanumRateControl::updateRateControl()
 {
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	if (_vehicle_angular_velocity_sub.updated()) {
 		vehicle_angular_velocity_s vehicle_angular_velocity{};
@@ -65,7 +65,7 @@ void MecanumSpeedControl::updateSpeedControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;

 	updateSubscriptions();

@@ -126,8 +126,8 @@ private:
 	// Controllers
 	PID _pid_speed_x;
 	PID _pid_speed_y;
-	SlewRate<float> _adjusted_speed_x_setpoint;
-	SlewRate<float> _adjusted_speed_y_setpoint;
+	SlewRate<float> _adjusted_speed_x_setpoint{0.f};
+	SlewRate<float> _adjusted_speed_y_setpoint{0.f};

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
@@ -98,7 +98,7 @@ void RoverMecanum::Run()
 void RoverMecanum::generateSetpoints()
 {
 	vehicle_status_s vehicle_status{};
-	_vehicle_status_sub.update(&vehicle_status);
+	_vehicle_status_sub.copy(&vehicle_status);

 	switch (vehicle_status.nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
@@ -183,6 +183,7 @@ void RoverMecanum::runSanityChecks()

 void RoverMecanum::reset()
 {
+	_mecanum_pos_control.reset();
 	_mecanum_speed_control.reset();
 	_mecanum_att_control.reset();
 	_mecanum_rate_control.reset();
@@ -42,30 +42,60 @@ set(BUILD_SHARED_LIBS OFF)
 set(BUILD_TESTING OFF)
 set(CHECK_THREADS NO)
 set(MESSAGE_QUIET ON)
-set(ZENOH_DEBUG ${CONFIG_ZENOH_DEBUG})
+set(ZENOH_DEBUG ${CONFIG_ZENOH_DEBUG} CACHE STRING "Debug print level" FORCE)
+set(PICO_STATIC ON)
+if(NOT DEFINED CONFIG_NET_TCPPROTO_OPTIONS AND CONFIG_PLATFORM_NUTTX)
+    set(Z_FEATURE_TCP_NODELAY 0 CACHE STRING "Toggle TCP_NODELAY")
+endif()
+
+if(NOT DEFINED CONFIG_PTHREAD_MUTEX_TYPES AND CONFIG_PLATFORM_NUTTX)
+    message( SEND_ERROR "Pthread mutex is diabled, Zenoh will not function." )
+endif()
+
+set(Z_TRANSPORT_LEASE 60000 CACHE STRING "Link lease duration in milliseconds to announce to other zenoh nodes") # Match ROS2 RMW_ZENOH
+
+set(FRAG_MAX_SIZE 512 CACHE STRING "Use this to override the maximum size for fragmented messages")
+set(BATCH_UNICAST_SIZE 256 CACHE STRING "Use this to override the maximum unicast batch size")
+set(BATCH_MULTICAST_SIZE 256 CACHE STRING "Use this to override the maximum multicast batch size")
+
+set(Z_FEATURE_UNSTABLE_API 1 CACHE STRING "Toggle unstable Zenoh-C API")

 px4_add_git_submodule(TARGET git_zenoh-pico PATH "zenoh-pico")
 add_subdirectory(zenoh-pico)
 unset(MESSAGE_QUIET)
-add_dependencies(zenohpico git_zenoh-pico px4_platform)
-target_compile_options(zenohpico PUBLIC -Wno-cast-align
+add_dependencies(zenohpico_static git_zenoh-pico px4_platform)
+target_compile_options(zenohpico_static PUBLIC -Wno-cast-align
 			-Wno-narrowing
 			-Wno-stringop-overflow
 			-Wno-stringop-truncation
 			-Wno-unused-result
-			-DZ_BATCH_SIZE_RX=512
-			-DZ_BATCH_SIZE_TX=512
-			-DZ_FRAG_MAX_SIZE=1024)
+			-Wno-type-limits
+			-Wno-unused-variable
+			-Wno-maybe-uninitialized
+			-Wno-conversion)

-target_compile_options(zenohpico PRIVATE -Wno-missing-prototypes)
+target_compile_options(zenohpico_static PRIVATE -Wno-missing-prototypes)
 if(CONFIG_PLATFORM_NUTTX)
-    target_compile_options(zenohpico PRIVATE -DUNIX_NO_MULTICAST_IF)
+    target_compile_options(zenohpico_static PRIVATE -DUNIX_NO_MULTICAST_IF)
 endif()

 if(CONFIG_ZENOH_SERIAL)
-    target_compile_options(zenohpico PRIVATE -DZ_LINK_SERIAL)
+    target_compile_options(zenohpico_static PRIVATE -DZ_LINK_SERIAL)
 endif()

+add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/default_topics.c
+COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/../uxrce_dds_client/generate_dds_topics.py
+	--client-outdir ${CMAKE_CURRENT_BINARY_DIR}
+	--dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
+	--template_file ${CMAKE_CURRENT_SOURCE_DIR}/default_topics.c.em
+DEPENDS
+	${CMAKE_CURRENT_SOURCE_DIR}/../uxrce_dds_client/generate_dds_topics.py
+	${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
+	${CMAKE_CURRENT_SOURCE_DIR}/default_topics.c.em
+COMMENT "Generating Default config"
+)
+add_custom_target(default_topics_config DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/default_topics.c)
+

 px4_add_module(
 		MODULE modules__zenoh
@@ -75,15 +105,17 @@ px4_add_module(
 		zenoh_config.cpp
 		publishers/zenoh_publisher.cpp
 		subscribers/zenoh_subscriber.cpp
+		${CMAKE_CURRENT_BINARY_DIR}/default_topics.c
 		MODULE_CONFIG
 			module.yaml
 		DEPENDS
 			cdr
 			uorb_msgs
 			px4_work_queue
-			zenohpico
+			zenohpico_static
 			zenoh_topics
 			git_zenoh-pico
+			default_topics_config
 		INCLUDES
 			${PX4_BINARY_DIR}/msg
 			zenoh-pico/include
@@ -16,6 +16,26 @@ if MODULES_ZENOH
            2: INFO + ERROR
            3: DEBUG + INFO + ERROR

+    config ZENOH_DEFAULT_LOCATOR
+        string "Zenoh default mode"
+        default "tcp/127.0.0.1:7447" if PLATFORM_POSIX
+        default "" if !PLATFORM_POSIX
+
+    config ZENOH_RMW_LIVELINESS
+        bool "[EXPERIMENTAL] rmw_zenoh liveliness implemenation"
+        default y
+        ---help---
+                Declares liveliness tokens with key expressions in the way rmw_zenoh expects them
+                Allowing to construct ROS2 graphs
+
+    config ZENOH_PUB_ON_MATCHING
+        bool "[EXPERIMENTAL] Only publish data when having matching subscribers"
+        default n
+        ---help---
+                Uses the Zenoh matching feature to check whether a publisher has subscribers.
+                If so, only then publish the data. This is still experimental
+
+
    # Choose exactly one item
    choice ZENOH_PUBSUB_SELECTION
            prompt "Publishers/Subscribers selection"
@@ -0,0 +1,155 @@
+#####
+#
+# This file maps all the topics that are to be used on the Zenoh client.
+#
+#####
+publications:
+
+  - topic: /fmu/out/register_ext_component_reply
+    type: px4_msgs::msg::RegisterExtComponentReply
+
+  - topic: /fmu/out/arming_check_request
+    type: px4_msgs::msg::ArmingCheckRequest
+
+  - topic: /fmu/out/mode_completed
+    type: px4_msgs::msg::ModeCompleted
+
+  - topic: /fmu/out/battery_status
+    type: px4_msgs::msg::BatteryStatus
+
+  - topic: /fmu/out/collision_constraints
+    type: px4_msgs::msg::CollisionConstraints
+
+  - topic: /fmu/out/estimator_status_flags
+    type: px4_msgs::msg::EstimatorStatusFlags
+
+  - topic: /fmu/out/failsafe_flags
+    type: px4_msgs::msg::FailsafeFlags
+
+  - topic: /fmu/out/manual_control_setpoint
+    type: px4_msgs::msg::ManualControlSetpoint
+
+  - topic: /fmu/out/position_setpoint_triplet
+    type: px4_msgs::msg::PositionSetpointTriplet
+
+  - topic: /fmu/out/sensor_combined
+    type: px4_msgs::msg::SensorCombined
+
+  - topic: /fmu/out/timesync_status
+    type: px4_msgs::msg::TimesyncStatus
+
+  # - topic: /fmu/out/vehicle_angular_velocity
+  #   type: px4_msgs::msg::VehicleAngularVelocity
+
+  - topic: /fmu/out/vehicle_land_detected
+    type: px4_msgs::msg::VehicleLandDetected
+
+  - topic: /fmu/out/vehicle_attitude
+    type: px4_msgs::msg::VehicleAttitude
+
+  - topic: /fmu/out/vehicle_control_mode
+    type: px4_msgs::msg::VehicleControlMode
+
+  - topic: /fmu/out/vehicle_command_ack
+    type: px4_msgs::msg::VehicleCommandAck
+
+  - topic: /fmu/out/vehicle_global_position
+    type: px4_msgs::msg::VehicleGlobalPosition
+
+  - topic: /fmu/out/vehicle_gps_position
+    type: px4_msgs::msg::SensorGps
+
+  - topic: /fmu/out/vehicle_local_position
+    type: px4_msgs::msg::VehicleLocalPosition
+
+  - topic: /fmu/out/vehicle_odometry
+    type: px4_msgs::msg::VehicleOdometry
+
+  - topic: /fmu/out/vehicle_status
+    type: px4_msgs::msg::VehicleStatus
+
+  - topic: /fmu/out/airspeed_validated
+    type: px4_msgs::msg::AirspeedValidated
+
+# Create uORB::Publication
+subscriptions:
+  - topic: /fmu/in/register_ext_component_request
+    type: px4_msgs::msg::RegisterExtComponentRequest
+
+  - topic: /fmu/in/unregister_ext_component
+    type: px4_msgs::msg::UnregisterExtComponent
+
+  - topic: /fmu/in/config_overrides_request
+    type: px4_msgs::msg::ConfigOverrides
+
+  - topic: /fmu/in/arming_check_reply
+    type: px4_msgs::msg::ArmingCheckReply
+
+  - topic: /fmu/in/mode_completed
+    type: px4_msgs::msg::ModeCompleted
+
+  - topic: /fmu/in/config_control_setpoints
+    type: px4_msgs::msg::VehicleControlMode
+
+  - topic: /fmu/in/distance_sensor
+    type: px4_msgs::msg::DistanceSensor
+
+  - topic: /fmu/in/manual_control_input
+    type: px4_msgs::msg::ManualControlSetpoint
+
+  - topic: /fmu/in/offboard_control_mode
+    type: px4_msgs::msg::OffboardControlMode
+
+  - topic: /fmu/in/onboard_computer_status
+    type: px4_msgs::msg::OnboardComputerStatus
+
+  - topic: /fmu/in/obstacle_distance
+    type: px4_msgs::msg::ObstacleDistance
+
+  - topic: /fmu/in/sensor_optical_flow
+    type: px4_msgs::msg::SensorOpticalFlow
+
+  - topic: /fmu/in/goto_setpoint
+    type: px4_msgs::msg::GotoSetpoint
+
+  - topic: /fmu/in/telemetry_status
+    type: px4_msgs::msg::TelemetryStatus
+
+  - topic: /fmu/in/trajectory_setpoint
+    type: px4_msgs::msg::TrajectorySetpoint
+
+  - topic: /fmu/in/vehicle_attitude_setpoint
+    type: px4_msgs::msg::VehicleAttitudeSetpoint
+
+  - topic: /fmu/in/vehicle_mocap_odometry
+    type: px4_msgs::msg::VehicleOdometry
+
+  - topic: /fmu/in/vehicle_rates_setpoint
+    type: px4_msgs::msg::VehicleRatesSetpoint
+
+  - topic: /fmu/in/vehicle_visual_odometry
+    type: px4_msgs::msg::VehicleOdometry
+
+  - topic: /fmu/in/vehicle_command
+    type: px4_msgs::msg::VehicleCommand
+
+  - topic: /fmu/in/vehicle_command_mode_executor
+    type: px4_msgs::msg::VehicleCommand
+
+  - topic: /fmu/in/vehicle_thrust_setpoint
+    type: px4_msgs::msg::VehicleThrustSetpoint
+
+  - topic: /fmu/in/vehicle_torque_setpoint
+    type: px4_msgs::msg::VehicleTorqueSetpoint
+
+  - topic: /fmu/in/actuator_motors
+    type: px4_msgs::msg::ActuatorMotors
+
+  - topic: /fmu/in/actuator_servos
+    type: px4_msgs::msg::ActuatorServos
+
+  - topic: /fmu/in/aux_global_position
+    type: px4_msgs::msg::VehicleGlobalPosition
+
+# Create uORB::PublicationMulti
+subscriptions_multi:
@@ -0,0 +1,32 @@
+@###############################################
+@#
+@# EmPy template
+@#
+@###############################################
+@# Generates default config fore Zenoh
+@#
+@# Context:
+@#  - msgs (List) list of all RTPS messages
+@#  - topics (List) list of all topic names
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@###############################################
+@{
+import os
+
+
+}@
+
+const char* default_pub_config =
+@[    for pub in publications]@
+	"@(pub['topic']);@(pub['simple_base_type']);0\n"
+@[    end for]@
+;
+
+const char* default_sub_config =
+@[    for sub in subscriptions]@
+	"@(sub['topic']);@(sub['simple_base_type']);0\n"
+@[    end for]@
+@[    for sub in subscriptions_multi]@
+	"@(sub['topic']);@(sub['simple_base_type']);-1\n"
+@[    end for]@
+;
@@ -45,17 +45,22 @@
 #include <uORB/Subscription.hpp>
 #include <dds_serializer.h>

-#define CDR_SAFETY_MARGIN 12
+#define CDR_SAFETY_MARGIN 24

 class uORB_Zenoh_Publisher : public Zenoh_Publisher
 {
 public:
-	uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) :
+	uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops, int instance) :
 		Zenoh_Publisher(),
 		_uorb_meta{meta},
 		_cdr_ops(ops)
 	{
-		_uorb_sub = orb_subscribe(meta);
+		if (instance <= 0) { // default (<0) or =0
+			_uorb_sub = orb_subscribe(meta); // orb_subscribe subscribes to the 0th/first instance by default
+
+		} else { // otherwise
+			_uorb_sub = orb_subscribe_multi(meta, instance);
+		}
 	};

 	~uORB_Zenoh_Publisher() override = default;
@@ -63,6 +68,14 @@ public:
 	// Update the uORB Subscription and broadcast a Zenoh ROS2 message
 	virtual int8_t update() override
 	{
+#ifdef CONFIG_ZENOH_PUB_ON_MATCHING
+		z_matching_status_t status;
+
+		if (z_publisher_get_matching_status(z_loan(_pub), &status) == _Z_RES_OK && !status.matching) {
+			return _Z_RES_OK;
+		}
+
+#endif
 		uint8_t data[_uorb_meta->o_size];
 		orb_copy(_uorb_meta, _uorb_sub, data);

@@ -70,10 +83,10 @@ public:
 		memcpy(buf, ros2_header, sizeof(ros2_header));

 		dds_ostream_t os;
-		os.m_buffer = buf;
-		os.m_index = (uint32_t)sizeof(ros2_header);
+		os.m_buffer = &buf[4];
+		os.m_index = 0;
 		os.m_size = (uint32_t)sizeof(ros2_header) + _uorb_meta->o_size + CDR_SAFETY_MARGIN;
-		os.m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2;
+		os.m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_1;

 		if (dds_stream_write(&os,
 				     &dds_allocator,
@@ -98,6 +111,11 @@ public:
 		Zenoh_Publisher::print();
 	}

+	const char *getName()
+	{
+		return _uorb_meta->o_name;
+	}
+
 private:
 	const orb_metadata *_uorb_meta;
 	int _uorb_sub;
@@ -44,7 +44,8 @@

 Zenoh_Publisher::Zenoh_Publisher()
 {
-	this->_topic[0] = 0x0;
+	_attachment.sequence_number = 0;
+	_attachment.rmw_gid_size = RMW_GID_STORAGE_SIZE;
 }

 Zenoh_Publisher::~Zenoh_Publisher()
@@ -58,22 +59,21 @@ int Zenoh_Publisher::undeclare_publisher()
 	return 0;
 }

-int Zenoh_Publisher::declare_publisher(z_owned_session_t s, const char *keyexpr)
+int Zenoh_Publisher::declare_publisher(z_owned_session_t s, const char *keyexpr, uint8_t *gid)
 {
-	strncpy(this->_topic, keyexpr, sizeof(this->_topic));
-
 	z_view_keyexpr_t ke;
-	z_view_keyexpr_from_str(&ke, this->_topic);

-	if (z_declare_publisher(&_pub, z_loan(s), z_loan(ke), NULL) < 0) {
+	if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) {
+		printf("%s is not a valid key expression\n", keyexpr);
+		return -1;
+	}
+
+	if (z_declare_publisher(z_loan(s), &_pub, z_loan(ke), NULL) < 0) {
 		printf("Unable to declare publisher for key expression!\n");
 		return -1;
 	}

-	if (!z_publisher_check(&_pub)) {
-		printf("Unable to declare publisher for key expression!\n");
-		return -1;
-	}
+	memcpy(_attachment.rmw_gid, gid, RMW_GID_STORAGE_SIZE);

 	return 0;
 }
@@ -82,14 +82,23 @@ int8_t Zenoh_Publisher::publish(const uint8_t *buf, int size)
 {
 	z_publisher_put_options_t options;
 	z_publisher_put_options_default(&options);
-	options.encoding = NULL;
+
+	_attachment.sequence_number++;
+	_attachment.time = hrt_absolute_time();
+
+	z_owned_bytes_t z_attachment;
+	z_bytes_from_static_buf(&z_attachment, (const uint8_t *)&_attachment, RMW_ATTACHEMENT_SIZE);
+
+	options.attachment = z_move(z_attachment);

 	z_owned_bytes_t payload;
-	z_bytes_serialize_from_slice(&payload, buf, size);
+	z_bytes_copy_from_buf(&payload, buf, size);
 	return z_publisher_put(z_loan(_pub), z_move(payload), &options);
 }

 void Zenoh_Publisher::print()
 {
-	printf("Topic: %s\n", this->_topic);
+	z_view_string_t keystr;
+	z_keyexpr_as_view_string(z_publisher_keyexpr(z_loan(_pub)), &keystr);
+	printf("Topic: %.*s\n", (int)z_string_len(z_loan(keystr)), z_string_data(z_loan(keystr)));
 }
@@ -41,6 +41,8 @@

 #pragma once

+#include "../rmw_attachment.h"
+
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/log.h>

@@ -54,7 +56,7 @@ public:
 	Zenoh_Publisher();
 	virtual ~Zenoh_Publisher();

-	virtual int declare_publisher(z_owned_session_t s, const char *keyexpr);
+	virtual int declare_publisher(z_owned_session_t s, const char *keyexpr, uint8_t *gid);

 	virtual int undeclare_publisher();

@@ -66,5 +68,5 @@ protected:
 	int8_t publish(const uint8_t *, int size);

 	z_owned_publisher_t _pub;
-	char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it.
+	RmwAttachment _attachment;
 };
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 rmw_attachment.h
+ *
+ * ROS2 RMW Attachment helper
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#include <zenoh-pico.h>
+
+#pragma once
+
+/* Derived from ROS2 rmw https://github.com/ros2/rmw/blob/e6addf2411b8ee8a2ac43d691533b8c05ae8f1b6/rmw/include/rmw/types.h#L44 */
+#define RMW_GID_STORAGE_SIZE 16u
+
+/* See rmw_zenoh design.md for more information https://github.com/ros2/rmw_zenoh/blob/rolling/docs/design.md#publishers */
+#define RMW_ATTACHEMENT_SIZE (8u + 8u + 1u + RMW_GID_STORAGE_SIZE)
+
+typedef struct __attribute__((__packed__)) RmwAttachment {
+	int64_t sequence_number;
+	int64_t time;
+	uint8_t rmw_gid_size;
+	uint8_t rmw_gid[RMW_GID_STORAGE_SIZE];
+} RmwAttachment;
@@ -49,29 +49,60 @@
 class uORB_Zenoh_Subscriber : public Zenoh_Subscriber
 {
 public:
-	uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) :
+	// d_instance: < (default if not in CSV) if we should create a new instance (safe), nonzero if we should use the 0 instance
+	uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops, int d_instance) :
 		Zenoh_Subscriber(),
 		_uorb_meta{meta},
 		_cdr_ops(ops)
 	{
-		int instance = 0;
-		_uorb_pub_handle = orb_advertise_multi(_uorb_meta, nullptr, &instance);
+		if (d_instance < 0) { // default=-1; allocate a new instance
+			int instance;
+			_uorb_pub_handle = orb_advertise_multi(_uorb_meta, nullptr, &instance);
+
+		} else {
+			_uorb_pub_handle = orb_advertise(_uorb_meta, nullptr);
+		}
 	};

-	~uORB_Zenoh_Subscriber() override = default;
+	~uORB_Zenoh_Subscriber()
+	{
+		undeclare_subscriber();
+		orb_unadvertise(_uorb_pub_handle);
+	}

 	// Update the uORB Subscription and broadcast a Zenoh ROS2 message
 	void data_handler(const z_loaned_sample_t *sample)
 	{
 		char data[_uorb_meta->o_size];

+		// TODO process rmw_zenoh attachment
 		const z_loaned_bytes_t *payload = z_sample_payload(sample);
 		size_t len = z_bytes_len(payload);

-		dds_istream_t is = {.m_buffer = (unsigned char *)(payload), .m_size = static_cast<int>(len),
-				    .m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2
-				   };
-		dds_stream_read(&is, data, &dds_allocator, _cdr_ops);
+#if defined(Z_FEATURE_UNSTABLE_API)
+		// Check if payload is contiguous so we can decode directly on that pointer
+		z_view_slice_t view;
+
+		if (z_bytes_get_contiguous_view(payload, &view) == Z_OK) {
+			const uint8_t *ptr = z_slice_data(z_loan(view));
+
+			dds_istream_t is = {.m_buffer = (unsigned char *)(ptr + 4), .m_size = static_cast<int>(len),
+					    .m_index = 0, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_1
+					   };
+			dds_stream_read(&is, data, &dds_allocator, _cdr_ops);
+
+		} else
+#endif
+		{
+			unsigned char reassembled_payload[len];
+			z_bytes_reader_t reader = z_bytes_get_reader(payload);
+			z_bytes_reader_read(&reader, reassembled_payload, len);
+
+			dds_istream_t is = {.m_buffer = &reassembled_payload[4], .m_size = static_cast<int>(len),
+					    .m_index = 0, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_1
+					   };
+			dds_stream_read(&is, data, &dds_allocator, _cdr_ops);
+		}

 		// As long as we don't have timesynchronization between Zenoh nodes
 		// we've to manually set the timestamp
@@ -41,7 +41,7 @@

 #include "zenoh_subscriber.hpp"

-static void data_handler_cb(const z_loaned_sample_t *sample, void *arg)
+static void data_handler_cb(z_loaned_sample_t *sample, void *arg)
 {
 	static_cast<Zenoh_Subscriber *>(arg)->data_handler(sample);
 }
@@ -50,15 +50,12 @@ void Zenoh_Subscriber::data_handler(const z_loaned_sample_t *sample)
 {
 	z_view_string_t keystr;
 	z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr);
-	z_owned_slice_t value;
-	z_bytes_deserialize_into_slice(z_sample_payload(sample), &value);
-	printf(">> [Subscriber] Received ('%s' size '%d')\n", z_string_data(z_loan(keystr)), (int)z_slice_len(z_loan(value)));
+	printf(">> [Subscriber] Received ('%s' size '%d')\n", z_string_data(z_loan(keystr)),
+	       (int)z_bytes_len(z_sample_payload(sample)));
 }

-
 Zenoh_Subscriber::Zenoh_Subscriber()
 {
-	this->_topic[0] = 0x0;
 }

 Zenoh_Subscriber::~Zenoh_Subscriber()
@@ -75,32 +72,30 @@ int Zenoh_Subscriber::undeclare_subscriber()
 int Zenoh_Subscriber::declare_subscriber(z_owned_session_t s, const char *keyexpr)
 {
 	z_owned_closure_sample_t callback;
-	z_closure_sample(&callback, data_handler_cb, NULL, this);
-
-	strncpy(this->_topic, keyexpr, sizeof(this->_topic));
+	z_closure(&callback, data_handler_cb, NULL, this);

 	z_view_keyexpr_t ke;
-	z_view_keyexpr_from_str(&ke, this->_topic);
+	z_view_keyexpr_from_str(&ke, keyexpr);

-	if (z_declare_subscriber(&_sub, z_loan(s), z_loan(ke), z_closure_sample_move(&callback), NULL) < 0) {
+	if (z_declare_subscriber(z_loan(s), &_sub, z_loan(ke), z_closure_sample_move(&callback), NULL) < 0) {
 		printf("Unable to declare subscriber.\n");
 		exit(-1);
 	}

-	if (!z_subscriber_check(&_sub)) {
-		printf("Unable to declare subscriber for key expression!\n %s\n", keyexpr);
-		return -1;
-	}
-
 	return 0;
 }

 void Zenoh_Subscriber::print()
 {
-	printf("Topic: %s\n", this->_topic);
+	z_view_string_t keystr;
+	z_keyexpr_as_view_string(z_subscriber_keyexpr(z_loan(_sub)), &keystr);
+	printf("Topic: %s\n", z_string_data(z_loan(keystr)));
 }

 void Zenoh_Subscriber::print(const char *type_string, const char *topic_string)
 {
-	printf("Topic: %s -> %s %s \n", this->_topic, type_string, topic_string);
+	z_view_string_t keystr;
+	z_keyexpr_as_view_string(z_subscriber_keyexpr(z_loan(_sub)), &keystr);
+	printf("Topic: %.*s -> %s %s \n", (int)z_string_len(z_loan(keystr)), z_string_data(z_loan(keystr)), type_string,
+	       topic_string);
 }
@@ -67,8 +67,7 @@ public:
 	virtual void print();

 protected:
-	virtual void  print(const char *type_string, const char *topic_string);
+	virtual void print(const char *type_string, const char *topic_string);

 	z_owned_subscriber_t _sub;
-	char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it.
 };
@@ -52,12 +52,35 @@
 // Auto-generated header to all uORB <-> CDR conversions
 #include <uorb_pubsub_factory.hpp>

-
 #define Z_PUBLISH
 #define Z_SUBSCRIBE

 extern "C" __EXPORT int zenoh_main(int argc, char *argv[]);

+void toCamelCase(char *input)
+{
+	bool capitalizeNext = true; // Capitalize the first letter
+	int j = 0;
+
+	for (int i = 0; input[i] != '\0'; i++) {
+		if (input[i] == '_') {
+			capitalizeNext = true; // Next letter should be capitalized
+
+		} else {
+			if (capitalizeNext && isalpha(input[i])) {
+				input[j++] = toupper(input[i]);
+				capitalizeNext = false;
+
+			} else {
+				input[j++] = input[i];
+			}
+		}
+	}
+
+	input[j] = '\0'; // Null-terminate the input string
+}
+
+
 ZENOH::ZENOH():
 	ModuleParams(nullptr)
 {
@@ -69,102 +92,316 @@ ZENOH::~ZENOH()

 }

-void ZENOH::run()
+int ZENOH::generate_rmw_zenoh_node_liveliness_keyexpr(const z_id_t *id, char *keyexpr)
+{
+	return snprintf(keyexpr, KEYEXPR_SIZE,
+			"@ros2_lv/0/%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/0/0/NN/%%/%%/"
+			"px4_%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
+			id->id[0], id->id[1],  id->id[2], id->id[3], id->id[4], id->id[5], id->id[6],
+			id->id[7], id->id[8],  id->id[9], id->id[10], id->id[11], id->id[12], id->id[13],
+			id->id[14], id->id[15],
+			_px4_guid[0], _px4_guid[1], _px4_guid[2], _px4_guid[3],
+			_px4_guid[4], _px4_guid[5], _px4_guid[6], _px4_guid[7],
+			_px4_guid[8], _px4_guid[9], _px4_guid[10], _px4_guid[11],
+			_px4_guid[12], _px4_guid[13], _px4_guid[14], _px4_guid[15]);
+}
+
+int ZENOH::generate_rmw_zenoh_topic_keyexpr(const char *topic, const uint8_t *rihs_hash, char *type, char *keyexpr)
+{
+	const char *type_name = getTypeName(type);
+
+	if (type_name) {
+		strncpy(type, type_name, TOPIC_INFO_SIZE);
+		toCamelCase(type); // Convert uORB type to camel case
+		return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRId32 "%s/"
+				KEYEXPR_MSG_NAME "%s_/RIHS01_"
+				"%02x%02x%02x%02x%02x%02x%02x%02x"
+				"%02x%02x%02x%02x%02x%02x%02x%02x"
+				"%02x%02x%02x%02x%02x%02x%02x%02x"
+				"%02x%02x%02x%02x%02x%02x%02x%02x",
+				_zenoh_domain_id.get(), topic, type,
+				rihs_hash[0], rihs_hash[1], rihs_hash[2], rihs_hash[3],
+				rihs_hash[4], rihs_hash[5], rihs_hash[6], rihs_hash[7],
+				rihs_hash[8], rihs_hash[9], rihs_hash[10], rihs_hash[11],
+				rihs_hash[12], rihs_hash[13], rihs_hash[14], rihs_hash[15],
+				rihs_hash[16], rihs_hash[17], rihs_hash[18], rihs_hash[19],
+				rihs_hash[20], rihs_hash[21], rihs_hash[22], rihs_hash[23],
+				rihs_hash[24], rihs_hash[25], rihs_hash[26], rihs_hash[27],
+				rihs_hash[28], rihs_hash[29], rihs_hash[30], rihs_hash[31]
+			       );
+	}
+
+	return -1;
+}
+
+int ZENOH::generate_rmw_zenoh_topic_liveliness_keyexpr(const z_id_t *id, const char *topic, const uint8_t *rihs_hash,
+		char *type_camel_case, char *keyexpr, const char *entity_str)
+{
+	// NOT REALLY COMPLIANT WITH RMW_ZENOH_CPP but get's the job done
+	// TODO build a correct keyexpr
+
+	char topic_lv[TOPIC_INFO_SIZE];
+	char *str = &topic_lv[0];
+
+	strncpy(topic_lv, topic, sizeof(topic_lv));
+
+	while (*str) {
+		if (*str == '/') {
+			*str = '%';
+		}
+
+		str++;
+	}
+
+	return snprintf(keyexpr, KEYEXPR_SIZE,
+			"@ros2_lv/%" PRId32 "/"
+			"%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/"
+			"0/11/%s/%%/%%/px4_%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/%s/"
+			KEYEXPR_MSG_NAME "%s_/RIHS01_"
+			"%02x%02x%02x%02x%02x%02x%02x%02x"
+			"%02x%02x%02x%02x%02x%02x%02x%02x"
+			"%02x%02x%02x%02x%02x%02x%02x%02x"
+			"%02x%02x%02x%02x%02x%02x%02x%02x"
+			"/::,7:,:,:,,",
+			_zenoh_domain_id.get(),
+			id->id[0], id->id[1],  id->id[2], id->id[3], id->id[4], id->id[5], id->id[6],
+			id->id[7], id->id[8],  id->id[9], id->id[10], id->id[11], id->id[12], id->id[13],
+			id->id[14], id->id[15],
+			entity_str,
+			_px4_guid[0], _px4_guid[1], _px4_guid[2], _px4_guid[3],
+			_px4_guid[4], _px4_guid[5], _px4_guid[6], _px4_guid[7],
+			_px4_guid[8], _px4_guid[9], _px4_guid[10], _px4_guid[11],
+			_px4_guid[12], _px4_guid[13], _px4_guid[14], _px4_guid[15],
+			topic_lv, type_camel_case,
+			rihs_hash[0], rihs_hash[1], rihs_hash[2], rihs_hash[3],
+			rihs_hash[4], rihs_hash[5], rihs_hash[6], rihs_hash[7],
+			rihs_hash[8], rihs_hash[9], rihs_hash[10], rihs_hash[11],
+			rihs_hash[12], rihs_hash[13], rihs_hash[14], rihs_hash[15],
+			rihs_hash[16], rihs_hash[17], rihs_hash[18], rihs_hash[19],
+			rihs_hash[20], rihs_hash[21], rihs_hash[22], rihs_hash[23],
+			rihs_hash[24], rihs_hash[25], rihs_hash[26], rihs_hash[27],
+			rihs_hash[28], rihs_hash[29], rihs_hash[30], rihs_hash[31]
+		       );
+}
+
+int ZENOH::setupSession()
 {
 	char mode[NET_MODE_SIZE];
 	char locator[NET_LOCATOR_SIZE];
-	int8_t ret;
-	int i;
-
-	Zenoh_Config z_config;
-
-	z_config.getNetworkConfig(mode, locator);
-
 	z_owned_config_t config;
-	z_config_default(&config);
-	zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, mode);
+	int ret = 0;

-	if (locator[0] != 0) {
-		zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, locator);
-
-	} else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) {
-		zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, Z_CONFIG_MULTICAST_LOCATOR_DEFAULT);
-	}
+	_config.getNetworkConfig(mode, locator);

 	PX4_INFO("Opening session...");
-	z_owned_session_t s;
-	ret = z_open(&s, z_move(config));

-	if (ret  < 0) {
-		PX4_ERR("Unable to open session, ret: %d", ret);
-		return;
+	do {
+		z_config_default(&config);
+		zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, mode);
+
+		if (locator[0] != 0) {
+			zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, locator);
+
+		} else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) {
+			zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, Z_CONFIG_MULTICAST_LOCATOR_DEFAULT);
+		}
+
+		if (ret == _Z_ERR_TRANSPORT_OPEN_FAILED) {
+			PX4_WARN("Unable to open session, make sure zenohd is running on %s", locator);
+
+		} else if (ret == _Z_ERR_SCOUT_NO_RESULTS) {
+			PX4_WARN("Unable to open session, scout no results");
+
+		} else if (ret < 0) {
+			PX4_WARN("Unable to open session, ret: %d", ret);
+		}
+
+		if (ret != 0) {
+			sleep(5); // Wait 5 seconds when doing a retry
+		}
+
+	} while ((ret = z_open(&_s, z_move(config), NULL)) < 0);
+
+	// Start read and lease tasks for zenoh-pico
+	if (zp_start_read_task(z_loan_mut(_s), NULL) < 0 || zp_start_lease_task(z_loan_mut(_s), NULL) < 0) {
+		PX4_ERR("Unable to start read and lease tasks");
+		z_drop(z_move(_s));
+		ret = -EINVAL;
 	}

-	PX4_INFO("Checking session...");
+	return ret;
+}

-	if (!z_session_check(&s)) {
-		PX4_ERR("Unable to check session!");
+int ZENOH::setupTopics(px4_pollfd_struct_t *pfds)
+{
+	char keyexpr[KEYEXPR_SIZE];
+	int i;
+	int ret = 0;
+
+#ifndef BOARD_HAS_NO_UUID
+	board_get_px4_guid(_px4_guid);
+#else
+	// TODO Fill ID with something reasonable
+	_px4_guid[0] = 0xAA;
+	_px4_guid[1] = 0xBB;
+	_px4_guid[2] = 0xCC;
+#endif
+
+#ifdef CONFIG_ZENOH_RMW_LIVELINESS
+	z_id_t self_id = z_info_zid(z_loan(_s));
+
+	if (generate_rmw_zenoh_node_liveliness_keyexpr(&self_id, keyexpr)) {
+		z_view_keyexpr_t ke;
+
+		if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) {
+			PX4_ERR("%s is not a valid key expression\n", keyexpr);
+			return -1;
+		}
+
+		z_owned_liveliness_token_t token;
+
+		if (z_liveliness_declare_token(z_loan(_s), &token, z_loan(ke), NULL) < 0) {
+			PX4_ERR("Unable to create liveliness token!\n");
+			return -1;
+		}
+	}
+
+#endif
+
+#ifdef Z_SUBSCRIBE
+	_zenoh_subscribers = (Zenoh_Subscriber **)malloc(sizeof(Zenoh_Subscriber *)*_sub_count);
+
+	if (_zenoh_subscribers) {
+		char topic[TOPIC_INFO_SIZE];
+		char type[TOPIC_INFO_SIZE];
+		int instance_no;
+
+		for (i = 0; i < _sub_count; i++) {
+			if (_config.getSubscriberMapping(topic, type, &instance_no)) {
+				_zenoh_subscribers[i] = genSubscriber(type, instance_no);
+				const uint8_t *rihs_hash = getRIHS01_Hash(type);
+
+				if (rihs_hash != NULL && _zenoh_subscribers[i] != 0 &&
+				    generate_rmw_zenoh_topic_keyexpr(topic, rihs_hash, type, keyexpr) > 0) {
+					_zenoh_subscribers[i]->declare_subscriber(_s, keyexpr);
+#ifdef CONFIG_ZENOH_RMW_LIVELINESS
+
+					if (generate_rmw_zenoh_topic_liveliness_keyexpr(&self_id, topic, rihs_hash, type, keyexpr, "MS") > 0) {
+						z_view_keyexpr_t ke;
+
+						if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) {
+							PX4_ERR("%s is not a valid key expression\n", keyexpr);
+							return -1;
+						}
+
+						z_owned_liveliness_token_t token;
+
+						if (z_liveliness_declare_token(z_loan(_s), &token, z_loan(ke), NULL) < 0) {
+							PX4_ERR("Unable to create liveliness token!\n");
+							return -1;
+						}
+					}
+
+#endif
+
+				} else {
+					PX4_ERR("Could not create a subscriber for type %s", type);
+				}
+
+			} else {
+				_zenoh_subscribers[i] = NULL;
+				PX4_ERR("Error parsing publisher config at index %i", i);
+			}
+		}
+
+		if (_config.getSubscriberMapping(topic, type, &instance_no) < 0) {
+			PX4_WARN("Subscriber mapping parsing error");
+		}
+
+		_config.closePubSubMapping();
+	}
+
+#endif
+
+#ifdef Z_PUBLISH
+	_zenoh_publishers = (uORB_Zenoh_Publisher **)malloc(_pub_count * sizeof(uORB_Zenoh_Publisher *));
+
+	if (_zenoh_publishers) {
+		char topic[TOPIC_INFO_SIZE];
+		char type[TOPIC_INFO_SIZE];
+		int instance;
+
+		for (i = 0; i < _pub_count; i++) {
+			if (_config.getPublisherMapping(topic, type, &instance)) {
+				_zenoh_publishers[i] = genPublisher(type, instance);
+				const uint8_t *rihs_hash = getRIHS01_Hash(type);
+
+				if (rihs_hash && _zenoh_publishers[i] != 0 &&
+				    generate_rmw_zenoh_topic_keyexpr(topic, rihs_hash, type, keyexpr) > 0) {
+					_zenoh_publishers[i]->declare_publisher(_s, keyexpr, (uint8_t *)&_px4_guid);
+					_zenoh_publishers[i]->setPollFD(&pfds[i]);
+#ifdef CONFIG_ZENOH_RMW_LIVELINESS
+
+					if (generate_rmw_zenoh_topic_liveliness_keyexpr(&self_id, topic, rihs_hash, type, keyexpr, "MP") > 0) {
+						z_view_keyexpr_t ke;
+
+						if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) {
+							PX4_ERR("%s is not a valid key expression\n", keyexpr);
+							return -1;
+						}
+
+						z_owned_liveliness_token_t token;
+
+						if (z_liveliness_declare_token(z_loan(_s), &token, z_loan(ke), NULL) < 0) {
+							PX4_ERR("Unable to create liveliness token!\n");
+							return -1;
+						}
+					}
+
+#endif
+
+				} else {
+					PX4_ERR("Could not create a publisher for type %s", type);
+				}
+
+			} else {
+				_zenoh_publishers[i] = NULL;
+				PX4_ERR("Error parsing publisher config at index %i", i);
+			}
+		}
+
+		if (_config.getPublisherMapping(topic, type, &instance) < 0) {
+			PX4_WARN("Publisher mapping parsing error");
+		}
+
+		_config.closePubSubMapping();
+	}
+
+#endif
+
+	return ret;
+}
+
+void ZENOH::run()
+{
+	int8_t ret;
+	int i;
+	_pub_count =  _config.getPubCount();
+	_sub_count =  _config.getSubCount();
+	px4_pollfd_struct_t pfds[_pub_count];
+
+	if (setupSession() < 0) {
+		PX4_ERR("Failed to setup Zenoh session");
 		return;
 	}

 	PX4_INFO("Starting reading/writing tasks...");

-	// Start read and lease tasks for zenoh-pico
-	if (zp_start_read_task(z_loan_mut(s), NULL) < 0 || zp_start_lease_task(z_loan_mut(s), NULL) < 0) {
-		PX4_ERR("Unable to start read and lease tasks");
-		z_close(z_move(s));
+	if (setupTopics(pfds) < 0) {
+		PX4_ERR("Failed to setup topics");
 		return;
 	}

-#ifdef Z_SUBSCRIBE
-	_sub_count =  z_config.getSubCount();
-	_zenoh_subscribers = (Zenoh_Subscriber **)malloc(sizeof(Zenoh_Subscriber *)*_sub_count);
-	{
-		char topic[TOPIC_INFO_SIZE];
-		char type[TOPIC_INFO_SIZE];
-
-		for (i = 0; i < _sub_count; i++) {
-			z_config.getSubscriberMapping(topic, type);
-			_zenoh_subscribers[i] = genSubscriber(type);
-
-			if (_zenoh_subscribers[i] != 0) {
-				_zenoh_subscribers[i]->declare_subscriber(s, topic);
-			}
-
-
-		}
-
-		if (z_config.getSubscriberMapping(topic, type) < 0) {
-			PX4_WARN("Subscriber mapping parsing error");
-		}
-	}
-#endif
-
-#ifdef Z_PUBLISH
-
-	_pub_count =  z_config.getPubCount();
-	_zenoh_publishers = (uORB_Zenoh_Publisher **)malloc(_pub_count * sizeof(uORB_Zenoh_Publisher *));
-	px4_pollfd_struct_t pfds[_pub_count];
-
-	{
-		char topic[TOPIC_INFO_SIZE];
-		char type[TOPIC_INFO_SIZE];
-
-		for (i = 0; i < _pub_count; i++) {
-			z_config.getPublisherMapping(topic, type);
-			_zenoh_publishers[i] = genPublisher(type);
-
-			if (_zenoh_publishers[i] != 0) {
-				_zenoh_publishers[i]->declare_publisher(s, topic);
-				_zenoh_publishers[i]->setPollFD(&pfds[i]);
-			}
-		}
-
-		if (z_config.getSubscriberMapping(topic, type) < 0) {
-			PX4_WARN("Publisher mapping parsing error");
-		}
-	}
-
 	if (_pub_count == 0) {
 		// Nothing to publish but we don't want to stop this thread
 		while (!should_exit()) {
@@ -184,7 +421,7 @@ void ZENOH::run()
 					ret = _zenoh_publishers[i]->update();

 					if (ret < 0) {
-						PX4_WARN("Publisher error %i", ret);
+						PX4_WARN("%s Publisher error %i", _zenoh_publishers[i]->getName(), ret);

 					}
 				}
@@ -192,26 +429,28 @@ void ZENOH::run()
 		}
 	}

-#endif
-
 	// Exiting cleaning up publisher and subscribers
 	for (i = 0; i < _sub_count; i++) {
-		delete _zenoh_subscribers[i];
+		if (_zenoh_subscribers[i]) {
+			delete _zenoh_subscribers[i];
+		}
 	}

 	free(_zenoh_subscribers);

 	for (i = 0; i < _pub_count; i++) {
-		delete _zenoh_publishers[i];
+		if (_zenoh_publishers[i]) {
+			delete _zenoh_publishers[i];
+		}
 	}

 	free(_zenoh_publishers);

 	// Stop read and lease tasks for zenoh-pico
-	zp_stop_read_task(z_session_loan_mut(&s));
-	zp_stop_lease_task(z_session_loan_mut(&s));
+	zp_stop_read_task(z_session_loan_mut(&_s));
+	zp_stop_lease_task(z_session_loan_mut(&_s));

-	z_close(z_session_move(&s));
+	z_drop(z_session_move(&_s));
 	exit_and_cleanup();
 }

@@ -244,8 +483,10 @@ Zenoh demo bridge
 	PRINT_MODULE_USAGE_COMMAND("stop");
 	PRINT_MODULE_USAGE_COMMAND("status");
 	PRINT_MODULE_USAGE_COMMAND("config");
-	PX4_INFO_RAW("     addpublisher  <zenoh_topic> <uorb_topic>  Publish uORB topic to Zenoh\n");
-	PX4_INFO_RAW("     addsubscriber <zenoh_topic> <uorb_topic>  Publish Zenoh topic to uORB\n");
+	PX4_INFO_RAW("     add publisher  <zenoh_topic> <uorb_topic> <optional uorb_instance>  Publish uORB topic to Zenoh\n");
+	PX4_INFO_RAW("     add subscriber <zenoh_topic> <uorb_topic> <optional uorb_instance>  Publish Zenoh topic to uORB\n");
+	PX4_INFO_RAW("     delete publisher  <zenoh_topic>\n");
+	PX4_INFO_RAW("     delete subscriber <zenoh_topic>\n");
 	PX4_INFO_RAW("     net           <mode> <locator>            Zenoh network mode\n");
 	PX4_INFO_RAW("          <mode>    values: client|peer   \n");
 	PX4_INFO_RAW("          <locator> client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n");
@@ -259,14 +500,22 @@ int ZENOH::print_status()

 	PX4_INFO("Publishers");

-	for (int i = 0; i < _pub_count; i++) {
-		_zenoh_publishers[i]->print();
+	if (_zenoh_publishers) {
+		for (int i = 0; i < _pub_count; i++) {
+			if (_zenoh_publishers[i]) {
+				_zenoh_publishers[i]->print();
+			}
+		}
 	}

 	PX4_INFO("Subscribers");

-	for (int i = 0; i < _sub_count; i++) {
-		_zenoh_subscribers[i]->print();
+	if (_zenoh_subscribers) {
+		for (int i = 0; i < _sub_count; i++) {
+			if (_zenoh_subscribers[i]) {
+				_zenoh_subscribers[i]->print();
+			}
+		}
 	}

 	return 0;
@@ -49,6 +49,7 @@
 #include "publishers/uorb_publisher.hpp"
 #include "subscribers/uorb_subscriber.hpp"

+
 class ZENOH : public ModuleBase<ZENOH>, public ModuleParams
 {
 public:
@@ -82,6 +83,16 @@ public:
 	void run() override;

 private:
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::ZENOH_DOMAIN_ID>) _zenoh_domain_id
+	)
+
+	int generate_rmw_zenoh_node_liveliness_keyexpr(const z_id_t *id, char *keyexpr);
+	int generate_rmw_zenoh_topic_keyexpr(const char *topic, const uint8_t *rihs_hash, char *type, char *keyexpr);
+	int generate_rmw_zenoh_topic_liveliness_keyexpr(const z_id_t *id, const char *topic, const uint8_t *rihs_hash,
+			char *type, char *keyexpr, const char *entity_str);
+	int setupSession();
+	int setupTopics(px4_pollfd_struct_t *pfds);

 	Zenoh_Config _config;

@@ -90,6 +101,10 @@ private:
 	int _sub_count;
 	Zenoh_Subscriber **_zenoh_subscribers;

+	z_owned_session_t _s;
+
+	px4_guid_t _px4_guid{};
+
 };

 #endif //ZENOH_MODULE_H
@@ -46,13 +46,16 @@
 #include <errno.h>
 #include <string.h>
 #include <stdlib.h>
+#include <ctype.h>

 #include <uORB/topics/uORBTopics.hpp>


-const char *default_net_config = Z_CONFIG_MODE_DEFAULT;
-const char *default_pub_config = "";
-const char *default_sub_config = ""; //TODO maybe use YAML
+const char *default_net_config = Z_CONFIG_MODE_DEFAULT ";" CONFIG_ZENOH_DEFAULT_LOCATOR;
+
+// Default config generated from default_topics.c.em and dds_topics.yaml
+extern const char *default_pub_config;
+extern const char *default_sub_config;


 Zenoh_Config::Zenoh_Config()
@@ -92,13 +95,14 @@ Zenoh_Config::~Zenoh_Config()
 	}
 }

-int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename)
+int Zenoh_Config::AddPubSub(char *topic, char *datatype, int instance_no, const char *filename)
 {
 	{
 		char f_topic[TOPIC_INFO_SIZE];
 		char f_type[TOPIC_INFO_SIZE];
+		int f_new_instance;

-		while (getPubSubMapping(f_topic, f_type, filename) > 0) {
+		while (getPubSubMapping(f_topic, f_type, &f_new_instance, filename) > 0) {
 			if (strcmp(topic, f_topic) == 0
 			    || strcmp(datatype, f_type) == 0) {
 				printf("Already mapped to uORB %s -> %s\n", f_type, f_topic);
@@ -116,7 +120,7 @@ int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename)
 			FILE *fp = fopen(filename, "a");

 			if (fp) {
-				fprintf(fp, "%s;%s\n", topic, datatype);
+				fprintf(fp, "%s;%s;%d\n", topic, datatype, instance_no);

 			} else {
 				return -1;
@@ -132,6 +136,79 @@ int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename)
 }


+int Zenoh_Config::DeletePubSub(char *topic, const char *filename)
+{
+	if (!filename || !topic) {
+		return -1;
+	}
+
+	FILE *file = fopen(filename, "r");
+
+	if (!file) {
+		return -1;
+	}
+
+	// Create a temporary file for writing
+	char temp_filename[256];
+	snprintf(temp_filename, sizeof(temp_filename), "%s.tmp", filename);
+	FILE *temp_file = fopen(temp_filename, "w");
+
+	if (!temp_file) {
+		fclose(file);
+		return -1;
+	}
+
+	char line[TOPIC_INFO_SIZE];
+	char line_copy[TOPIC_INFO_SIZE];
+	const char *fields[1];  // We only need the topic
+	int found = 0;
+
+	while (fgets(line, sizeof(line), file)) {
+		// Remove newline if present
+		size_t len = strlen(line);
+
+		if (len > 0 && line[len - 1] == '\n') {
+			line[len - 1] = '\0';
+		}
+
+		// Make a copy of the line before parsing since parse_csv_line will replace ;s with \0s
+		strncpy(line_copy, line, sizeof(line_copy) - 1);
+		line_copy[sizeof(line_copy) - 1] = '\0';
+		int num_fields = parse_csv_line(line_copy, fields, 1);
+
+		// If the topic doesn't match the topic, write the line to temp file
+		if (num_fields == 0 || strcmp(fields[0], topic) != 0) {
+			fprintf(temp_file, "%s\n", line);
+
+		} else {
+			found = 1;
+		}
+	}
+
+	fclose(file);
+	fclose(temp_file);
+
+	// replace the original file if deletion was successful
+	if (found) {
+		if (remove(filename) != 0) {
+			remove(temp_filename);
+			return -1;
+		}
+
+		if (rename(temp_filename, filename) != 0) {
+			remove(temp_filename);
+			return -1;
+		}
+
+	} else {
+		// Otherwise, if no line was deleted, remove the temp file
+		remove(temp_filename);
+	}
+
+	return 0;
+}
+
+
 int Zenoh_Config::SetNetworkConfig(char *mode, char *locator)
 {

@@ -158,53 +235,136 @@ int Zenoh_Config::cli(int argc, char *argv[])
 	if (argc == 1) {
 		dump_config();

+	} else if (argc == 2) {
+		if (strcmp(argv[1], "delete") == 0) {
+			printf("The type of resource to be deleted (publisher/subscriber) must be specified.\n");
+
+		} else if (strcmp(argv[1], "add") == 0) {
+			printf("The type of resource to be added (publisher/subscriber) must be specified.\n");
+
+		} else {
+			printf("Unrecognized command\n");
+		}
+
 	} else if (argc == 3) {
 		if (strcmp(argv[1], "net") == 0) {
 			SetNetworkConfig(argv[2], 0);
+
+		} else if (strcmp(argv[1], "delete") == 0) {
+			printf("The name of the resource to be deleted needs to be specified.\n");
+
+		} else if (strcmp(argv[1], "add") == 0) {
+			printf("The name of the Zenoh/uOrb topic pair to be linked needs to be specified.\n");
+
+		} else {
+			printf("Unrecognized command\n");
 		}

 	} else if (argc == 4) {
-		if (strcmp(argv[1], "addpublisher") == 0) {
-			if (AddPubSub(argv[2], argv[3], ZENOH_PUB_CONFIG_PATH) > 0) {
-				printf("Added %s %s to publishers\n", argv[2], argv[3]);
-
-			} else {
-				printf("Could not add uORB %s -> %s to publishers\n",  argv[3], argv[2]);
-			}
-
-		} else if (strcmp(argv[1], "addsubscriber") == 0) {
-			if (AddPubSub(argv[2], argv[3], ZENOH_SUB_CONFIG_PATH) > 0) {
-				printf("Added %s -> uORB %s to subscribers\n", argv[2], argv[3]);
-
-			} else {
-				printf("Could not add %s -> uORB %s to subscribers\n",  argv[2], argv[3]);
-			}
-
-		} else if (strcmp(argv[1], "net") == 0) {
+		if (strcmp(argv[1], "net") == 0) {
 			SetNetworkConfig(argv[2], argv[3]);
+
+		} else if (strcmp(argv[1], "delete") == 0) {
+			if (strcmp(argv[2], "publisher") == 0) {
+				int res = DeletePubSub(argv[3], ZENOH_PUB_CONFIG_PATH);
+
+				if (res < 0) {
+					printf("Could not delete publisher topic %s\n", argv[3]);
+				}
+
+			} else if (strcmp(argv[2], "subscriber") == 0) {
+				int res = DeletePubSub(argv[3], ZENOH_SUB_CONFIG_PATH);
+
+				if (res < 0) {
+					printf("Could not delete subscriber topic %s\n", argv[3]);
+				}
+
+			} else {
+				printf("Unrecognized command\n");
+			}
+
+		} else {
+			printf("Unrecognized command\n");
 		}
-	}
+
+	} else if (argc >= 5) {
+		if (strcmp(argv[1], "add") == 0) {
+			if (strcmp(argv[2], "publisher") == 0) {
+				int instance = 0;
+
+				if (argc == 6) {
+					if (sscanf(argv[5], "%d", &instance) != 1 || instance < 0) {
+						printf("Invalid instance %s (must be an integer, 0 for the default instance or a specific instance's index)\n",
+						       argv[5]);
+						return 0;
+					}
+				}
+
+				if (AddPubSub(argv[3], argv[4], instance, ZENOH_PUB_CONFIG_PATH) > 0) {
+					printf("Added %s %s to publishers (instance %d)\n", argv[3], argv[4], instance);
+
+				} else {
+					printf("Could not add uORB %s:%d -> %s to publishers\n",  argv[3], instance, argv[4]);
+				}
+
+			} else if (strcmp(argv[2], "subscriber") == 0) {
+				int instance = 0;
+
+				if (argc == 6) {
+					if (sscanf(argv[5], "%d", &instance) != 1 || instance == 0 || instance == -1) {
+						printf("Invalid instance %s (must be an integer, 0 for the default instance or -1 for a new uOrb instance)\n", argv[5]);
+						return 0;
+					}
+				}
+
+				if (AddPubSub(argv[3], argv[4], instance, ZENOH_SUB_CONFIG_PATH) > 0) {
+					printf("Added %s -> uORB %s:%d to subscribers\n", argv[3], argv[4], instance);
+
+				} else {
+					printf("Could not add %s -> uORB %s to subscribers\n",  argv[3], argv[4]);
+				}
+
+			} else {
+				printf("Unrecognized command\n");
+			}
+
+		} else {
+			printf("Unrecognized command\n");
+		}
+	} // doesn't need an else because negative argc would be... weird

 	//TODO make CLI to modify configuration now you would have to manually modify the files
 	return 0;
 }

-const char *Zenoh_Config::get_csv_field(char *line, int num)
+int Zenoh_Config::parse_csv_line(char *line, const char **fields, int max_fields)
 {
-	const char *tok;
-
-	for (
-		tok = strtok(line, ";");
-		tok && *tok;
-		tok = strtok(NULL, ";\n")) {
-		if (!--num) {
-			return tok;
-		}
+	if (!line || !fields || max_fields <= 0) {
+		return 0;
 	}

-	return NULL;
-}
+	int field_count = 0;
+	char *token = strtok(line, ";");

+	while (token && field_count < max_fields) {
+		// Trim leading whitespace
+		while (isspace((unsigned char)*token)) { token++; }
+
+		// Trim trailing whitespace
+		char *end = token + strlen(token) - 1;
+
+		while (end > token && isspace((unsigned char)*end)) {
+			*end = '\0';
+			end--;
+		}
+
+		fields[field_count] = token;
+		field_count++;
+		token = strtok(NULL, ";\n\r");
+	}
+
+	return field_count;
+}
 void Zenoh_Config::getNetworkConfig(char *mode, char *locator)
 {
 	FILE *fp;
@@ -215,8 +375,16 @@ void Zenoh_Config::getNetworkConfig(char *mode, char *locator)
 	// If file opened successfully, then read the file
 	if (fp) {
 		fgets(buffer, NET_CONFIG_LINE_SIZE, fp);
-		const char *config_locator = get_csv_field(buffer, 2);
-		char *config_mode = (char *)get_csv_field(buffer, 1);
+		const char *fields[2];
+		int nfields = parse_csv_line(buffer, fields, 2);
+
+		if (nfields < 1) {
+			PX4_ERR("Invalid Zenoh net config file (must contain the mode and optional locator separated by a ;).");
+			fclose(fp);
+			return;
+		}
+
+		char *config_mode = (char *)fields[0];

 		if (config_mode) {
 			config_mode[strcspn(config_mode, "\n")] = 0;
@@ -226,7 +394,8 @@ void Zenoh_Config::getNetworkConfig(char *mode, char *locator)
 			mode[0] = 0;
 		}

-		if (config_locator) {
+		if (nfields >= 2) {
+			const char *config_locator = fields[1];
 			strncpy(locator, config_locator, NET_LOCATOR_SIZE);

 		} else {
@@ -261,8 +430,21 @@ int Zenoh_Config::getLineCount(const char *filename)
 	return lines;
 }

+int Zenoh_Config::closePubSubMapping()
+{
+	if (fp_mapping != NULL) {
+		//Close the file
+		fclose(fp_mapping);
+		fp_mapping = NULL;
+		return 0;
+	}
+
+	return 0;
+}
+
+
 // Very rudamentary here but we've to wait for a more advanced param system
-int Zenoh_Config::getPubSubMapping(char *topic, char *type, const char *filename)
+int Zenoh_Config::getPubSubMapping(char *topic, char *type, int *instance, const char *filename)
 {
 	char buffer[MAX_LINE_SIZE];

@@ -272,13 +454,30 @@ int Zenoh_Config::getPubSubMapping(char *topic, char *type, const char *filename

 	if (fp_mapping) {
 		while (fgets(buffer, MAX_LINE_SIZE, fp_mapping) != NULL) {
-			if (buffer[0] != '\n') {
-				const char *config_type = get_csv_field(buffer, 2);
-				const char *config_topic = get_csv_field(buffer, 1);

-				strncpy(type, config_type, TOPIC_INFO_SIZE);
-				strncpy(topic, config_topic, TOPIC_INFO_SIZE);
-				return 1;
+			if (buffer[0] != '\n') {
+				const char *fields[3];
+				int nfields = parse_csv_line(buffer, fields, 3);
+
+
+				if (nfields >= 2) {
+					if (nfields == 3) {
+						if (sscanf(fields[2], "%d", instance) != 1) {
+							PX4_WARN("Malformed zenoh config instance %s (instance field should be an integer following the type)\n", fields[2]);
+							return -1;
+						}
+
+					} else {
+						*instance = -1;
+					}
+
+					strncpy(type, fields[1], TOPIC_INFO_SIZE);
+					strncpy(topic, fields[0], TOPIC_INFO_SIZE);
+					return 1;
+
+				} else {
+					return -1;
+				}
 			}

 		}
@@ -289,9 +488,7 @@ int Zenoh_Config::getPubSubMapping(char *topic, char *type, const char *filename
 	}

 	//Close the file
-	fclose(fp_mapping);
-	fp_mapping = NULL;
-	return 0;
+	return closePubSubMapping();

 }

@@ -319,19 +516,22 @@ void Zenoh_Config::dump_config()
 	{
 		char topic[TOPIC_INFO_SIZE];
 		char type[TOPIC_INFO_SIZE];
+		int instance_no;

 		printf("Publisher config:\n");

-		while (getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH) > 0) {
+		while (getPubSubMapping(topic, type, &instance_no, ZENOH_PUB_CONFIG_PATH) > 0) {
 			printf("Topic: %s\n", topic);
 			printf("Type: %s\n", type);
+			printf("Instance: %d\n", instance_no);
 		}

 		printf("\nSubscriber config:\n");

-		while (getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH) > 0) {
+		while (getPubSubMapping(topic, type, &instance_no, ZENOH_SUB_CONFIG_PATH) > 0) {
 			printf("Topic: %s\n", topic);
 			printf("Type: %s\n", type);
+			printf("Instance: %d\n", instance_no);
 		}
 	}
 }
@@ -56,9 +56,13 @@

 #define NET_MODE_SIZE sizeof("client")
 #define NET_LOCATOR_SIZE 64
-#define NET_CONFIG_LINE_SIZE NET_MODE_SIZE + NET_LOCATOR_SIZE
-#define TOPIC_INFO_SIZE 64
-#define MAX_LINE_SIZE 2*TOPIC_INFO_SIZE
+#define NET_CONFIG_LINE_SIZE (NET_MODE_SIZE + NET_LOCATOR_SIZE)
+#define KEYEXPR_RIHS01_SIZE sizeof("RIHS01_XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX")
+#define KEYEXPR_MSG_NAME "px4_msgs::msg::dds_::"
+#define KEYEXPR_MSG_NAME_SIZE sizeof(KEYEXPR_MSG_NAME)
+#define TOPIC_INFO_SIZE (96)
+#define MAX_LINE_SIZE (2 * TOPIC_INFO_SIZE)
+#define KEYEXPR_SIZE (MAX_LINE_SIZE + KEYEXPR_MSG_NAME_SIZE + KEYEXPR_RIHS01_SIZE + 128)

 class Zenoh_Config
 {
@@ -77,23 +81,26 @@ public:
 	{
 		return getLineCount(ZENOH_SUB_CONFIG_PATH);
 	}
-	int getPublisherMapping(char *topic, char *type)
+	int getPublisherMapping(char *topic, char *type, int *instance)
 	{
-		return getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH);
+		return getPubSubMapping(topic, type, instance, ZENOH_PUB_CONFIG_PATH);
 	}
-	int getSubscriberMapping(char *topic, char *type)
+	// existing_instance will be either 0 (should create a new instance) or nonzero (should reuse the existing 0 instance)
+	int getSubscriberMapping(char *topic, char *type, int *existing_instance)
 	{
-		return getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH);
+		return getPubSubMapping(topic, type, existing_instance, ZENOH_SUB_CONFIG_PATH);
 	}
+	int closePubSubMapping();


 private:
-	int getPubSubMapping(char *topic, char *type, const char *filename);
-	int AddPubSub(char *topic, char *datatype, const char *filename);
+	int getPubSubMapping(char *topic, char *type, int *new_instance, const char *filename);
+	int AddPubSub(char *topic, char *datatype, int new_instance, const char *filename);
+	int DeletePubSub(char *topic, const char *filename);
 	int SetNetworkConfig(char *mode, char *locator);
 	int getLineCount(const char *filename);

-	const char *get_csv_field(char *line, int num);
+	int parse_csv_line(char *line, const char **fields, int max_fields);
 	void generate_clean_config();
 	void dump_config();

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 zenoh_params.c
+ *
+ * Parameters defined by Zenoh
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+/**
+ * ROS2 RMW_ZENOH_CPP Domain id
+ *
+ * @min 0
+ * @max 232
+ */
+PARAM_DEFINE_INT32(ZENOH_DOMAIN_ID, 0);
Author	SHA1	Message	Date
Marco Hauswirth	c47b3ba1b0	* parameter renaming * move hor-motion check to method	2025-08-26 15:08:55 +02:00
Marco Hauswirth	db468f78a1	set state to unknown during init (re-init)	2025-08-26 15:08:55 +02:00
Marco Hauswirth	18bee5e08b	revert unnecessary unit-test changes	2025-08-26 15:08:55 +02:00
Marco Hauswirth	58f63909ca	* naming conventions * add landed-state for range-finder-cons-check * ajdust unit-tests	2025-08-26 15:08:55 +02:00
Marco Hauswirth	85b116f14e	remove terrain checks form baroRef unit-tests, vehicle vertical motion is not defined in test	2025-08-26 15:08:53 +02:00
Marco Hauswirth	c83debd64b	always run consistency-KF but limit state change to vertical velocity. this fixes behavior for jumps in fixed wing	2025-08-26 15:08:43 +02:00
Marco Hauswirth	9df07a9542	minor cleanup	2025-08-26 15:08:43 +02:00
Marco Hauswirth	f2df6c7264	only run range validator on regular data	2025-08-26 15:08:43 +02:00
Marco Hauswirth	f89f73cf37	adjust IMU falling detection baro-range unittest	2025-08-26 15:08:43 +02:00
Marco Hauswirth	a97cc72791	also run range consistency check while landed	2025-08-26 15:08:43 +02:00
Marco Hauswirth	c98a7ef8ae	change consistency to only be valid if running for a second	2025-08-26 15:08:43 +02:00
Marco Hauswirth	86725c0491	* clean up method calls * move P-init to ekf-derivation script	2025-08-26 15:08:43 +02:00
Marco Hauswirth	a078d6e306	fix uncertainty initialization	2025-08-26 15:08:43 +02:00
Marco Hauswirth	f56c4cc370	clean up range-filter fusion step	2025-08-26 15:08:41 +02:00
Marco Hauswirth	bb17333bf4	reset state to unknown on init	2025-08-26 15:08:09 +02:00
Marco Hauswirth	670d19c382	overhaul approach to work on unit tests and snow/blocking/normal flights	2025-08-26 15:08:09 +02:00
Marco Hauswirth	ce6fe59c8a	adjust threshold unit tests	2025-08-26 15:08:09 +02:00
Marco Hauswirth	ba72d37f9b	add symforce, adjust smaller things	2025-08-26 15:08:09 +02:00
Marco Hauswirth	dade5c3a5c	always update lpf	2025-08-26 15:08:09 +02:00
Marco Hauswirth	8c13115e27	fix bugs, rejection was too sensitive	2025-08-26 15:08:08 +02:00
Marco Hauswirth	4e40a86971	fix wrong miniEKF init	2025-08-26 15:07:33 +02:00
Marco Hauswirth	0058953e4f	handle noise while hovering	2025-08-26 15:07:31 +02:00
Marco Hauswirth	f1f1e6f059	add mini KF to validate range sensor measurements	2025-08-26 15:04:40 +02:00
Beat Küng	6ec8dec63a	commander: add valid_registrations_mask to ArmingCheckRequest.msg This allows external modes to individually check if they are flagged as invalid/unresponsive. Previously this was done only based on whether or not ArmingCheckRequest was received, which does not work when multiple modes are running.	2025-08-26 14:38:05 +02:00
Beat Küng	edfcdaa008	commander: check for stale arming_check_reply messages based on the message timestamp. Previously it was possible to run into the following case: - 2 external modes are registered (running inside the same ROS node) - they time out due to the micro xrce agent being blocked for some reason - PX4 removes them - the latest arming check replies still arrive to PX4 - the application restarts - the first mode gets registered - PX4 handles the previous arming check reply, and clears waiting_for_first_response, which reduces the timeout - the second mode registers and as part of that checks for message compatibility. This takes ~1s, triggering a timeout of the first mode	2025-08-26 14:38:05 +02:00
Silvan	a1ee9eb2c4	mavlink: remove streams from LOW_BANDWIDTH that are deprecated Signed-off-by: Silvan <silvan@auterion.com>	2025-08-25 17:34:34 +02:00
bresch	4a697d0191	ekf2: stop GNSS altittude and velocity aiding when gnss_fault is set	2025-08-25 10:22:23 +02:00
mahima-yoga	db3f33760e	docs: add instructions for controlling actuators in SIH	2025-08-25 09:28:43 +02:00
mahima-yoga	dd09cdf986	Commander: remove HIL_STATE_ON from arming lockdown Removing this from the boolean allows users to send pwm values in SIH.	2025-08-25 09:28:43 +02:00
chfriedrich98	4a5eabb61e	rover: constrain update steps	2025-08-22 12:13:01 +02:00
chfriedrich98	248f113141	rover: improve hold position logic (#25466 )	2025-08-22 12:10:16 +02:00
Peter van der Perk	c1d15d0e09	Zenoh: use strncpy and bool	2025-08-22 08:22:59 +02:00
Peter van der Perk	8689c00be7	Zenoh: cleanup and review	2025-08-22 08:22:59 +02:00
Peter van der Perk	17e843a985	zenoh: remove MessageFormat since Zenoh RIHS01 already provides it RIHS01 has the same functionality and already provides type safety with rmw_zenoh_cpp. The user on ROS2 can compare the PX4 ros2_lv hashes with their own px4_msgs for a mismatch	2025-08-22 08:22:59 +02:00
Peter van der Perk	44ff6d9c62	zenoh: exclude src/modules/zenoh/dds_topics.yaml from yaml check	2025-08-22 08:22:59 +02:00
Peter van der Perk	747bcc9db5	zenoh: Move ROS2 Rmw attachment code to rmw_attachment.h Allows for re-use for later ROS2 Service / Zenoh queryable	2025-08-22 08:22:59 +02:00
Benjamin Chung	c41216376a	Add an explicatory comment to Zenoh publisher's handling of the uOrb topic number (or lack thereof)	2025-08-22 08:22:59 +02:00
Benjamin Chung	88c1412d25	Zenoh CLI improvements	2025-08-22 08:22:59 +02:00
Benjamin Chung	01bf700f3d	Fix dds topics naming	2025-08-22 08:22:59 +02:00
Benjamin Chung	0bb9e5952a	Pubsub constructor template fix	2025-08-22 08:22:59 +02:00
Benjamin Chung	70054fc567	Implement instance selection & pub/sub deletion for Zenoh	2025-08-22 08:22:59 +02:00
Benjamin Chung	7a98c87fcb	Copy the uxrce config for Zenoh	2025-08-22 08:22:59 +02:00
Benjamin Chung	80b5cf2ed7	Prevent conversion warnings from static integers in zenoh-pico	2025-08-22 08:22:59 +02:00
Peter van der Perk	9ffd31097d	zenoh: Use CDRv1 to match ROS2 Fixes various padding related serialization issues.	2025-08-22 08:22:59 +02:00
Peter van der Perk	f99759db87	zenoh: Fix status keyexpr printf	2025-08-22 08:22:59 +02:00
Peter van der Perk	231128c68e	Zenoh set transport lease to 60000 to match ros2	2025-08-22 08:22:59 +02:00
Peter van der Perk	5622565eea	Zenoh optimize memory usage and add optional publish on matching	2025-08-22 08:22:59 +02:00
Peter van der Perk	7887f16daa	Update NuttX config for use with Zenoh	2025-08-22 08:22:59 +02:00
Peter van der Perk	0763bbe2cf	Generate default Zenoh config from dds_topics.yaml For easy transition from uxrce to zenoh and a sane base config to begin with	2025-08-22 08:22:59 +02:00
Peter van der Perk	bac009c2b8	Enable Zenoh by default on big-flash targets These targets can easily support Zenoh by default eases adoption and testing	2025-08-22 08:22:59 +02:00
Peter van der Perk	ac2627cca9	rmw attachment serialization changes Use new atachment serialization format Subscriber fix parsing payload and remove uorb publisher on destructor	2025-08-22 08:22:59 +02:00
Peter van der Perk	61e2f566ca	Zenoh config, lv and connection fixes Fixes a bug in the csv parsing Use % for / seperators in ros2_lv On startup retry connecting	2025-08-22 08:22:59 +02:00
Peter van der Perk	3d30eaae5f	Fix NuttX keepalive socketoption	2025-08-22 08:22:59 +02:00
Peter van der Perk	e052f35664	zenoh: omit timestamp attachment This isn't needed for the rmw_zenoh zenohd configuration	2025-08-22 08:22:59 +02:00
Peter van der Perk	2bc9cb4ead	zenoh: implemement experimental liveliness to get ROS2 graph to work	2025-08-22 08:22:59 +02:00
Peter van der Perk	5211d9c92e	zenoh: pubsub factory fix datatype naming convention	2025-08-22 08:22:59 +02:00
Peter van der Perk	575923b534	Zenoh: fix topic_name and datatype mapping Using substring was buggy instad we make dictionary based on datatypes and the get_topics function	2025-08-22 08:22:59 +02:00
Peter van der Perk	e37f20e94d	zenoh: Don't use uORB o_name as type but check for parent type For example vehicle_local_position_groundtruth has ROS2 type vehicle_local_position, so we've to use px4_msgs/VehicleLocalPosition as typename	2025-08-22 08:22:59 +02:00
Peter van der Perk	cb74cee970	zenoh: Increase CDR safety margin Figure out by trial and error with padding on vehicle_local_position	2025-08-22 08:22:59 +02:00
Peter van der Perk	70536766db	zenoh: Handle parsing errors in config	2025-08-22 08:22:59 +02:00
Peter van der Perk	40bba0069d	zenoh: Fix handling for non-existing types	2025-08-22 08:22:59 +02:00
Peter van der Perk	35004e357c	zenoh: Add px4_sitl_zenoh to cmake-variants.yaml	2025-08-22 08:22:59 +02:00
Peter van der Perk	923257779a	zenoh: Default to 127.0.0.1 when using sitl/posix Also improve error message when connection failed	2025-08-22 08:22:59 +02:00
Peter van der Perk	a24b3a121c	zenoh: Improve error message when there are no scouting results	2025-08-22 08:22:59 +02:00
Peter van der Perk	85cab5a4db	sitl: autostart zenoh if enabled	2025-08-22 08:22:59 +02:00
Peter van der Perk	859ba81e33	Zenoh fix gcc/sitl compile errors	2025-08-22 08:22:59 +02:00
Peter van der Perk	4aff095f9b	IDL 2 RIHS01 remove tempfile and print	2025-08-22 08:22:59 +02:00
Peter van der Perk	796efeebe7	Implement Domain id parameter and move gid to zenoh	2025-08-22 08:22:59 +02:00
Peter van der Perk	9d02698987	Update Zenoh for library and implement rmw_zenoh features New zenoh-pico library Keyexpr instrospection RIHS01 Types	2025-08-22 08:22:59 +02:00
Peter van der Perk	e1a7fbce71	Update cdrstream code generator including typehash	2025-08-22 08:22:59 +02:00
Peter van der Perk	a87456b38b	Update rosidl Adds support for typehashes	2025-08-22 08:22:59 +02:00
Peter van der Perk	33a5122916	Update Zenoh-pico	2025-08-22 08:22:59 +02:00
ljarvela	b53ecf7f68	uavcan: increase battery filter sample interval to 500ms (#25454 ) Fixes issue #25430 Co-authored-by: Lasse Järvelä <lasse.jarvela@iceye.com>	2025-08-21 19:19:52 -07:00
Alexander Lerach	138427b3a8	config: add dynamic init file * config: add dynamic init file * added review feedback * added docs	2025-08-21 16:46:06 +02:00
Alexander Lerach	785ea1a137	ubx: add new mode for GCS usage * ubx: add new mode for GCS usage * use head ref	2025-08-21 15:45:40 +02:00
chfriedrich98	8e5cd59502	rover: fix setpoint generation	2025-08-21 13:33:19 +02:00
Beat Küng	df11aa1d69	fix commander: handle mode executor correctly on disarm There were a number of cases where the state was not correct or not as desired after disarming, when running an external mode 'MyMission' with executor: - run MyMission, which triggers Hold, then Land - before: Mode: Hold, executor_in_charge: 1 - after: Mode: MyMission, executor_in_charge: 1 - run MyMission, then user switches to RTL - before: Mode: MyMission, executor_in_charge: 0 - after: Mode: MyMission, executor_in_charge: 1 - run MyMission, then while in Hold mode, low battery failsafe (RTL) - before: Mode: Hold, executor_in_charge: 1 - after: Mode: MyMission, executor_in_charge: 1 - run MyMission, then stop external mode (terminate the process) - before: Mode: (mode not available), executor_in_charge: 0 - after: Mode: Hold, executor_in_charge: 0 This case is unchanged: - run MyMission, then low battery failsafe (RTL) - before: Mode: MyMission, executor_in_charge: 1 - after: Mode: MyMission, executor_in_charge: 1	2025-08-21 10:06:30 +02:00