mixer_module: send a last sample out after all outputs were disabled

This matters for PWM when the last output gets disabled on either FMU or IO
it would just keep on running.

Also when rebooting with a parameters reset or new airframe with no mapped outputs
it would previously keep outputting PWM with the disarmed value of the new airframe
e.g. 1000us which is a safety hazard because servos could break the physical limit of the
model or miscalibrated ESCs spinning motors.

.editorconfig

@@ -0,0 +1,14 @@
+root = true
+
+[*]
+insert_final_newline = false
+
+[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
+indent_style = tab
+tab_width = 8
+# Not in the official standard, but supported by many editors
+max_line_length = 120
+
+[*.yaml]
+indent_style = space
+indent_size = 2

.gitmodules

@@ -81,5 +81,5 @@
 	url = https://github.com/PX4/PX4-gazebo-models.git
 	branch = main
 [submodule "boards/modalai/voxl2/libfc-sensor-api"]
-	path = src/modules/muorb/apps/libfc-sensor-api
+	path = boards/modalai/voxl2/libfc-sensor-api
 	url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git

Tools/astyle/files_to_check_code_style.sh

@@ -30,4 +30,5 @@ exec find boards msg src platforms test \
     -path src/lib/cdrstream/cyclonedds -prune -o \
     -path src/lib/cdrstream/rosidl -prune -o \
     -path src/modules/zenoh/zenoh-pico -prune -o \
+    -path boards/modalai/voxl2/libfc-sensor-api -prune -o \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

Tools/msg/templates/uorb/msg.cpp.em

@@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>
 
+@{
+queue_length = 1
+for constant in spec.constants:
+	if constant.name == 'ORB_QUEUE_LENGTH':
+		queue_length = constant.val
+}@
+
 @[for topic in topics]@
 static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
-ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
+ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic), @queue_length);
 @[end for]
 
 void print_message(const orb_metadata *meta, const @uorb_struct& message)

Tools/simulation/gazebo-classic/sitl_gazebo-classic

@@ -1 +1 @@
-Subproject commit 33ac87a37676fb597de110e926bbf0a080169ffe
+Subproject commit da7206e057703cc645770f02437013358b71e1c0

Tools/simulation/gz

@@ -1 +1 @@
-Subproject commit 222833656802532ec2271986a65fd198cfa48259
+Subproject commit 6b4ed09d1b495fbff663f098979cc046df013abd

boards/ark/can-flow/firmware.prototype

@@ -4,7 +4,7 @@
     "description": "Firmware for the ARK flow board",
     "image": "",
     "build_time": 0,
-    "summary": "ARFFLOW",
+    "summary": "ARKFLOW",
     "version": "0.1",
     "image_size": 0,
     "image_maxsize": 2080768,

boards/ark/septentrio-gps/default.px4board

@@ -20,6 +20,7 @@ CONFIG_UAVCANNODE_RTK_DATA=y
 CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
+CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y

boards/ark/septentrio-gps/init/rc.board_defaults

@@ -11,3 +11,5 @@ param set-default SENS_IMU_CLPNOTI 0
 
 safety_button start
 tone_alarm start
+
+ekf2 start

boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig

@@ -50,6 +50,7 @@ CONFIG_STACK_COLORATION=y
 CONFIG_START_DAY=30
 CONFIG_START_MONTH=11
 CONFIG_STDIO_DISABLE_BUFFERING=y
+CONFIG_STM32_FLASH_CONFIG_G=y
 CONFIG_STM32_NOEXT_VECTORS=y
 CONFIG_STM32_TIM8=y
 CONFIG_TASK_NAME_SIZE=0

boards/ark/septentrio-gps/nuttx-config/nsh/defconfig

@@ -123,6 +123,7 @@ CONFIG_STM32_ADC1=y
 CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
 CONFIG_STM32_DMA1=y
 CONFIG_STM32_DMA2=y
+CONFIG_STM32_FLASH_CONFIG_G=y
 CONFIG_STM32_FLASH_PREFETCH=y
 CONFIG_STM32_FLOWCONTROL_BROKEN=y
 CONFIG_STM32_I2C1=y

boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld

@@ -33,7 +33,7 @@
  *
  ****************************************************************************/
 
-/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
+/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
  * 256Kb of SRAM. SRAM is split up into three blocks:
  *
  * 1) 112Kb of SRAM beginning at address 0x2000:0000

boards/ark/septentrio-gps/nuttx-config/scripts/script.ld

@@ -33,7 +33,7 @@
  *
  ****************************************************************************/
 
-/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
+/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
  * 256Kb of SRAM. SRAM is split up into three blocks:
  *
  * 1) 112Kb of SRAM beginning at address 0x2000:0000

boards/atl/mantis-edu/src/board_config.h

@@ -64,9 +64,6 @@
 // Hacks for MAVLink RC button input
 #define ATL_MANTIS_RC_INPUT_HACKS
 
-// Hacks for MAVLink RC button input
-#define ATL_MANTIS_RC_INPUT_HACKS
-
 /*
  * ADC channels
  *

boards/modalai/fc-v2/default.px4board

@@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y

boards/modalai/voxl2-slpi/default.px4board

@@ -4,6 +4,8 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
@@ -29,3 +31,4 @@ CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_ORB_COMMUNICATOR=y
+CONFIG_PARAM_REMOTE=y

boards/modalai/voxl2-slpi/src/CMakeLists.txt

@@ -44,7 +44,6 @@ add_library(drivers_board
     )
 
 # Add custom drivers for SLPI
-# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)

boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp

@@ -62,6 +62,8 @@
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@@ -88,21 +90,22 @@ static bool _is_running = false;
 volatile bool _task_should_exit = false;
 static px4_task_t _task_handle = -1;
 int _uart_fd = -1;
-bool debug = false;
+bool _debug = false;
 std::string port = "2";
 int baudrate = 921600;
 const unsigned mode_flag_custom = 1;
 const unsigned mode_flag_armed = 128;
-bool _send_gps = false;
 bool _send_mag = false;
+bool _send_distance = false;
 
 uORB::Publication<battery_status_s>				_battery_pub{ORB_ID(battery_status)};
 uORB::PublicationMulti<sensor_gps_s>			_sensor_gps_pub{ORB_ID(sensor_gps)};
-uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};
 uORB::Publication<vehicle_odometry_s>			_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 uORB::Publication<vehicle_odometry_s>			_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 uORB::PublicationMulti<sensor_baro_s>			_sensor_baro_pub{ORB_ID(sensor_baro)};
 uORB::Publication<esc_status_s>					_esc_status_pub{ORB_ID(esc_status)};
+uORB::Publication<distance_sensor_s>			_distance_sensor_pub{ORB_ID(distance_sensor)};
+uORB::Publication<sensor_optical_flow_s>        _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 uORB::Subscription 								_battery_status_sub{ORB_ID(battery_status)};
 
 int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
@@ -128,13 +131,42 @@ float x_gyro = 0;
 float y_gyro = 0;
 float z_gyro = 0;
 uint64_t gyro_accel_time = 0;
-bool _use_software_mav_throttling{false};
 
-int heartbeat_counter = 0;
-int imu_counter = 0;
-int hil_sensor_counter = 0;
-int vision_msg_counter = 0;
-int gps_counter = 0;
+// Status counters
+uint32_t heartbeat_received_counter = 0;
+uint32_t heartbeat_sent_counter = 0;
+uint32_t imu_counter = 0;
+uint32_t hil_sensor_counter = 0;
+uint32_t mag_counter = 0;
+uint32_t baro_counter = 0;
+uint32_t actuator_sent_counter = 0;
+uint32_t odometry_received_counter = 0;
+uint32_t odometry_sent_counter = 0;
+uint32_t gps_received_counter = 0;
+uint32_t gps_sent_counter = 0;
+uint32_t distance_received_counter = 0;
+uint32_t distance_sent_counter = 0;
+uint32_t flow_received_counter = 0;
+uint32_t flow_sent_counter = 0;
+uint32_t unknown_msg_received_counter = 0;
+
+enum class position_source {GPS, VIO, FLOW, NUM_POSITION_SOURCES};
+
+struct position_source_data_s {
+	char label[8];
+	bool send;
+	bool fail;
+	uint32_t failure_duration;
+	uint64_t failure_duration_start;
+} position_source_data[(int) position_source::NUM_POSITION_SOURCES] = {
+	{"GPS", false, false, 0, 0},
+	{"VIO", false, false, 0, 0},
+	{"FLOW", false, false, 0, 0}
+};
+
+uint64_t first_sensor_msg_timestamp = 0;
+uint64_t first_sensor_report_timestamp = 0;
+uint64_t last_sensor_report_timestamp = 0;
 
 vehicle_status_s _vehicle_status{};
 vehicle_control_mode_s _control_mode{};
@@ -144,7 +176,6 @@ battery_status_s _battery_status{};
 sensor_accel_fifo_s accel_fifo{};
 sensor_gyro_fifo_s gyro_fifo{};
 
-
 int openPort(const char *dev, speed_t speed);
 int closePort();
 
@@ -153,7 +184,8 @@ int writeResponse(void *buf, size_t len);
 
 int start(int argc, char *argv[]);
 int stop();
-int get_status();
+void print_status();
+void clear_status_counters();
 bool isOpen() { return _uart_fd >= 0; };
 
 void usage();
@@ -163,50 +195,65 @@ void *send_actuator(void *);
 void send_actuator_data();
 
 void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
+void handle_message_hil_optical_flow(mavlink_message_t *msg);
+void handle_message_distance_sensor(mavlink_message_t *msg);
 void handle_message_hil_gps_dsp(mavlink_message_t *msg);
 void handle_message_odometry_dsp(mavlink_message_t *msg);
-void handle_message_vision_position_estimate_dsp(mavlink_message_t *msg);
 void handle_message_command_long_dsp(mavlink_message_t *msg);
 
 void handle_message_dsp(mavlink_message_t *msg);
 void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
-void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control);
+void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control);
 
 void
 handle_message_dsp(mavlink_message_t *msg)
 {
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_HIL_SENSOR:
+		hil_sensor_counter++;
 		handle_message_hil_sensor_dsp(msg);
 		break;
 
 	case MAVLINK_MSG_ID_HIL_GPS:
-		if (_send_gps) { handle_message_hil_gps_dsp(msg); }
+		gps_received_counter++;
 
-		break;
+		if (position_source_data[(int) position_source::GPS].send) { handle_message_hil_gps_dsp(msg); }
 
-	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
-		handle_message_vision_position_estimate_dsp(msg);
 		break;
 
 	case MAVLINK_MSG_ID_ODOMETRY:
-		handle_message_odometry_dsp(msg);
-		break;
+		odometry_received_counter++;
+
+		if (position_source_data[(int) position_source::VIO].send) { handle_message_odometry_dsp(msg); }
 
-	case MAVLINK_MSG_ID_COMMAND_LONG:
-		handle_message_command_long_dsp(msg);
 		break;
 
 	case MAVLINK_MSG_ID_HEARTBEAT:
-		PX4_DEBUG("Heartbeat msg received");
+		heartbeat_received_counter++;
+
+		if (_debug) { PX4_INFO("Heartbeat msg received"); }
+
 		break;
 
-	case MAVLINK_MSG_ID_SYSTEM_TIME:
-		PX4_DEBUG("MAVLINK SYSTEM TIME");
+	case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
+		flow_received_counter++;
+
+		if (position_source_data[(int) position_source::FLOW].send) { handle_message_hil_optical_flow(msg); }
+
+		break;
+
+	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
+		distance_received_counter++;
+
+		if (_send_distance) { handle_message_distance_sensor(msg); }
+
 		break;
 
 	default:
-		PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
+		unknown_msg_received_counter++;
+
+		if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
+
 		break;
 	}
 }
@@ -228,7 +275,6 @@ void send_actuator_data()
 	bool first_sent = false;
 
 	while (true) {
-
 		bool controls_updated = false;
 		(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
 
@@ -239,45 +285,50 @@ void send_actuator_data()
 		bool actuator_updated = false;
 		(void) orb_check(_actuator_outputs_sub, &actuator_updated);
 
+		uint8_t  newBuf[512];
+		uint16_t newBufLen = 0;
+
+		mavlink_hil_actuator_controls_t hil_act_control;
+		actuator_controls_from_outputs_dsp(&hil_act_control);
+
+		mavlink_message_t message{};
+		mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
+
 		if (actuator_updated) {
 			orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
-			px4_lockstep_wait_for_components();
 
 			if (_actuator_outputs.timestamp > 0) {
-				mavlink_hil_actuator_controls_t hil_act_control;
-				actuator_controls_from_outputs_dsp(&hil_act_control);
-
-				mavlink_message_t message{};
-				mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
 				previous_timestamp = _actuator_outputs.timestamp;
 				previous_uorb_timestamp = _actuator_outputs.timestamp;
-				uint8_t  newBuf[512];
-				uint16_t newBufLen = 0;
+
 				newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
 				int writeRetval = writeResponse(&newBuf, newBufLen);
-				PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
+
+				actuator_sent_counter++;
+
+				if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
+
 				first_sent = true;
-				send_esc_telemetry_dsp(hil_act_control);
+
+				send_esc_status(hil_act_control);
 			}
 
-		} else if (!actuator_updated && first_sent && differential > 4000) {
-			mavlink_hil_actuator_controls_t hil_act_control;
-			actuator_controls_from_outputs_dsp(&hil_act_control);
+		} else if (! actuator_updated && first_sent && differential > 4000) {
 			previous_timestamp = hrt_absolute_time();
 
-			mavlink_message_t message{};
-			mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
-			uint8_t  newBuf[512];
-			uint16_t newBufLen = 0;
 			newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
 			int writeRetval = writeResponse(&newBuf, newBufLen);
-			//PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential);
 
-			PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
-			send_esc_telemetry_dsp(hil_act_control);
+			actuator_sent_counter++;
+
+			if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
+
+			send_esc_status(hil_act_control);
 		}
 
 		differential = hrt_absolute_time() - previous_timestamp;
+
+		px4_usleep(1000);
 	}
 }
 
@@ -287,14 +338,10 @@ void task_main(int argc, char *argv[])
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 
-	while ((ch = px4_getopt(argc, argv, "vsdcmgp:b:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "odmghfp:b:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		case 's':
-			_use_software_mav_throttling = true;
-			break;
-
 		case 'd':
-			debug = true;
+			_debug = true;
 			break;
 
 		case 'p':
@@ -310,7 +357,19 @@ void task_main(int argc, char *argv[])
 			break;
 
 		case 'g':
-			_send_gps = true;
+			position_source_data[(int) position_source::GPS].send = true;
+			break;
+
+		case 'o':
+			position_source_data[(int) position_source::VIO].send = true;
+			break;
+
+		case 'h':
+			_send_distance = true;
+			break;
+
+		case 'f':
+			position_source_data[(int) position_source::FLOW].send = true;
 			break;
 
 		default:
@@ -319,11 +378,13 @@ void task_main(int argc, char *argv[])
 	}
 
 	const char *charport = port.c_str();
-	int openRetval = openPort(charport, (speed_t) baudrate);
-	int open = isOpen();
+	(void) openPort(charport, (speed_t) baudrate);
 
-	if (open) {
-		PX4_ERR("Port is open: %d", openRetval);
+	if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
+
+	if (! isOpen()) {
+		PX4_ERR("DSP HITL failed to open serial port");
+		return;
 	}
 
 	uint64_t last_heartbeat_timestamp = hrt_absolute_time();
@@ -342,14 +403,11 @@ void task_main(int argc, char *argv[])
 	pthread_attr_destroy(&sender_thread_attr);
 
 	int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
 
 	_is_running = true;
 
 	while (!_task_should_exit) {
-
 		uint8_t rx_buf[1024];
-		//rx_buf[511] = '\0';
 
 		uint64_t timestamp = hrt_absolute_time();
 
@@ -357,8 +415,8 @@ void task_main(int argc, char *argv[])
 		if (got_first_sensor_msg) {
 			uint64_t delta_time = timestamp - last_imu_update_timestamp;
 
-			if (delta_time > 15000) {
-				PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
+			if ((imu_counter) && (delta_time > 15000)) {
+				PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time);
 			}
 
 			uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time();
@@ -396,7 +454,7 @@ void task_main(int argc, char *argv[])
 			hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
 			(void) writeResponse(&hb_newBuf, hb_newBufLen);
 			last_heartbeat_timestamp = timestamp;
-			heartbeat_counter++;
+			heartbeat_sent_counter++;
 		}
 
 		bool vehicle_updated = false;
@@ -416,7 +474,7 @@ void task_main(int argc, char *argv[])
 	_is_running = false;
 }
 
-void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
+void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
 {
 	esc_status_s esc_status{};
 	esc_status.timestamp = hrt_absolute_time();
@@ -448,17 +506,13 @@ void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
 	_esc_status_pub.publish(esc_status);
 }
 
-
 void
 handle_message_command_long_dsp(mavlink_message_t *msg)
 {
-	/* command */
 	mavlink_command_long_t cmd_mavlink;
 	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
 
-	if (debug) {
-		PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
-	}
+	if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
 
 	mavlink_command_ack_t ack = {};
 	ack.result = MAV_RESULT_UNSUPPORTED;
@@ -470,46 +524,140 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
 	uint16_t acknewBufLen = 0;
 	acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
 	int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
-	PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time());
+
+	if (_debug) { PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); }
 }
 
+int	flow_debug_counter = 0;
+
 void
-handle_message_vision_position_estimate_dsp(mavlink_message_t *msg)
+handle_message_hil_optical_flow(mavlink_message_t *msg)
 {
-	mavlink_vision_position_estimate_t vpe;
-	mavlink_msg_vision_position_estimate_decode(msg, &vpe);
+	mavlink_hil_optical_flow_t flow;
+	mavlink_msg_hil_optical_flow_decode(msg, &flow);
 
-	// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
-	vehicle_odometry_s odom{};
-	uint64_t timestamp = hrt_absolute_time();
-	odom.timestamp_sample = timestamp;
+	if ((_debug) && (!(flow_debug_counter % 10))) {
+		PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
+		PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
+	}
 
-	odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
-	odom.position[0] = vpe.x;
-	odom.position[1] = vpe.y;
-	odom.position[2] = vpe.z;
+	flow_debug_counter++;
 
-	const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
-	q.copyTo(odom.q);
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
+	device_id.devid_s.bus = 1;
+	device_id.devid_s.address = msg->sysid;
+	device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
 
-	// VISION_POSITION_ESTIMATE covariance
-	//  Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
-	//  (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
-	//  If unknown, assign NaN value to first element in the array.
-	odom.position_variance[0] = vpe.covariance[0];  // X  row 0, col 0
-	odom.position_variance[1] = vpe.covariance[6];  // Y  row 1, col 1
-	odom.position_variance[2] = vpe.covariance[11]; // Z  row 2, col 2
+	sensor_optical_flow_s sensor_optical_flow{};
 
-	odom.orientation_variance[0] = vpe.covariance[15]; // R  row 3, col 3
-	odom.orientation_variance[1] = vpe.covariance[18]; // P  row 4, col 4
-	odom.orientation_variance[2] = vpe.covariance[20]; // Y  row 5, col 5
+	sensor_optical_flow.timestamp_sample = hrt_absolute_time();
+	sensor_optical_flow.device_id = device_id.devid;
 
-	odom.reset_counter = vpe.reset_counter;
+	sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
+	sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
 
-	odom.timestamp = hrt_absolute_time();
+	sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
+	sensor_optical_flow.quality = flow.quality;
 
-	_visual_odometry_pub.publish(odom);
-	vision_msg_counter++;
+	int index = (int) position_source::FLOW;
+
+	if (position_source_data[index].fail) {
+		uint32_t duration = position_source_data[index].failure_duration;
+		hrt_abstime start = position_source_data[index].failure_duration_start;
+
+		if (duration) {
+			if (hrt_elapsed_time(&start) > (duration * 1000000)) {
+				PX4_INFO("Optical flow failure ending");
+				position_source_data[index].fail = false;
+				position_source_data[index].failure_duration = 0;
+				position_source_data[index].failure_duration_start = 0;
+
+			} else {
+				sensor_optical_flow.quality = 0;
+			}
+
+		} else {
+			sensor_optical_flow.quality = 0;
+		}
+	}
+
+	const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
+
+	if (integrated_gyro.isAllFinite()) {
+		integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
+		sensor_optical_flow.delta_angle_available = true;
+	}
+
+	sensor_optical_flow.max_flow_rate       = NAN;
+	sensor_optical_flow.min_ground_distance = NAN;
+	sensor_optical_flow.max_ground_distance = NAN;
+
+	// Use distance value for distance sensor topic
+	// if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
+	// 	// Positive value (including zero): distance known. Negative value: Unknown distance.
+	// 	sensor_optical_flow.distance_m = flow.distance;
+	// 	sensor_optical_flow.distance_available = true;
+	// }
+
+	// Emulate voxl-flow-server where distance comes in a separate
+	// distance sensor topic message
+	sensor_optical_flow.distance_m = 0.0f;
+	sensor_optical_flow.distance_available = false;
+
+	sensor_optical_flow.timestamp = hrt_absolute_time();
+
+	_sensor_optical_flow_pub.publish(sensor_optical_flow);
+
+	flow_sent_counter++;
+}
+
+int distance_debug_counter = 0;
+
+void handle_message_distance_sensor(mavlink_message_t *msg)
+{
+	mavlink_distance_sensor_t dist_sensor;
+	mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
+
+	if ((_debug) && (!(distance_debug_counter % 10))) {
+		PX4_INFO("distance: time: %u, quality: %u, height: %u",
+			 dist_sensor.time_boot_ms, dist_sensor.signal_quality,
+			 dist_sensor.current_distance);
+	}
+
+	distance_debug_counter++;
+
+	distance_sensor_s ds{};
+
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
+	device_id.devid_s.bus = 1;
+	device_id.devid_s.address = msg->sysid;
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
+
+	ds.timestamp        = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
+	ds.min_distance     = static_cast<float>(dist_sensor.min_distance) * 1e-2f;     /* cm to m */
+	ds.max_distance     = static_cast<float>(dist_sensor.max_distance) * 1e-2f;     /* cm to m */
+	ds.current_distance = static_cast<float>(dist_sensor.current_distance) * 1e-2f; /* cm to m */
+	ds.variance         = dist_sensor.covariance * 1e-4f;                           /* cm^2 to m^2 */
+	ds.h_fov            = dist_sensor.horizontal_fov;
+	ds.v_fov            = dist_sensor.vertical_fov;
+	ds.q[0]             = dist_sensor.quaternion[0];
+	ds.q[1]             = dist_sensor.quaternion[1];
+	ds.q[2]             = dist_sensor.quaternion[2];
+	ds.q[3]             = dist_sensor.quaternion[3];
+	ds.type             = dist_sensor.type;
+	ds.device_id        = device_id.devid;
+	ds.orientation      = dist_sensor.orientation;
+
+	// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown
+	// quality value. Also it comes normalised between 1 and 100 while the uORB
+	// signal quality is normalised between 0 and 100.
+	ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
+
+	_distance_sensor_pub.publish(ds);
+
+	distance_sent_counter++;
 }
 
 void
@@ -518,6 +666,8 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
 	mavlink_odometry_t odom_in;
 	mavlink_msg_odometry_decode(msg, &odom_in);
 
+	odometry_sent_counter++;
+
 	// fill vehicle_odometry from Mavlink ODOMETRY
 	vehicle_odometry_s odom{};
 	uint64_t timestamp = hrt_absolute_time();
@@ -699,6 +849,28 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
 	odom.reset_counter = odom_in.reset_counter;
 	odom.quality = odom_in.quality;
 
+	int index = (int) position_source::VIO;
+
+	if (position_source_data[index].fail) {
+		uint32_t duration = position_source_data[index].failure_duration;
+		hrt_abstime start = position_source_data[index].failure_duration_start;
+
+		if (duration) {
+			if (hrt_elapsed_time(&start) > (duration * 1000000)) {
+				PX4_INFO("VIO failure ending");
+				position_source_data[index].fail = false;
+				position_source_data[index].failure_duration = 0;
+				position_source_data[index].failure_duration_start = 0;
+
+			} else {
+				odom.quality = 0;
+			}
+
+		} else {
+			odom.quality = 0;
+		}
+	}
+
 	switch (odom_in.estimator_type) {
 	case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
 	case MAV_ESTIMATOR_TYPE_NAIVE:
@@ -745,10 +917,6 @@ void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg)
 	msg->mode = mode_flag_custom;
 	msg->mode |= (armed) ? mode_flag_armed : 0;
 	msg->flags = 0;
-
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-	msg->flags |= 1;
-#endif
 }
 
 int openPort(const char *dev, speed_t speed)
@@ -759,7 +927,8 @@ int openPort(const char *dev, speed_t speed)
 	}
 
 	_uart_fd = qurt_uart_open(dev, speed);
-	PX4_DEBUG("qurt_uart_opened");
+
+	if (_debug) { PX4_INFO("qurt_uart_opened"); }
 
 	if (_uart_fd < 0) {
 		PX4_ERR("Error opening port: %s (%i)", dev, errno);
@@ -840,25 +1009,50 @@ int stop()
 
 void usage()
 {
-	PX4_INFO("Usage: dsp_hitl {start|info|status|stop}");
+	PX4_INFO("Usage: dsp_hitl {start|status|clear|failure|stop}");
+	PX4_INFO("       failure  <source> <duration>");
+	PX4_INFO("                source: gps, vio, flow");
+	PX4_INFO("                duration: 0 (toggle state), else seconds");
 }
 
-int get_status()
+void print_status()
 {
 	PX4_INFO("Running: %s", _is_running ? "yes" : "no");
-	PX4_INFO("Status of IMU_Data counter: %i", imu_counter);
-	PX4_INFO("Value of current accel x, y, z data: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
-	PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
-	PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
-	PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
-	PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
-	PX4_INFO("Value of GPS Data counter: %i", gps_counter);
-	return 0;
+	PX4_INFO("HIL Sensor received: %i", hil_sensor_counter);
+	PX4_INFO("IMU updates: %i", imu_counter);
+	PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
+	PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
+	PX4_INFO("Magnetometer sent: %i", mag_counter);
+	PX4_INFO("Barometer sent: %i", baro_counter);
+	PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
+	PX4_INFO("Odometry received: %i, sent: %i", odometry_received_counter, odometry_sent_counter);
+	PX4_INFO("GPS received: %i, sent: %i", gps_received_counter, gps_sent_counter);
+	PX4_INFO("Distance sensor received: %i, sent: %i", distance_received_counter, distance_sent_counter);
+	PX4_INFO("Optical flow received: %i, sent: %i", flow_received_counter, flow_sent_counter);
+	PX4_INFO("Actuator updates sent: %i", actuator_sent_counter);
+	PX4_INFO("Unknown messages received: %i", unknown_msg_received_counter);
 }
 
-uint64_t first_sensor_msg_timestamp = 0;
-uint64_t first_sensor_report_timestamp = 0;
-uint64_t last_sensor_report_timestamp = 0;
+void
+clear_status_counters()
+{
+	heartbeat_received_counter = 0;
+	heartbeat_sent_counter = 0;
+	imu_counter = 0;
+	hil_sensor_counter = 0;
+	mag_counter = 0;
+	baro_counter = 0;
+	actuator_sent_counter = 0;
+	odometry_received_counter = 0;
+	odometry_sent_counter = 0;
+	gps_received_counter = 0;
+	gps_sent_counter = 0;
+	distance_received_counter = 0;
+	distance_sent_counter = 0;
+	flow_received_counter = 0;
+	flow_sent_counter = 0;
+	unknown_msg_received_counter = 0;
+}
 
 void
 handle_message_hil_sensor_dsp(mavlink_message_t *msg)
@@ -928,6 +1122,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
 			}
 
 			_px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
+
+			mag_counter++;
 		}
 	}
 
@@ -942,17 +1138,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
 		sensor_baro.error_count = 0;
 		sensor_baro.timestamp = hrt_absolute_time();
 		_sensor_baro_pub.publish(sensor_baro);
-	}
 
-	// differential pressure
-	if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
-		differential_pressure_s report{};
-		report.timestamp_sample = gyro_accel_time;
-		report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
-		report.temperature = hil_sensor.temperature;
-		report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
-		report.timestamp = hrt_absolute_time();
-		_differential_pressure_pub.publish(report);
+		baro_counter++;
 	}
 
 	// battery status
@@ -970,7 +1157,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
 
 		_battery_pub.publish(hil_battery_status);
 	}
-	hil_sensor_counter++;
 }
 
 void
@@ -989,15 +1175,41 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
 
 	gps.device_id = device_id.devid;
 
-	gps.latitude_deg = hil_gps.lat;
-	gps.longitude_deg = hil_gps.lon;
-	gps.altitude_msl_m = hil_gps.alt;
-	gps.altitude_ellipsoid_m = hil_gps.alt;
+	gps.latitude_deg = hil_gps.lat * 1e-7;
+	gps.longitude_deg = hil_gps.lon * 1e-7;
+	gps.altitude_msl_m = hil_gps.alt * 1e-3;
+	gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
 
 	gps.s_variance_m_s = 0.25f;
 	gps.c_variance_rad = 0.5f;
+
+	gps.satellites_used = hil_gps.satellites_visible;
 	gps.fix_type = hil_gps.fix_type;
 
+	int index = (int) position_source::GPS;
+
+	if (position_source_data[index].fail) {
+		uint32_t duration = position_source_data[index].failure_duration;
+		hrt_abstime start = position_source_data[index].failure_duration_start;
+
+		if (duration) {
+			if (hrt_elapsed_time(&start) > (duration * 1000000)) {
+				PX4_INFO("GPS failure ending");
+				position_source_data[index].fail = false;
+				position_source_data[index].failure_duration = 0;
+				position_source_data[index].failure_duration_start = 0;
+
+			} else {
+				gps.satellites_used = 1;
+				gps.fix_type = 0;
+			}
+
+		} else {
+			gps.satellites_used = 1;
+			gps.fix_type = 0;
+		}
+	}
+
 	gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
 	gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
 
@@ -1021,7 +1233,6 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
 	gps.timestamp_time_relative = 0;
 	gps.time_utc_usec = hil_gps.time_usec;
 
-	gps.satellites_used = hil_gps.satellites_visible;
 
 	gps.heading = NAN;
 	gps.heading_offset = NAN;
@@ -1029,10 +1240,51 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
 	gps.timestamp = hrt_absolute_time();
 
 	_sensor_gps_pub.publish(gps);
-	gps_counter++;
+
+	gps_sent_counter++;
 }
 
+int
+process_failure(dsp_hitl::position_source src, int duration)
+{
+	if (src >= position_source::NUM_POSITION_SOURCES) {
+		return 1;
+	}
+
+	int index = (int) src;
+
+	if (position_source_data[index].send) {
+		if (duration <= 0) {
+			// Toggle state
+			if (position_source_data[index].fail) {
+				PX4_INFO("Ending indefinite %s failure", position_source_data[index].label);
+				position_source_data[index].fail = false;
+
+			} else {
+				PX4_INFO("Starting indefinite %s failure", position_source_data[index].label);
+				position_source_data[index].fail = true;
+			}
+
+			position_source_data[index].failure_duration = 0;
+			position_source_data[index].failure_duration_start = 0;
+
+		} else {
+			PX4_INFO("%s failure for %d seconds", position_source_data[index].label, duration);
+			position_source_data[index].fail = true;
+			position_source_data[index].failure_duration = duration;
+			position_source_data[index].failure_duration_start = hrt_absolute_time();
+		}
+
+	} else {
+		PX4_ERR("%s not active, cannot create failure", position_source_data[index].label);
+		return 1;
+	}
+
+	return 0;
 }
+
+} // End dsp_hitl namespace
+
 int dsp_hitl_main(int argc, char *argv[])
 {
 	int myoptind = 1;
@@ -1044,20 +1296,47 @@ int dsp_hitl_main(int argc, char *argv[])
 
 	const char *verb = argv[myoptind];
 
-
 	if (!strcmp(verb, "start")) {
 		return dsp_hitl::start(argc - 1, argv + 1);
-	}
 
-	else if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return dsp_hitl::stop();
-	}
 
-	else if (!strcmp(verb, "status")) {
-		return dsp_hitl::get_status();
-	}
+	} else if (!strcmp(verb, "status")) {
+		dsp_hitl::print_status();
+		return 0;
 
-	else {
+	} else if (!strcmp(verb, "clear")) {
+		dsp_hitl::clear_status_counters();
+		return 0;
+
+	} else if (!strcmp(verb, "failure")) {
+		if (argc != 4) {
+			dsp_hitl::usage();
+			return 1;
+		}
+
+		const char *source = argv[myoptind + 1];
+		int duration = atoi(argv[myoptind + 2]);
+
+		if (!strcmp(source, "gps")) {
+			return dsp_hitl::process_failure(dsp_hitl::position_source::GPS, duration);
+
+		} else if (!strcmp(source, "vio")) {
+			return dsp_hitl::process_failure(dsp_hitl::position_source::VIO, duration);
+
+		} else if (!strcmp(source, "flow")) {
+			return dsp_hitl::process_failure(dsp_hitl::position_source::FLOW, duration);
+
+		} else {
+			PX4_ERR("Unknown failure source %s, duration %d", source, duration);
+			dsp_hitl::usage();
+			return 1;
+		}
+
+		return 0;
+
+	} else {
 		dsp_hitl::usage();
 		return 1;
 	}

boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp

@@ -1,991 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "ICM42688P.hpp"
-
-bool hitl_mode = false;
-
-using namespace time_literals;
-
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
-}
-
-ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
-	// SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency),
-	// I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	// _drdy_gpio(drdy_gpio)
-	SPI(config),
-	I2CSPIDriver(config),
-	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
-{
-	if (config.drdy_gpio != 0) {
-		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
-	}
-
-	if (!hitl_mode) {
-		// _px4_accel = std::make_shared<PX4Accelerometer>(get_device_id(), rotation);
-		// _px4_gyro = std::make_shared<PX4Gyroscope>(get_device_id(), rotation);
-		ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
-		// _imu_server_pub.advertise();
-
-	} else {
-		ConfigureSampleRate(0);
-	}
-}
-
-ICM42688P::~ICM42688P()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_fifo_empty_perf);
-	perf_free(_fifo_overflow_perf);
-	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_missed_perf);
-
-	// if (!hitl_mode){
-	//  		_imu_server_pub.unadvertise();
-	// }
-}
-
-int ICM42688P::init()
-{
-	int ret = SPI::init();
-
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
-		return ret;
-	}
-
-	return Reset() ? 0 : -1;
-}
-
-bool ICM42688P::Reset()
-{
-	_state = STATE::RESET;
-	DataReadyInterruptDisable();
-	ScheduleClear();
-	ScheduleNow();
-	return true;
-}
-
-void ICM42688P::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
-}
-
-void ICM42688P::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
-
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_missed_perf);
-}
-
-int ICM42688P::probe()
-{
-	for (int i = 0; i < 3; i++) {
-		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
-
-		if (whoami == WHOAMI) {
-			PX4_INFO("ICM42688P::probe successful!");
-			return PX4_OK;
-
-		} else {
-			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-
-			uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
-			int bank = reg_bank_sel >> 4;
-
-			if (bank >= 1 && bank <= 3) {
-				DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
-				// force bank selection and retry
-				SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
-			}
-		}
-	}
-
-	return PX4_ERROR;
-}
-
-void ICM42688P::RunImpl()
-{
-	PX4_INFO(">>> ICM42688P this: %p", this);
-
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// DEVICE_CONFIG: Software reset configuration
-		RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(2_ms); // to be safe wait 2 ms for soft reset to be effective
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
-		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
-		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
-
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(10_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-
-		if (Configure()) {
-
-			// Wakeup accel and gyro after configuring registers
-			ScheduleDelayed(1_ms); // add a delay here to be safe
-			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
-			ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
-
-			// if configure succeeded then start reading from FIFO
-			_state = STATE::FIFO_READ;
-
-			if (DataReadyInterruptConfigure()) {
-				_data_ready_interrupt_enabled = true;
-
-				// backup schedule as a watchdog timeout
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_ERR("ICM42688P::RunImpl interrupt configuration failed");
-
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
-
-		} else {
-
-			PX4_ERR("ICM42688P::RunImpl configuration failed");
-
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::FIFO_READ: {
-#ifndef __PX4_QURT
-			uint32_t samples = 0;
-
-			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
-
-				} else {
-					samples = _fifo_gyro_samples;
-				}
-
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			if (samples == 0) {
-				// check current FIFO count
-				const uint16_t fifo_count = FIFOReadCount();
-
-				if (fifo_count >= FIFO::SIZE) {
-					FIFOReset();
-					perf_count(_fifo_overflow_perf);
-
-				} else if (fifo_count == 0) {
-					perf_count(_fifo_empty_perf);
-
-				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
-
-					if (samples > FIFO_MAX_SAMPLES) {
-						// not technically an overflow, but more samples than we expected or can publish
-						FIFOReset();
-						perf_count(_fifo_overflow_perf);
-						samples = 0;
-					}
-				}
-			}
-
-			bool success = false;
-
-			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
-					success = true;
-
-					if (_failure_count > 0) {
-						_failure_count--;
-					}
-				}
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			// check configuration registers periodically or immediately following any failure
-			if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
-			    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
-			    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
-			   ) {
-				_last_config_check_timestamp = now;
-				_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-				_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
-				_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
-
-			} else {
-				// register check failed, force reset
-				perf_count(_bad_register_perf);
-				Reset();
-			}
-
-#endif
-		}
-
-		break;
-	}
-}
-
-void ICM42688P::ConfigureSampleRate(int sample_rate)
-{
-	if (sample_rate == 0) {
-		sample_rate = 800; // default to 800 Hz
-	}
-
-	// round down to nearest FIFO sample dt
-	const float min_interval = FIFO_SAMPLE_DT;
-	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
-
-	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
-
-	// recompute FIFO empty interval (us) with actual gyro sample limit
-	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
-
-	ConfigureFIFOWatermark(_fifo_gyro_samples);
-}
-
-void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
-{
-	// FIFO watermark threshold in number of bytes
-	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
-
-	for (auto &r : _register_bank0_cfg) {
-		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
-			// FIFO_WM[7:0]  FIFO_CONFIG2
-			r.set_bits = fifo_watermark_threshold & 0xFF;
-
-		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
-			// FIFO_WM[11:8] FIFO_CONFIG3
-			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
-		}
-	}
-}
-
-void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
-{
-	if (bank != _last_register_bank || force) {
-		// select BANK_0
-		uint8_t cmd_bank_sel[2] {};
-		cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
-		cmd_bank_sel[1] = bank;
-		transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
-
-		_last_register_bank = bank;
-	}
-}
-
-bool ICM42688P::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_bank0_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	for (const auto &reg_cfg : _register_bank1_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	for (const auto &reg_cfg : _register_bank2_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	// now check that all are configured
-	bool success = true;
-
-	for (const auto &reg_cfg : _register_bank0_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	for (const auto &reg_cfg : _register_bank1_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	for (const auto &reg_cfg : _register_bank2_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	// // 20-bits data format used
-	// //  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
-	if (!hitl_mode) {
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_gyro.set_range(math::radians(2000.f));
-		_px4_gyro.set_scale(math::radians(1.f / 131.f));
-	}
-
-	return success;
-}
-
-static bool interrupt_debug = false;
-static uint32_t interrupt_debug_count = 0;
-static const uint32_t interrupt_debug_trigger = 800;
-static hrt_abstime last_interrupt_time = 0;
-static hrt_abstime avg_interrupt_delta = 0;
-static hrt_abstime max_interrupt_delta = 0;
-static hrt_abstime min_interrupt_delta = 60 * 1000 * 1000;
-static hrt_abstime cumulative_interrupt_delta = 0;
-
-int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	hrt_abstime current_interrupt_time = hrt_absolute_time();
-
-	if (interrupt_debug) {
-		if (last_interrupt_time) {
-			hrt_abstime interrupt_delta_time = current_interrupt_time - last_interrupt_time;
-
-			if (interrupt_delta_time > max_interrupt_delta) { max_interrupt_delta = interrupt_delta_time; }
-
-			if (interrupt_delta_time < min_interrupt_delta) { min_interrupt_delta = interrupt_delta_time; }
-
-			cumulative_interrupt_delta += interrupt_delta_time;
-		}
-
-		last_interrupt_time = current_interrupt_time;
-
-		interrupt_debug_count++;
-
-		if (interrupt_debug_count == interrupt_debug_trigger) {
-			avg_interrupt_delta = cumulative_interrupt_delta / interrupt_debug_trigger;
-			PX4_INFO(">>> Max: %llu, Min: %llu, Avg: %llu", max_interrupt_delta,
-				 min_interrupt_delta, avg_interrupt_delta);
-			interrupt_debug_count = 0;
-			cumulative_interrupt_delta = 0;
-		}
-	}
-
-	static_cast<ICM42688P *>(arg)->DataReady();
-
-	return 0;
-}
-
-void ICM42688P::DataReady()
-{
-#ifndef __PX4_QURT
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
-
-#else
-	uint16_t fifo_byte_count = FIFOReadCount();
-
-	FIFORead(hrt_absolute_time(), fifo_byte_count / sizeof(FIFO::DATA));
-#endif
-}
-
-bool ICM42688P::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
-
-	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
-}
-
-bool ICM42688P::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
-
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
-}
-
-template <typename T>
-bool ICM42688P::RegisterCheck(const T &reg_cfg)
-{
-	bool success = true;
-
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
-
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
-
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
-
-	return success;
-}
-
-template <typename T>
-uint8_t ICM42688P::RegisterRead(T reg)
-{
-	uint8_t cmd[2] {};
-	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
-	SelectRegisterBank(reg);
-	transfer(cmd, cmd, sizeof(cmd));
-	return cmd[1];
-}
-
-template <typename T>
-void ICM42688P::RegisterWrite(T reg, uint8_t value)
-{
-	uint8_t cmd[2] { (uint8_t)reg, value };
-	SelectRegisterBank(reg);
-	transfer(cmd, cmd, sizeof(cmd));
-}
-
-template <typename T>
-void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
-
-	uint8_t val = (orig_val & ~clearbits) | setbits;
-
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
-}
-
-uint16_t ICM42688P::FIFOReadCount()
-{
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
-
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return 0;
-	}
-
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
-}
-
-// static uint32_t debug_decimator = 0;
-// static hrt_abstime last_sample_time = 0;
-// static bool imu_debug = true;
-
-bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t max_transfer_size = 10 * sizeof(FIFO::DATA) + 4;
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, max_transfer_size);
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
-
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
-	}
-
-	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
-	}
-
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
-
-	if (fifo_count_bytes >= FIFO::SIZE) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
-	}
-
-	const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
-
-	if (fifo_count_samples == 0) {
-		perf_count(_fifo_empty_perf);
-		return false;
-	}
-
-	// check FIFO header in every sample
-	uint16_t valid_samples = 0;
-
-	// for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
-	for (int i = 0; i < math::min(samples, (uint16_t) 10); i++) {
-		bool valid = true;
-
-		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
-		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
-
-		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
-			// FIFO sample empty if HEADER_MSG set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
-			// accel bit not set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
-			// gyro bit not set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
-			// Packet does not contain a new and valid extended 20-bit data
-			valid = false;
-
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
-			// accel ODR changed
-			valid = false;
-
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
-			// gyro ODR changed
-			valid = false;
-		}
-
-		if (valid) {
-			valid_samples++;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-			break;
-		}
-	}
-
-	// if (imu_debug) {
-	// 	debug_decimator++;
-	// 	if (debug_decimator == 801) {
-	// 		debug_decimator = 0;
-	// 		PX4_INFO("Initial: %u Next: %u Valid: %u Delta: %llu", samples, fifo_count_samples, valid_samples, timestamp_sample - last_sample_time);
-	// 	}
-	// 	last_sample_time = timestamp_sample;
-	// }
-
-	if (valid_samples > 0) {
-		if (ProcessTemperature(buffer.f, valid_samples)) {
-			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-			ProcessIMU(timestamp_sample, buffer.f, valid_samples);
-			return true;
-		}
-	}
-
-	return false;
-}
-
-void ICM42688P::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
-
-	// SIGNAL_PATH_RESET: FIFO flush
-	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
-
-	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
-}
-
-static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
-{
-	// 0xXXXAABBC
-	uint32_t high   = ((a << 12) & 0x000FF000);
-	uint32_t low    = ((b << 4)  & 0x00000FF0);
-	uint32_t lowest = (c         & 0x0000000F);
-
-	uint32_t x = high | low | lowest;
-
-	if (a & Bit7) {
-		// sign extend
-		x |= 0xFFF00000u;
-	}
-
-	return static_cast<int32_t>(x);
-}
-
-void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
-	float gyro_x = 0.0,  gyro_y = 0.0,  gyro_z = 0.0;
-
-	for (int i = 0; i < samples; i++) {
-		_imu_server_decimator++;
-
-		if (_imu_server_decimator == 8) {
-			_imu_server_decimator = 0;
-			// 20 bit hires mode
-
-			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-			// Accel data is 18 bit
-			int32_t temp_accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
-								fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
-			int32_t temp_accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
-								fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
-			int32_t temp_accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
-								fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
-
-			// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
-			int32_t temp_gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0,
-							       fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
-			int32_t temp_gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0,
-							       fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
-			int32_t temp_gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0,
-							       fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
-
-			// accel samples invalid if -524288
-			if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) {
-				// shift accel by 2 (2 least significant bits are always 0)
-				accel_x = (float) temp_accel_x / 4.f;
-				accel_y = (float) temp_accel_y / 4.f;
-				accel_z = (float) temp_accel_z / 4.f;
-
-				// shift gyro by 1 (least significant bit is always 0)
-				gyro_x = (float) temp_gyro_x / 2.f;
-				gyro_y = (float) temp_gyro_y / 2.f;
-				gyro_z = (float) temp_gyro_z / 2.f;
-
-				// correct frame for publication
-				// sensor's frame is +x forward, +y left, +z up
-				// flip y & z to publish right handed with z down (x forward, y right, z down)
-				accel_y = -accel_y;
-				accel_z = -accel_z;
-				gyro_y  = -gyro_y;
-				gyro_z  = -gyro_z;
-
-				// Scale everything appropriately
-				float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f);
-				accel_x *= accel_scale_factor;
-				accel_y *= accel_scale_factor;
-				accel_z *= accel_scale_factor;
-
-				float gyro_scale_factor = math::radians(1.f / 131.f);
-				gyro_x *= gyro_scale_factor;
-				gyro_y *= gyro_scale_factor;
-				gyro_z *= gyro_scale_factor;
-
-				// Store the data in our array
-				_imu_server_data.accel_x[_imu_server_index] = accel_x;
-				_imu_server_data.accel_y[_imu_server_index] = accel_y;
-				_imu_server_data.accel_z[_imu_server_index] = accel_z;
-				_imu_server_data.gyro_x[_imu_server_index]  = gyro_x;
-				_imu_server_data.gyro_y[_imu_server_index]  = gyro_y;
-				_imu_server_data.gyro_z[_imu_server_index]  = gyro_z;
-				_imu_server_data.ts[_imu_server_index]      = timestamp_sample - (125 * (samples - 1 - i));
-				_imu_server_index++;
-
-				// If array is full, publish the data
-				if (_imu_server_index == 10) {
-					_imu_server_index = 0;
-					_imu_server_data.timestamp = hrt_absolute_time();
-					_imu_server_data.temperature = 0; // Not used right now
-					_imu_server_pub.publish(_imu_server_data);
-				}
-			}
-		}
-	}
-}
-
-void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
-
-	// 18-bits of accelerometer data
-	bool scale_20bit = false;
-
-	// first pass
-	for (int i = 0; i < samples; i++) {
-		// 20 bit hires mode
-		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-		// Accel data is 18 bit ()
-		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
-						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
-		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
-						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
-		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
-						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
-
-		// sample invalid if -524288
-		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
-			// check if any values are going to exceed int16 limits
-			static constexpr int16_t max_accel = INT16_MAX;
-			static constexpr int16_t min_accel = INT16_MIN;
-
-			if (accel_x >= max_accel || accel_x <= min_accel) {
-				scale_20bit = true;
-			}
-
-			if (accel_y >= max_accel || accel_y <= min_accel) {
-				scale_20bit = true;
-			}
-
-			if (accel_z >= max_accel || accel_z <= min_accel) {
-				scale_20bit = true;
-			}
-
-			// shift by 2 (2 least significant bits are always 0)
-			accel.x[accel.samples] = accel_x / 4;
-			accel.y[accel.samples] = accel_y / 4;
-			accel.z[accel.samples] = accel_z / 4;
-			accel.samples++;
-		}
-	}
-
-	if (!scale_20bit) {
-		// if highres enabled accel data is always 8192 LSB/g
-		if (!hitl_mode) {
-			_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		}
-
-	} else {
-		// 20 bit data scaled to 16 bit (2^4)
-		for (int i = 0; i < samples; i++) {
-			// 20 bit hires mode
-			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-			// Accel data is 18 bit ()
-			int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
-			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
-			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
-
-			accel.x[i] = accel_x;
-			accel.y[i] = accel_y;
-			accel.z[i] = accel_z;
-		}
-
-		if (!hitl_mode) {
-			_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		}
-	}
-
-	// correct frame for publication
-	for (int i = 0; i < accel.samples; i++) {
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[i] = accel.x[i];
-		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
-		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
-	}
-
-	if (!hitl_mode) {
-		_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-					   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-	}
-
-	if (accel.samples > 0) {
-		if (!hitl_mode) {
-			_px4_accel.updateFIFO(accel);
-		}
-	}
-}
-
-void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = 0;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	// 20-bits of gyroscope data
-	bool scale_20bit = false;
-
-	// first pass
-	for (int i = 0; i < samples; i++) {
-		// 20 bit hires mode
-		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
-		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
-		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
-		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
-
-		// check if any values are going to exceed int16 limits
-		static constexpr int16_t max_gyro = INT16_MAX;
-		static constexpr int16_t min_gyro = INT16_MIN;
-
-		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		gyro.x[gyro.samples] = gyro_x / 2;
-		gyro.y[gyro.samples] = gyro_y / 2;
-		gyro.z[gyro.samples] = gyro_z / 2;
-		gyro.samples++;
-	}
-
-	if (!scale_20bit) {
-		// if highres enabled gyro data is always 131 LSB/dps
-		if (!hitl_mode) {
-			_px4_gyro.set_scale(math::radians(1.f / 131.f));
-		}
-
-	} else {
-		// 20 bit data scaled to 16 bit (2^4)
-		for (int i = 0; i < samples; i++) {
-			gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
-			gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
-			gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
-		}
-
-		if (!hitl_mode) {
-			_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
-		}
-	}
-
-	// correct frame for publication
-	for (int i = 0; i < gyro.samples; i++) {
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro.x[i];
-		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
-		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
-	}
-
-	if (!hitl_mode) {
-		_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-					  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-	}
-
-	if (gyro.samples > 0) {
-		if (!hitl_mode) {
-			_px4_gyro.updateFIFO(gyro);
-		}
-	}
-}
-
-bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
-{
-	int16_t temperature[FIFO_MAX_SAMPLES];
-	float temperature_sum{0};
-
-	int valid_samples = 0;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
-
-		// sample invalid if -32768
-		if (t != -32768) {
-			temperature_sum += t;
-			temperature[valid_samples] = t;
-			valid_samples++;
-		}
-	}
-
-	if (valid_samples > 0) {
-		const float temperature_avg = temperature_sum / valid_samples;
-
-		for (int i = 0; i < valid_samples; i++) {
-			// temperature changing wildly is an indication of a transfer error
-			if (fabsf(temperature[i] - temperature_avg) > 1000) {
-				perf_count(_bad_transfer_perf);
-				return false;
-			}
-		}
-
-		// use average temperature reading
-		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
-
-		if (PX4_ISFINITE(TEMP_degC)) {
-			if (!hitl_mode) {
-				_px4_accel.set_temperature(TEMP_degC);
-				_px4_gyro.set_temperature(TEMP_degC);
-				return true;
-			}
-
-		} else {
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	return false;
-}

boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp

@@ -1,235 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 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 ICM42688P.hpp
- *
- * Driver for the Invensense ICM42688P connected via SPI.
- *
- */
-
-#pragma once
-
-#include "InvenSense_ICM42688P_registers.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/device/spi.h>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-#include <lib/geo/geo.h>
-#include <lib/perf/perf_counter.h>
-#include <px4_platform_common/atomic.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-#include <uORB/topics/imu_server.h>
-#include <uORB/topics/sensor_accel_fifo.h>
-#include <uORB/topics/sensor_gyro_fifo.h>
-#include <memory>
-
-using namespace InvenSense_ICM42688P;
-
-extern bool hitl_mode;
-
-class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
-{
-public:
-	// ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
-	// 	  spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
-	ICM42688P(const I2CSPIDriverConfig &config);
-	~ICM42688P() override;
-
-	// static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-	// 				     int runtime_instance);
-	static void print_usage();
-
-	void RunImpl();
-
-	int init() override;
-	void print_status() override;
-
-private:
-	void exit_and_cleanup() override;
-
-	// Sensor Configuration
-	static constexpr float IMU_ODR{8000.f}; // 8kHz accel & gyro ODR configured
-	static constexpr float FIFO_SAMPLE_DT{1e6f / IMU_ODR};
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
-
-	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	// static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
-	static constexpr uint32_t FIFO_MAX_SAMPLES{10};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
-		uint8_t INT_STATUS{0};
-		uint8_t FIFO_COUNTH{0};
-		uint8_t FIFO_COUNTL{0};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)),
-		      "Invalid FIFOTransferBuffer size");
-
-	struct register_bank0_config_t {
-		Register::BANK_0 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	struct register_bank1_config_t {
-		Register::BANK_1 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	struct register_bank2_config_t {
-		Register::BANK_2 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Reset();
-
-	bool Configure();
-	void ConfigureSampleRate(int sample_rate);
-	void ConfigureFIFOWatermark(uint8_t samples);
-
-	void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
-	void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
-	void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
-	void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
-
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
-
-	template <typename T> bool RegisterCheck(const T &reg_cfg);
-	template <typename T> uint8_t RegisterRead(T reg);
-	template <typename T> void RegisterWrite(T reg, uint8_t value);
-	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
-	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
-	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
-
-	uint16_t FIFOReadCount();
-	bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
-	void FIFOReset();
-
-	void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
-
-	const spi_drdy_gpio_t _drdy_gpio;
-
-	// std::shared_ptr<PX4Accelerometer> _px4_accel;
-	// std::shared_ptr<PX4Gyroscope> _px4_gyro;
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
-	perf_counter_t _drdy_missed_perf{nullptr};
-
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
-	hrt_abstime _temperature_update_timestamp{0};
-	int _failure_count{0};
-
-	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
-
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	bool _data_ready_interrupt_enabled{false};
-
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		FIFO_READ,
-	} _state{STATE::RESET};
-
-	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
-
-	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{12};
-	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
-		{ Register::BANK_0::FIFO_CONFIG,          FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
-		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
-		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
-		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
-		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
-		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
-		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
-		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
-		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
-		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
-		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
-	};
-
-	uint8_t _checked_register_bank1{0};
-	static constexpr uint8_t size_register_bank1_cfg{4};
-	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
-		{ Register::BANK_1::GYRO_CONFIG_STATIC3,  GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_CLEAR},
-		{ Register::BANK_1::GYRO_CONFIG_STATIC4,  GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_CLEAR},
-		{ Register::BANK_1::GYRO_CONFIG_STATIC5,  GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_CLEAR},
-	};
-
-	uint8_t _checked_register_bank2{0};
-	static constexpr uint8_t size_register_bank2_cfg{3};
-	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_CLEAR },
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_CLEAR },
-	};
-
-	uint32_t _temperature_samples{0};
-
-	// Support for the IMU server
-	uint32_t _imu_server_index{0};
-	uint32_t _imu_server_decimator{0};
-	imu_server_s _imu_server_data;
-	uORB::Publication<imu_server_s> _imu_server_pub{ORB_ID(imu_server)};
-
-};

boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp

@@ -1,430 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 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 InvenSense_ICM42688P_registers.hpp
- *
- * Invensense ICM-42688-P registers.
- *
- */
-
-#pragma once
-
-#include <cstdint>
-
-namespace InvenSense_ICM42688P
-{
-// TODO: move to a central header
-static constexpr uint8_t Bit0 = (1 << 0);
-static constexpr uint8_t Bit1 = (1 << 1);
-static constexpr uint8_t Bit2 = (1 << 2);
-static constexpr uint8_t Bit3 = (1 << 3);
-static constexpr uint8_t Bit4 = (1 << 4);
-static constexpr uint8_t Bit5 = (1 << 5);
-static constexpr uint8_t Bit6 = (1 << 6);
-static constexpr uint8_t Bit7 = (1 << 7);
-
-static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
-static constexpr uint8_t DIR_READ = 0x80;
-
-static constexpr uint8_t WHOAMI = 0x47;
-
-static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
-static constexpr float TEMPERATURE_OFFSET = 25.f; // C
-
-namespace Register
-{
-
-enum class BANK_0 : uint8_t {
-	DEVICE_CONFIG      = 0x11,
-
-	INT_CONFIG         = 0x14,
-
-	FIFO_CONFIG        = 0x16,
-
-	TEMP_DATA1         = 0x1D,
-	TEMP_DATA0         = 0x1E,
-
-	INT_STATUS         = 0x2D,
-	FIFO_COUNTH        = 0x2E,
-	FIFO_COUNTL        = 0x2F,
-	FIFO_DATA          = 0x30,
-
-	SIGNAL_PATH_RESET  = 0x4B,
-	INTF_CONFIG0       = 0x4C,
-	INTF_CONFIG1       = 0x4D,
-	PWR_MGMT0          = 0x4E,
-	GYRO_CONFIG0       = 0x4F,
-	ACCEL_CONFIG0      = 0x50,
-	GYRO_CONFIG1       = 0x51,
-	GYRO_ACCEL_CONFIG0 = 0x52,
-	ACCEL_CONFIG1      = 0x53,
-
-	FIFO_CONFIG1       = 0x5F,
-	FIFO_CONFIG2       = 0x60,
-	FIFO_CONFIG3       = 0x61,
-
-	INT_CONFIG0        = 0x63,
-
-	INT_SOURCE0        = 0x65,
-
-	SELF_TEST_CONFIG   = 0x70,
-
-	WHO_AM_I           = 0x75,
-	REG_BANK_SEL       = 0x76,
-};
-
-enum class BANK_1 : uint8_t {
-	GYRO_CONFIG_STATIC2 = 0x0B,
-	GYRO_CONFIG_STATIC3 = 0x0C,
-	GYRO_CONFIG_STATIC4 = 0x0D,
-	GYRO_CONFIG_STATIC5 = 0x0E,
-	INTF_CONFIG5        = 0x7B,
-};
-enum class BANK_2 : uint8_t {
-	ACCEL_CONFIG_STATIC2 = 0x03,
-	ACCEL_CONFIG_STATIC3 = 0x04,
-	ACCEL_CONFIG_STATIC4 = 0x05,
-};
-
-};
-
-//---------------- BANK0 Register bits
-
-// DEVICE_CONFIG
-enum DEVICE_CONFIG_BIT : uint8_t {
-	SOFT_RESET_CONFIG = Bit0, //
-};
-
-// INT_CONFIG
-enum INT_CONFIG_BIT : uint8_t {
-	INT1_MODE           = Bit2,
-	INT1_DRIVE_CIRCUIT  = Bit1,
-	INT1_POLARITY       = Bit0,
-};
-
-// FIFO_CONFIG
-enum FIFO_CONFIG_BIT : uint8_t {
-	// 7:6 FIFO_MODE
-	FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
-};
-
-// INT_STATUS
-enum INT_STATUS_BIT : uint8_t {
-	RESET_DONE_INT = Bit4,
-	DATA_RDY_INT   = Bit3,
-	FIFO_THS_INT   = Bit2,
-	FIFO_FULL_INT  = Bit1,
-};
-
-// SIGNAL_PATH_RESET
-enum SIGNAL_PATH_RESET_BIT : uint8_t {
-	ABORT_AND_RESET = Bit3,
-	FIFO_FLUSH      = Bit1,
-};
-
-// PWR_MGMT0
-enum PWR_MGMT0_BIT : uint8_t {
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
-};
-
-// GYRO_CONFIG0
-enum GYRO_CONFIG0_BIT : uint8_t {
-	// 7:5 GYRO_FS_SEL
-	GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
-	GYRO_FS_SEL_1000_DPS = Bit5,
-	GYRO_FS_SEL_500_DPS  = Bit6,
-	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,
-	GYRO_FS_SEL_125_DPS  = Bit7,
-
-
-	// 3:0 GYRO_ODR
-	//  0001: 32kHz
-	GYRO_ODR_32KHZ_SET   = Bit0,
-	GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0010: 16kHz
-	GYRO_ODR_16KHZ_SET   = Bit1,
-	GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0011: 8kHz
-	GYRO_ODR_8KHZ_SET    = Bit1 | Bit0,
-	GYRO_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
-	//  0110: 1kHz (default)
-	GYRO_ODR_1KHZ_SET    = Bit2 | Bit1,
-	GYRO_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
-};
-
-// ACCEL_CONFIG0
-enum ACCEL_CONFIG0_BIT : uint8_t {
-	// 7:5 ACCEL_FS_SEL
-	ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
-	ACCEL_FS_SEL_8G  = Bit5,
-	ACCEL_FS_SEL_4G  = Bit6,
-	ACCEL_FS_SEL_2G  = Bit6 | Bit5,
-
-
-	// 3:0 ACCEL_ODR
-	//  0001: 32kHz
-	ACCEL_ODR_32KHZ_SET   = Bit0,
-	ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0010: 16kHz
-	ACCEL_ODR_16KHZ_SET   = Bit1,
-	ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0011: 8kHz
-	ACCEL_ODR_8KHZ_SET    = Bit1 | Bit0,
-	ACCEL_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
-	//  0110: 1kHz (default)
-	ACCEL_ODR_1KHZ_SET    = Bit2 | Bit1,
-	ACCEL_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
-};
-
-// GYRO_CONFIG1
-enum GYRO_CONFIG1_BIT : uint8_t {
-	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
-};
-
-// GYRO_ACCEL_CONFIG0
-enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
-	// 7:4 ACCEL_UI_FILT_BW
-	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
-
-	// 3:0 GYRO_UI_FILT_BW
-	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
-};
-
-// ACCEL_CONFIG1
-enum ACCEL_CONFIG1_BIT : uint8_t {
-	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
-};
-
-// FIFO_CONFIG1
-enum FIFO_CONFIG1_BIT : uint8_t {
-	FIFO_RESUME_PARTIAL_RD = Bit6,
-	FIFO_WM_GT_TH          = Bit5,
-	FIFO_HIRES_EN          = Bit4,
-	FIFO_TEMP_EN           = Bit2,
-	FIFO_GYRO_EN           = Bit1,
-	FIFO_ACCEL_EN          = Bit0,
-};
-
-// INT_CONFIG0
-enum INT_CONFIG0_BIT : uint8_t {
-	// 3:2 FIFO_THS_INT_CLEAR
-	CLEAR_ON_FIFO_READ = Bit3,
-};
-
-// INT_SOURCE0
-enum INT_SOURCE0_BIT : uint8_t {
-	UI_FSYNC_INT1_EN   = Bit6,
-	PLL_RDY_INT1_EN    = Bit5,
-	RESET_DONE_INT1_EN = Bit4,
-	UI_DRDY_INT1_EN    = Bit3,
-	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
-	FIFO_FULL_INT1_EN  = Bit1,
-	UI_AGC_RDY_INT1_EN = Bit0,
-};
-
-// REG_BANK_SEL
-enum REG_BANK_SEL_BIT : uint8_t {
-	USER_BANK_0 = 0,           // 0: Select USER BANK 0.
-	USER_BANK_1 = Bit0,        // 1: Select USER BANK 1.
-	USER_BANK_2 = Bit1,        // 2: Select USER BANK 2.
-	USER_BANK_3 = Bit1 | Bit0, // 3: Select USER BANK 3.
-};
-
-
-//---------------- BANK1 Register bits
-
-// GYRO_CONFIG_STATIC2
-enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
-	GYRO_AAF_DIS = Bit1,
-	GYRO_NF_DIS  = Bit0,
-};
-
-// GYRO_CONFIG_STATIC3
-enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
-
-	// 585 Hz
-	GYRO_AAF_DELT_SET = Bit3 | Bit2 | Bit0, //13
-	GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit1,
-
-	// 213 Hz
-	// GYRO_AAF_DELT_SET = Bit2 | Bit0, //5
-	// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit1,
-
-	// 126 Hz
-	//GYRO_AAF_DELT_SET = Bit1 | Bit0, //3
-	//GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2,
-
-	// 42 Hz
-	// GYRO_AAF_DELT_SET = Bit0, //1
-	// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
-
-};
-
-// GYRO_CONFIG_STATIC4
-enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
-
-	// 585 Hz
-	GYRO_AAF_DELTSQR_LOW_SET = Bit7 | Bit5 | Bit3 | Bit1, //170
-	GYRO_AAF_DELTSQR_LOW_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
-
-	// 213 Hz
-	// GYRO_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
-	// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
-
-	// 126 Hz
-	//GYRO_AAF_DELTSQR_LOW_SET = Bit3 | Bit0, //9
-	//GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1,
-
-	// 42 Hz
-	// GYRO_AAF_DELTSQR_LOW_SET = Bit0, //1
-	// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
-};
-
-// GYRO_CONFIG_STATIC5
-enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
-
-	// 585 Hz
-	GYRO_AAF_DELTSQR_HIGH_SET = 0,
-	GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
-	GYRO_AAF_BITSHIFT_SET = Bit7, // 8 << 4
-	GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit5 | Bit4,
-
-	// 213 Hz
-	// GYRO_AAF_DELTSQR_HIGH_SET = 0,
-	// GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
-	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
-	// GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
-
-	// 126 Hz
-	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6, //12
-	// GYRO_AAF_BITSHIFT_CLEAR = Bit5 | Bit4,
-
-	// 42 Hz
-	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
-	// GYRO_AAF_BITSHIFT_CLEAR = 0,
-
-
-};
-
-
-//---------------- BANK2 Register bits
-
-// ACCEL_CONFIG_STATIC2
-enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
-	ACCEL_AAF_DIS  = Bit0,
-	ACCEL_AAF_DELT = Bit3 | Bit1,
-
-	// 213 Hz
-	ACCEL_AAF_DELT_SET = Bit3 | Bit1, //5
-	ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit2,
-
-	// 42 Hz
-	// ACCEL_AAF_DELT_SET = Bit1, //1
-	// ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit3 | Bit2,
-};
-
-// ACCEL_CONFIG_STATIC3
-enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
-	ACCEL_AAF_DELTSQR_LOW = Bit4 | Bit3 | Bit0,
-	// 213 Hz
-	ACCEL_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
-	ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
-
-	// 42 Hz
-	// ACCEL_AAF_DELTSQR_LOW_SET = Bit0, //1
-	// ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
-
-};
-
-// ACCEL_CONFIG_STATIC4
-enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
-	ACCEL_AAF_BITSHIFT     = Bit7 | Bit5,
-	ACCEL_AAF_DELTSQR_HIGH = 0,
-	// 213 Hz
-	ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
-	ACCEL_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
-
-	// 42 Hz
-	// ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
-	// ACCEL_AAF_BITSHIFT_CLEAR = 0,
-
-	ACCEL_AAF_DELTSQR_HIGH_SET = 0,
-	ACCEL_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
-};
-
-
-namespace FIFO
-{
-static constexpr size_t SIZE = 2048;
-
-// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
-
-// Packet 4
-struct DATA {
-	uint8_t FIFO_Header;
-	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
-	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
-	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
-	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
-	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
-	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
-	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
-	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
-	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
-	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
-	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
-	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
-	uint8_t TEMP_DATA1;    // Temperature[15:8]
-	uint8_t TEMP_DATA0;    // Temperature[7:0]
-	uint8_t TimeStamp_h;   // TimeStamp[15:8]
-	uint8_t TimeStamp_l;   // TimeStamp[7:0]
-	uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
-	uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
-	uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
-};
-
-// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
-enum FIFO_HEADER_BIT : uint8_t {
-	HEADER_MSG             = Bit7, // 1: FIFO is empty
-	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
-	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
-	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
-	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
-	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
-	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
-};
-
-}
-} // namespace InvenSense_ICM42688P

boards/modalai/voxl2-slpi/src/drivers/icm42688p/icm42688p_main.cpp

@@ -1,116 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "ICM42688P.hpp"
-
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
-#include <string>
-
-void ICM42688P::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-// I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-// 		int runtime_instance)
-// {
-// 	ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
-// 					    cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
-//
-// 	if (!instance) {
-// 		PX4_ERR("alloc failed");
-// 		return nullptr;
-// 	}
-//
-// 	if (OK != instance->init()) {
-// 		delete instance;
-// 		return nullptr;
-// 	}
-//
-// 	return instance;
-// }
-
-extern "C" int icm42688p_main(int argc, char *argv[])
-{
-
-	for (int i = 0; i <= argc - 1; i++) {
-		if (std::string(argv[i]) == "-h") {
-			argv[i] = 0;
-			hitl_mode = true;
-			break;
-		}
-	}
-
-	int ch;
-	using ThisDriver = ICM42688P;
-	BusCLIArguments cli{false, true};
-	cli.default_spi_frequency = SPI_SPEED;
-
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
-
-	const char *verb = cli.optArg();
-
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42688P);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}

boards/modalai/voxl2/cmake/init.cmake

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

boards/modalai/voxl2/cmake/link_libraries.cmake

@@ -1,7 +1,7 @@
 
 # Link against the public stub version of the proprietary fc sensor library
 target_link_libraries(px4 PRIVATE
-		${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
+		${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
 		px4_layer
 		${module_libraries}
 )

boards/modalai/voxl2/default.px4board

@@ -11,7 +11,6 @@ CONFIG_DRIVERS_VOXL2_IO=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MUORB_APPS=y
@@ -26,3 +25,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_ORB_COMMUNICATOR=y
+CONFIG_PARAM_PRIMARY=y

boards/modalai/voxl2/scripts/build-deps.sh

@@ -1,10 +1,9 @@
 #!/bin/bash
 
-cd src/modules/muorb/apps/libfc-sensor-api
+cd boards/modalai/voxl2/libfc-sensor-api
 rm -fR build
 mkdir build
 cd build
 CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
 make
-cd ../../../../../..
-
+cd ../../../../..

boards/modalai/voxl2/target/voxl-px4-hitl-start

@@ -76,12 +76,13 @@ qshell flight_mode_manager start
 # Start all of the processing modules on the applications processor
 dataman start
 navigator start
-load_mon start
 
 # Start microdds_client for ros2 offboard messages from agent over localhost
 microdds_client start -t udp -h 127.0.0.1 -p 8888
 
 qshell pwm_out_sim start -m hil
+# g = gps, m = mag, o = odometry (vio), h = distance sensor, f = optic flow
+# qshell dsp_hitl start -g -m -o -h -f
 qshell dsp_hitl start -g -m
 
 # start the onboard fast link to connect to voxl-mavlink-server

boards/modalai/voxl2/target/voxl-px4-start

@@ -207,7 +207,6 @@ qshell flight_mode_manager start
 # Start all of the processing modules on the applications processor
 dataman start
 navigator start
-load_mon start
 
 # This bridge allows raw data packets to be sent over UART to the ESC
 voxl2_io_bridge start

boards/mro/ctrl-zero-classic/default.px4board

@@ -1,9 +1,11 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
-CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
-CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
-CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
+CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
@@ -25,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -46,7 +49,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -96,4 +98,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y

boards/mro/ctrl-zero-classic/init/rc.board_defaults

@@ -6,4 +6,4 @@
 param set-default BAT1_V_DIV 10.1
 param set-default BAT1_A_PER_V 17
 
-safety_button start
+param set-default TEL_FRSKY_CONFIG 103

boards/mro/ctrl-zero-classic/init/rc.board_sensors

@@ -16,3 +16,5 @@ icm20948 -s -b 1 -R 8 -M start
 
 # Interal DPS310 (barometer)
 dps310 -s -b 2 start
+
+safety_button start

boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig

@@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
-CONFIG_ARCH_CHIP_STM32H743II=y
+CONFIG_ARCH_CHIP_STM32H743ZI=y
 CONFIG_ARCH_CHIP_STM32H7=y
 CONFIG_ARCH_INTERRUPTSTACK=768
 CONFIG_ARMV7M_BASEPRI_WAR=y

boards/mro/ctrl-zero-classic/nuttx-config/include/board.h

@@ -222,6 +222,9 @@
 
 
 /* UART/USART */
+#define GPIO_USART1_TX	 GPIO_USART1_TX_3      /* PB6  */
+#define GPIO_USART1_RX	 GPIO_USART1_RX_3      /* PB7  */
+
 #define GPIO_USART2_TX   GPIO_USART2_TX_2      /* PD5  */
 #define GPIO_USART2_RX   GPIO_USART2_RX_2      /* PD6  */
 #define GPIO_USART2_CTS  GPIO_USART2_CTS_NSS_2 /* PD3  */
@@ -235,9 +238,6 @@
 #define GPIO_UART4_TX    GPIO_UART4_TX_2       /* PA0  */
 #define GPIO_UART4_RX    GPIO_UART4_RX_2       /* PA1  */
 
-#define GPIO_USART6_TX   0                     /* USART6 is RX-only */
-#define GPIO_USART6_RX   GPIO_USART6_RX_1      /* PC7  */
-
 #define GPIO_UART7_TX    GPIO_UART7_TX_3       /* PE8  */
 #define GPIO_UART7_RX    GPIO_UART7_RX_3       /* PE7  */
 
@@ -268,13 +268,14 @@
 #define GPIO_SPI5_MISO   GPIO_SPI5_MISO_1               /* PF8  */
 #define GPIO_SPI5_MOSI   GPIO_SPI5_MOSI_2               /* PF9  */
 
+#define GPIO_SPI6_SCK    ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13  */
+#define GPIO_SPI6_MISO   GPIO_SPI6_MISO_1               /* PG12  */
+#define GPIO_SPI6_MOSI   GPIO_SPI6_MOSI_1               /* PG14  */
+
 
 /* I2C */
 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2       /* PB9  */
 
-#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2       /* PH7 */
-#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2       /* PH8 */
-
-#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4       /* PB6 */
-#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4       /* PB7 */
+#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2       /* PF14 */
+#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2       /* PF15 */

boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig

@@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
-CONFIG_ARCH_CHIP_STM32H743II=y
+CONFIG_ARCH_CHIP_STM32H743ZI=y
 CONFIG_ARCH_CHIP_STM32H7=y
 CONFIG_ARCH_INTERRUPTSTACK=768
 CONFIG_ARCH_STACKDUMP=y
@@ -192,7 +192,6 @@ CONFIG_STM32H7_DMA2=y
 CONFIG_STM32H7_DMACAPABLE=y
 CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
 CONFIG_STM32H7_I2C1=y
-CONFIG_STM32H7_I2C3=y
 CONFIG_STM32H7_I2C4=y
 CONFIG_STM32H7_I2C_DYNTIMEO=y
 CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
@@ -212,17 +211,20 @@ CONFIG_STM32H7_SPI2=y
 CONFIG_STM32H7_SPI5=y
 CONFIG_STM32H7_SPI5_DMA=y
 CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI6=y
+CONFIG_STM32H7_TIM15=y
+CONFIG_STM32H7_TIM16=y
 CONFIG_STM32H7_TIM1=y
 CONFIG_STM32H7_TIM2=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
 CONFIG_STM32H7_TIM8=y
+CONFIG_STM32H7_USART1=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
 CONFIG_STM32H7_UART4=y
 CONFIG_STM32H7_UART7=y
 CONFIG_STM32H7_UART8=y
-CONFIG_STM32H7_USART2=y
-CONFIG_STM32H7_USART3=y
-CONFIG_STM32H7_USART6=y
 CONFIG_STM32H7_USART_BREAKS=y
 CONFIG_STM32H7_USART_INVERT=y
 CONFIG_STM32H7_USART_SINGLEWIRE=y
@@ -252,9 +254,6 @@ CONFIG_USART3_IFLOWCONTROL=y
 CONFIG_USART3_OFLOWCONTROL=y
 CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_TXBUFSIZE=3000
-CONFIG_USART6_BAUD=57600
-CONFIG_USART6_RXBUFSIZE=600
-CONFIG_USART6_TXBUFSIZE=1500
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500

boards/mro/ctrl-zero-classic/src/board_config.h

@@ -45,95 +45,111 @@
 #include <stm32_gpio.h>
 
 /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
-#define GPIO_nLED_RED   /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
-#define GPIO_nLED_GREEN /* PB1  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_nLED_RED   /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_nLED_GREEN /* PB5  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
 #define GPIO_nLED_BLUE  /* PB3  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
 
+#define GPIO_LED_SAFETY /* PE6  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
+
 #define BOARD_HAS_CONTROL_STATUS_LEDS      1
 #define BOARD_OVERLOAD_LED     LED_RED
 #define BOARD_ARMED_STATE_LED  LED_BLUE
 
 /* ADC channels */
 #define PX4_ADC_GPIO  \
-	/* PA2 */  GPIO_ADC12_INP14,  \
+	/* PC4 */  GPIO_ADC12_INP4,   \
 	/* PA3 */  GPIO_ADC12_INP15,  \
 	/* PA4 */  GPIO_ADC12_INP18,  \
-	/* PC1 */  GPIO_ADC123_INP11
+	/* PC0 */  GPIO_ADC123_INP10, \
+	/* PC5 */  GPIO_ADC12_INP8,   \
+	/* PB0 */  GPIO_ADC12_INP9,   \
+	/* PB1 */  GPIO_ADC12_INP5
 
 /* Define Channel numbers must match above GPIO pins */
-#define ADC_BATTERY_VOLTAGE_CHANNEL       14 /* PA2 BATT_VOLT_SENS */
+#define ADC_BATTERY_VOLTAGE_CHANNEL       4 /* PC4 BATT_VOLT_SENS */
 #define ADC_BATTERY_CURRENT_CHANNEL       15 /* PA3 BATT_CURRENT_SENS */
 #define ADC_SCALED_V5_CHANNEL             18 /* PA4 VDD_5V_SENS */
-#define ADC_RC_RSSI_CHANNEL               11 /* PC1 */
+#define ADC_RC_RSSI_CHANNEL               10 /* PC0 */
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL       8 /* PC5 */
+#define ADC_AUX1_VOLTAGE_CHANNEL       	   9 /* PB0 */
+#define ADC_AUX2_VOLTAGE_CHANNEL       	   5 /* PB1 */
 
 #define ADC_CHANNELS \
 	((1 << ADC_BATTERY_VOLTAGE_CHANNEL)       | \
 	 (1 << ADC_BATTERY_CURRENT_CHANNEL)       | \
 	 (1 << ADC_SCALED_V5_CHANNEL)             | \
-	 (1 << ADC_RC_RSSI_CHANNEL))
+	 (1 << ADC_RC_RSSI_CHANNEL)		  | \
+	 (1 << ADC_AIRSPEED_VOLTAGE_CHANNEL)	  | \
+	 (1 << ADC_AUX1_VOLTAGE_CHANNEL)	  | \
+	 (1 << ADC_AUX2_VOLTAGE_CHANNEL))
 
 /* HW has to large of R termination on ADC todo:change when HW value is chosen */
 #define BOARD_ADC_OPEN_CIRCUIT_V     (5.6f)
 
 /* CAN Silence: Silent mode control */
-#define GPIO_CAN1_SILENT_S0  /* PF5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
-#define GPIO_CAN2_SILENT_S0  /* PF5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
+#define GPIO_CAN1_SILENT_S0  /* PF11  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11)
+#define GPIO_CAN2_SILENT_S0  /* PF12  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)
 
 /* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS  12
 
 /* Power supply control and monitoring GPIOs */
-#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_nPOWER_IN_A                /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
 
 #define GPIO_VDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
 #define BOARD_NUMBER_BRICKS             1
 
 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
 
+#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+
 /* Define True logic Power Control in arch agnostic form */
 #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
 #define READ_VDD_3V3_SPEKTRUM_POWER_EN()   (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
 
 /* Tone alarm output */
-#define TONE_ALARM_TIMER        2  /* timer 2 */
-#define TONE_ALARM_CHANNEL      1  /* PA15 TIM2_CH1 */
+#define TONE_ALARM_TIMER        16  /* timer 16 */
+#define TONE_ALARM_CHANNEL      1  /* PF6 TIM16_CH1 */
 
-#define GPIO_BUZZER_1           /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_BUZZER_1           /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6)
 
 #define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
-#define GPIO_TONE_ALARM         GPIO_TIM2_CH1OUT_2
+#define GPIO_TONE_ALARM         GPIO_TIM16_CH1OUT_2
 
 /* USB OTG FS */
 #define GPIO_OTGFS_VBUS         /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
 
 /* High-resolution timer */
 #define HRT_TIMER               3  /* use timer3 for the HRT */
-#define HRT_TIMER_CHANNEL       1 /* use capture/compare channel 1 */
+#define HRT_TIMER_CHANNEL       2 /* use capture/compare channel 1 */
 
-#define HRT_PPM_CHANNEL         /* T3C3 */  3  /* use capture/compare channel 3 */
-#define GPIO_PPM_IN             /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
+#define HRT_PPM_CHANNEL         /* T3C2 */  2  /* use capture/compare channel 2 */
+#define GPIO_PPM_IN             /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
 
 /* RC Serial port */
-#define RC_SERIAL_PORT          "/dev/ttyS3"
+#define RC_SERIAL_PORT          "/dev/ttyS5"
 
-#define GPIO_RSSI_IN            /* PC1  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
+#define GPIO_RSSI_IN            /* PC0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0)
 
 /* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
-#define GPIO_SAFETY_SWITCH_IN              /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
-/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
+#define GPIO_SAFETY_SWITCH_IN              /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
+/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
 #define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
 
+/* No PX4IO processor present */
+#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0
+
 /* Power switch controls ******************************************************/
 #define SPEKTRUM_POWER(_on_true)           VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
 
 /*
  * Board has a separate RC_IN
  *
- * GPIO PPM_IN on PB0 T3CH3
+ * GPIO PPM_IN on PC7 T3CH2
  * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
  *   Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
  */
-#define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
 #define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
@@ -156,7 +172,7 @@
 #define BOARD_HAS_STATIC_MANIFEST 1
 
 
-#define BOARD_NUM_IO_TIMERS 5
+#define BOARD_NUM_IO_TIMERS 6
 
 
 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -171,9 +187,12 @@
 		GPIO_CAN2_SILENT_S0,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
+		GPIO_LEVEL_SHIFTER_OE, 		  \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_SAFETY_SWITCH_IN,            \
 		GPIO_OTGFS_VBUS,                  \
+		GPIO_BTN_SAFETY,                  \
+		GPIO_LED_SAFETY,                  \
 	}
 
 __BEGIN_DECLS

boards/mro/ctrl-zero-classic/src/i2c.cpp

@@ -35,6 +35,5 @@
 
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(1),
-	initI2CBusExternal(3),
 	initI2CBusExternal(4),
 };

boards/mro/ctrl-zero-classic/src/spi.cpp

@@ -37,7 +37,7 @@
 
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}),
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
 	}, {GPIO::PortE, GPIO::Pin3}),
 	initSPIBus(SPI::Bus::SPI2, {
@@ -46,7 +46,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	}),
 	initSPIBus(SPI::Bus::SPI5, {
 		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
-		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
+		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
+	}),
+	initSPIBusExternal(SPI::Bus::SPI6, {
+		initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}),
 	}),
 };
 

boards/mro/ctrl-zero-classic/src/timer_config.cpp

@@ -33,6 +33,28 @@
 
 #include <px4_arch/io_timer_hw_description.h>
 
+/* Timer allocation
+ *
+ * TIM1_CH4	T FMU_CH1
+ * TIM1_CH3	T FMU_CH2
+ * TIM1_CH2	T FMU_CH3
+ * TIM1_CH1	T FMU_CH4
+ *
+ * TIM4_CH2	T FMU_CH5
+ * TIM4_CH3	T FMU_CH6
+ * TIM2_CH3	T FMU_CH7
+ * TIM2_CH1	T FMU_CH8
+ *
+ * TIM2_CH4	T FMU_CH9
+ * TIM15_CH1	T FMU_CH10
+ *
+ * TIM8_CH1	T FMU_CH11
+ *
+ * TIM4_CH4	T FMU_CH12
+ *
+ * TIM16_CH1	T BUZZER	- Driven by other driver
+ */
+
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 	initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
 	initIOTimer(Timer::Timer4, DMA{DMA::Index1}),

boards/px4/fmu-v5x/default.px4board

@@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_PAYLOAD_DELIVERER=y
 CONFIG_MODULES_RC_UPDATE=y
-CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -102,9 +101,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y

boards/px4/fmu-v5x/init/rc.board_defaults

@@ -16,7 +16,7 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1
 
-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	# Skynode: use the "custom participant" config for uxrce_dds_client
 	param set-default UXRCE_DDS_PTCFG 2

boards/px4/fmu-v5x/init/rc.board_mavlink

@@ -3,7 +3,7 @@
 # board specific MAVLink startup script.
 #------------------------------------------------------------------------------
 
-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	# Start MAVLink on the UART connected to the mission computer
 	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z

boards/px4/fmu-v5x/init/rc.board_sensors

@@ -49,7 +49,7 @@ then
 	fi
 fi
 
-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	#SKYNODE base fmu board orientation
 

boards/px4/fmu-v6x/init/rc.board_defaults

@@ -18,7 +18,7 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 0
 
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	# Skynode: use the "custom participant" config for uxrce_dds_client
 	param set-default UXRCE_DDS_PTCFG 2

boards/px4/fmu-v6x/init/rc.board_mavlink

@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------
 
 # if skynode base board is detected start Mavlink on Telem2
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
 

boards/px4/fmu-v6x/init/rc.board_sensors

@@ -56,7 +56,7 @@ then
 fi
 
 #Start Auterion Power Module selector for Skynode boards
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	pm_selector_auterion start
 else
@@ -93,7 +93,7 @@ else
 		icm20649 -s -R 6 start
 	else
 		# Internal SPI BMI088
-		if ver hwbasecmp 009 010
+		if ver hwbasecmp 009 010 011
 		then
 			bmi088 -A -R 6 -s start
 			bmi088 -G -R 6 -s start
@@ -110,7 +110,7 @@ else
 	fi
 
 	# Internal SPI bus ICM42688p
-	if ver hwbasecmp 009 010
+	if ver hwbasecmp 009 010 011
 	then
 		icm42688p -R 12 -s start
 	else
@@ -127,7 +127,7 @@ else
 		# Internal SPI bus ICM-42670-P (hard-mounted)
 		icm42670p -R 10 -s start
 	else
-		if ver hwbasecmp 009 010
+		if ver hwbasecmp 009 010 011
 		then
 			icm20602 -R 6 -s start
 		else

boards/px4/fmu-v6xrt/default.px4board

@@ -6,21 +6,30 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6"
+CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
+CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
-CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
-CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
-CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
-CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
+CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
@@ -32,14 +41,20 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_COMMON_UWB=y
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -57,11 +72,15 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_UXRCE_DDS_CLIENT=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y

boards/px4/fmu-v6xrt/init/rc.board_mavlink

@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------
 
 # if skynode base board is detected start Mavlink on Telem2
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
 

boards/px4/fmu-v6xrt/rover.px4board

@@ -1,4 +1,9 @@
+CONFIG_MODULES_AIRSPEED_SELECTOR=n
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
+CONFIG_MODULES_FW_ATT_CONTROL=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -6,6 +11,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
+CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

msg/CMakeLists.txt

@@ -58,6 +58,7 @@ set(msg_files
 	CameraCapture.msg
 	CameraStatus.msg
 	CameraTrigger.msg
+	CanInterfaceStatus.msg
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg
@@ -99,6 +100,7 @@ set(msg_files
 	FollowTargetStatus.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
+	GeofenceStatus.msg
 	GimbalControls.msg
 	GimbalDeviceAttitudeStatus.msg
 	GimbalDeviceInformation.msg
@@ -153,6 +155,10 @@ set(msg_files
 	OrbTest.msg
 	OrbTestLarge.msg
 	OrbTestMedium.msg
+	ParameterResetRequest.msg
+	ParameterSetUsedRequest.msg
+	ParameterSetValueRequest.msg
+	ParameterSetValueResponse.msg
 	ParameterUpdate.msg
 	Ping.msg
 	PositionControllerLandingStatus.msg
@@ -284,6 +290,9 @@ foreach(msg_file ${msg_files})
 	list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json)
 endforeach()
 
+# set parent scope msg_files for ROS
+set(msg_files ${msg_files} PARENT_SCOPE)
+
 # Generate uORB headers
 add_custom_command(
 	OUTPUT

msg/CanInterfaceStatus.msg

@@ -0,0 +1,6 @@
+uint64 timestamp		# time since system start (microseconds)
+uint8 interface
+
+uint64 io_errors
+uint64 frames_tx
+uint64 frames_rx

msg/GeofenceStatus.msg

@@ -0,0 +1,7 @@
+uint64 timestamp                        # time since system start (microseconds)
+
+uint32 geofence_id 			# loaded geofence id
+uint8 status 				# Current geofence status
+
+uint8 GF_STATUS_LOADING = 0
+uint8 GF_STATUS_READY = 1

msg/MissionResult.msg

@@ -1,8 +1,8 @@
 uint64 timestamp		# time since system start (microseconds)
 
-uint16 mission_id   		# Id for the mission for which the result was generated
-uint16 geofence_id  		# Id for the corresponding geofence for which the result was generated (used for mission feasibility)
-uint64 home_position_counter  	# Counter of the home position for which the result was generated (used for mission feasibility)
+uint32 mission_id   		# Id for the mission for which the result was generated
+uint32 geofence_id  		# Id for the corresponding geofence for which the result was generated (used for mission feasibility)
+uint32 home_position_counter  	# Counter of the home position for which the result was generated (used for mission feasibility)
 
 int32 seq_reached		# Sequence of the mission item which has been reached, default -1
 uint16 seq_current		# Sequence of the current mission item

msg/OrbTestMedium.msg

@@ -4,4 +4,6 @@ int32 val
 
 uint8[64] junk
 
+uint8 ORB_QUEUE_LENGTH = 16
+
 # TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll

msg/ParameterResetRequest.msg

@@ -0,0 +1,8 @@
+# ParameterResetRequest : Used by the primary to reset one or all parameter value(s) on the remote
+
+uint64 timestamp
+uint16 parameter_index
+
+bool reset_all              # If this is true then ignore parameter_index
+
+uint8 ORB_QUEUE_LENGTH = 4

msg/ParameterSetUsedRequest.msg

@@ -0,0 +1,6 @@
+# ParameterSetUsedRequest : Used by a remote to update the used flag for a parameter on the primary
+
+uint64 timestamp
+uint16 parameter_index
+
+uint8 ORB_QUEUE_LENGTH = 64

msg/ParameterSetValueRequest.msg

@@ -0,0 +1,11 @@
+# ParameterSetValueRequest : Used by a remote or primary to update the value for a parameter at the other end
+
+uint64 timestamp
+uint16 parameter_index
+
+int32 int_value             # Optional value for an integer parameter
+float32 float_value         # Optional value for a float parameter
+
+uint8 ORB_QUEUE_LENGTH = 32
+
+# TOPICS parameter_set_value_request parameter_remote_set_value_request parameter_primary_set_value_request

msg/ParameterSetValueResponse.msg

@@ -0,0 +1,9 @@
+# ParameterSetValueResponse : Response to a set value request by either primary or secondary
+
+uint64 timestamp
+uint64 request_timestamp
+uint16 parameter_index
+
+uint8 ORB_QUEUE_LENGTH = 4
+
+# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response

msg/TransponderReport.msg

@@ -47,4 +47,4 @@ uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18
 uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19
 uint16 ADSB_EMITTER_TYPE_ENUM_END=20
 
-uint8 ORB_QUEUE_LENGTH = 8
+uint8 ORB_QUEUE_LENGTH = 16

platforms/common/CMakeLists.txt

@@ -52,6 +52,7 @@ add_library(px4_platform STATIC
 	shutdown.cpp
 	spi.cpp
 	pab_manifest.c
+	Serial.cpp
 	${SRCS}
 )
 target_link_libraries(px4_platform prebuild_targets px4_work_queue)

platforms/common/Serial.cpp

@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/Serial.hpp>
+
+namespace device
+{
+
+Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+	       FlowControl flowcontrol) :
+	_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
+{
+	// If no baudrate was specified then set it to a reasonable default value
+	if (baudrate == 0) {
+		(void) _impl.setBaudrate(9600);
+	}
+}
+
+Serial::~Serial()
+{
+}
+
+bool Serial::open()
+{
+	return _impl.open();
+}
+
+bool Serial::isOpen() const
+{
+	return _impl.isOpen();
+}
+
+bool Serial::close()
+{
+	return _impl.close();
+}
+
+ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
+{
+	return _impl.read(buffer, buffer_size);
+}
+
+ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
+}
+
+ssize_t Serial::write(const void *buffer, size_t buffer_size)
+{
+	return _impl.write(buffer, buffer_size);
+}
+
+uint32_t Serial::getBaudrate() const
+{
+	return _impl.getBaudrate();
+}
+
+bool Serial::setBaudrate(uint32_t baudrate)
+{
+	return _impl.setBaudrate(baudrate);
+}
+
+ByteSize Serial::getBytesize() const
+{
+	return _impl.getBytesize();
+}
+
+bool Serial::setBytesize(ByteSize bytesize)
+{
+	return _impl.setBytesize(bytesize);
+}
+
+Parity Serial::getParity() const
+{
+	return _impl.getParity();
+}
+
+bool Serial::setParity(Parity parity)
+{
+	return _impl.setParity(parity);
+}
+
+StopBits Serial::getStopbits() const
+{
+	return _impl.getStopbits();
+}
+
+bool Serial::setStopbits(StopBits stopbits)
+{
+	return _impl.setStopbits(stopbits);
+}
+
+FlowControl Serial::getFlowcontrol() const
+{
+	return _impl.getFlowcontrol();
+}
+
+bool Serial::setFlowcontrol(FlowControl flowcontrol)
+{
+	return _impl.setFlowcontrol(flowcontrol);
+}
+
+const char *Serial::getPort() const
+{
+	return _impl.getPort();
+}
+
+} // namespace device

platforms/common/include/px4_platform_common/Serial.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <SerialImpl.hpp>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device __EXPORT
+{
+
+class Serial
+{
+public:
+	Serial(const char *port, uint32_t baudrate = 57600,
+	       ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
+	       StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
+	virtual ~Serial();
+
+	// Open sets up the port and gets it configured based on desired configuration
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	// If port is already open then the following configuration functions
+	// will reconfigure the port. If the port is not yet open then they will
+	// simply store the configuration in preparation for the port to be opened.
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+	const char *getPort() const;
+
+private:
+	// Disable copy constructors
+	Serial(const Serial &);
+	Serial &operator=(const Serial &);
+
+	// platform implementation
+	SerialImpl _impl;
+};
+
+} // namespace device

platforms/common/include/px4_platform_common/SerialCommon.hpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+namespace device
+{
+namespace SerialConfig
+{
+
+
+// ByteSize: number of data bits
+enum class ByteSize {
+	FiveBits  = 5,
+	SixBits   = 6,
+	SevenBits = 7,
+	EightBits = 8,
+};
+
+// Parity: enable parity checking
+enum class Parity {
+	None = 0,
+	Odd  = 1,
+	Even = 2,
+};
+
+// StopBits: number of stop bits
+enum class StopBits {
+	One = 1,
+	Two = 2
+};
+
+// FlowControl: enable flow control
+enum class FlowControl {
+	Disabled = 0,
+	Enabled  = 1,
+};
+
+} // namespace SerialConfig
+} // namespace device

platforms/common/include/px4_platform_common/time.h

@@ -13,11 +13,21 @@ __END_DECLS
 #define px4_clock_gettime system_clock_gettime
 #endif
 
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)
 
 __BEGIN_DECLS
 __EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
+__END_DECLS
 
+#else
+
+#define px4_clock_settime system_clock_settime
+
+#endif
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+__BEGIN_DECLS
 __EXPORT int px4_usleep(useconds_t usec);
 __EXPORT unsigned int px4_sleep(unsigned int seconds);
 __EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
@@ -27,7 +37,6 @@ __END_DECLS
 
 #else
 
-#define px4_clock_settime system_clock_settime
 #define px4_usleep system_usleep
 #define px4_sleep system_sleep
 #define px4_pthread_cond_timedwait system_pthread_cond_timedwait

platforms/common/pab_manifest.c

@@ -293,12 +293,6 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
 		.mandatory   = 1,
 		.connection  = px4_hw_con_onboard,
 	},
-	{
-		.id          =  PX4_MFT_PM2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
 	{
 		.id          = PX4_MFT_ETHERNET,
 		.present     = 1,
@@ -316,6 +310,52 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
 // BASE ID 10  Skynode QS no USB Alaised to ID 9
 // BASE ID 16  Auterion Skynode RC10, RC11, RC12, RC13 Alaised to ID 0
 
+// BASE ID 17  Auterion Skynode RC13 with many parts removed
+static const px4_hw_mft_item_t base_configuration_17[] = {
+	{
+		.id          = PX4_MFT_PX4IO,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_USB,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		.id          = PX4_MFT_CAN2,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_CAN3,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		.id          = PX4_MFT_PM2,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_ETHERNET,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_connector,
+	},
+	{
+		.id          = PX4_MFT_T100_ETH,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
 //  ver_rev
@@ -329,6 +369,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{HW_BASE_ID(9),  base_configuration_9, arraySize(base_configuration_9)},  // Auterion Skynode ver 9
 	{HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)},  // Auterion Skynode ver 10
 	{HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)},  // Auterion Skynode ver 16
+	{HW_BASE_ID(17), base_configuration_17, arraySize(base_configuration_17)},  // Auterion Skynode ver 17
 };
 
 /************************************************************************************

platforms/common/px4_log.cpp

@@ -74,7 +74,7 @@ void px4_log_initialize(void)
 	log_message.severity = 6; // info
 	strcpy((char *)log_message.text, "initialized uORB logging");
 	log_message.timestamp = hrt_absolute_time();
-	orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, log_message_s::ORB_QUEUE_LENGTH);
+	orb_log_message_pub = orb_advertise(ORB_ID(log_message), &log_message);
 }
 
 __EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...)

platforms/common/uORB/Publication.hpp

@@ -48,24 +48,6 @@
 namespace uORB
 {
 
-template <typename U> class DefaultQueueSize
-{
-private:
-	template <typename T>
-	static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *)
-	{
-		return T::ORB_QUEUE_LENGTH;
-	}
-
-	template <typename T> static constexpr uint8_t get_queue_size(...)
-	{
-		return 1;
-	}
-
-public:
-	static constexpr unsigned value = get_queue_size<U>(nullptr);
-};
-
 class PublicationBase
 {
 public:
@@ -97,7 +79,7 @@ protected:
 /**
  * uORB publication wrapper class
  */
-template<typename T, uint8_t ORB_QSIZE = DefaultQueueSize<T>::value>
+template<typename T>
 class Publication : public PublicationBase
 {
 public:
@@ -113,7 +95,7 @@ public:
 	bool advertise()
 	{
 		if (!advertised()) {
-			_handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE);
+			_handle = orb_advertise(get_topic(), nullptr);
 		}
 
 		return advertised();

platforms/common/uORB/PublicationMulti.hpp

@@ -51,7 +51,7 @@ namespace uORB
 /**
  * Base publication multi wrapper class
  */
-template<typename T, uint8_t QSIZE = DefaultQueueSize<T>::value>
+template<typename T>
 class PublicationMulti : public PublicationBase
 {
 public:
@@ -73,7 +73,7 @@ public:
 	{
 		if (!advertised()) {
 			int instance = 0;
-			_handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE);
+			_handle = orb_advertise_multi(get_topic(), nullptr, &instance);
 		}
 
 		return advertised();

platforms/common/uORB/SubscriptionBlocking.hpp

@@ -56,31 +56,6 @@ public:
 	SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
 		SubscriptionCallback(meta, interval_us, instance)
 	{
-		// pthread_mutexattr_init
-		pthread_mutexattr_t attr;
-		int ret_attr_init = pthread_mutexattr_init(&attr);
-
-		if (ret_attr_init != 0) {
-			PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
-		}
-
-#if defined(PTHREAD_PRIO_NONE)
-		// pthread_mutexattr_settype
-		//  PTHREAD_PRIO_NONE not available on cygwin
-		int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
-
-		if (ret_mutexattr_settype != 0) {
-			PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
-		}
-
-#endif // PTHREAD_PRIO_NONE
-
-		// pthread_mutex_init
-		int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
-
-		if (ret_mutex_init != 0) {
-			PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init);
-		}
 	}
 
 	virtual ~SubscriptionBlocking()

platforms/common/uORB/uORB.cpp

@@ -118,22 +118,11 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
 	return uORB::Manager::get_instance()->orb_advertise(meta, data);
 }
 
-orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
-{
-	return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
-}
-
 orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance);
 }
 
-orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
-				       unsigned int queue_size)
-{
-	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size);
-}
-
 int orb_unadvertise(orb_advert_t handle)
 {
 	return uORB::Manager::get_instance()->orb_unadvertise(handle);
@@ -227,6 +216,14 @@ const char *orb_get_c_type(unsigned char short_type)
 	return nullptr;
 }
 
+uint8_t orb_get_queue_size(const struct orb_metadata *meta)
+{
+	if (meta) {
+		return meta->o_queue;
+	}
+
+	return 0;
+}
 
 void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name)
 {

platforms/common/uORB/uORB.h

@@ -53,6 +53,8 @@ struct orb_metadata {
 	const uint16_t o_size_no_padding;   /**< object size w/o padding at the end (for logger) */
 	uint32_t message_hash;	/**< Hash over all fields for message compatibility checks */
 	orb_id_size_t  o_id;                /**< ORB_ID enum */
+	uint8_t o_queue;					/**< queue size */
+
 };
 
 typedef const struct orb_metadata *orb_id_t;
@@ -102,14 +104,16 @@ typedef const struct orb_metadata *orb_id_t;
  * @param _size_no_padding	Struct size w/o padding at the end
  * @param _message_hash	32 bit message hash over all fields
  * @param _orb_id_enum	ORB ID enum e.g.: ORB_ID::vehicle_status
+ * @param _queue_size Queue size from topic definition
  */
-#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum)		\
-	const struct orb_metadata __orb_##_name = {	\
-		#_name,					\
-		sizeof(_struct),		\
-		_size_no_padding,			\
-		_message_hash,				\
-		_orb_id_enum				\
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum, _queue_size)               \
+	const struct orb_metadata __orb_##_name = {     \
+		#_name,                                 \
+		sizeof(_struct),                \
+		_size_no_padding,                       \
+		_message_hash,                          \
+		_orb_id_enum,                           \
+		_queue_size                             \
 	}; struct hack
 
 __BEGIN_DECLS
@@ -135,23 +139,11 @@ typedef void 	*orb_advert_t;
  */
 extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
 
-/**
- * @see uORB::Manager::orb_advertise()
- */
-extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data,
-					unsigned int queue_size) __EXPORT;
-
 /**
  * @see uORB::Manager::orb_advertise_multi()
  */
 extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT;
 
-/**
- * @see uORB::Manager::orb_advertise_multi()
- */
-extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size) __EXPORT;
-
 /**
  * @see uORB::Manager::orb_unadvertise()
  */
@@ -160,7 +152,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT;
 /**
  * @see uORB::Manager::orb_publish()
  */
-extern int	orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
+extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
 
 /**
  * Advertise as the publisher of a topic.
@@ -241,6 +233,12 @@ extern int	orb_get_interval(int handle, unsigned *interval) __EXPORT;
  */
 const char *orb_get_c_type(unsigned char short_type);
 
+/**
+ * Returns the queue size of a topic
+ * @param meta orb topic metadata
+ */
+extern uint8_t orb_get_queue_size(const struct orb_metadata *meta);
+
 /**
  * Print a topic to console. Do not call this directly, use print_message() instead.
  * @param meta orb topic metadata

platforms/common/uORB/uORBDeviceNode.cpp

@@ -48,37 +48,10 @@
 
 static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
 
-// round up to nearest power of two
-// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
-// Note: When the input value > 128, the output is always 128
-static inline uint8_t round_pow_of_two_8(uint8_t n)
-{
-	if (n == 0) {
-		return 1;
-	}
-
-	// Avoid is already a power of 2
-	uint8_t value = n - 1;
-
-	// Fill 1
-	value |= value >> 1U;
-	value |= value >> 2U;
-	value |= value >> 4U;
-
-	// Unable to round-up, take the value of round-down
-	if (value == UINT8_MAX) {
-		value >>= 1U;
-	}
-
-	return value + 1;
-}
-
-uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path,
-			     uint8_t queue_size) :
+uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) :
 	CDev(strdup(path)), // success is checked in CDev::init
 	_meta(meta),
-	_instance(instance),
-	_queue_size(round_pow_of_two_8(queue_size))
+	_instance(instance)
 {
 }
 
@@ -186,7 +159,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 
 			/* re-check size */
 			if (nullptr == _data) {
-				const size_t data_size = _meta->o_size * _queue_size;
+				const size_t data_size = _meta->o_size * _meta->o_queue;
 				_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
 
 				if (_data) {
@@ -217,7 +190,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
 	unsigned generation = _generation.fetch_add(1);
 
-	memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size);
+	memcpy(_data + (_meta->o_size * (generation % _meta->o_queue)), buffer, _meta->o_size);
 
 	// callbacks
 	for (auto item : _callbacks) {
@@ -254,13 +227,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 		*(uintptr_t *)arg = (uintptr_t)this;
 		return PX4_OK;
 
-	case ORBIOCSETQUEUESIZE: {
-			lock();
-			int ret = update_queue_size(arg);
-			unlock();
-			return ret;
-		}
-
 	case ORBIOCGETINTERVAL:
 		*(unsigned *)arg = filp_to_subscription(filp)->get_interval_us();
 		return PX4_OK;
@@ -389,12 +355,11 @@ uORB::DeviceNode::print_statistics(int max_topic_length)
 
 	const uint8_t instance = get_instance();
 	const int8_t sub_count = subscriber_count();
-	const uint8_t queue_size = get_queue_size();
 
 	unlock();
 
 	PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
-		     queue_size, get_meta()->o_size, get_devname());
+		     get_meta()->o_queue, get_meta()->o_size, get_devname());
 
 	return true;
 }
@@ -447,7 +412,10 @@ int16_t uORB::DeviceNode::process_add_subscription()
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
 
 	if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
-		ch->send_message(_meta->o_name, _meta->o_size, _data);
+		// Only send the most recent data to initialize the remote end.
+		if (_data_valid) {
+			ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue)));
+		}
 	}
 
 	return PX4_OK;
@@ -483,21 +451,6 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data
 }
 #endif /* CONFIG_ORB_COMMUNICATOR */
 
-int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
-{
-	if (_queue_size == queue_size) {
-		return PX4_OK;
-	}
-
-	//queue size is limited to 255 for the single reason that we use uint8 to store it
-	if (_data || _queue_size > queue_size || queue_size > 255) {
-		return PX4_ERROR;
-	}
-
-	_queue_size = round_pow_of_two_8(queue_size);
-	return PX4_OK;
-}
-
 unsigned uORB::DeviceNode::get_initial_generation()
 {
 	ATOMIC_ENTER;

platforms/common/uORB/uORBDeviceNode.hpp

@@ -62,7 +62,7 @@ class UnitTest;
 class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode<uORB::DeviceNode *>
 {
 public:
-	DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1);
+	DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path);
 	virtual ~DeviceNode();
 
 	// no copy, assignment, move, move assignment
@@ -179,15 +179,6 @@ public:
 
 	void mark_as_advertised() { _advertised = true; }
 
-	/**
-	 * Try to change the size of the queue. This can only be done as long as nobody published yet.
-	 * This is the case, for example when orb_subscribe was called before an orb_advertise.
-	 * The queue size can only be increased.
-	 * @param queue_size new size of the queue
-	 * @return PX4_OK if queue size successfully set
-	 */
-	int update_queue_size(unsigned int queue_size);
-
 	/**
 	 * Print statistics
 	 * @param max_topic_length max topic name length for printing
@@ -195,7 +186,7 @@ public:
 	 */
 	bool print_statistics(int max_topic_length);
 
-	uint8_t get_queue_size() const { return _queue_size; }
+	uint8_t get_queue_size() const { return _meta->o_queue; }
 
 	int8_t subscriber_count() const { return _subscriber_count; }
 
@@ -234,7 +225,7 @@ public:
 	bool copy(void *dst, unsigned &generation)
 	{
 		if ((dst != nullptr) && (_data != nullptr)) {
-			if (_queue_size == 1) {
+			if (_meta->o_queue == 1) {
 				ATOMIC_ENTER;
 				memcpy(dst, _data, _meta->o_size);
 				generation = _generation.load();
@@ -253,12 +244,12 @@ public:
 				}
 
 				// Compatible with normal and overflow conditions
-				if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
+				if (!is_in_range(current_generation - _meta->o_queue, generation, current_generation - 1)) {
 					// Reader is too far behind: some messages are lost
-					generation = current_generation - _queue_size;
+					generation = current_generation - _meta->o_queue;
 				}
 
-				memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
+				memcpy(dst, _data + (_meta->o_size * (generation % _meta->o_queue)), _meta->o_size);
 				ATOMIC_LEAVE;
 
 				++generation;
@@ -295,7 +286,7 @@ private:
 
 	const uint8_t _instance; /**< orb multi instance identifier */
 	bool _advertised{false};  /**< has ever been advertised (not necessarily published data yet) */
-	uint8_t _queue_size; /**< maximum number of elements in the queue */
+
 	int8_t _subscriber_count{0};
 
 

platforms/common/uORB/uORBManager.cpp

@@ -265,8 +265,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 	return ret;
 }
 
-orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size)
+orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 #ifdef ORB_USE_PUBLISHER_RULES
 
@@ -300,19 +299,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}
 
-	/* Set the queue size. This must be done before the first publication; thus it fails if
-	 * this is not the first advertiser.
-	 */
-	int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
-
-	if (result < 0 && queue_size > 1) {
-		PX4_WARN("orb_advertise_multi: failed to set queue size");
-	}
-
 	/* get the advertiser handle and close the node */
 	orb_advert_t advertiser;
 
-	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
 	px4_close(fd);
 
 	if (result == PX4_ERROR) {
@@ -602,6 +592,22 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 {
 	PX4_DEBUG("entering process_remote_topic: name: %s", topic_name);
 
+	// First make sure this is a valid topic
+	const struct orb_metadata *const *topic_list = orb_get_topics();
+	orb_id_t topic_ptr = nullptr;
+
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
+			topic_ptr = topic_list[i];
+			break;
+		}
+	}
+
+	if (! topic_ptr) {
+		PX4_ERR("process_remote_topic meta not found for %s\n", topic_name);
+		return -1;
+	}
+
 	// Look to see if we already have a node for this topic
 	char nodepath[orb_maxpath];
 	int ret = uORB::Utils::node_mkpath(nodepath, topic_name);
@@ -613,7 +619,7 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 			uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
 
 			if (node) {
-				PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
+				PX4_DEBUG("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
 				node->mark_as_advertised();
 				_remote_topics.insert(topic_name);
 				return 0;
@@ -622,27 +628,9 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 	}
 
 	// We didn't find a node so we need to create it via an advertisement
-	const struct orb_metadata *const *topic_list = orb_get_topics();
-	orb_id_t topic_ptr = nullptr;
-
-	for (size_t i = 0; i < orb_topics_count(); i++) {
-		if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
-			topic_ptr = topic_list[i];
-			break;
-		}
-	}
-
-	if (topic_ptr) {
-		PX4_INFO("Advertising remote topic %s", topic_name);
-		_remote_topics.insert(topic_name);
-		// Add some queue depth when advertising remote topics. These
-		// topics may get aggregated and thus delivered in a batch that
-		// requires some buffering in a queue.
-		orb_advertise(topic_ptr, nullptr, 5);
-
-	} else {
-		PX4_INFO("process_remote_topic meta not found for %s\n", topic_name);
-	}
+	PX4_DEBUG("Advertising remote topic %s", topic_name);
+	_remote_topics.insert(topic_name);
+	orb_advertise(topic_ptr, nullptr);
 
 	return 0;
 }
@@ -663,8 +651,11 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName)
 			PX4_DEBUG("DeviceNode(%s) not created yet", messageName);
 
 		} else {
-			// node is present.
-			node->process_add_subscription();
+			// node is present. But don't send any data to it if it
+			// is a node advertised by the remote side
+			if (_remote_topics.find(messageName) == false) {
+				node->process_add_subscription();
+			}
 		}
 
 	} else {

platforms/common/uORB/uORBManager.hpp

@@ -215,17 +215,15 @@ public:
 	 * @param data    A pointer to the initial data to be published.
 	 *      For topics updated by interrupt handlers, the advertisement
 	 *      must be performed from non-interrupt context.
-	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
-	 *      used.
 	 * @return    nullptr on error, otherwise returns an object pointer
 	 *      that can be used to publish to the topic.
 	 *      If the topic in question is not known (due to an
 	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
 	 *      this function will return nullptr and set errno to ENOENT.
 	 */
-	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
+	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data = nullptr)
 	{
-		return orb_advertise_multi(meta, data, nullptr, queue_size);
+		return orb_advertise_multi(meta, data, nullptr);
 	}
 
 	/**
@@ -250,16 +248,13 @@ public:
 	 * @param instance  Pointer to an integer which will yield the instance ID (0-based)
 	 *      of the publication. This is an output parameter and will be set to the newly
 	 *      created instance, ie. 0 for the first advertiser, 1 for the next and so on.
-	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
-	 *      used.
 	 * @return    nullptr on error, otherwise returns a handle
 	 *      that can be used to publish to the topic.
 	 *      If the topic in question is not known (due to an
 	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
 	 *      this function will return nullptr and set errno to ENOENT.
 	 */
-	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-					 unsigned int queue_size = 1);
+	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance);
 
 	/**
 	 * Unadvertise a topic.

platforms/common/uORB/uORBManagerUsr.cpp

@@ -89,8 +89,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 	return data.ret;
 }
 
-orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size)
+orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 	/* open the node as an advertiser */
 	int fd = node_open(meta, true, instance);
@@ -100,19 +99,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}
 
-	/* Set the queue size. This must be done before the first publication; thus it fails if
-	 * this is not the first advertiser.
-	 */
-	int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
-
-	if (result < 0 && queue_size > 1) {
-		PX4_WARN("orb_advertise_multi: failed to set queue size");
-	}
-
 	/* get the advertiser handle and close the node */
 	orb_advert_t advertiser;
 
-	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
 	px4_close(fd);
 
 	if (result == PX4_ERROR) {

platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp

@@ -574,8 +574,8 @@ int uORBTest::UnitTest::test_wrap_around()
 	bool updated{false};
 
 	// Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation
-	const int queue_size = 16;
-	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_wrap_around), nullptr, queue_size);
+	const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_wrap_around));
+	ptopic = orb_advertise(ORB_ID(orb_test_medium_wrap_around), nullptr);
 
 	if (ptopic == nullptr) {
 		return test_fail("advertise failed: %d", errno);
@@ -828,9 +828,9 @@ int uORBTest::UnitTest::test_queue()
 		return test_fail("subscribe failed: %d", errno);
 	}
 
-	const int queue_size = 16;
+	const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue));
 	orb_test_medium_s t{};
-	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
+	ptopic = orb_advertise(ORB_ID(orb_test_medium_queue), &t);
 
 	if (ptopic == nullptr) {
 		return test_fail("advertise failed: %d", errno);
@@ -935,9 +935,9 @@ int uORBTest::UnitTest::pub_test_queue_main()
 {
 	orb_test_medium_s t{};
 	orb_advert_t ptopic{nullptr};
-	const int queue_size = 50;
+	const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue_poll));
 
-	if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
+	if ((ptopic = orb_advertise(ORB_ID(orb_test_medium_queue_poll), &t)) == nullptr) {
 		_thread_should_exit = true;
 		return test_fail("advertise failed: %d", errno);
 	}

platforms/nuttx/NuttX/apps

@@ -1 +1 @@
-Subproject commit 616f7024a479bf209eadce133bba5dc8820a7f99
+Subproject commit e37940d8535f603a16b8f6f21c21edaf584218aa

platforms/nuttx/src/px4/common/SerialImpl.cpp

@@ -0,0 +1,394 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h>
+#include <px4_log.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+#define MODULE_NAME "SerialImpl"
+
+namespace device
+{
+
+SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		       FlowControl flowcontrol) :
+	_baudrate(baudrate),
+	_bytesize(bytesize),
+	_parity(parity),
+	_stopbits(stopbits),
+	_flowcontrol(flowcontrol)
+{
+	if (port) {
+		strncpy(_port, port, sizeof(_port) - 1);
+		_port[sizeof(_port) - 1] = '\0';
+
+	} else {
+		_port[0] = 0;
+	}
+}
+
+SerialImpl::~SerialImpl()
+{
+	if (isOpen()) {
+		close();
+	}
+}
+
+bool SerialImpl::validateBaudrate(uint32_t baudrate)
+{
+	return ((baudrate == 9600) ||
+		(baudrate == 19200) ||
+		(baudrate == 38400) ||
+		(baudrate == 57600) ||
+		(baudrate == 115200) ||
+		(baudrate == 230400) ||
+		(baudrate == 460800) ||
+		(baudrate == 921600));
+}
+
+bool SerialImpl::configure()
+{
+	/* process baud rate */
+	int speed;
+
+	if (! validateBaudrate(_baudrate)) {
+		PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
+		return false;
+	}
+
+	switch (_baudrate) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
+#endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
+		return false;
+	}
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
+		return false;
+	}
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
+		return false;
+	}
+
+	return true;
+}
+
+bool SerialImpl::open()
+{
+	if (isOpen()) {
+		return true;
+	}
+
+	// Open the serial port
+	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open %s err: %d", _port, errno);
+		return false;
+	}
+
+	_serial_fd = serial_fd;
+
+	// Configure the serial port
+	if (! configure()) {
+		PX4_ERR("failed to configure %s err: %d", _port, errno);
+		return false;
+	}
+
+	_open = true;
+
+	return _open;
+}
+
+bool SerialImpl::isOpen() const
+{
+	return _open;
+}
+
+bool SerialImpl::close()
+{
+
+	if (_serial_fd >= 0) {
+		::close(_serial_fd);
+	}
+
+	_serial_fd = -1;
+	_open = false;
+
+	return true;
+}
+
+ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot read from serial device until it has been opened");
+		return -1;
+	}
+
+	int ret = ::read(_serial_fd, buffer, buffer_size);
+
+	if (ret < 0) {
+		PX4_DEBUG("%s read error %d", _port, ret);
+	}
+
+	return ret;
+}
+
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	if (!_open) {
+		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
+		return -1;
+	}
+
+	if (buffer_size < character_count) {
+		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
+		return -1;
+	}
+
+	const hrt_abstime start_time_us = hrt_absolute_time();
+	int total_bytes_read = 0;
+
+	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
+		// Poll for incoming UART data.
+		pollfd fds[1];
+		fds[0].fd = _serial_fd;
+		fds[0].events = POLLIN;
+
+		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
+
+		if (remaining_time <= 0) { break; }
+
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
+
+		if (ret > 0) {
+			if (fds[0].revents & POLLIN) {
+				const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
+
+				int err = 0;
+				int bytes_available = 0;
+				err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
+
+				if (err != 0 || bytes_available < (int)character_count) {
+					px4_usleep(sleeptime);
+				}
+
+				ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
+
+				if (ret > 0) {
+					total_bytes_read += ret;
+				}
+
+			} else {
+				PX4_ERR("Got a poll error");
+				return -1;
+			}
+		}
+	}
+
+	return total_bytes_read;
+}
+
+ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot write to serial device until it has been opened");
+		return -1;
+	}
+
+	int written = ::write(_serial_fd, buffer, buffer_size);
+	::fsync(_serial_fd);
+
+	if (written < 0) {
+		PX4_ERR("%s write error %d", _port, written);
+	}
+
+	return written;
+}
+
+const char *SerialImpl::getPort() const
+{
+	return _port;
+}
+
+uint32_t SerialImpl::getBaudrate() const
+{
+	return _baudrate;
+}
+
+bool SerialImpl::setBaudrate(uint32_t baudrate)
+{
+	if (! validateBaudrate(baudrate)) {
+		PX4_ERR("ERR: invalid baudrate: %lu", baudrate);
+		return false;
+	}
+
+	// check if already configured
+	if ((baudrate == _baudrate) && _open) {
+		return true;
+	}
+
+	_baudrate = baudrate;
+
+	// process baud rate change now if port is already open
+	if (_open) {
+		return configure();
+	}
+
+	return true;
+}
+
+ByteSize SerialImpl::getBytesize() const
+{
+	return _bytesize;
+}
+
+bool SerialImpl::setBytesize(ByteSize bytesize)
+{
+	return bytesize == ByteSize::EightBits;
+}
+
+Parity SerialImpl::getParity() const
+{
+	return _parity;
+}
+
+bool SerialImpl::setParity(Parity parity)
+{
+	return parity == Parity::None;
+}
+
+StopBits SerialImpl::getStopbits() const
+{
+	return _stopbits;
+}
+
+bool SerialImpl::setStopbits(StopBits stopbits)
+{
+	return stopbits == StopBits::One;
+}
+
+FlowControl SerialImpl::getFlowcontrol() const
+{
+	return _flowcontrol;
+}
+
+bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
+{
+	return flowcontrol == FlowControl::Disabled;
+}
+
+} // namespace device

platforms/nuttx/src/px4/common/include/SerialImpl.hpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	const char *getPort() const;
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+private:
+
+	int _serial_fd{-1};
+
+	bool _open{false};
+
+	char _port[32] {};
+
+	uint32_t _baudrate{0};
+
+	ByteSize _bytesize{ByteSize::EightBits};
+	Parity _parity{Parity::None};
+	StopBits _stopbits{StopBits::One};
+	FlowControl _flowcontrol{FlowControl::Disabled};
+
+	bool validateBaudrate(uint32_t baudrate);
+	bool configure();
+
+};
+
+} // namespace device

platforms/nuttx/src/px4/common/px4_layer.cmake

@@ -3,6 +3,8 @@
 add_library(px4_layer
 		${KERNEL_SRCS}
 		cdc_acm_check.cpp
+		${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+		SerialImpl.cpp
 	)
 
 target_link_libraries(px4_layer

platforms/nuttx/src/px4/common/px4_protected_layers.cmake

@@ -15,6 +15,8 @@ add_library(px4_layer
 	usr_board_ctrl.c
 	usr_hrt.cpp
 	usr_mcu_version.cpp
+	${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+	SerialImpl.cpp
 )
 
 target_link_libraries(px4_layer

platforms/posix/include/SerialImpl.hpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	const char *getPort() const;
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+private:
+
+	int _serial_fd{-1};
+
+	bool _open{false};
+
+	char _port[32] {};
+
+	uint32_t _baudrate{0};
+
+	ByteSize _bytesize{ByteSize::EightBits};
+	Parity _parity{Parity::None};
+	StopBits _stopbits{StopBits::One};
+	FlowControl _flowcontrol{FlowControl::Disabled};
+
+	bool validateBaudrate(uint32_t baudrate);
+	bool configure();
+};
+
+} // namespace device

platforms/posix/src/px4/common/CMakeLists.txt

@@ -46,6 +46,8 @@ add_library(px4_layer
 	drv_hrt.cpp
 	cpuload.cpp
 	print_load.cpp
+	${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+	SerialImpl.cpp
 )
 target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
 target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable

platforms/posix/src/px4/common/SerialImpl.cpp

@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h>
+#include <px4_log.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+namespace device
+{
+
+SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		       FlowControl flowcontrol) :
+	_baudrate(baudrate),
+	_bytesize(bytesize),
+	_parity(parity),
+	_stopbits(stopbits),
+	_flowcontrol(flowcontrol)
+{
+	if (port) {
+		strncpy(_port, port, sizeof(_port) - 1);
+		_port[sizeof(_port) - 1] = '\0';
+
+	} else {
+		_port[0] = 0;
+	}
+}
+
+SerialImpl::~SerialImpl()
+{
+	if (isOpen()) {
+		close();
+	}
+}
+
+bool SerialImpl::validateBaudrate(uint32_t baudrate)
+{
+	return ((baudrate == 9600) ||
+		(baudrate == 19200) ||
+		(baudrate == 38400) ||
+		(baudrate == 57600) ||
+		(baudrate == 115200) ||
+		(baudrate == 230400) ||
+		(baudrate == 460800) ||
+		(baudrate == 921600));
+}
+
+bool SerialImpl::configure()
+{
+	/* process baud rate */
+	int speed;
+
+	if (! validateBaudrate(_baudrate)) {
+		PX4_ERR("ERR: unknown baudrate: %u", _baudrate);
+		return false;
+	}
+
+	switch (_baudrate) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
+#endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %d", _baudrate);
+		return false;
+	}
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
+		return false;
+	}
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
+		return false;
+	}
+
+	return true;
+}
+
+bool SerialImpl::open()
+{
+	if (isOpen()) {
+		return true;
+	}
+
+	// Open the serial port
+	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open %s err: %d", _port, errno);
+		return false;
+	}
+
+	_serial_fd = serial_fd;
+
+	// Configure the serial port
+	if (! configure()) {
+		PX4_ERR("failed to configure %s err: %d", _port, errno);
+		return false;
+	}
+
+	_open = true;
+
+	return _open;
+}
+
+bool SerialImpl::isOpen() const
+{
+	return _open;
+}
+
+bool SerialImpl::close()
+{
+
+	if (_serial_fd >= 0) {
+		::close(_serial_fd);
+	}
+
+	_serial_fd = -1;
+	_open = false;
+
+	return true;
+}
+
+ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot read from serial device until it has been opened");
+		return -1;
+	}
+
+	int ret = ::read(_serial_fd, buffer, buffer_size);
+
+	if (ret < 0) {
+		PX4_DEBUG("%s read error %d", _port, ret);
+
+	}
+
+	return ret;
+}
+
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	if (!_open) {
+		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
+		return -1;
+	}
+
+	if (buffer_size < character_count) {
+		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
+		return -1;
+	}
+
+	const hrt_abstime start_time_us = hrt_absolute_time();
+	int total_bytes_read = 0;
+
+	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
+		// Poll for incoming UART data.
+		pollfd fds[1];
+		fds[0].fd = _serial_fd;
+		fds[0].events = POLLIN;
+
+		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
+
+		if (remaining_time <= 0) { break; }
+
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
+
+		if (ret > 0) {
+			if (fds[0].revents & POLLIN) {
+				const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
+				px4_usleep(sleeptime);
+
+				ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
+
+				if (ret > 0) {
+					total_bytes_read += ret;
+				}
+
+			} else {
+				PX4_ERR("Got a poll error");
+				return -1;
+			}
+		}
+	}
+
+	return total_bytes_read;
+}
+
+ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot write to serial device until it has been opened");
+		return -1;
+	}
+
+	int written = ::write(_serial_fd, buffer, buffer_size);
+	::fsync(_serial_fd);
+
+	if (written < 0) {
+		PX4_ERR("%s write error %d", _port, written);
+
+	}
+
+	return written;
+}
+
+const char *SerialImpl::getPort() const
+{
+	return _port;
+}
+
+uint32_t SerialImpl::getBaudrate() const
+{
+	return _baudrate;
+}
+
+bool SerialImpl::setBaudrate(uint32_t baudrate)
+{
+	if (! validateBaudrate(baudrate)) {
+		PX4_ERR("ERR: invalid baudrate: %u", baudrate);
+		return false;
+	}
+
+	// check if already configured
+	if ((baudrate == _baudrate) && _open) {
+		return true;
+	}
+
+	_baudrate = baudrate;
+
+	// process baud rate change now if port is already open
+	if (_open) {
+		return configure();
+	}
+
+	return true;
+}
+
+ByteSize SerialImpl::getBytesize() const
+{
+	return _bytesize;
+}
+
+bool SerialImpl::setBytesize(ByteSize bytesize)
+{
+	return bytesize == ByteSize::EightBits;
+}
+
+Parity SerialImpl::getParity() const
+{
+	return _parity;
+}
+
+bool SerialImpl::setParity(Parity parity)
+{
+	return parity == Parity::None;
+}
+
+StopBits SerialImpl::getStopbits() const
+{
+	return _stopbits;
+}
+
+bool SerialImpl::setStopbits(StopBits stopbits)
+{
+	return stopbits == StopBits::One;
+}
+
+FlowControl SerialImpl::getFlowcontrol() const
+{
+	return _flowcontrol;
+}
+
+bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
+{
+	return flowcontrol == FlowControl::Disabled;
+}
+
+} // namespace device

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -51,6 +51,11 @@
 #include <errno.h>
 #include "hrt_work.h"
 
+// Voxl2 board specific API definitions to get time offset
+#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+#include "fc_sensor.h"
+#endif
+
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <lockstep_scheduler/lockstep_scheduler.h>
 static LockstepScheduler lockstep_scheduler {true};
@@ -107,6 +112,29 @@ hrt_abstime hrt_absolute_time()
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	struct timespec ts;
 	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+
+# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+	hrt_abstime temp_abstime = ts_to_abstime(&ts);
+	int apps_time_offset = fc_sensor_get_time_offset();
+
+	if (apps_time_offset < 0) {
+		hrt_abstime temp_offset = -apps_time_offset;
+
+		if (temp_offset >= temp_abstime) {
+			temp_abstime = 0;
+
+		} else {
+			temp_abstime -= temp_offset;
+		}
+
+	} else {
+		temp_abstime += (hrt_abstime) apps_time_offset;
+	}
+
+	ts.tv_sec = temp_abstime / 1000000;
+	ts.tv_nsec = (temp_abstime % 1000000) * 1000;
+# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+
 	return ts_to_abstime(&ts);
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 }
@@ -449,6 +477,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	return system_clock_gettime(clk_id, tp);
+
 }
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)