compensate airspeed setpoint based on weight ratio

Signed-off-by: RomanBapst <bapstroman@gmail.com>

.vscode/settings.json

@@ -127,6 +127,5 @@
         "terminal.integrated.scrollback": 15000,
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
-        },
-        "ros.distro": "humble"
+        }
 }

ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow

@@ -31,7 +31,6 @@ param set-default PWM_MAIN_FUNC4 104
 
 # EKF2
 param set-default EKF2_GPS_CTRL 0
-param set-default EKF2_HGT_REF 0
 param set-default EKF2_EVP_NOISE 0.05
 param set-default EKF2_EVA_NOISE 0.05
 param set-default EKF2_OF_CTRL 1
@@ -40,9 +39,6 @@ param set-default EKF2_OF_CTRL 1
 param set-default LPE_FUSION 242
 param set-default LPE_FAKE_ORIGIN 1
 
-# Commander
-# param set-default COM_HOME_EN 0 # Disable setting of home position
-
 param set-default MPC_ALT_MODE 2
 
 param set-default SENS_FLOW_ROT 6

ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult

@@ -7,7 +7,6 @@
 
 param set-default FW_LAUN_DETCN_ON 1
 param set-default FW_THR_IDLE 0.1 # needs to be running before throw as that's how gazebo detects arming
-param set-default FW_LAUN_AC_THLD 10
 
 param set-default FW_LND_ANG 8
 

ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie

@@ -43,6 +43,7 @@ param set-default MC_ROLLRATE_P 0.07
 param set-default MC_YAW_P 3
 
 param set-default MPC_THR_HOVER 0.7
+param set-default MPC_THR_MAX 1
 param set-default MPC_Z_P 1.5
 param set-default MPC_Z_VEL_P_ACC 8
 param set-default MPC_Z_VEL_I_ACC 6

ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21

@@ -42,6 +42,7 @@ param set-default MC_ROLLRATE_P 0.07
 param set-default MC_YAW_P 3
 
 param set-default MPC_THR_HOVER 0.7
+param set-default MPC_THR_MAX 1
 param set-default MPC_Z_P 1.5
 param set-default MPC_Z_VEL_P_ACC 8
 param set-default MPC_Z_VEL_I_ACC 6

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -34,7 +34,6 @@ param set-default EKF2_REQ_EPV 10
 param set-default EKF2_REQ_HDRIFT 0.5
 param set-default EKF2_REQ_SACC 1
 param set-default EKF2_REQ_VDRIFT 1.0
-param set-default EKF2_RNG_QLTY_T 3.0
 
 param set-default RTL_TYPE 1
 param set-default RTL_RETURN_ALT 100

boards/bitcraze/crazyflie21/default.px4board

@@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 # CONFIG_EKF2_GNSS_YAW is not set
-# CONFIG_EKF2_MAGNETOMETER is not set
 # CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
@@ -34,7 +33,6 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MTD=y

boards/cubepilot/cubeorange/default.px4board

@@ -19,7 +19,6 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
-CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -50,6 +49,7 @@ 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

boards/cubepilot/cubeorangeplus/default.px4board

@@ -20,7 +20,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
-CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/holybro/kakutef7/default.px4board

@@ -17,19 +17,11 @@ CONFIG_DRIVERS_OSD_ATXXXX=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_AUXVEL is not set
-# CONFIG_EKF2_BARO_COMPENSATION is not set
-# CONFIG_EKF2_DRAG_FUSION is not set
-# CONFIG_EKF2_EXTERNAL_VISION is not set
-# CONFIG_EKF2_GNSS_YAW is not set
-# CONFIG_EKF2_MAGNETOMETER is not set
-# CONFIG_EKF2_RANGE_FINDER is not set
-# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
@@ -41,9 +33,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
-# CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_TOP=y

boards/holybro/kakutef7/init/rc.board_defaults

@@ -12,10 +12,12 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050
 
-param set-default SYS_HAS_MAG 0
+# use the Q attitude estimator, it works w/o mag or GPS.
+param set-default SYS_MC_EST_GROUP 3
+param set-default ATT_W_ACC 0.4
+param set-default ATT_W_GYRO_BIAS 0
 
-# enable gravity fusion
-param set-default EKF2_IMU_CONTROL 7
+param set-default SYS_HAS_MAG 0
 
 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090

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

@@ -102,8 +102,13 @@ if ver hwtypecmp V6X002001
 then
 	rm3100 -I -b 4 start
 else
-	# Internal magnetometer on I2C
-	bmm150 -I -R 0 start
+	if ver hwtypecmp V6X009010 V6X010010
+	then
+		# Internal magnetometer on I2C
+		bmm150 -I -R 0 start
+	else
+		bmm150 -I -R 6 start
+	fi
 fi
 
 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)

cmake/kconfig.cmake

@@ -216,17 +216,15 @@ if(EXISTS ${BOARD_DEFCONFIG})
 
 	endforeach()
 
-	if (CONFIG_BOARD_PROTECTED)
-	    # Put every module not in userspace also to kernel list
-	    foreach(modpath ${config_module_list})
+	# Put every module not in userspace also to kernel list
+	foreach(modpath ${config_module_list})
 		get_filename_component(module ${modpath} NAME)
 		list(FIND config_user_list ${module} _index)
 
 		if (${_index} EQUAL -1)
 			list(APPEND config_kernel_list ${modpath})
 		endif()
-	    endforeach()
-	endif()
+	endforeach()
 
 	if(PLATFORM)
 		# set OS, and append specific platform module path

integrationtests/python_src/px4_it/mavros/mission_test.py

@@ -303,12 +303,9 @@ class MavrosMissionTest(MavrosTestCommon):
         self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res))
         self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res))
         self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res))
-
         self.assertTrue(res['roll_error_std'] < 5.0, str(res))
         self.assertTrue(res['pitch_error_std'] < 5.0, str(res))
-
-        # TODO: fix by excluding initial heading init and reset preflight
-        self.assertTrue(res['yaw_error_std'] < 10.0, str(res))
+        self.assertTrue(res['yaw_error_std'] < 5.0, str(res))
 
 
 if __name__ == '__main__':

msg/ActuatorTest.msg

@@ -18,4 +18,4 @@ float32 value					# range: [-1, 1], where 1 means maximum positive output,
                    				# and NaN maps to disarmed (stop the motors)
 uint32 timeout_ms				# timeout in ms after which to exit test mode (if 0, do not time out)
 
-uint8 ORB_QUEUE_LENGTH = 16                     # >= MAX_NUM_MOTORS to support code in esc_calibration
+uint8 ORB_QUEUE_LENGTH = 12                     # same as MAX_NUM_MOTORS to support code in esc_calibration

msg/EstimatorAidSource1d.msg

@@ -22,4 +22,3 @@ bool fused                   # true if the sample was successfully fused
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
 # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
-# TOPICS estimator_aid_src_terrain_range_finder

platforms/common/uORB/uORBDeviceNode.cpp

@@ -188,10 +188,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 			if (nullptr == _data) {
 				const size_t data_size = _meta->o_size * _queue_size;
 				_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
-
-				if (_data) {
-					memset(_data, 0, data_size);
-				}
+				memset(_data, 0, data_size);
 			}
 
 			unlock();

src/drivers/camera_capture/camera_capture.cpp

@@ -226,9 +226,7 @@ void
 CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
 				  uint32_t overflow)
 {
-	if (camera_capture::g_camera_capture) {
-		camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow);
-	}
+	camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow);
 }
 
 void
@@ -361,11 +359,6 @@ CameraCapture::stop()
 
 	work_cancel(HPWORK, &_work_publisher);
 
-	if (_capture_channel >= 0) {
-		up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr);
-	}
-
-
 	if (camera_capture::g_camera_capture != nullptr) {
 		delete (camera_capture::g_camera_capture);
 	}

src/drivers/differential_pressure/asp5033/ASP5033.cpp

@@ -222,7 +222,7 @@ int ASP5033::collect()
 
 	// k is a shift based on the pressure range of the device. See
 	// table in the datasheet
-	constexpr uint8_t k = 7;
+	constexpr uint8_t k = 8;
 	constexpr float press_scale = 1.0f / (1U << k); //= 1.0f / (1U << k);
 	press_sum += press * press_scale;
 	press_count++;

src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp

@@ -33,7 +33,6 @@
 
 /* Include Files */
 #include "AFBRS50.hpp"
-#include "argus_hal_test.h"
 
 #include <lib/drivers/device/Device.hpp>
 
@@ -43,6 +42,9 @@
 /*! Define the SPI baud rate (to be used in the SPI module). */
 #define SPI_BAUD_RATE 5000000
 
+#define LONG_RANGE_MODE_HZ 25
+#define SHORT_RANGE_MODE_HZ 50
+
 #include "s2pi.h"
 #include "timer.h"
 #include "argus_hal_test.h"
@@ -50,7 +52,6 @@
 AFBRS50 *g_dev{nullptr};
 
 AFBRS50::AFBRS50(uint8_t device_orientation):
-	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
 	_px4_rangefinder(0, device_orientation)
 {
@@ -60,7 +61,6 @@ AFBRS50::AFBRS50(uint8_t device_orientation):
 	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_AFBRS50;
 
 	_px4_rangefinder.set_device_id(device_id.devid);
-	_px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
 }
 
 AFBRS50::~AFBRS50()
@@ -70,12 +70,12 @@ AFBRS50::~AFBRS50()
 	perf_free(_sample_perf);
 }
 
-status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd)
+status_t AFBRS50::measurement_ready_callback(status_t status, void *data)
 {
 	if (!up_interrupt_context()) {
 		if (status == STATUS_OK) {
 			if (g_dev) {
-				g_dev->ProcessMeasurement(hnd);
+				g_dev->ProcessMeasurement(data);
 			}
 
 		} else {
@@ -86,33 +86,35 @@ status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd)
 	return status;
 }
 
-void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd)
+void AFBRS50::ProcessMeasurement(void *data)
 {
-	perf_count(_sample_perf);
+	if (data != nullptr) {
+		perf_count(_sample_perf);
 
-	argus_results_t res{};
-	status_t evaluate_status = Argus_EvaluateData(hnd, &res);
+		argus_results_t res{};
+		status_t evaluate_status = Argus_EvaluateData(_hnd, &res, data);
 
-	if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) {
-		uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000);
-		float result_m = static_cast<float>(result_mm) / 1000.f;
-		int8_t quality = res.Bin.SignalQuality;
+		if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) {
+			uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000);
+			float result_m = static_cast<float>(result_mm) / 1000.f;
+			int8_t quality = res.Bin.SignalQuality;
 
-		// Signal quality indicates 100% for good signals, 50% and lower for weak signals.
-		// 1% is an errored signal (not reliable). Signal Quality of 0% is unknown.
-		if (quality == 1) {
-			quality = 0;
+			// Signal quality indicates 100% for good signals, 50% and lower for weak signals.
+			// 1% is an errored signal (not reliable). Signal Quality of 0% is unknown.
+			if (quality == 1) {
+				quality = 0;
+			}
+
+			// distance quality check
+			if (result_m > _max_distance) {
+				result_m = 0.0;
+				quality = 0;
+			}
+
+			_current_distance = result_m;
+			_current_quality = quality;
+			_px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality);
 		}
-
-		// distance quality check
-		if (result_m > _max_distance) {
-			result_m = 0.0;
-			quality = 0;
-		}
-
-		_current_distance = result_m;
-		_current_quality = quality;
-		_px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality);
 	}
 }
 
@@ -135,37 +137,7 @@ int AFBRS50::init()
 	// Initialize the S2PI hardware required by the API.
 	S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE);
 
-	int32_t mode_param = _p_sens_afbr_mode.get();
-
-	if (mode_param < 0 || mode_param > 3) {
-		PX4_ERR("Invalid mode parameter: %li", mode_param);
-		return PX4_ERROR;
-	}
-
-	argus_mode_t mode = ARGUS_MODE_LONG_RANGE;
-
-	switch (mode_param) {
-	case 0:
-		mode = ARGUS_MODE_SHORT_RANGE;
-		break;
-
-	case 1:
-		mode = ARGUS_MODE_LONG_RANGE;
-		break;
-
-	case 2:
-		mode = ARGUS_MODE_HIGH_SPEED_SHORT_RANGE;
-		break;
-
-	case 3:
-		mode = ARGUS_MODE_HIGH_SPEED_LONG_RANGE;
-		break;
-
-	default:
-		break;
-	}
-
-	status_t status = Argus_InitMode(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS, mode);
+	status_t status = Argus_Init(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS);
 
 	if (status == STATUS_OK) {
 		uint32_t id = Argus_GetChipID(_hnd);
@@ -176,6 +148,7 @@ int AFBRS50::init()
 		PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c);
 
 		argus_module_version_t mv = Argus_GetModuleVersion(_hnd);
+		argus_laser_type_t lt = Argus_GetLaserType(_hnd);
 
 		switch (mv) {
 		case AFBR_S50MV85G_V1:
@@ -195,20 +168,19 @@ int AFBRS50::init()
 
 		case AFBR_S50LV85D_V1:
 			_min_distance = 0.08f;
-			_max_distance = 30.f;
-			_px4_rangefinder.set_min_distance(_min_distance);
-			_px4_rangefinder.set_max_distance(_max_distance);
-			_px4_rangefinder.set_fov(math::radians(6.f));
-			PX4_INFO_RAW("AFBR-S50LV85D\n");
-			break;
 
-		case AFBR_S50LX85D_V1:
-			_min_distance = 0.08f;
-			_max_distance = 50.f;
+			if (lt == LASER_H_V2X) {
+				_max_distance = 50.f;
+				PX4_INFO_RAW("AFBR-S50LX85D (v2)\n");
+
+			} else {
+				_max_distance = 30.f;
+				PX4_INFO_RAW("AFBR-S50LV85D (v1)\n");
+			}
+
 			_px4_rangefinder.set_min_distance(_min_distance);
 			_px4_rangefinder.set_max_distance(_max_distance);
 			_px4_rangefinder.set_fov(math::radians(6.f));
-			PX4_INFO_RAW("AFBR-S50LX85D\n");
 			break;
 
 		case AFBR_S50MV68B_V1:
@@ -251,9 +223,6 @@ int AFBRS50::init()
 
 		ScheduleDelayed(_measure_interval);
 		return PX4_OK;
-
-	} else {
-		PX4_ERR("Argus_InitMode failed: %ld", status);
 	}
 
 	return PX4_ERROR;
@@ -261,15 +230,6 @@ int AFBRS50::init()
 
 void AFBRS50::Run()
 {
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
-	}
-
 	switch (_state) {
 	case STATE::TEST: {
 			if (_testing) {
@@ -283,8 +243,7 @@ void AFBRS50::Run()
 		break;
 
 	case STATE::CONFIGURE: {
-			_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
-			status_t status = set_rate(_current_rate);
+			status_t status = set_rate(SHORT_RANGE_MODE_HZ);
 
 			if (status != STATUS_OK) {
 				PX4_ERR("CONFIGURE status not okay: %i", (int)status);
@@ -292,18 +251,24 @@ void AFBRS50::Run()
 				ScheduleNow();
 			}
 
-			status = Argus_SetConfigurationDFMMode(_hnd, DFM_MODE_8X);
+			status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_B, DFM_MODE_8X);
 
 			if (status != STATUS_OK) {
 				PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status);
-				_state = STATE::STOP;
-				ScheduleNow();
 			}
 
-			status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false);
+			status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_A, DFM_MODE_8X);
 
 			if (status != STATUS_OK) {
-				PX4_ERR("Argus_SetConfigurationSmartPowerSaveEnabled status not okay: %i", (int)status);
+				PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status);
+			}
+
+			// start in short range mode
+			_mode = ARGUS_MODE_B;
+			set_mode(_mode);
+
+			if (status != STATUS_OK) {
+				PX4_ERR("CONFIGURE status not okay: %i", (int)status);
 				ScheduleNow();
 
 			} else {
@@ -323,7 +288,7 @@ void AFBRS50::Run()
 				}
 			}
 
-			Evaluate_rate();
+			UpdateMode();
 		}
 		break;
 
@@ -341,41 +306,49 @@ void AFBRS50::Run()
 	ScheduleDelayed(_measure_interval);
 }
 
-void AFBRS50::Evaluate_rate()
+void AFBRS50::UpdateMode()
 {
-	// only update mode if _current_distance is a valid measurement and if the last rate switch was more than 1 second ago
-	if ((_current_distance > 0) && (_current_quality > 0) && ((hrt_absolute_time() - _last_rate_switch) > 1_s)) {
+	// only update mode if _current_distance is a valid measurement
+	if ((_current_distance > 0) && (_current_quality > 0)) {
 
-		status_t status = STATUS_OK;
-
-		if ((_current_distance >= (_p_sens_afbr_thresh.get() + _p_sens_afbr_hyster.get()))
-		    && (_current_rate != (uint32_t)_p_sens_afbr_l_rate.get())) {
-
-			_current_rate = (uint32_t)_p_sens_afbr_l_rate.get();
-			status = set_rate(_current_rate);
+		if ((_current_distance >= _long_range_threshold) && (_mode != ARGUS_MODE_A)) {
+			// change to long range mode
+			argus_mode_t mode = ARGUS_MODE_A;
+			status_t status = set_mode(mode);
 
 			if (status != STATUS_OK) {
-				PX4_ERR("set_rate status not okay: %i", (int)status);
-
-			} else {
-				PX4_INFO("switched to long range rate: %i", (int)_current_rate);
-				_last_rate_switch = hrt_absolute_time();
+				PX4_ERR("set_mode status not okay: %i", (int)status);
 			}
 
-		} else if ((_current_distance <= (_p_sens_afbr_thresh.get() - _p_sens_afbr_hyster.get()))
-			   && (_current_rate != (uint32_t)_p_sens_afbr_s_rate.get())) {
-
-			_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
-			status = set_rate(_current_rate);
+			status = set_rate(LONG_RANGE_MODE_HZ);
 
 			if (status != STATUS_OK) {
 				PX4_ERR("set_rate status not okay: %i", (int)status);
+			}
 
-			} else {
-				PX4_INFO("switched to short range rate: %i", (int)_current_rate);
-				_last_rate_switch = hrt_absolute_time();
+			status = set_rate(LONG_RANGE_MODE_HZ);
+
+			if (status != STATUS_OK) {
+				PX4_ERR("set_rate status not okay: %i", (int)status);
+			}
+
+		} else if ((_current_distance <= _short_range_threshold) && (_mode != ARGUS_MODE_B)) {
+			// change to short range mode
+			argus_mode_t mode = ARGUS_MODE_B;
+			status_t status = set_mode(mode);
+
+			if (status != STATUS_OK) {
+				PX4_ERR("set_mode status not okay: %i", (int)status);
+			}
+
+			status = set_rate(SHORT_RANGE_MODE_HZ);
+
+			if (status != STATUS_OK) {
+				PX4_ERR("set_rate status not okay: %i", (int)status);
 			}
 		}
+
+		ScheduleDelayed(1000_ms); // don't switch again for at least 1 second
 	}
 }
 
@@ -400,6 +373,33 @@ void AFBRS50::print_info()
 	get_info();
 }
 
+status_t AFBRS50::set_mode(argus_mode_t mode)
+{
+	while (Argus_GetStatus(_hnd) != STATUS_IDLE) {
+		px4_usleep(1_ms);
+	}
+
+	status_t status = Argus_SetConfigurationMeasurementMode(_hnd, mode);
+
+	if (status != STATUS_OK) {
+		PX4_ERR("Argus_SetConfigurationMeasurementMode status not okay: %i", (int)status);
+		return status;
+	}
+
+	argus_mode_t current_mode;
+	status = Argus_GetConfigurationMeasurementMode(_hnd, &current_mode);
+
+	if (status != STATUS_OK) {
+		PX4_ERR("Argus_GetConfigurationMeasurementMode status not okay: %i", (int)status);
+		return status;
+
+	} else {
+		_mode = current_mode;
+	}
+
+	return status;
+}
+
 status_t AFBRS50::set_rate(uint32_t rate_hz)
 {
 	while (Argus_GetStatus(_hnd) != STATUS_IDLE) {
@@ -429,10 +429,13 @@ status_t AFBRS50::set_rate(uint32_t rate_hz)
 
 void AFBRS50::get_info()
 {
+	argus_mode_t current_mode;
 	argus_dfm_mode_t dfm_mode;
-	Argus_GetConfigurationDFMMode(_hnd, &dfm_mode);
+	Argus_GetConfigurationMeasurementMode(_hnd, &current_mode);
+	Argus_GetConfigurationDFMMode(_hnd, current_mode, &dfm_mode);
 
 	PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance);
+	PX4_INFO_RAW("mode: %d\n", current_mode);
 	PX4_INFO_RAW("dfm mode: %d\n", dfm_mode);
 	PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measure_interval));
 }

src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp

@@ -44,16 +44,13 @@
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
 #include <lib/perf/perf_counter.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/parameter_update.h>
 
 using namespace time_literals;
 
-class AFBRS50 : public ModuleParams, public px4::ScheduledWorkItem
+class AFBRS50 : public px4::ScheduledWorkItem
 {
 public:
 	AFBRS50(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
@@ -78,16 +75,18 @@ public:
 private:
 	void Run() override;
 
-	void Evaluate_rate();
+	void UpdateMode();
 
-	void ProcessMeasurement(argus_hnd_t *hnd);
+	void ProcessMeasurement(void *data);
 
-	static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd);
+	static status_t measurement_ready_callback(status_t status, void *data);
 
 	void get_info();
+	status_t set_mode(argus_mode_t mode);
 	status_t set_rate(uint32_t rate_hz);
 
 	argus_hnd_t *_hnd{nullptr};
+	argus_mode_t _mode{ARGUS_MODE_B}; // Short-Range
 
 	enum class STATE : uint8_t {
 		TEST,
@@ -99,24 +98,14 @@ private:
 	PX4Rangefinder _px4_rangefinder;
 
 	hrt_abstime _measurement_time{0};
-	hrt_abstime _last_rate_switch{0};
 
 	perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")};
 
 	uint32_t _measure_interval{1000000 / 50}; // 50Hz
 	float _current_distance{0};
 	int8_t _current_quality{0};
+	const float _short_range_threshold = 4.0; //meters
+	const float _long_range_threshold = 6.0; //meters
 	float _max_distance;
 	float _min_distance;
-	uint32_t _current_rate{0};
-
-	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::SENS_AFBR_MODE>)   _p_sens_afbr_mode,
-		(ParamInt<px4::params::SENS_AFBR_S_RATE>)  _p_sens_afbr_s_rate,
-		(ParamInt<px4::params::SENS_AFBR_L_RATE>)  _p_sens_afbr_l_rate,
-		(ParamInt<px4::params::SENS_AFBR_THRESH>) _p_sens_afbr_thresh,
-		(ParamInt<px4::params::SENS_AFBR_HYSTER>)    _p_sens_afbr_hyster
-	);
 };

src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c

@@ -85,8 +85,6 @@ static int gpio_falling_edge(int irq, void *context, void *arg)
 
 status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps)
 {
-	(void)defaultSlave;
-
 	px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_CS);
 
 	s2pi_.spidev = px4_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS);
@@ -109,24 +107,11 @@ status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps)
 * - #STATUS_BUSY: An SPI transfer is in progress.
 * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode.
 *****************************************************************************/
-status_t S2PI_GetStatus(s2pi_slave_t slave)
+status_t S2PI_GetStatus(void)
 {
-	(void)slave;
-
 	return s2pi_.Status;
 }
 
-status_t S2PI_TryGetMutex(s2pi_slave_t slave)
-{
-	(void) slave;
-	return STATUS_OK;
-}
-
-void S2PI_ReleaseMutex(s2pi_slave_t slave)
-{
-	(void) slave;
-}
-
 /*!***************************************************************************
 * @brief Sets the SPI baud rate in bps.
 * @param baudRate_Bps The default SPI baud rate in bauds-per-second.
@@ -150,10 +135,8 @@ status_t S2PI_SetBaudRate(uint32_t baudRate_Bps)
 * switch back to ordinary SPI functionality.
 * @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
 *****************************************************************************/
-status_t S2PI_CaptureGpioControl(s2pi_slave_t slave)
+status_t S2PI_CaptureGpioControl(void)
 {
-	(void)slave;
-
 	/* Check if something is ongoing. */
 	IRQ_LOCK();
 	status_t status = s2pi_.Status;
@@ -182,10 +165,8 @@ status_t S2PI_CaptureGpioControl(s2pi_slave_t slave)
 * the #S2PI_CaptureGpioControl function.
 * @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
 *****************************************************************************/
-status_t S2PI_ReleaseGpioControl(s2pi_slave_t slave)
+status_t S2PI_ReleaseGpioControl(void)
 {
-	(void)slave;
-
 	/* Check if something is ongoing. */
 	IRQ_LOCK();
 	status_t status = s2pi_.Status;
@@ -221,8 +202,6 @@ status_t S2PI_ReleaseGpioControl(s2pi_slave_t slave)
 *****************************************************************************/
 status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value)
 {
-	(void)slave;
-
 	/* Check if pin is valid. */
 	if (pin > S2PI_IRQ || value > 1) {
 		return ERROR_INVALID_ARGUMENT;
@@ -249,8 +228,6 @@ status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value)
 *****************************************************************************/
 status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value)
 {
-	(void)slave;
-
 	/* Check if pin is valid. */
 	if (pin > S2PI_IRQ || !value) {
 		return ERROR_INVALID_ARGUMENT;
@@ -278,8 +255,6 @@ status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value)
 *****************************************************************************/
 status_t S2PI_CycleCsPin(s2pi_slave_t slave)
 {
-	(void)slave;
-
 	/* Check the driver status. */
 	IRQ_LOCK();
 	status_t status = s2pi_.Status;
@@ -397,10 +372,8 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8
 * invoked with the #ERROR_ABORTED error byte.
 * @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
 *****************************************************************************/
-status_t S2PI_Abort(s2pi_slave_t slave)
+status_t S2PI_Abort(void)
 {
-	(void)slave;
-
 	status_t status = s2pi_.Status;
 
 	/* Check if something is ongoing. */
@@ -432,8 +405,6 @@ status_t S2PI_Abort(s2pi_slave_t slave)
 *****************************************************************************/
 status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, void *callbackData)
 {
-	(void)slave;
-
 	s2pi_.IrqCallback = callback;
 	s2pi_.IrqCallbackData = callbackData;
 
@@ -459,7 +430,5 @@ status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, v
 *****************************************************************************/
 uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave)
 {
-	(void)slave;
-
 	return px4_arch_gpioread(s2pi_.GPIOs[S2PI_IRQ]);
 }

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h

@@ -1,12 +1,12 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Defines the dynamic configuration adaption (DCA) setup parameters
- *              and data structure.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Defines the dynamic configuration adaption (DCA) setup parameters
+ * 				and data structure.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -37,37 +37,34 @@
 
 #ifndef ARGUS_DCA_H
 #define ARGUS_DCA_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_dca Dynamic Configuration Adaption
- * @ingroup     argus_api
+ * @defgroup 	argusdca Dynamic Configuration Adaption
+ * @ingroup		argusapi
  *
- * @brief       Dynamic Configuration Adaption (DCA) parameter definitions and API functions.
+ * @brief		Dynamic Configuration Adaption (DCA) parameter definitions and API functions.
  *
- * @details     The DCA contains an algorithms that detect ambient conditions
- *              and adopt the device configuration to the changing parameters
- *              dynamically while operating the sensor. This is achieved by
- *              rating the currently received signal quality and changing the
- *              device configuration accordingly to the gathered information
- *              from the current measurement frame results before the next
- *              integration cycle starts.
+ * @details		The DCA contains an algorithms that detect ambient conditions
+ * 				and adopt the device configuration to the changing parameters
+ * 				dynamically while operating the sensor. This is achieved by
+ * 				rating the currently received signal quality and changing the
+ * 				device configuration accordingly to the gathered information
+ * 				from the current measurement frame results before the next
+ * 				integration cycle starts.
  *
- *              The DCA consists of the following features:
- *              - Static or Dynamic mode. The first is utilizing the nominal
- *                values while the latter is dynamically adopting between min.
- *                and max. value and starting from the nominal values.
- *              - Analog Integration Depth Adaption (from multiple patterns down to single pulses)
- *              - Optical Output Power Adaption
- *              - Pixel Input Gain Adaption (w/ ambient light rejection)
- *              - ADC Sensitivity (i.e. ADC Range) Adaption
- *              - Power Saving Ratio (to decrease the average output power and thus the current consumption)
- *              - All that features are heeding the Laser Safety limits.
- *              .
+ * 				The DCA consists of the following features:
+ * 			 	- Static or Dynamic mode. The first is utilizing the nominal
+ * 			 	  values while the latter is dynamically adopting between min.
+ * 			 	  and max. value and starting from the nominal values.
+ * 			 	- Analog Integration Depth Adaption (from multiple patterns down to single pulses)
+ * 			 	- Optical Output Power Adaption
+ * 			 	- Pixel Input Gain Adaption (w/ ambient light rejection)
+ * 			 	- ADC Sensitivity (i.e. ADC Range) Adaption
+ * 			 	- Power Saving Ratio (to decrease the average output power and thus the current consumption)
+ * 			 	- All that features are heeding the Laser Safety limits.
+ * 				.
  *
- * @addtogroup  argus_dca
+ * @addtogroup 	argusdca
  * @{
  *****************************************************************************/
 
@@ -76,26 +73,39 @@ extern "C" {
 
 
 /*! The minimum amplitude threshold value. */
-#define ARGUS_CFG_DCA_ATH_MIN       (1U << 6U)
+#define ARGUS_CFG_DCA_ATH_MIN		(1U << 6U)
 
 /*! The maximum amplitude threshold value. */
-#define ARGUS_CFG_DCA_ATH_MAX       (0xFFFFU)
+#define ARGUS_CFG_DCA_ATH_MAX		(0xFFFFU)
 
 
 /*! The minimum saturated pixel threshold value. */
-#define ARGUS_CFG_DCA_PXTH_MIN      (1U)
+#define ARGUS_CFG_DCA_PXTH_MIN		(1U)
 
 /*! The maximum saturated pixel threshold value. */
-#define ARGUS_CFG_DCA_PXTH_MAX      (33U)
+#define ARGUS_CFG_DCA_PXTH_MAX		(33U)
 
 
 /*! The maximum analog integration depth in UQ10.6 format,
  * i.e. the maximum pattern count per sample. */
-#define ARGUS_CFG_DCA_DEPTH_MAX     ((uq10_6_t)(0xFFC0U))
+#define ARGUS_CFG_DCA_DEPTH_MAX 	((uq10_6_t)(ADS_SEQCT_N_MASK << (6U - ADS_SEQCT_N_SHIFT)))
 
 /*! The minimum analog integration depth in UQ10.6 format,
  *  i.e. the minimum pattern count per sample. */
-#define ARGUS_CFG_DCA_DEPTH_MIN     ((uq10_6_t)(1U)) // 1/64, i.e. 1/2 nibble
+#define ARGUS_CFG_DCA_DEPTH_MIN 	((uq10_6_t)(1U)) // 1/64, i.e. 1/2 nibble
+
+
+/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */
+#define ARGUS_CFG_DCA_POWER_MAX_LSB (ADS_LASET_VCSEL_HC1_MASK >> ADS_LASET_VCSEL_HC1_SHIFT)
+
+/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */
+#define ARGUS_CFG_DCA_POWER_MIN_LSB (1)
+
+/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */
+#define ARGUS_CFG_DCA_POWER_MAX		(ADS0032_HIGH_CURRENT_LSB2MA(ARGUS_CFG_DCA_POWER_MAX_LSB + 1))
+
+/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */
+#define ARGUS_CFG_DCA_POWER_MIN 	(1)
 
 
 /*! The dynamic configuration algorithm Pixel Input Gain stage count. */
@@ -129,9 +139,9 @@ extern "C" {
 
 
 /*!***************************************************************************
- * @brief   The dynamic configuration algorithm enable flags.
+ * @brief	The dynamic configuration algorithm enable flags.
  *****************************************************************************/
-typedef enum argus_dca_enable_t {
+typedef enum {
 	/*! @internal
 	 *
 	 *  DCA is disabled and will be completely skipped.
@@ -150,9 +160,9 @@ typedef enum argus_dca_enable_t {
 } argus_dca_enable_t;
 
 /*!***************************************************************************
- * @brief   The DCA amplitude evaluation method.
+ * @brief	The DCA amplitude evaluation method.
  *****************************************************************************/
-typedef enum argus_dca_amplitude_mode_t {
+typedef enum {
 	/*! Evaluate the DCA amplitude as the maximum of all valid amplitudes. */
 	DCA_AMPLITUDE_MAX = 1U,
 
@@ -162,9 +172,9 @@ typedef enum argus_dca_amplitude_mode_t {
 } argus_dca_amplitude_mode_t;
 
 /*!***************************************************************************
- * @brief   The dynamic configuration algorithm Optical Output Power stages enumerator.
+ * @brief	The dynamic configuration algorithm Optical Output Power stages enumerator.
  *****************************************************************************/
-typedef enum argus_dca_power_t {
+typedef enum {
 	/*! Use low output power stage. */
 	DCA_POWER_LOW = 0,
 
@@ -177,9 +187,9 @@ typedef enum argus_dca_power_t {
 } argus_dca_power_t;
 
 /*!***************************************************************************
- * @brief   The dynamic configuration algorithm Pixel Input Gain stages enumerator.
+ * @brief	The dynamic configuration algorithm Pixel Input Gain stages enumerator.
  *****************************************************************************/
-typedef enum argus_dca_gain_t {
+typedef enum {
 	/*! Low gain stage. */
 	DCA_GAIN_LOW = 0,
 
@@ -196,113 +206,113 @@ typedef enum argus_dca_gain_t {
 
 
 /*!***************************************************************************
- * @brief   State flags for the current frame.
- * @details State flags determine the current state of the measurement frame:
- *          - [0]: #ARGUS_STATE_XTALK_MONITOR_ACTIVE
- *          - [1]: #ARGUS_STATE_DUAL_FREQ_MODE
- *          - [2]: #ARGUS_STATE_MEASUREMENT_FREQ
- *          - [3]: #ARGUS_STATE_DEBUG_MODE
- *          - [4]: #ARGUS_STATE_WEAK_SIGNAL
- *          - [5]: #ARGUS_STATE_BGL_WARNING
- *          - [6]: #ARGUS_STATE_BGL_ERROR
- *          - [7]: #ARGUS_STATE_PLL_LOCKED
- *          - [8]: #ARGUS_STATE_LASER_WARNING
- *          - [9]: #ARGUS_STATE_LASER_ERROR
- *          - [10]: #ARGUS_STATE_HAS_DATA
- *          - [11]: #ARGUS_STATE_HAS_AUX_DATA
- *          - [12]: #ARGUS_STATE_DCA_MAX
- *          - [13]: DCA Power Stage
- *          - [14-15]: DCA Gain Stages
- *          .
+ * @brief	State flags for the current frame.
+ * @details	State flags determine the current state of the measurement frame:
+ * 			- [0]: #ARGUS_STATE_MEASUREMENT_MODE
+ * 			- [1]: #ARGUS_STATE_DUAL_FREQ_MODE
+ * 			- [2]: #ARGUS_STATE_MEASUREMENT_FREQ
+ * 			- [3]: #ARGUS_STATE_DEBUG_MODE
+ * 			- [4]: #ARGUS_STATE_WEAK_SIGNAL
+ * 			- [5]: #ARGUS_STATE_BGL_WARNING
+ * 			- [6]: #ARGUS_STATE_BGL_ERROR
+ * 			- [7]: #ARGUS_STATE_PLL_LOCKED
+ * 			- [8]: #ARGUS_STATE_LASER_WARNING
+ * 			- [9]: #ARGUS_STATE_LASER_ERROR
+ * 			- [10]: #ARGUS_STATE_HAS_DATA
+ * 			- [11]: #ARGUS_STATE_HAS_AUX_DATA
+ * 			- [12]: #ARGUS_STATE_DCA_MAX
+ * 			- [13]: DCA Power Stage
+ * 			- [14-15]: DCA Gain Stages
+ * 			.
  *****************************************************************************/
-typedef enum argus_state_t {
+typedef enum {
 	/*! No state flag set. */
 	ARGUS_STATE_NONE = 0,
 
-	/*! 0x0001: Crosstalk Monitor is enabled and updating.
-	 *          - 0: Inactive: crosstalk monitor values are not updated,
-	 *          - 1: Active: crosstalk monitor values are updated. */
-	ARGUS_STATE_XTALK_MONITOR_ACTIVE = 1U << 0U,
+	/*! 0x0001: Measurement Mode.
+	 *  		- 0: Mode A: Long Range / Medium Precision
+	 *  		- 1: Mode B: Short Range / High Precision */
+	ARGUS_STATE_MEASUREMENT_MODE = 1U << 0U,
 
 	/*! 0x0002: Dual Frequency Mode Enabled.
-	 *          - 0: Disabled: measurements with base frequency,
-	 *          - 1: Enabled: measurement with detuned frequency. */
+	 *  		- 0: Disabled: measurements with base frequency,
+	 *  		- 1: Enabled: measurement with detuned frequency. */
 	ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U,
 
 	/*! 0x0004: Measurement Frequency for Dual Frequency Mode
 	 *          (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set).
-	 *          - 0: A-Frame w/ detuned frequency,
-	 *          - 1: B-Frame w/ detuned frequency */
+	 *  		- 0: A-Frame w/ detuned frequency,
+	 *  		- 1: B-Frame w/ detuned frequency */
 	ARGUS_STATE_MEASUREMENT_FREQ = 1U << 2U,
 
 	/*! 0x0008: Debug Mode. If set, the range value of erroneous pixels
-	 *          are not cleared or reset.
-	 *          - 0: Disabled (default).
-	 *          - 1: Enabled. */
+	 * 		  	are not cleared or reset.
+	 * 			- 0: Disabled (default).
+	 *  		- 1: Enabled. */
 	ARGUS_STATE_DEBUG_MODE = 1U << 3U,
 
 	/*! 0x0010: Weak Signal Flag.
-	 *          Set whenever the Pixel Binning Algorithm is detecting a
-	 *          weak signal, i.e. if the amplitude dies not reach its
-	 *          (absolute) threshold. If the Golden Pixel is enabled,
-	 *          this also indicates that the Pixel Binning Algorithm
-	 *          falls back to the Golden Pixel.
-	 *          - 0: Normal Signal.
-	 *          - 1: Weak Signal or Golden Pixel Mode. */
+	 * 			Set whenever the Pixel Binning Algorithm is detecting a
+	 * 			weak signal, i.e. if the amplitude dies not reach its
+	 * 			(absolute) threshold. If the Golden Pixel is enabled,
+	 *  	  	this also indicates that the Pixel Binning Algorithm
+	 *  	  	falls back to the Golden Pixel.
+	 * 			- 0: Normal Signal.
+	 *  		- 1: Weak Signal or Golden Pixel Mode. */
 	ARGUS_STATE_WEAK_SIGNAL = 1U << 4U,
 
 	/*! 0x0020: Background Light Warning Flag.
-	 *          Set whenever the background light is very high and the
-	 *          measurement data might be unreliable.
-	 *          - 0: No Warning: Background Light is within valid range.
-	 *          - 1: Warning: Background Light is very high. */
+	 *        	Set whenever the background light is very high and the
+	 *        	measurement data might be unreliable.
+	 *  		- 0: No Warning: Background Light is within valid range.
+	 *  		- 1: Warning: Background Light is very high. */
 	ARGUS_STATE_BGL_WARNING = 1U << 5U,
 
 	/*! 0x0040: Background Light Error Flag.
-	 *          Set whenever the background light is too high and the
-	 *          measurement data is unreliable or invalid.
-	 *          - 0: No Error: Background Light is within valid range.
-	 *          - 1: Error: Background Light is too high. */
+	 *        	Set whenever the background light is too high and the
+	 *        	measurement data is unreliable or invalid.
+	 *  		- 0: No Error: Background Light is within valid range.
+	 *  		- 1: Error: Background Light is too high. */
 	ARGUS_STATE_BGL_ERROR = 1U << 6U,
 
 	/*! 0x0080: PLL_LOCKED bit.
-	 *          - 0: PLL not locked at start of integration.
-	 *          - 1: PLL locked at start of integration. */
+	 *  		- 0: PLL not locked at start of integration.
+	 *  		- 1: PLL locked at start of integration. */
 	ARGUS_STATE_PLL_LOCKED = 1U << 7U,
 
 	/*! 0x0100: Laser Failure Warning Flag.
-	 *          Set whenever the an invalid system condition is detected.
-	 *          (i.e. DCA at max state but no amplitude on any (incl. reference)
-	 *          pixel, not amplitude but any saturated pixel).
-	 *          - 0: No Warning: Laser is operating properly.
-	 *          - 1: Warning: Invalid laser conditions detected. If the invalid
-	 *               condition stays, a laser malfunction error is raised. */
+	 *        	Set whenever the an invalid system condition is detected.
+	 *        	(i.e. DCA at max state but no amplitude on any (incl. reference)
+	 *        	pixel, not amplitude but any saturated pixel).
+	 *  		- 0: No Warning: Laser is operating properly.
+	 *  		- 1: Warning: Invalid laser conditions detected. If the invalid
+	 *  		     condition stays, a laser malfunction error is raised. */
 	ARGUS_STATE_LASER_WARNING = 1U << 8U,
 
 	/*! 0x0200: Laser Failure Error Flag.
-	 *          Set whenever a laser malfunction error is raised and the
-	 *          system is put into a safe state.
-	 *          - 0: No Error: Laser is operating properly.
-	 *          - 1: Error: Invalid laser conditions are detected for a certain
-	 *               soak time and the system is put into a safe state. */
+	 * 			Set whenever a laser malfunction error is raised and the
+	 * 			system is put into a safe state.
+	 *  		- 0: No Error: Laser is operating properly.
+	 *  		- 1: Error: Invalid laser conditions are detected for a certain
+	 *  		     soak time and the system is put into a safe state. */
 	ARGUS_STATE_LASER_ERROR = 1U << 9U,
 
 	/*! 0x0400: Set if current frame has distance measurement data available.
-	 *          - 0: No measurement data available, all values are 0 or stalled.
-	 *          - 1: Measurement data is available and correctly evaluated. */
+	 *  		- 0: No measurement data available, all values are 0 or stalled.
+	 *  		- 1: Measurement data is available and correctly evaluated. */
 	ARGUS_STATE_HAS_DATA = 1U << 10U,
 
 	/*! 0x0800: Set if current frame has auxiliary measurement data available.
-	 *          - 0: No auxiliary data available, all values are 0 or stalled.
-	 *          - 1: Auxiliary data is available and correctly evaluated. */
+	 *  		- 0: No auxiliary data available, all values are 0 or stalled.
+	 *  		- 1: Auxiliary data is available and correctly evaluated. */
 	ARGUS_STATE_HAS_AUX_DATA = 1U << 11U,
 
 	/*! 0x1000: DCA Maximum State Flag.
-	 *          Set whenever the DCA has extended all its parameters to their
-	 *          maximum values and can not increase the integration energy any
-	 *          further.
-	 *          - 0: DCA has not yet reached its maximum state.
-	 *          - 1: DCA has reached its maximum state and can not increase any further. */
+	 *  		Set whenever the DCA has extended all its parameters to their
+	 *  		maximum values and can not increase the integration energy any
+	 *  		further.
+	 *  		- 0: DCA has not yet reached its maximum state.
+	 *  		- 1: DCA has reached its maximum state and can not increase any further. */
 	ARGUS_STATE_DCA_MAX = 1U << 12U,
 
 	/*! 0x2000: DCA is in high Optical Output Power stage. */
@@ -323,20 +333,20 @@ typedef enum argus_state_t {
 } argus_state_t;
 
 /*!***************************************************************************
- * @brief   Dynamic Configuration Adaption (DCA) Parameters.
- * @details DCA contains:
- *           - Static or dynamic mode. The first is utilizing the nominal values
- *             while the latter is dynamically adopting between min. and max.
- *             value and starting form the nominal values.
- *           - Analog Integration Depth Adaption down to single pulses.
- *           - Optical Output Power Adaption
- *           - Pixel Input Gain Adaption
- *           - Digital Integration Depth Adaption
- *           - Dynamic Global Phase Shift Injection.
- *           - All that features are heeding the Laser Safety limits.
- *           .
+ * @brief	Dynamic Configuration Adaption (DCA) Parameters.
+ * @details	DCA contains:
+ * 			 - Static or dynamic mode. The first is utilizing the nominal values
+ * 			   while the latter is dynamically adopting between min. and max.
+ * 			   value and starting form the nominal values.
+ * 			 - Analog Integration Depth Adaption down to single pulses.
+ * 			 - Optical Output Power Adaption
+ * 			 - Pixel Input Gain Adaption
+ * 			 - Digital Integration Depth Adaption
+ * 			 - Dynamic Global Phase Shift Injection.
+ * 			 - All that features are heeding the Laser Safety limits.
+ * 			 .
  *****************************************************************************/
-typedef struct argus_cfg_dca_t {
+typedef struct {
 	/*! Enables the automatic configuration adaption features.
 	 *  Enables the dynamic part if #DCA_ENABLE_DYNAMIC and the static only if
 	 *  #DCA_ENABLE_STATIC. */
@@ -484,7 +494,4 @@ typedef struct argus_cfg_dca_t {
 } argus_cfg_dca_t;
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_DCA_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h

@@ -1,12 +1,12 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 hardware API.
- * @details     This file provides generic definitions belonging to all
- *              devices from the AFBR-S50 product family.
+ * @brief    	This file is part of the AFBR-S50 hardware API.
+ * @details		This file provides generic definitions belonging to all
+ * 				devices from the AFBR-S50 product family.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -37,9 +37,6 @@
 
 #ifndef ARGUS_DEF_H
 #define ARGUS_DEF_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
  * Include files
@@ -55,41 +52,36 @@ extern "C" {
 #include <string.h>
 
 /*!***************************************************************************
- * @addtogroup  argus_api
+ * @addtogroup 	argusapi
  * @{
  *****************************************************************************/
 
 /*!***************************************************************************
- * @brief   Maximum number of phases per measurement cycle.
- * @details The actual phase number is defined in the register configuration.
- *          However the software does only support a fixed value of 4 yet.
+ * @brief	Maximum number of phases per measurement cycle.
+ * @details	The actual phase number is defined in the register configuration.
+ * 			However the software does only support a fixed value of 4 yet.
  *****************************************************************************/
-#define ARGUS_PHASECOUNT 4
+#define ARGUS_PHASECOUNT 4U
 
 /*!***************************************************************************
- * @brief   The device pixel field size in x direction (long edge).
+ * @brief	The device pixel field size in x direction (long edge).
  *****************************************************************************/
-#define ARGUS_PIXELS_X  8
+#define ARGUS_PIXELS_X	8U
 
 /*!***************************************************************************
- * @brief   The device pixel field size in y direction (short edge).
+ * @brief	The device pixel field size in y direction (short edge).
  *****************************************************************************/
-#define ARGUS_PIXELS_Y  4
+#define ARGUS_PIXELS_Y	4U
 
 /*!***************************************************************************
- * @brief   The total device pixel count.
+ * @brief	The total device pixel count.
  *****************************************************************************/
-#define ARGUS_PIXELS    ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y))
+#define ARGUS_PIXELS	((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y))
 
 /*!***************************************************************************
- * @brief   A flag indicating that the device is a extended range device.
+ * @brief	The AFBR-S50 module types.
  *****************************************************************************/
-#define MODULE_EXTENDED_FLAG (0x40U)
-
-/*!***************************************************************************
- * @brief   The AFBR-S50 module types.
- *****************************************************************************/
-typedef enum argus_module_version_t {
+typedef enum {
 	/*! No device connected or not recognized. */
 	MODULE_NONE = 0,
 
@@ -97,80 +89,54 @@ typedef enum argus_module_version_t {
 	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
 	 *  medium range 3D applications.
 	 *  Version 1 - legacy version! */
-	AFBR_S50MV85G_V1 = 0x01,
+	AFBR_S50MV85G_V1 = 1,
 
 	/*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device
 	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
 	 *  medium range 3D applications.
 	 *  Version 2 - legacy version! */
-	AFBR_S50MV85G_V2 = 0x02,
+	AFBR_S50MV85G_V2 = 2,
+
+	/*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device
+	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
+	 *  medium range 3D applications.
+	 *  Version 7 - current version! */
+	AFBR_S50MV85G_V3 = 7,
 
 	/*! AFBR-S50LV85D: an ADS0032 based multi-pixel range finder device
 	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
 	 *  long range 1D applications.
 	 *  Version 1 - current version! */
-	AFBR_S50LV85D_V1 = 0x03,
+	AFBR_S50LV85D_V1 = 3,
 
 	/*! AFBR-S50MV68B: an ADS0032 based multi-pixel range finder device
 	 *  w/ 4x8 pixel matrix and red, 680 nm, laser source for
 	 *  medium range 1D applications.
 	 *  Version 1 - current version! */
-	AFBR_S50MV68B_V1 = 0x04,
+	AFBR_S50MV68B_V1 = 4,
 
 	/*! AFBR-S50MV85I: an ADS0032 based multi-pixel range finder device
 	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
 	 *  medium range 3D applications.
 	 *  Version 1 - current version! */
-	AFBR_S50MV85I_V1 = 0x05,
+	AFBR_S50MV85I_V1 = 5,
 
 	/*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device
 	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
 	 *  short range 3D applications.
 	 *  Version 1 - current version! */
-	AFBR_S50SV85K_V1 = 0x06,
+	AFBR_S50SV85K_V1 = 6,
 
-	/*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device
-	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
-	 *  medium range 3D applications.
-	 *  Version 3 - current version! */
-	AFBR_S50MV85G_V3 = 0x07,
 
-	/*! AFBR-S50LX85D: an ADS0032 based multi-pixel range finder device
-	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
-	 *  extended long range 1D applications.
-	 *  Version 1 - current version! */
-	AFBR_S50LX85D_V1 = AFBR_S50LV85D_V1 | MODULE_EXTENDED_FLAG,
-
-	/*! AFBR-S50MX68B: an ADS0032 based multi-pixel range finder device
-	 *  w/ 4x8 pixel matrix and red, 680 nm, laser source for
-	 *  extended medium range 1D applications.
-	 *  Version 1 - current version! */
-	AFBR_S50MX68B_V1 = AFBR_S50MV68B_V1 | MODULE_EXTENDED_FLAG,
-
-	/*! AFBR-S50MX85I: an ADS0032 based multi-pixel range finder device
-	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
-	 *  extended medium range 3D applications.
-	 *  Version 1 - current version! */
-	AFBR_S50MX85I_V1 = AFBR_S50MV85I_V1 | MODULE_EXTENDED_FLAG,
-
-	/*! AFBR-S50MX85G: an ADS0032 based multi-pixel range finder device
-	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
-	 *  extended short range 3D applications.
-	 *  Version 1 - current version! */
-	AFBR_S50SX85K_V1 = AFBR_S50SV85K_V1 | MODULE_EXTENDED_FLAG,
-
-	/*! AFBR-S50MX85G: an ADS0032 based multi-pixel range finder device
-	 *  w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for
-	 *  extended medium range 3D applications.
-	 *  Version 1 - current version! */
-	AFBR_S50MX85G_V1 = AFBR_S50MV85G_V3 | MODULE_EXTENDED_FLAG,
+	/*! Reserved for future extensions. */
+	Reserved = 0x3F
 
 } argus_module_version_t;
 
 /*!***************************************************************************
- * @brief   The AFBR-S50 laser configurations.
+ * @brief	The AFBR-S50 laser configurations.
  *****************************************************************************/
-typedef enum argus_laser_type_t {
+typedef enum {
 	/*! No laser connected. */
 	LASER_NONE = 0,
 
@@ -186,15 +152,12 @@ typedef enum argus_laser_type_t {
 	/*! 850nm Infra-Red VCSEL v2 /w extended mode. */
 	LASER_H_V2X = 4,
 
-	/*! 680nm Red VCSEL v1 w/ extended mode. */
-	LASER_R_V1X = 5,
-
 } argus_laser_type_t;
 
 /*!***************************************************************************
- * @brief   The AFBR-S50 chip versions.
+ * @brief	The AFBR-S50 chip versions.
  *****************************************************************************/
-typedef enum argus_chip_version_t {
+typedef enum {
 	/*! No device connected or not recognized. */
 	ADS0032_NONE = 0,
 
@@ -215,102 +178,37 @@ typedef enum argus_chip_version_t {
 
 } argus_chip_version_t;
 
+/*!***************************************************************************
+ * @brief	The number of measurement modes with distinct configuration and
+ * 			calibration records.
+ *****************************************************************************/
+#define ARGUS_MODE_COUNT (2)
 
 /*!***************************************************************************
- * @brief   The measurement mode flags.
- * @details The measurement mode flags that can be combined to a measurement
- *          mode, e.g. high speed short range mode. See #argus_mode_t for
- *          a complete list of available measurement modes.
- *
- *          - Bit 0: Short Range Mode
- *          - Bit 1: Long Range Mode
- *          - Bit 2: High Speed Mode
- *
- *          Note that the Long and Short Range Flags are mutual exclusive but
- *          any of those 2 must be set. Thus the value 0 is invalid!
- *          All other flags enhance the base configurations, e.g. the High
- *          Speed flag create the high speed mode of the selected base
- *          measurement mode.
+ * @brief	The measurement modes.
  *****************************************************************************/
-typedef enum argus_mode_flags_t {
-	/*! Measurement Mode Flag for Short Range Base Mode. */
-	ARGUS_MODE_FLAG_SHORT_RANGE = 0x01 << 0,
+typedef enum {
+	/*! Measurement Mode A: Long Range Mode. */
+	ARGUS_MODE_A = 1,
 
-	/*! Measurement Mode Flag for Long Range Base Mode. */
-	ARGUS_MODE_FLAG_LONG_RANGE = 0x01 << 1,
-
-	/*! Measurement Mode Flag for High Speed Mode. */
-	ARGUS_MODE_FLAG_HIGH_SPEED = 0x01 << 2
-
-} argus_mode_flags_t;
-
-/*!***************************************************************************
- * @brief   The measurement modes.
- * @details The measurement modes are composed in binary from of the flags
- *          define in #argus_mode_flags_t, i.e. each bit has a special meaning:
- *
- *          - Bit 0: Short Range Mode
- *          - Bit 1: Long Range Mode
- *          - Bit 2: High Speed Mode
- *
- *          Note that the Long and Short Range Bits are mutual exclusive but any
- *          of those 2 must be set. Thus the value 0 is invalid!
- *****************************************************************************/
-typedef enum argus_mode_t {
-	/*! Measurement Mode: Short Range Mode. */
-	ARGUS_MODE_SHORT_RANGE =                                // = 0x01 = 0b0001
-		ARGUS_MODE_FLAG_SHORT_RANGE,
-
-	/*! Measurement Mode: Long Range Mode. */
-	ARGUS_MODE_LONG_RANGE =                                 // = 0x02 = 0b0010
-		ARGUS_MODE_FLAG_LONG_RANGE,
-
-	/*! Measurement Mode: High Speed Short Range Mode. */
-	ARGUS_MODE_HIGH_SPEED_SHORT_RANGE =                     // = 0x05 = 0b0101
-		ARGUS_MODE_FLAG_SHORT_RANGE | ARGUS_MODE_FLAG_HIGH_SPEED,
-
-	/*! Measurement Mode: High Speed Long Range Mode. */
-	ARGUS_MODE_HIGH_SPEED_LONG_RANGE =                      // = 0x06 = 0b0110
-		ARGUS_MODE_FLAG_LONG_RANGE | ARGUS_MODE_FLAG_HIGH_SPEED,
+	/*! Measurement Mode B: Short Range Mode. */
+	ARGUS_MODE_B = 2,
 
 } argus_mode_t;
 
-/*! The data structure for the API representing a AFBR-S50 device instance. */
-typedef struct argus_hnd_t argus_hnd_t;
-
 /*!***************************************************************************
- * @brief   Measurement Ready API callback function.
- *
- * @details Invoked by the API whenever a measurement cycle is finished and
- *          new data is ready to be evaluated via the #Argus_EvaluateData API
- *          function.
- *          The callback is passed to the API via the #Argus_TriggerMeasurement
- *          or #Argus_StartMeasurementTimer API functions.
- *          The API passes the status of the currently finished measurement
- *          cycle to the callback as first parameters. The second parameter is
- *          a pointer the API handle structure. The latter is used to identify
- *          the calling instance of the API in case of multiple devices.
- *          Further it can be passed to the #Argus_EvaluateData function.
- *
- * @warning Since the callback is called from an interrupt context, the
- *          callback execution must return as fast as possible. The usual task
- *          in the callback is to post an event to the main thread to inform it
- *          about the new data and that is must call the #Argus_EvaluateData
- *          function.
- *
- * @param   status The module status that caused the callback. #STATUS_OK if
- *                 everything was as expected.
- *
- * @param   hnd The API handle pointer to the calling instance. Identifies the
- *              instance of the API that was invoking the callback and thus the
- *              instance that must call the #Argus_EvaluateData for.
- *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @brief	Generic API callback function.
+ * @details	Invoked by the API. The content of the abstract data pointer
+ * 			depends upon the context.
+ * @param	status The module status that caused the callback. #STATUS_OK if
+ * 				   everything was as expected.
+ * @param	data An abstract pointer to an user defined data. This will usually
+ * 				 be passed to the function that also takes the callback as an
+ * 				 parameter. Otherwise it has a special meaning such as
+ * 				 configuration or calibration data.
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-typedef status_t (*argus_measurement_ready_callback_t)(status_t status, argus_hnd_t *hnd);
+typedef status_t (*argus_callback_t)(status_t status, void *data);
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_DEF_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Defines the dual frequency mode (DFM) setup parameters.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Defines the dual frequency mode (DFM) setup parameters.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,27 +36,24 @@
 
 #ifndef ARGUS_DFM_H
 #define ARGUS_DFM_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_dfm Dual Frequency Mode
- * @ingroup     argus_api
+ * @defgroup 	argusdfm Dual Frequency Mode
+ * @ingroup		argusapi
  *
- * @brief       Dual Frequency Mode (DFM) parameter definitions and API functions.
+ * @brief		Dual Frequency Mode (DFM) parameter definitions and API functions.
  *
- * @details     The DFM is an algorithm to extend the unambiguous range of the
- *              sensor by utilizing two detuned measurement frequencies.
+ * @details		The DFM is an algorithm to extend the unambiguous range of the
+ * 				sensor by utilizing two detuned measurement frequencies.
  *
- *              The AFBR-S50 API provides three measurement modes:
- *              - 1X: Single Frequency Measurement
- *              - 4X: Dual Frequency Measurement w/ 4 times the unambiguous
- *                    range of the Single Frequency Measurement
- *              - 8X: Dual Frequency Measurement w/ 8 times the unambiguous
- *                    range of the Single Frequency Measurement
+ *				The AFBR-S50 API provides three measurement modes:
+ *				- 1X: Single Frequency Measurement
+ *				- 4X: Dual Frequency Measurement w/ 4 times the unambiguous
+ *				      range of the Single Frequency Measurement
+ *				- 8X: Dual Frequency Measurement w/ 8 times the unambiguous
+ *				      range of the Single Frequency Measurement
  *
- * @addtogroup  argus_dfm
+ * @addtogroup 	argusdfm
  * @{
  *****************************************************************************/
 
@@ -64,10 +61,10 @@ extern "C" {
 #define ARGUS_DFM_FRAME_COUNT (2U)
 
 /*! The Dual Frequency Mode measurement modes count. Excluding the disabled mode. */
-#define ARGUS_DFM_MODE_COUNT (2U) // except off-mode!
+#define ARGUS_DFM_MODE_COUNT (2U) // expect off-mode!
 
 /*! The Dual Frequency Mode measurement modes enumeration. */
-typedef enum argus_dfm_mode_t {
+typedef enum {
 	/*! Single Frequency Measurement Mode (w/ 1x Unambiguous Range). */
 	DFM_MODE_OFF = 0U,
 
@@ -81,7 +78,4 @@ typedef enum argus_dfm_mode_t {
 
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_DFM_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Defines macros to work with pixel and ADC channel masks.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Defines macros to work with pixel and ADC channel masks.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -37,409 +37,217 @@
 
 #ifndef ARGUS_MAP_H
 #define ARGUS_MAP_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_map Pixel Channel Mapping
- * @ingroup     argus_api
+ * @defgroup	argusmap ADC Channel Mapping
+ * @ingroup		argusres
  *
- * @brief       Pixel Channel Mapping
+ * @brief		Pixel ADC Channel Mapping
  *
- * @details     The ADC Channels of each pixel or auxiliary channel on the device
- *              are numbered in a way that is convenient on the chip architecture.
- *              The macros in this module are defined in order to map between the
- *              chip internal channel number (ch) to the two-dimensional
- *              x-y-indices or one-dimensional n-index representation.
+ * @details		The ADC Channels of each pixel or auxiliary channel on the device
+ * 				are numbered in a way that is convenient on the chip architecture.
+ * 				The macros in this module are defined in order to map between the
+ * 				chip internal channel number (ch) to the two-dimensional
+ * 				x-y-indices or one-dimensional n-index representation.
  *
- * @addtogroup  argus_map
+ * @addtogroup 	argusmap
  * @{
  *****************************************************************************/
 
+#include "api/argus_def.h"
 #include "utility/int_math.h"
-#include <assert.h>
 
 
-/*!***************************************************************************
- * @brief   The device pixel field size in x direction (long edge).
- *****************************************************************************/
-#define ARGUS_PIXELS_X  8
-
-/*!***************************************************************************
- * @brief   The device pixel field size in y direction (short edge).
- *****************************************************************************/
-#define ARGUS_PIXELS_Y  4
-
-/*!***************************************************************************
- * @brief   The total device pixel count.
- *****************************************************************************/
-#define ARGUS_PIXELS    ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y))
 
 
 /*!*****************************************************************************
- * @brief   Macro to determine the pixel ADC channel number from the x-z-indices.
- * @param   x The x-index of the pixel.
- * @param   y The y-index of the pixel.
- * @return  The ADC channel number of the pixel.
+ * @brief	Macro to determine the pixel ADC channel number from the x-z-indices.
+ * @param	x The x-index of the pixel.
+ * @param	y The y-index of the pixel.
+ * @return	The ADC channel number of the pixel.
  ******************************************************************************/
 #define PIXEL_XY2CH(x, y) ((((y) << 3U) & 0x10U) | (((x) ^ 0x07U) << 1U) | ((y) & 0x01U))
 
 /*!*****************************************************************************
- * @brief   Macro to determine the pixel x-index from the ADC channel number.
- * @param   c The ADC channel number of the pixel.
- * @return  The x-index of the pixel.
+ * @brief	Macro to determine the pixel x-index from the ADC channel number.
+ * @param	c The ADC channel number of the pixel.
+ * @return	The x-index of the pixel.
  ******************************************************************************/
 #define PIXEL_CH2X(c) ((((c) >> 1U) ^ 0x07U) & 0x07U)
 
 /*!*****************************************************************************
- * @brief   Macro to determine the pixel y-index from the ADC channel number.
- * @param   c The ADC channel number of the pixel.
- * @return  The y-index of the pixel.
+ * @brief	Macro to determine the pixel y-index from the ADC channel number.
+ * @param	c The ADC channel number of the pixel.
+ * @return	The y-index of the pixel.
  ******************************************************************************/
 #define PIXEL_CH2Y(c) ((((c) >> 3U) & 0x02U) | ((c) & 0x01U))
 
 
 /*!*****************************************************************************
- * @brief   Macro to determine the n-index from the x-y-indices.
- * @param   x The x-index of the pixel.
- * @param   y The y-index of the pixel.
- * @return  The n-index of the pixel.
+ * @brief	Macro to determine the n-index from the x-y-indices.
+ * @param	x The x-index of the pixel.
+ * @param	y The y-index of the pixel.
+ * @return	The n-index of the pixel.
  ******************************************************************************/
 #define PIXEL_XY2N(x, y) (((x) << 2U) | (y))
 
 /*!*****************************************************************************
- * @brief   Macro to determine the pixel x-index from the n-index.
- * @param   n The n-index of the pixel.
- * @return  The x-index number of the pixel.
+ * @brief	Macro to determine the pixel x-index from the n-index.
+ * @param	n The n-index of the pixel.
+ * @return	The x-index number of the pixel.
  ******************************************************************************/
 #define PIXEL_N2X(n) ((n) >> 2U)
 
 /*!*****************************************************************************
- * @brief   Macro to determine the pixel y-index from the n-index.
- * @param   n The n-index of the pixel.
- * @return  The y-index number of the pixel.
+ * @brief	Macro to determine the pixel y-index from the n-index.
+ * @param	n The n-index of the pixel.
+ * @return	The y-index number of the pixel.
  ******************************************************************************/
 #define PIXEL_N2Y(n) ((n) & 0x03U)
 
 
 /*!*****************************************************************************
- * @brief   Macro to determine the pixel n-index from the ADC channel number.
- * @param   n The n-index of the pixel.
- * @return  The ADC channel number of the pixel.
+ * @brief	Macro to determine the pixel n-index from the ADC channel number.
+ * @param	n The n-index of the pixel.
+ * @return	The ADC channel number of the pixel.
  ******************************************************************************/
 #define PIXEL_N2CH(n) ((((n) << 3U) & 0x10U) | ((((n) >> 1U) ^ 0x0EU) & 0x0EU) | ((n) & 0x01U))
 
 /*!*****************************************************************************
- * @brief   Macro to determine the pixel
- * @param   c The ADC channel number of the pixel.
- * @return  The n-index of the pixel.
+ * @brief	Macro to determine the pixel
+ * @param	c The ADC channel number of the pixel.
+ * @return	The n-index of the pixel.
  ******************************************************************************/
 #define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U))
 
 
 /*!*****************************************************************************
- * @brief   Macro to determine if a pixel given by the n-index is enabled in a pixel mask.
- * @param   msk 32-bit pixel mask
- * @param   n n-index of the pixel.
- * @return  True if the pixel (n) is enabled.
+ * @brief	Macro to determine if a pixel given by the n-index is enabled in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	n n-index of the pixel.
+ * @return 	True if the pixel (n) is enabled.
  ******************************************************************************/
 #define PIXELN_ISENABLED(msk, n) (((msk) >> (n)) & 0x01U)
 
 /*!*****************************************************************************
- * @brief   Macro to enable a pixel given by the n-index in a pixel mask.
- * @param   msk 32-bit pixel mask
- * @param   n n-index of the pixel to enable.
+ * @brief	Macro to enable a pixel given by the n-index in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	n n-index of the pixel to enable.
  ******************************************************************************/
 #define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (n)))
 
 /*!*****************************************************************************
- * @brief   Macro disable a pixel given by the n-index in a pixel mask.
- * @param   msk 32-bit pixel mask
- * @param   n n-index of the pixel to disable.
+ * @brief	Macro disable a pixel given by the n-index in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	n n-index of the pixel to disable.
  ******************************************************************************/
 #define PIXELN_DISABLE(msk, n) ((msk) &= (~(0x01U << (n))))
 
 
 /*!*****************************************************************************
- * @brief   Macro to determine if an ADC pixel channel is enabled from a pixel mask.
- * @param   msk The 32-bit pixel mask
- * @param   c The ADC channel number of the pixel.
- * @return  True if the specified pixel ADC channel is enabled.
+ * @brief	Macro to determine if an ADC pixel channel is enabled from a pixel mask.
+ * @param	msk The 32-bit pixel mask
+ * @param	c The ADC channel number of the pixel.
+ * @return 	True if the specified pixel ADC channel is enabled.
  ******************************************************************************/
 #define PIXELCH_ISENABLED(msk, c) (PIXELN_ISENABLED(msk, PIXEL_CH2N(c)))
 
 /*!*****************************************************************************
- * @brief   Macro to enable an ADC pixel channel in a pixel mask.
- * @param   msk The 32-bit pixel mask
- * @param   c The pixel ADC channel number to enable.
+ * @brief	Macro to enable an ADC pixel channel in a pixel mask.
+ * @param	msk The 32-bit pixel mask
+ * @param	c The pixel ADC channel number to enable.
  ******************************************************************************/
 #define PIXELCH_ENABLE(msk, c) (PIXELN_ENABLE(msk, PIXEL_CH2N(c)))
 
 /*!*****************************************************************************
- * @brief   Macro to disable an ADC pixel channel in a pixel mask.
- * @param   msk The 32-bit pixel mask
- * @param   c The pixel ADC channel number to disable.
+ * @brief	Macro to disable an ADC pixel channel in a pixel mask.
+ * @param	msk The 32-bit pixel mask
+ * @param	c The pixel ADC channel number to disable.
  ******************************************************************************/
 #define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c)))
 
 
 /*!*****************************************************************************
- * @brief   Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask.
- * @param   msk 32-bit pixel mask
- * @param   x x-index of the pixel.
- * @param   y y-index of the pixel.
- * @return  True if the pixel (x,y) is enabled.
+ * @brief	Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	x x-index of the pixel.
+ * @param	y y-index of the pixel.
+ * @return 	True if the pixel (x,y) is enabled.
  ******************************************************************************/
 #define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y)))
 
 /*!*****************************************************************************
- * @brief   Macro to enable a pixel given by the x-y-indices in a pixel mask.
- * @param   msk 32-bit pixel mask
- * @param   x x-index of the pixel to enable.
- * @param   y y-index of the pixel to enable.
+ * @brief	Macro to enable a pixel given by the x-y-indices in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	x x-index of the pixel to enable.
+ * @param	y y-index of the pixel to enable.
  ******************************************************************************/
 #define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y)))
 
 /*!*****************************************************************************
- * @brief   Macro disable a pixel given by the x-y-indices in a pixel mask.
- * @param   msk 32-bit pixel mask
- * @param   x x-index of the pixel to disable.
- * @param   y y-index of the pixel to disable.
+ * @brief	Macro disable a pixel given by the x-y-indices in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	x x-index of the pixel to disable.
+ * @param	y y-index of the pixel to disable.
  ******************************************************************************/
 #define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y)))
 
 
 /*!*****************************************************************************
- * @brief   Macro to determine if an ADC channel is enabled in a channel mask.
- * @param   msk 32-bit channel mask
- * @param   ch channel number of the ADC channel.
- * @return  True if the ADC channel is enabled.
+ * @brief	Macro to determine if an ADC channel is enabled in a channel mask.
+ * @param	msk 32-bit channel mask
+ * @param	ch channel number of the ADC channel.
+ * @return 	True if the ADC channel is enabled.
  ******************************************************************************/
 #define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U)
 
 /*!*****************************************************************************
- * @brief   Macro to determine if an ADC channel is enabled in a channel mask.
- * @param   msk 32-bit channel mask
- * @param   ch channel number of the ADC channel to enabled.
+ * @brief	Macro to determine if an ADC channel is enabled in a channel mask.
+ * @param	msk 32-bit channel mask
+ * @param	ch channel number of the ADC channel to enabled.
  ******************************************************************************/
 #define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U)))
 
 /*!*****************************************************************************
- * @brief   Macro to determine if an ADC channel is disabled in a channel mask.
- * @param   msk 32-bit channel mask
- * @param   ch channel number of the ADC channel to disable.
+ * @brief	Macro to determine if an ADC channel is disabled in a channel mask.
+ * @param	msk 32-bit channel mask
+ * @param	ch channel number of the ADC channel to disable.
  ******************************************************************************/
 #define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U))))
 
 
 /*!*****************************************************************************
- * @brief   Macro to determine the number of enabled pixel/channels in a mask
- *          via a popcount algorithm.
- * @param   pxmsk 32-bit pixel mask
- * @return  The count of enabled pixel channels.
+ * @brief	Macro to determine the number of enabled pixel/channels in a mask
+ * 			via a popcount algorithm.
+ * @param	pxmsk 32-bit pixel mask
+ * @return 	The count of enabled pixel channels.
  ******************************************************************************/
 #define PIXEL_COUNT(pxmsk) popcount(pxmsk)
 
 /*!*****************************************************************************
- * @brief   Macro to determine the number of enabled channels via a popcount
- *          algorithm.
- * @param   pxmsk 32-bit pixel mask
- * @param   chmsk 32-bit channel mask
- * @return  The count of enabled ADC channels.
+ * @brief	Macro to determine the number of enabled channels via a popcount
+ * 			algorithm.
+ * @param	pxmsk 32-bit pixel mask
+ * @param	chmsk 32-bit channel mask
+ * @return 	The count of enabled ADC channels.
  ******************************************************************************/
 #define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk))
 
 /*!*****************************************************************************
- * @brief   Converts a raw ADC channel mask to a x-y-sorted pixel mask.
- * @param   msk The raw ADC channel mask to be converted.
- * @return  The converted x-y-sorted pixel mask.
+ * @brief	Converts a raw ADC channel mask to a x-y-sorted pixel mask.
+ * @param	msk The raw ADC channel mask to be converted.
+ * @return 	The converted x-y-sorted pixel mask.
  ******************************************************************************/
 static inline uint32_t ChannelToPixelMask(uint32_t msk)
 {
 	uint32_t res = 0;
 
 	for (uint_fast8_t n = 0; n < 32; n += 2) {
-		res |= ((msk >> PIXEL_N2CH(n)) & 0x3U) << n; // sets 2 bits at once
+		res |= ((msk >> PIXEL_N2CH(n)) & 0x3U) << n;
 	}
 
 	return res;
 }
 
-/*!*****************************************************************************
- * @brief   Converts a x-y-sorted pixel mask to a raw ADC channel mask.
- * @param   msk The x-y-sorted pixel channel mask to be converted.
- * @return  The converted raw ADC channel mask.
- ******************************************************************************/
-static inline uint32_t PixelToChannelMask(uint32_t msk)
-{
-	uint32_t res = 0;
-
-	for (uint_fast8_t ch = 0; ch < 32; ch += 2) {
-		res |= ((msk >> PIXEL_CH2N(ch)) & 0x3U) << ch; // sets 2 bits at once
-	}
-
-	return res;
-}
-
-
-/*!*****************************************************************************
- * @brief   Shifts a pixel mask by a given offset.
- *
- * @details This moves the selected pixel pattern by a specified number of
- *          pixels in x and y direction.
- *          If the shift in y direction is odd (e.g +1), the pattern will be
- *          shifted by +0.5 or -0.5 in x direction due to the hexagonal shape
- *          of the pixel field. Thus, a center pixel (usually the Golden Pixel)
- *          is determined that is used to determine if the pattern is shifted
- *          by +0.5 or -0.5 pixels in x direction. The center pixel is then
- *          always shifted without changing the x index and the surrounding
- *          pixels are adopting its x index accordingly.
- *
- *          Example: Consider the flower pattern, i.e. the Golden Pixel (e.g.
- *          5/2) is selected and all is direct neighbors (i.e. 5/1, 6/1, 6/2,
- *          6/3, 5/3, 4/2). If the pattern is shifted by -1 in y direction, the
- *          new Golden Pixel would be 5/1. Now all surrounding pixels are
- *          selected, namely 4/0, 4/1, 4/2, 5/0, 5/2, 6/1). This yields again
- *          the flower around the Golden Pixel.
- *
- *          Thus, the pixels can not all be shifted by the same dx/dy values due
- *          to the hexagonal shape of the pixel field, e.g. the upper right
- *          neighbor of 5/2 is 5/1 but the upper right neighbor of 5/1 is NOT
- *          5/0 but 4/0!
- *          This happens only if the shift in y direction is an odd number.
- *          The algorithm to determine new indices is as follows:
- *          - If the shift in y direction is even (e.g. +2, -2), no compensation
- *            of the hexagonal shape is needed; skip compensation, simply
- *            add/subtract indices.
- *          - If the center pixel y index is even, pixels that will have even y
- *            index after the shift will be additionally shifted by -1 in x
- *            direction.
- *          - If the center pixel y index is odd, pixel that will have odd y
- *            index after the shift will be additionally shifted by +1 in x
- *            direction.
- *
- * @see     Please also refer to the function #Argus_GetCalibrationGoldenPixel
- *          to obtain the current Golden Pixel location.
- *
- * @param   pixel_mask The x-y-sorted pixel mask to be shifted.
- * @param   dx The number of pixel to shift in x direction.
- * @param   dy The number of pixel to shift in y direction.
- * @param   center_y The center y index of the pattern that is shifted.
- * @return  The shifted pixel mask.
- ******************************************************************************/
-static inline uint32_t ShiftSelectedPixels(const uint32_t pixel_mask,
-		const int8_t dx,
-		const int8_t dy,
-		const uint8_t center_y)
-{
-	if (dx == 0 && dy == 0) { return pixel_mask; }
-
-	uint32_t shifted_mask = 0;
-
-	for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
-		for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
-			int8_t x_src = x - dx;
-			int8_t y_src = y - dy;
-
-			if (dy & 0x1) {
-				/* Compensate for hexagonal pixel shape. */
-				if ((center_y & 0x1) && (y & 0x1)) {
-					x_src--;
-				}
-
-				if (!(center_y & 0x1) && !(y & 0x1)) {
-					x_src++;
-				}
-			}
-
-			if (x_src < 0 || x_src >= ARGUS_PIXELS_X) { continue; }
-
-			if (y_src < 0 || y_src >= ARGUS_PIXELS_Y) { continue; }
-
-			if (PIXELXY_ISENABLED(pixel_mask, x_src, y_src)) {
-				PIXELXY_ENABLE(shifted_mask, x, y);
-			}
-		}
-	}
-
-	return shifted_mask;
-}
-
-/*!*****************************************************************************
- * @brief   Fills a pixel mask to a specified number of pixels around a center pixel.
- *
- * @details The pixel mask is iteratively filled with the nearest pixel to a
- *          specified center pixel until a specified number of pixels is achieved.
- *          The distance between two pixel is determined via a quadratic metric,
- *          i.e. dx^2 + dy^2. Pixels towards the lower x indices are preferred.
- *
- *          Note that the distance of only calculated approximately, e.g. the
- *          y distance of pixels is considered to be 2 instead of cos(60)*2.
- *
- *          Nothing is done if the number of pixels already exceeds the specified
- *          /p pixel_count parameter.
- *
- * @see     Please also refer to the function #Argus_GetCalibrationGoldenPixel
- *          to obtain the current Golden Pixel location.
- *
- * @param   pixel_mask The x-y-sorted pixel mask to be filled with pixels.
- * @param   pixel_count The final number of pixels in the pixel mask.
- * @param   center_x The center pixel x-index.
- * @param   center_y The center pixel y-index.
- * @return  The filled pixel mask with at least /p pixel_count pixels selected.
- ******************************************************************************/
-static inline uint32_t FillPixelMask(uint32_t pixel_mask,
-				     const uint8_t pixel_count,
-				     const uint8_t center_x,
-				     const uint8_t center_y)
-{
-	assert(pixel_count <= ARGUS_PIXELS);
-	assert(center_x < ARGUS_PIXELS_X);
-	assert(center_y < ARGUS_PIXELS_Y);
-
-	if (pixel_count == ARGUS_PIXELS) { return 0xFFFFFFFFU; }
-
-	/* If the pattern was shifted towards boundaries, the pixel count may have
-	 * decreased. In this case, the pixels closest to the reference pixel are
-	 * selected. Pixel towards lower x index are prioritized. */
-	while (pixel_count > PIXEL_COUNT(pixel_mask)) {
-		int32_t min_dist = INT32_MAX;
-		int8_t min_x = -1;
-		int8_t min_y = -1;
-
-		/* Find nearest not selected pixel. */
-		for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
-			for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
-				if (!PIXELXY_ISENABLED(pixel_mask, x, y)) {
-					int32_t distx = (x - center_x) << 1;
-
-					if (!(y & 0x1)) { distx++; }
-
-					if (!(center_y & 0x1)) { distx--; }
-
-					const int32_t disty = (y - center_y) << 1;
-					int32_t dist = distx * distx + disty * disty;
-
-					if (dist < min_dist) {
-						min_dist = dist;
-						min_x = x;
-						min_y = y;
-					}
-				}
-			}
-		}
-
-		assert(min_x >= 0 && min_x < ARGUS_PIXELS_X);
-		assert(min_y >= 0 && min_y < ARGUS_PIXELS_Y);
-		assert(!PIXELXY_ISENABLED(pixel_mask, min_x, min_y));
-		PIXELXY_ENABLE(pixel_mask, min_x, min_y);
-	}
-
-	return pixel_mask;
-}
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_MAP_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 hardware API.
- * @details     Defines the generic measurement parameters and data structures.
+ * @brief    	This file is part of the AFBR-S50 hardware API.
+ * @details		Defines the generic measurement parameters and data structures.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,20 +36,17 @@
 
 #ifndef ARGUS_MEAS_H
 #define ARGUS_MEAS_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_meas Measurement/Device Control
- * @ingroup     argus_api
+ * @defgroup 	argusmeas Measurement/Device Control
+ * @ingroup		argusapi
  *
- * @brief       Measurement/Device control module
+ * @brief 		Measurement/Device control module
  *
- * @details     This module contains measurement and device control specific
- *              definitions and methods.
+ * @details		This module contains measurement and device control specific
+ * 				definitions and methods.
  *
- * @addtogroup  argus_meas
+ * @addtogroup 	argusmeas
  * @{
  *****************************************************************************/
 
@@ -69,11 +66,11 @@ extern "C" {
 #define ARGUS_AUX_DATA_SIZE (3U * ARGUS_AUX_CHANNEL_COUNT) // 3 bytes * x channels * 1 phase
 
 /*!***************************************************************************
- * @brief   The device measurement configuration structure.
- * @details The portion of the configuration data that belongs to the
- *          measurement cycle. I.e. the data that defines a measurement frame.
+ * @brief	The device measurement configuration structure.
+ * @details	The portion of the configuration data that belongs to the
+ * 			measurement cycle. I.e. the data that defines a measurement frame.
  *****************************************************************************/
-typedef struct argus_meas_frame_t {
+typedef struct {
 	/*! Frame integration time in microseconds.
 	 *  The integration time determines the measured time between
 	 *  the start signal and the IRQ. Note that this value will be
@@ -85,13 +82,13 @@ typedef struct argus_meas_frame_t {
 
 	/*! Pixel enabled mask for the 32 pixels sorted
 	 *  by x-y-indices.
-	 *  See [pixel mapping](@ref argus_map) for more
+	 *  See [pixel mapping](@ref argusmap) for more
 	 *  details on the pixel mask. */
 	uint32_t PxEnMask;
 
 	/*! ADS channel enabled mask for the remaining
 	 *  channels 31 .. 63 (miscellaneous values).
-	 *  See [pixel mapping](@ref argus_map) for more
+	 *  See [pixel mapping](@ref argusmap) for more
 	 *  details on the ADC channel mask. */
 	uint32_t ChEnMask;
 
@@ -116,6 +113,9 @@ typedef struct argus_meas_frame_t {
 	 *  Determines the optical output power. */
 	uq12_4_t OutputPower;
 
+	/*! The amplitude that is evaluated and used in the DCA module. */
+	uq12_4_t DCAAmplitude;
+
 	/*! Laser Bias Current Settings in LSB. */
 	uint8_t BiasCurrent;
 
@@ -133,7 +133,4 @@ typedef struct argus_meas_frame_t {
 } argus_meas_frame_t;
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_MEAS_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h

@@ -1,59 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 hardware API.
- * @details     Defines the generic device calibration API.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef ARGUS_OFFSET_H
-#define ARGUS_OFFSET_H
-
-/*!***************************************************************************
- * @addtogroup  argus_cal
- * @{
- *****************************************************************************/
-
-#include "argus_def.h"
-
-/*!***************************************************************************
- * @brief   Pixel Range Offset Table.
- * @details Contains pixel range offset values for all 32 active pixels.
- *****************************************************************************/
-typedef struct argus_cal_offset_table_t {
-	/*! The offset values per pixel in meter and Q0.15 format. */
-	q0_15_t Table[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
-
-} argus_cal_offset_table_t;
-
-
-/*! @} */
-#endif /* ARGUS_OFFSET_T */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h

@@ -1,12 +1,12 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Defines the pixel binning algorithm (PBA) setup parameters and
- *              data structure.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Defines the pixel binning algorithm (PBA) setup parameters and
+ * 				data structure.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -37,98 +37,86 @@
 
 #ifndef ARGUS_PBA_H
 #define ARGUS_PBA_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_pba Pixel Binning Algorithm
- * @ingroup     argus_api
+ * @defgroup 	arguspba Pixel Binning Algorithm
+ * @ingroup		argusapi
  *
- * @brief       Pixel Binning Algorithm (PBA) parameter definitions and API functions.
+ * @brief		Pixel Binning Algorithm (PBA) parameter definitions and API functions.
  *
- * @details     Defines the generic Pixel Binning Algorithm (PBA) setup parameters
- *              and data structure.
+ * @details		Defines the generic pixel binning algorithm (PBA) setup parameters
+ * 				and data structure.
  *
- *              The PBA module contains filter algorithms that determine the
- *              pixels with the best signal quality and extract an 1D distance
- *              information from the filtered pixels by averaging them in a
- *              specified way.
+ * 				The PBA module contains filter algorithms that determine the
+ * 				pixels with the best signal quality and extract an 1d distance
+ * 				information from the filtered pixels.
  *
- *              The Pixel Binning Algorithm is a three-stage filter with a
- *              fallback value:
+ * 				The pixel filter algorithm is a three-stage filter with a
+ * 				fallback value:
  *
- *              -# A fixed pre-filter mask is applied to statically disable
- *                  specified pixels.
- *              -# A relative and absolute amplitude filter is applied in the
- *                  second stage. The relative filter is determined by a ratio
- *                  of the maximum amplitude off all available (i.e. not filtered
- *                  in stage 1) pixels. Pixels that have an amplitude below the
- *                  relative threshold are dismissed. The same holds true for
- *                  the absolute amplitude threshold. All pixel with smaller
- *                  amplitude are dismissed.\n
- *                  Note that the absolute amplitude threshold is disabled if
- *                  the Golden Pixel (see below) is also disabled in order to
- *                  prevent invalid filtering for multi-pixel devices.\n
- *                  The relative threshold is useful to setup a distance
- *                  measurement scenario. All well illuminated pixels are
- *                  selected and considered for the final 1D distance. The
- *                  absolute threshold is used to dismiss pixels that are below
- *                  the noise level. The latter would be considered for the 1D
- *                  result if the maximum amplitude is already very low.
- *              -# An absolute minimum distance filter is applied in addition
- *                  to the amplitude filter. This removes all pixel that have
- *                  a lower distance than the specified threshold. This is used
- *                  to remove invalid pixels that can be detected by a physically
- *                  not correct negative distance.
- *              -# A distance filter is used to distinguish pixels that target
- *                  the actual object from pixels that see the brighter background,
- *                  e.g. white walls. Thus, the pixel with the minimum distance
- *                  is referenced and all pixel that have a distance between
- *                  the minimum and the given minimum distance scope are selected
- *                  for the 1D distance result. The minimum distance scope is
- *                  determined by an relative (to the current minimum distance)
- *                  and an absolute value. The larger scope value is the
- *                  relevant one, i.e. the relative distance scope can be used
- *                  to heed the increasing noise at larger distances.
- *              -# If all of the above filters fail to determine a single valid
- *                  pixel, the Golden Pixel is used as a fallback value. The
- *                  Golden Pixel is the pixel that sits right at the focus point
- *                  of the optics at large distances.
- *              .
+ * 				-# A fixed pre-filter mask is applied to statically disable
+ * 				    specified pixels.
+ * 				-# A relative and absolute amplitude filter is applied in the
+ * 				 	second stage. The relative filter is determined by a ratio
+ * 				 	of the maximum amplitude off all available (i.e. not filtered
+ * 				 	in stage 1) pixels. Pixels that have an amplitude below the
+ * 				 	relative threshold are dismissed. The same holds true for
+ * 				 	the absolute amplitude threshold. All pixel with smaller
+ * 				 	amplitude are dismissed.\n
+ * 				 	The relative threshold is useful to setup a distance
+ * 				 	measurement scenario. All well illuminated pixels are
+ * 				 	selected and considered for the final 1d distance. The
+ * 				 	absolute threshold is used to dismiss pixels that are below
+ * 				 	the noise level. The latter would be considered for the 1d
+ * 				 	result if the maximum amplitude is already very low.
+ * 				-# A distance filter is used to distinguish pixels that target
+ * 				    the actual object from pixels that see the brighter background,
+ * 				    e.g. white walls. Thus, the pixel with the minimum distance
+ * 				    is referenced and all pixel that have a distance between
+ * 				    the minimum and the given minimum distance scope are selected
+ * 				    for the 1d distance result. The minimum distance scope is
+ * 				    determined by an relative (to the current minimum distance)
+ * 				    and an absolute value. The larger scope value is the
+ * 				    relevant one, i.e. the relative distance scope can be used
+ * 				    to heed the increasing noise at larger distances.
+ * 				-# If all of the above filters fail to determine a single valid
+ * 				    pixel, the golden pixel is used as a fallback value. The
+ * 				    golden pixel is the pixel that sits right at the focus point
+ * 				    of the optics at large distances.
+ * 				.
  *
- *              After filtering is done, there may be more than a single pixel
- *              left to determine the 1D signal. Therefore several averaging
- *              methods are implemented to obtain the best 1D result from many
- *              pixels. See #argus_pba_averaging_mode_t for details.
+ *				After filtering is done, there may be more than a single pixel
+ *				left to determine the 1d signal. Therefore several averaging
+ *				methods are implemented to obtain the best 1d result from many
+ *				pixels. See #argus_pba_averaging_mode_t for details.
  *
  *
- * @addtogroup  argus_pba
+ * @addtogroup 	arguspba
  * @{
  *****************************************************************************/
 
 #include "argus_def.h"
 
 /*!***************************************************************************
- * @brief   Enable flags for the pixel binning algorithm.
+ * @brief	Enable flags for the pixel binning algorithm.
  *
  * @details Determines the pixel binning algorithm feature enable status.
- *          - [0]: #PBA_ENABLE: Enables the pixel binning feature.
- *          - [1]: reserved
- *          - [2]: reserved
- *          - [3]: reserved
- *          - [4]: reserved
- *          - [5]: #PBA_ENABLE_GOLDPX: Enables the Golden Pixel feature.
- *          - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope
- *                   feature.
- *          - [7]: reserved
- *          .
+ * 			- [0]: #PBA_ENABLE: Enables the pixel binning feature.
+ * 			- [1]: reserved
+ * 			- [2]: reserved
+ * 			- [3]: reserved
+ * 			- [4]: reserved
+ * 			- [5]: #PBA_ENABLE_GOLDPX: Enables the golden pixel feature.
+ * 			- [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope
+ * 					 feature.
+ * 			- [7]: reserved
+ * 			.
  *****************************************************************************/
-typedef enum argus_pba_flags_t {
+typedef enum {
 	/*! Enables the pixel binning feature. */
 	PBA_ENABLE = 1U << 0U,
 
-	/*! Enables the Golden Pixel. */
+	/*! Enables the golden pixel. */
 	PBA_ENABLE_GOLDPX = 1U << 5U,
 
 	/*! Enables the minimum distance scope filter. */
@@ -137,9 +125,9 @@ typedef enum argus_pba_flags_t {
 } argus_pba_flags_t;
 
 /*!***************************************************************************
- * @brief   The averaging modes for the pixel binning algorithm.
+ * @brief	The averaging modes for the pixel binning algorithm.
  *****************************************************************************/
-typedef enum argus_pba_averaging_mode_t {
+typedef enum {
 	/*! Evaluate the 1D range from all available pixels using
 	 *  a simple average. */
 	PBA_SIMPLE_AVG = 1U,
@@ -152,12 +140,11 @@ typedef enum argus_pba_averaging_mode_t {
 } argus_pba_averaging_mode_t;
 
 /*!***************************************************************************
- * @brief   The pixel binning algorithm settings data structure.
- * @details Describes the pixel binning algorithm settings.
+ * @brief	The pixel binning algorithm settings data structure.
+ * @details	Describes the pixel binning algorithm settings.
  *****************************************************************************/
 typedef struct {
-	/*! Enables the Pixel Binning Algorithm.
-	 *
+	/*! Enables the pixel binning features.
 	 *  Each bit may enable a different feature. See #argus_pba_flags_t
 	 *  for details about the enabled flags. */
 	argus_pba_flags_t Enabled;
@@ -169,7 +156,6 @@ typedef struct {
 	argus_pba_averaging_mode_t AveragingMode;
 
 	/*! The Relative amplitude threshold value (in %) of the max. amplitude.
-	 *
 	 *  Pixels with amplitude below this threshold value are dismissed.
 	 *
 	 *  All available values from the 8-bit representation are valid.
@@ -179,27 +165,22 @@ typedef struct {
 	uq0_8_t RelAmplThreshold;
 
 	/*! The relative minimum distance scope value in %.
-	 *
-	 *  Pixels that have a range value within [x0, x0 + dx] are considered
-	 *  for the pixel binning, where x0 is the minimum distance of all
-	 *  amplitude picked pixels and dx is the minimum distance scope value.
-	 *  The minimum distance scope value will be the maximum of relative
-	 *  and absolute value.
+	 * 	Pixels that have a range value within [x0, x0 + dx] are considered
+	 * 	for the pixel binning, where x0 is the minimum distance of all
+	 * 	amplitude picked pixels and dx is the minimum distance scope value.
+	 * 	The minimum distance scope value will be the maximum of relative
+	 * 	and absolute value.
 	 *
 	 *  All available values from the 8-bit representation are valid.
 	 *  The actual percentage value is determined by 100%/256*x.
 	 *
-	 *  Special values:
+	 *	Special values:
 	 *  - 0: Use 0 for absolute value only or to choose the pixel with the
-	 *       minimum distance only (of also the absolute value is 0)! */
+	 *  	 minimum distance only (of also the absolute value is 0)! */
 	uq0_8_t RelMinDistanceScope;
 
-	/*! The absolute amplitude threshold value in LSB.
-	 *
-	 *  Pixels with amplitude below this threshold value are dismissed.
-	 *
-	 *  The absolute amplitude threshold is only valid if the Golden Pixel
-	 *  mode is enabled. Otherwise, the threshold is set to 0 LSB internally.
+	/*! The Absolute amplitude threshold value in LSB.
+	 * 	Pixels with amplitude below this threshold value are dismissed.
 	 *
 	 *  All available values from the 16-bit representation are valid.
 	 *  The actual LSB value is determined by x/16.
@@ -208,42 +189,33 @@ typedef struct {
 	uq12_4_t AbsAmplThreshold;
 
 	/*! The absolute minimum distance scope value in m.
-	 *
-	 *  Pixels that have a range value within [x0, x0 + dx] are considered
-	 *  for the pixel binning, where x0 is the minimum distance of all
-	 *  amplitude picked pixels and dx is the minimum distance scope value.
-	 *  The minimum distance scope value will be the maximum of relative
-	 *  and absolute value.
+	 * 	Pixels that have a range value within [x0, x0 + dx] are considered
+	 * 	for the pixel binning, where x0 is the minimum distance of all
+	 * 	amplitude picked pixels and dx is the minimum distance scope value.
+	 * 	The minimum distance scope value will be the maximum of relative
+	 * 	and absolute value.
 	 *
 	 *  All available values from the 16-bit representation are valid.
 	 *  The actual LSB value is determined by x/2^15.
 	 *
-	 *  Special values:
+	 *	Special values:
 	 *  - 0: Use 0 for relative value only or to choose the pixel with the
-	 *       minimum distance only (of also the relative value is 0)! */
+	 *  	 minimum distance only (of also the relative value is 0)! */
 	uq1_15_t AbsMinDistanceScope;
 
-	/*! The absolute minimum distance threshold value in m.
-	 *
-	 *  Pixels with distance below this threshold value are dismissed. */
-	q9_22_t AbsMinDistanceThreshold;
-
 	/*! The pre-filter pixel mask determines the pixel channels that are
-	 *  statically excluded from the pixel binning (i.e. 1D distance) result.
+	 * 	statically excluded from the pixel binning (i.e. 1D distance) result.
 	 *
-	 *  The pixel enabled mask is an 32-bit mask that determines the
-	 *  device internal channel number. It is recommended to use the
-	 *   - #PIXELXY_ISENABLED(msk, x, y)
-	 *   - #PIXELXY_ENABLE(msk, x, y)
-	 *   - #PIXELXY_DISABLE(msk, x, y)
-	 *   .
-	 *  macros to work with the pixel enable masks. */
+	 * 	The pixel enabled mask is an 32-bit mask that determines the
+	 * 	device internal channel number. It is recommended to use the
+	 * 	 - #PIXELXY_ISENABLED(msk, x, y)
+	 * 	 - #PIXELXY_ENABLE(msk, x, y)
+	 * 	 - #PIXELXY_DISABLE(msk, x, y)
+	 * 	 .
+	 * 	macros to work with the pixel enable masks. */
 	uint32_t PrefilterMask;
 
 } argus_cfg_pba_t;
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_PBA_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Defines the device pixel measurement results data structure.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Defines the device pixel measurement results data structure.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,40 +36,34 @@
 
 #ifndef ARGUS_PX_H
 #define ARGUS_PX_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @addtogroup  argus_res
+ * @addtogroup 	argusres
  * @{
  *****************************************************************************/
 
-#include "argus_def.h"
-
-
 /*! Maximum amplitude value in UQ12.4 format. */
-#define ARGUS_AMPLITUDE_MAX     (0xFFF0U)
+#define ARGUS_AMPLITUDE_MAX		(0xFFF0U)
 
 /*! Maximum range value in Q9.22 format.
  * Also used as a special value to determine no object detected or infinity range. */
 #define ARGUS_RANGE_MAX (Q9_22_MAX)
 
 /*!***************************************************************************
- * @brief   Status flags for the evaluated pixel structure.
+ * @brief	Status flags for the evaluated pixel structure.
  *
  * @details Determines the pixel status. 0 means OK (#PIXEL_OK).
- *          - [0]: #PIXEL_OFF: Pixel was disabled and not read from the device.
- *          - [1]: #PIXEL_SAT: The pixel was saturated.
- *          - [2]: #PIXEL_BIN_EXCL: The pixel was excluded from the 1D result.
- *          - [3]: #PIXEL_INVALID: The pixel data is invalid.
- *          - [4]: #PIXEL_PREFILTERED: The was pre-filtered by static mask.
- *          - [5]: #PIXEL_NO_SIGNAL: The pixel has no valid signal.
- *          - [6]: #PIXEL_OUT_OF_SYNC: The pixel has lost signal trace.
- *          - [7]: #PIXEL_STALLED: The pixel value is stalled due to errors.
- *          .
+ * 			- [0]: #PIXEL_OFF: Pixel was disabled and not read from the device.
+ * 			- [1]: #PIXEL_SAT: The pixel was saturated.
+ * 			- [2]: #PIXEL_BIN_EXCL: The pixel was excluded from the 1D result.
+ * 			- [3]: #PIXEL_AMPL_MIN: The pixel amplitude has evaluated to 0.
+ * 			- [4]: #PIXEL_PREFILTERED: The was pre-filtered by static mask.
+ * 			- [5]: #PIXEL_NO_SIGNAL: The pixel has no valid signal.
+ * 			- [6]: #PIXEL_OUT_OF_SYNC: The pixel has lost signal trace.
+ * 			- [7]: #PIXEL_STALLED: The pixel value is stalled due to errors.
+ * 			.
  *****************************************************************************/
-typedef enum argus_px_status_t {
+typedef enum {
 	/*! 0x00: Pixel status OK. */
 	PIXEL_OK = 0,
 
@@ -83,45 +77,43 @@ typedef enum argus_px_status_t {
 	/*! 0x04: Pixel is excluded from the pixel binning (1d) result. */
 	PIXEL_BIN_EXCL = 1U << 2U,
 
-	/*! 0x08: Pixel has invalid data due to miscellaneous reasons, e.g.
-	 *        - Amplitude calculates to 0 (i.e. division by 0)
-	 *        - Golden Pixel is invalid due to other saturated pixel.
-	 *        - Range/distance is negative. */
-	PIXEL_INVALID = 1U << 3U,
+	/*! 0x08: Pixel amplitude minimum underrun
+	 *        (i.e. the amplitude calculation yields 0). */
+	PIXEL_AMPL_MIN = 1U << 3U,
 
 	/*! 0x10: Pixel is pre-filtered by the static pixel binning pre-filter mask,
-	 *        i.e. the pixel is disabled by software. */
+	 * 	      i.e. the pixel is disabled by software. */
 	PIXEL_PREFILTERED = 1U << 4U,
 
 	/*! 0x20: Pixel amplitude is below its threshold value. The received signal
-	 *        strength is too low to evaluate a valid signal. The range value is
-	 *        set to the maximum possible value (approx. 512 m). */
+	 *  	  strength is too low to evaluate a valid signal. The range value is
+	 *  	  set to the maximum possible value (approx. 512 m). */
 	PIXEL_NO_SIGNAL = 1U << 5U,
 
 	/*! 0x40: Pixel is not in sync with respect to the dual frequency algorithm.
-	 *        I.e. the pixel may have a correct value but is estimated into the
-	 *        wrong unambiguous window. */
+	 * 		  I.e. the pixel may have a correct value but is estimated into the
+	 * 		  wrong unambiguous window. */
 	PIXEL_OUT_OF_SYNC = 1U << 6U,
 
 	/*! 0x80: Pixel is stalled due to one of the following reasons:
-	 *        - #PIXEL_SAT
-	 *        - #PIXEL_INVALID
-	 *        - #PIXEL_OUT_OF_SYNC
-	 *        - Global Measurement Error
-	 *        .
-	 *        A stalled pixel does not update its measurement data and keeps the
-	 *        previous values. If the issue is resolved, the stall disappears and
-	 *        the pixel is updating again. */
+	 * 		  - #PIXEL_SAT
+	 * 		  - #PIXEL_AMPL_MIN
+	 * 		  - #PIXEL_OUT_OF_SYNC
+	 * 		  - Global Measurement Error
+	 * 		  .
+	 * 		  A stalled pixel does not update its measurement data and keeps the
+	 * 		  previous values. If the issue is resolved, the stall disappears and
+	 * 		  the pixel is updating again. */
 	PIXEL_STALLED = 1U << 7U
 
 } argus_px_status_t;
 
 /*!***************************************************************************
- * @brief   The evaluated measurement results per pixel.
- * @details This structure contains the evaluated data for a single pixel.\n
- *          If the amplitude is 0, the pixel is turned off or has invalid data.
+ * @brief	The evaluated measurement results per pixel.
+ * @details	This structure contains the evaluated data for a single pixel.\n
+ * 			If the amplitude is 0, the pixel is turned off or has invalid data.
  *****************************************************************************/
-typedef struct argus_pixel_t {
+typedef struct {
 	/*! Range Values from the device in meter. It is the actual distance before
 	 *  software adjustments/calibrations. */
 	q9_22_t Range;
@@ -149,23 +141,14 @@ typedef struct argus_pixel_t {
 /*!***************************************************************************
  * @brief Representation of a correlation vector containing sine/cosine components.
  *****************************************************************************/
-typedef struct argus_vector_t {
-	union {
-		/*! The sine [0] and cosine [1] components. */
-		q15_16_t SC[2];
+typedef struct {
+	/*! The sine component. */
+	q15_16_t S;
 
-		struct {
-			/*! The sine component. */
-			q15_16_t S;
+	/*! The cosine component. */
+	q15_16_t C;
 
-			/*! The cosine component. */
-			q15_16_t C;
-		};
-	};
 } argus_vector_t;
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_PX_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Defines the generic measurement results data structure.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Defines the generic measurement results data structure.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,39 +36,35 @@
 
 #ifndef ARGUS_RES_H
 #define ARGUS_RES_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_res Measurement Data
- * @ingroup     argus_api
+ * @defgroup	argusres Measurement Data
+ * @ingroup		argusapi
  *
- * @brief       Measurement results data structures.
+ * @brief		Measurement results data structures.
  *
- * @details     The interface defines all data structures that correspond to
- *              the AFBR-S50 measurement results, e.g.
- *                  - 1D distance and amplitude values,
- *                  - 3D distance and amplitude values (i.e. per pixel),
- *                  - Auxiliary channel measurement results (VDD, IAPD, temperature, ...)
- *                  - Device and result status
- *                  - ...
- *                  .
+ * @details		The interface defines all data structures that correspond to
+ * 				the AFBR-S50 measurement results, e.g.
+ * 					- 1D distance and amplitude values,
+ * 					- 3D distance and amplitude values (i.e. per pixel),
+ * 					- Auxiliary channel measurement results (VDD, IAPD, temperature, ...)
+ * 					- Device and result status
+ * 					- ...
+ * 					.
  *
- * @addtogroup  argus_res
+ * @addtogroup 	argusres
  * @{
  *****************************************************************************/
 
-#include "argus_px.h"
 #include "argus_def.h"
+#include "argus_px.h"
 #include "argus_meas.h"
-#include "argus_xtalk.h"
 
 /*!***************************************************************************
- * @brief   The 1d measurement results data structure.
+ * @brief	The 1d measurement results data structure.
  * @details The 1d measurement results obtained by the Pixel Binning Algorithm.
  *****************************************************************************/
-typedef struct argus_results_bin_t {
+typedef struct {
 	/*! Raw 1D range value in meter (Q9.22 format). The distance obtained by
 	 *  the Pixel Binning Algorithm from the current measurement frame. */
 	q9_22_t Range;
@@ -87,11 +83,11 @@ typedef struct argus_results_bin_t {
 } argus_results_bin_t;
 
 /*!***************************************************************************
- * @brief   The auxiliary measurement results data structure.
- * @details The auxiliary measurement results obtained by the auxiliary task.\n
- *          Special values, i.e. 0xFFFFU, indicate no readout value available.
+ * @brief	The auxiliary measurement results data structure.
+ * @details	The auxiliary measurement results obtained by the auxiliary task.\n
+ * 			Special values, i.e. 0xFFFFU, indicate no readout value available.
  *****************************************************************************/
-typedef struct argus_results_aux_t {
+typedef struct {
 	/*! VDD ADC channel readout value.\n
 	 *  Special Value if no value has been measured:\n
 	 *  Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */
@@ -133,66 +129,32 @@ typedef struct argus_results_aux_t {
 } argus_results_aux_t;
 
 /*!***************************************************************************
- * @brief   The debug data of measurement results data structure.
- * @details This data structure will be filled with API internal data for
- *          debugging purposes.
- *****************************************************************************/
-typedef struct argus_results_debug_t {
-	/*! The amplitude that is evaluated and used in the DCA module. */
-	uq12_4_t DCAAmplitude;
-
-	/*! Raw x-y-sorted ADC results from the device.\n
-	 *  Data is arranged as 32-bit values in following order:
-	 *  index > phase; where index is pixel number n and auxiliary ADC channel.\n
-	 *  Note that disabled pixels are skipped.\n
-	 *  e.g. [n=0,p=0][n=0,p=1]..[n=0,p=3][n=1,p=0]...[n=1,p=3]...[n=31,p=3] */
-	uint32_t Data[ARGUS_RAW_DATA_VALUES];
-
-	/*! The current crosstalk correction values as determined by the
-	 *  crosstalk predictor algorithm. This is basically the temperature
-	 *  dependent portion of the crosstalk correction.\n
-	 *  Note that there are two values for the upper and lower two rows
-	 *  respectively. */
-	xtalk_t XtalkPredictor[ARGUS_PIXELS_Y / 2U];
-
-	/*! The current crosstalk correction values as determined by the
-	 *  crosstalk monitor algorithm. This is a dynamic portion of the
-	 *  crosstalk correction that is determined by monitoring passive
-	 *  pixels.\n
-	 *  Note that the values are valid row-wise. */
-	xtalk_t XtalkMonitor[ARGUS_PIXELS_Y];
-
-} argus_results_debug_t;
-
-/*!***************************************************************************
- * @brief   The measurement results data structure.
+ * @brief	The measurement results data structure.
  * @details This structure contains all information obtained by a single
- *          distance measurement on the device:
- *           - The measurement status can be read from the #Status.
- *           - A timing information is given via the #TimeStamp.
- *           - Information about the frame state is in the #Frame structure.
- *           - The 1D distance results are gathered under #Bin.
- *           - The 3D distance results for each pixel is at #Pixels or #Pixel.
- *           - Auxiliary values such as temperature can be found at #Auxiliary.
- *           - Raw data and debug information from the device and API is stored
- *             in the optional #Debug data structure. Note that this points to
- *             an optional structure and can be null!
- *           .
+ *			distance measurement on the device:
+ *			 - The measurement status can be read from the #Status.
+ *			 - A timing information is given via the #TimeStamp.
+ *			 - Information about the frame state is in the #Frame structure.
+ *			 - The 1D distance results are gathered under #Bin.
+ *			 - The 3D distance results for each pixel is at #Pixels or #Pixel.
+ *			 - Auxiliary values such as temperature can be found at #Auxiliary.
+ *			 - Raw data from the device is stored in the #Data array.
+ *			 .
  *
- *          The pixel x-y orientation is sketched in the following graph. Note that
- *          the laser source would be on the right side beyond the reference pixel.
- *          See also \link argus_map ADC Channel Mapping\endlink
+ *			The pixel x-y orientation is sketched in the following graph. Note that
+ *			the laser source would be on the right side beyond the reference pixel.
+ *			See also \link argusmap ADC Channel Mapping\endlink
  * @code
- *          // Pixel Field: Pixel[x][y]
- *          //
- *          // 0  -----------> x
- *          // |  O O O O O O O O
- *          // |   O O O O O O O O
- *          // |  O O O O O O O O          O (ref. Px)
- *          // y   O O O O O O O O
+ * 			// Pixel Field: Pixel[x][y]
+ * 			//
+ * 			// 0  -----------> x
+ * 			// |  O O O O O O O O
+ * 			// |   O O O O O O O O
+ * 			// |  O O O O O O O O          O (ref. Px)
+ * 			// y   O O O O O O O O
  * @endcode
  *****************************************************************************/
-typedef struct argus_results_t {
+typedef struct {
 	/*! The \link #status_t status\endlink of the current measurement frame.
 	 *   - 0 (i.e. #STATUS_OK) for a good measurement signal.
 	 *   - > 0 for warnings and weak measurement signal.
@@ -206,6 +168,13 @@ typedef struct argus_results_t {
 	/*! The configuration for the current measurement frame. */
 	argus_meas_frame_t Frame;
 
+	/*! Raw x-y-sorted ADC results from the device.\n
+	 *  Data is arranged as 32-bit values in following order:
+	 *  index > phase; where index is pixel number n and auxiliary ADC channel.\n
+	 *  Note that disabled pixels are skipped.\n
+	 *  e.g. [n=0,p=0][n=0,p=1]..[n=0,p=3][n=1,p=0]...[n=1,p=3]...[n=31,p=3] */
+	uint32_t Data[ARGUS_RAW_DATA_VALUES];
+
 	union {
 		/*! Pixel data indexed by channel number n.\n
 		 *  Contains calibrated range, amplitude and pixel status among others.
@@ -214,14 +183,14 @@ typedef struct argus_results_t {
 		 *  - 0..31: active pixels
 		 *  - 32:    reference pixel
 		 *
-		 *  See also \link argus_map ADC Channel Mapping\endlink */
+		 *  See also \link argusmap ADC Channel Mapping\endlink */
 		argus_pixel_t Pixels[ARGUS_PIXELS + 1U];
 
 		struct {
 			/*! Pixel data indexed by x-y-indices.\n
 			 *  The pixels are ordered in a two dimensional array that represent
 			 *  the x and y indices of the pixel.\n
-			*   See also \link argus_map ADC Channel Mapping\endlink
+			*	See also \link argusmap ADC Channel Mapping\endlink
 			 *
 			 *  Contains calibrated range, amplitude and pixel status among others. */
 			argus_pixel_t Pixel[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
@@ -244,17 +213,8 @@ typedef struct argus_results_t {
 	/*! The auxiliary ADC channel data, e.g. sensor temperature. */
 	argus_results_aux_t Auxiliary;
 
-	/*! Optional Debug Data.
-	 *  If the pointer is set to a #argus_results_debug_t data structure before
-	 *  passing it to the #Argus_EvaluateData function, the data structure is
-	 *  filled with internal parameters for debugging purposes. */
-	argus_results_debug_t *Debug;
-
 } argus_results_t;
 
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_RES_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Defines the Shot Noise Monitor (SNM) setup parameters.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Defines the Shot Noise Monitor (SNM) setup parameters.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,33 +36,30 @@
 
 #ifndef ARGUS_SNM_H
 #define ARGUS_SNM_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_snm Shot Noise Monitor
- * @ingroup     argus_api
+ * @defgroup 	argussnm Shot Noise Monitor
+ * @ingroup		argusapi
  *
- * @brief       Shot Noise Monitor (SNM) parameter definitions and API functions.
+ * @brief		Shot Noise Monitor (SNM) parameter definitions and API functions.
  *
- * @details     The SNM is an algorithm to monitor and react on shot noise
- *              induced by harsh environment conditions like high ambient
- *              light.
+ * @details		The SNM is an algorithm to monitor and react on shot noise
+ * 				induced by harsh environment conditions like high ambient
+ * 				light.
  *
- *              The AFBR-S50 API provides three modes:
- *              - Dynamic: Automatic mode, automatically adopts to current
- *                         ambient conditions.
- *              - Static (Outdoor): Static mode, optimized for outdoor applications.
- *              - Static (Indoor): Static mode, optimized for indoor applications.
- *              .
+ *				The AFBR-S50 API provides three modes:
+ *				- Dynamic: Automatic mode, automatically adopts to current
+ *						   ambient conditions.
+ *				- Static (Outdoor): Static mode, optimized for outdoor applications.
+ *				- Static (Indoor): Static mode, optimized for indoor applications.
+ *				.
  *
- * @addtogroup  argus_snm
+ * @addtogroup 	argussnm
  * @{
  *****************************************************************************/
 
 /*! The Shot Noise Monitor modes enumeration. */
-typedef enum argus_snm_mode_t {
+typedef enum {
 	/*! Static Shot Noise Monitoring Mode, optimized for indoor applications.
 	 *  Assumes the best case scenario, i.e. no bad influence from ambient conditions.
 	 *  Thus it uses a fixed setting that will result in the best performance.
@@ -82,7 +79,4 @@ typedef enum argus_snm_mode_t {
 
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_SNM_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Provides status codes for the AFBR-S50 API.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Provides status codes for the AFBR-S50 API.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,32 +36,25 @@
 
 #ifndef ARGUS_STATUS_H
 #define ARGUS_STATUS_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 #include <stdint.h>
 
 /*!***************************************************************************
- * @defgroup    argus_status Status Codes
- * @ingroup     argus
- *
+ * @defgroup    status Status Codes
  * @brief       Status and Error Code Definitions
- *
  * @details     Defines status and error codes for function return values.
  *              Basic status number structure:
  *              - 0 is OK or no error.
  *              - negative values determine errors.
  *              - positive values determine warnings or status information.
  *              .
- *
- * @addtogroup  argus_status
+ * @addtogroup  status
  * @{
  *****************************************************************************/
 
 /*!***************************************************************************
- * @brief   Type used for all status and error return values.
- * @details Basic status number structure:
+ * @brief  	Type used for all status and error return values.
+ * @details	Basic status number structure:
  *           - 0 is OK or no error.
  *           - negative values determine errors.
  *           - positive values determine warnings or status information.
@@ -145,8 +138,8 @@ enum Status {
 	 ********** NVM / Flash  Layer Status *********************************************************
 	 *********************************************************************************************/
 
-	/*! -98: Flash Error: The read memory block was not written previously and contains no data. */
-	ERROR_NVM_EMPTY = -98,
+	/*! -98: Flash Error: The version of the settings in the flash memory is not compatible. */
+	ERROR_NVM_INVALID_FILE_VERSION = -98,
 
 	/*! -99: Flash Error: The memory is out of range. */
 	ERROR_NVM_OUT_OF_RANGE = -99,
@@ -190,13 +183,6 @@ enum Status {
 	/*! -102: AFBR-S50 Error: Inconsistent configuration parameters. */
 	ERROR_ARGUS_INVALID_CFG = -102,
 
-	/*! -103: AFBR-S50 Error: The evaluation function has been called but no
-	 *  raw data is available yet.
-	 *  See also #Argus_EvaluateData for more information. */
-	ERROR_ARGUS_BUFFER_EMPTY = -103,
-
-	/*! -104: AFBR-S50 Error: Invalid slave identifier is passed to the module. */
-	ERROR_ARGUS_INVALID_SLAVE = -104,
 
 	/*! -105: AFBR-S50 Error: Invalid measurement mode configuration parameter. */
 	ERROR_ARGUS_INVALID_MODE = -105,
@@ -205,6 +191,7 @@ enum Status {
 	 *  The current measurement data set is invalid! */
 	ERROR_ARGUS_BIAS_VOLTAGE_REINIT = -107,
 
+
 	/*! -109: AFBR-S50 Error: The EEPROM readout has failed. The failure is detected
 	 *  by three distinct read attempts, each resulting in invalid data.
 	 *  Note: this state differs from that #STATUS_ARGUS_EEPROM_BIT_ERROR
@@ -237,6 +224,7 @@ enum Status {
 	 *  requested command. */
 	ERROR_ARGUS_BUSY = -191,
 
+
 	/*! -199: AFBR-S50 Error: Unknown module number. */
 	ERROR_ARGUS_UNKNOWN_MODULE = -199,
 
@@ -247,22 +235,24 @@ enum Status {
 	ERROR_ARGUS_UNKNOWN_LASER = -197,
 
 
-	/*! 191: AFBR-S50 Status (internal): The device is currently busy with testing the
-	 *  SPI connection to the device. */
-	STATUS_ARGUS_BUSY_TEST = 191,
 
-	/*! 192: AFBR-S50 Status (internal): The device is currently busy with updating the
-	 *  settings parameter (i.e. with writing register values). */
-	STATUS_ARGUS_BUSY_UPDATE = 192,
+	/*! 193: AFBR-S50 Status (internal): The device is currently busy with updating the
+	 *  configuration (i.e. with writing register values). */
+	STATUS_ARGUS_BUSY_CFG_UPDATE = 193,
+
+	/*! 194: AFBR-S50 Status (internal): The device is currently busy with updating the
+	 *  calibration data (i.e. writing to register values). */
+	STATUS_ARGUS_BUSY_CAL_UPDATE = 194,
 
 	/*! 195: AFBR-S50 Status (internal): The device is currently executing a calibration
-	 *  sequence. */
+	 * 	sequence. */
 	STATUS_ARGUS_BUSY_CAL_SEQ = 195,
 
 	/*! 196: AFBR-S50 Status (internal): The device is currently executing a measurement
 	 *  cycle. */
 	STATUS_ARGUS_BUSY_MEAS = 196,
 
+
 	/*! 100: AFBR-S50 Status (internal): The ASIC is initializing a new measurement, i.e.
 	 *  a register value is written that starts an integration cycle on the ASIC. */
 	STATUS_ARGUS_STARTING = 100,
@@ -270,10 +260,9 @@ enum Status {
 	/*! 103: AFBR-S50 Status (internal): The ASIC is performing an integration cycle. */
 	STATUS_ARGUS_ACTIVE = 103,
 
+
+
 };
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_STATUS_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file contains the current API version number.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		This file contains the current API version number.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,19 +36,16 @@
 
 #ifndef ARGUS_VERSION_H
 #define ARGUS_VERSION_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_version API Version
- * @ingroup     argus_api
+ * @defgroup	version API Version
+ * @ingroup		argusapi
  *
- * @brief       API and library core version number
+ * @brief		API and library core version number
  *
- * @details     Contains the AFBR-S50 API and Library Core Version Number.
+ * @details		Contains the AFBR-S50 API and Library Core Version Number.
  *
- * @addtogroup  argus_version
+ * @addtogroup 	version
  * @{
  *****************************************************************************/
 
@@ -56,13 +53,13 @@ extern "C" {
 #define ARGUS_API_VERSION_MAJOR    1
 
 /*! Minor version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_MINOR    4
+#define ARGUS_API_VERSION_MINOR    3
 
 /*! Bugfix version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_BUGFIX   4
+#define ARGUS_API_VERSION_BUGFIX   5
 
-/*! Build version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_BUILD    "20230327150535"
+/*! Build version nunber of the AFBR-S50 API. */
+#define ARGUS_API_VERSION_BUILD    "20210812171515"
 
 /*****************************************************************************/
 
@@ -76,7 +73,4 @@ extern "C" {
 				       (ARGUS_API_VERSION_BUGFIX))
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_VERSION_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 hardware API.
- * @details     Defines the generic device calibration API.
+ * @brief    	This file is part of the AFBR-S50 hardware API.
+ * @details		Defines the generic device calibration API.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,71 +36,44 @@
 
 #ifndef ARGUS_XTALK_H
 #define ARGUS_XTALK_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @addtogroup  argus_cal
+ * @addtogroup 	arguscal
  * @{
  *****************************************************************************/
 
-#include "argus_def.h"
-#include "argus_dfm.h"
+#include "api/argus_def.h"
 
 /*!***************************************************************************
- * @brief   Pixel Crosstalk Compensation Vector.
- * @details Contains calibration data (per pixel) that belongs to the
- *          RX-TX-Crosstalk compensation feature.
- *          The crosstalk vector consists of a Sine and Cosine component in LSB.
+ * @brief	Pixel Crosstalk Compensation Vector.
+ * @details	Contains calibration data (per pixel) that belongs to the
+ * 			RX-TX-Crosstalk compensation feature.
  *****************************************************************************/
-typedef struct xtalk_t {
+
+/*! Pixel Crosstalk Vector */
+typedef struct {
 	/*! Crosstalk Vector - Sine component.
-	 *  Units: LSB
 	 *  Special Value: Q11_4_MIN == not available */
 	q11_4_t dS;
 
 	/*! Crosstalk Vector - Cosine component.
-	 *  Units: LSB
 	 *  Special Value: Q11_4_MIN == not available */
 	q11_4_t dC;
 
 } xtalk_t;
 
 /*!***************************************************************************
- * @brief   Pixel Crosstalk Vector Table.
- * @details Contains crosstalk vector values for all 32 active pixels,
- *          separated for A/B-Frames.
+ * @brief	Pixel-To-Pixel Crosstalk Compensation Parameters.
+ * @details	Contains calibration data that belongs to the pixel-to-pixel
+ * 			crosstalk compensation feature.
  *****************************************************************************/
-typedef struct argus_cal_xtalk_table_t {
-	union {
-		struct {
-			/*! The crosstalk vector table for A-Frames. */
-			xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
-
-			/*! The crosstalk vector table for B-Frames. */
-			xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
-		};
-
-		/*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/
-		xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
-	};
-
-} argus_cal_xtalk_table_t;
-
-
-/*!***************************************************************************
- * @brief   Pixel-To-Pixel Crosstalk Compensation Parameters.
- * @details Contains calibration data that belongs to the pixel-to-pixel
- *          crosstalk compensation feature.
- *****************************************************************************/
-typedef struct argus_cal_p2pxtalk_t {
+typedef struct {
 	/*! Pixel-To-Pixel Compensation on/off. */
 	bool Enabled;
 
 	/*! The relative threshold determines when the compensation is active for
 	 *  each individual pixel. The value determines the ratio of the individual
-	 *  pixel signal with respect to the overall average signal. If the
+	 *  pixel signal is with respect to the overall average signal. If the
 	 *  ratio is smaller than the value, the compensation is active. Absolute
 	 *  and relative conditions are connected with AND logic. */
 	uq0_8_t RelativeThreshold;
@@ -138,7 +111,4 @@ typedef struct argus_cal_p2pxtalk_t {
 
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_XTALK_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file the main header of the AFBR-S50 API.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		This file the main header of the AFBR-S50 API.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,6 +36,7 @@
 
 #ifndef ARGUS_H
 #define ARGUS_H
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -43,6 +44,7 @@ extern "C" {
 #include "api/argus_api.h"
 
 #ifdef __cplusplus
-} // extern "C"
+}
 #endif
+
 #endif /* ARGUS_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file provides an interface for enabling/disabling interrupts.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		This file provides an interface for enabling/disabling interrupts.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,94 +36,96 @@
 
 #ifndef ARGUS_IRQ_H
 #define ARGUS_IRQ_H
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 /*!***************************************************************************
- * @defgroup    argus_irq IRQ: Global Interrupt Control Layer
- * @ingroup     argus_hal
+ * @defgroup	argus_irq IRQ: Global Interrupt Control Layer
+ * @ingroup		argus_platform
  *
- * @brief       Global Interrupt Control Layer
+ * @brief		Global Interrupt Control Layer
  *
- * @details     This module provides functionality to globally enable/disable
- *              interrupts in a nested way.
+ * @details		This module provides functionality to globally enable/disable
+ *				interrupts in a nested way.
  *
- *              Here is a simple example implementation using the CMSIS functions
- *              "__enable_irq()" and "__disable_irq()". An integer counter is
- *              used to achieve nested interrupt disabling:
+ *				Here is a simple example implementation using the CMSIS functions
+ *				"__enable_irq()" and "__disable_irq()". An integer counter is
+ *				used to achieve nested interrupt disabling:
  *
- *              @code
+ *				@code
  *
- *              // Global lock level counter value.
- *              static volatile int g_irq_lock_ct;
+ *				// Global lock level counter value.
+ *				static volatile int g_irq_lock_ct;
  *
- *              // Global unlock all interrupts using CMSIS function "__enable_irq()".
- *              void IRQ_UNLOCK(void)
- *              {
- *                  assert(g_irq_lock_ct > 0);
- *                  if (--g_irq_lock_ct <= 0)
- *                  {
- *                      g_irq_lock_ct = 0;
- *                      __enable_irq();
- *                  }
- *              }
+ *				// Global unlock all interrupts using CMSIS function "__enable_irq()".
+ *				void IRQ_UNLOCK(void)
+ *				{
+ *					assert(g_irq_lock_ct > 0);
+ *					if (--g_irq_lock_ct <= 0)
+ *					{
+ *						g_irq_lock_ct = 0;
+ *						__enable_irq();
+ *					}
+ *				}
  *
- *              // Global lock all interrupts using CMSIS function "__disable_irq()".
- *              void IRQ_LOCK(void)
- *              {
- *                  __disable_irq();
- *                  g_irq_lock_ct++;
- *              }
+ *				// Global lock all interrupts using CMSIS function "__disable_irq()".
+ *				void IRQ_LOCK(void)
+ *				{
+ *					__disable_irq();
+ *					g_irq_lock_ct++;
+ *				}
  *
- *              @endcode
+ *				@endcode
  *
- * @note        The IRQ locking mechanism is used to create atomic sections
- *              (within the scope of the AFBR-S50 API) that are very few processor
- *              instruction only. It does NOT lock interrupts for considerable
- *              amounts of time.
+ * @note		The IRQ locking mechanism is used to create atomic sections
+ * 				(within the scope of the AFBR-S50 API) that	are very few processor
+ * 				instruction only. It does NOT lock interrupts for considerable
+ * 				amounts of time.
  *
- * @note        The IRQ_LOCK might get called multiple times. Therefore, the
- *              API expects that the IRQ_UNLOCK must be called as many times as
- *              the IRQ_LOCK was called before the interrupts are enabled.
+ * @note 		The IRQ_LOCK might get called multiple times. Therefore, the
+ * 				API expects that the IRQ_UNLOCK must be called as many times as
+ * 				the IRQ_LOCK was called before the interrupts are enabled.
  *
- * @note        The interrupts utilized by the AFBR-S50 API can be interrupted
- *              by other, higher prioritized interrupts, e.g. some system
- *              critical interrupts. In this case, the IRQ_LOCK/IRQ_UNLOCK
- *              mechanism can be implemented such that only the interrupts
- *              required for the AFBR-S50 API are locked. The above example is
- *              dedicated to a ARM Corex-M0 architecture, where interrupts
- *              can only disabled at a global scope. Other architectures like
- *              ARM Cortex-M4 allow selective disabling of interrupts.
+ * @note		The interrupts utilized by the AFBR-S50 API can be interrupted
+ * 				by other, higher prioritized interrupts, e.g. some system
+ * 				critical interrupts. In this case, the IRQ_LOCK/IRQ_UNLOCK
+ * 				mechanism can be implemented such that only the interrupts
+ * 				required for the AFBR-S50 API are locked. The above	example is
+ * 				dedicated to a ARM Corex-M0 architecture, where interrupts
+ *				can only disabled at a global scope. Other architectures like
+ *				ARM Cortex-M4 allow selective disabling of interrupts.
  *
- * @addtogroup  argus_irq
+ * @addtogroup 	argus_irq
  * @{
  *****************************************************************************/
 
 /*!***************************************************************************
- * @brief   Enable IRQ Interrupts
+ * @brief	Enable IRQ Interrupts
  *
- * @details Enables IRQ interrupts and enters an atomic or critical section.
+ * @details	Enables IRQ interrupts and enters an atomic or critical section.
  *
- * @note    The IRQ_LOCK might get called multiple times. Therefore, the
- *          API expects that the IRQ_UNLOCK must be called as many times as
- *          the IRQ_LOCK was called before the interrupts are enabled.
+ * @note 	The IRQ_LOCK might get called multiple times. Therefore, the
+ * 			API expects that the IRQ_UNLOCK must be called as many times as
+ * 			the IRQ_LOCK was called before the interrupts are enabled.
  *****************************************************************************/
 void IRQ_UNLOCK(void);
 
 /*!***************************************************************************
- * @brief   Disable IRQ Interrupts
+ * @brief	Disable IRQ Interrupts
  *
- * @details Disables IRQ interrupts and leaves the atomic or critical section.
+ * @details	Disables IRQ interrupts and leaves the atomic or critical section.
  *
- * @note    The IRQ_LOCK might get called multiple times. Therefore, the
- *          API expects that the IRQ_UNLOCK must be called as many times as
- *          the IRQ_LOCK was called before the interrupts are enabled.
+ * @note 	The IRQ_LOCK might get called multiple times. Therefore, the
+ * 			API expects that the IRQ_UNLOCK must be called as many times as
+ * 			the IRQ_LOCK was called before the interrupts are enabled.
  *****************************************************************************/
 void IRQ_LOCK(void);
 
-/*! @} */
 #ifdef __cplusplus
-} // extern "C"
+}
 #endif
+
+/*! @} */
 #endif // ARGUS_IRQ_H

src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file provides an interface for the optional non-volatile memory.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		This file provides an interface for the optional non-volatile memory.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,113 +36,99 @@
 
 #ifndef ARGUS_NVM_H
 #define ARGUS_NVM_H
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 /*!***************************************************************************
- * @defgroup    argus_nvm NVM: Non-Volatile Memory Layer
- * @ingroup     argus_hal
+ * @defgroup	argus_nvm NVM: Non-Volatile Memory Layer
+ * @ingroup		argus_platform
  *
- * @brief       Non-Volatile Memory Layer
+ * @brief		Non-Volatile Memory Layer
  *
- * @details     This module provides functionality to access the non-volatile
- *              memory (e.g. flash) on the underlying platform.
+ * @details		This module provides functionality to access the non-volatile
+ * 				memory (e.g. flash) on the underlying platform.
  *
- *              This module is optional and only required if calibration data
- *              needs to be stored within the API.
+ * 				This module is optional and only required if calibration data
+ * 				needs to be stored within the API.
  *
- * @note        The implementation of this module is optional for the correct
- *              execution of the API. If not implemented, a weak implementation
- *              within the API will be used that disables the NVM feature.
+ * @note		The implementation of this module is optional for the correct
+ * 				execution of the API. If not implemented, a weak implementation
+ * 				within the API will be used that disables the NVM feature.
  *
- * @addtogroup  argus_nvm
+ * @addtogroup 	argus_nvm
  * @{
  *****************************************************************************/
 
-#include "api/argus_def.h"
-
-/*! The NVM block size in the non-volatile memory. */
-#define ARGUS_NVM_BLOCK_SIZE 0x300 // 768 bytes
+#include "argus.h"
 
 /*!***************************************************************************
- * @brief   Write a block of data to the non-volatile memory.
+ * @brief	Initializes the non-volatile memory unit and reserves a chunk of memory.
  *
- * @details The function is called whenever the API wants to write data into
- *          non-volatile memory, e.g. flash. Later, the API reads the written
- *          data via the #NVM_ReadBlock function.
+ * @details The function is called upon API initialization sequence. If available,
+ * 			the non-volatile memory module reserves a chunk of memory with the
+ * 			provides number of bytes (size) and returns with #STATUS_OK.
  *
- *          The data shall be written to a specified memory block that is
- *          uniquely dedicated to each individual device. The /p id parameter
- *          is passed to the function that identifies the device. The /p id
- *          is composed of the device ID and module type, i.e. it is unique
- *          among all devices. If only a single device is used anyway, the
- *          /p id parameter can be ignored.
+ * 			If not implemented, the function should return #ERROR_NOT_IMPLEMENTED
+ * 			in oder to inform the API to not use the NVM module.
  *
- *          If no NVM module is available, the function can return with error
- *          #ERROR_NOT_IMPLEMENTED and the API ignores the NVM.
+ * 			After initialization, the API calls the #NVM_Write and #NVM_Read
+ * 			methods to write within the reserved chunk of memory.
  *
- *          If write fails, e.g. due to lack of memory, a negative status
- *          must be returned, e.g. #ERROR_NVM_OUT_OF_RANGE.
+ * @note	The implementation of this function is optional for the correct
+ * 			execution of the API. If not implemented, a weak implementation
+ * 			within the API will be used that disables the NVM feature.
  *
- *          The block size is fixed for a single device. The actual block size
- *          is defined with #ARGUS_NVM_BLOCK_SIZE.
- *
- * @note    The implementation of this function is optional for the correct
- *          execution of the API. If not implemented, a weak implementation
- *          within the API will be used that disables the NVM feature.
- *
- * @param   id The 32-bit ID number to identify the corresponding memory block.
- * @param   block_size The number of bytes to be written. Note that this value
- *                     is fixed, i.e. the API always writes the same data size.
- *                     The size is defined here: #ARGUS_NVM_BLOCK_SIZE.
- * @param   buf The pointer to the data buffer of size /p block_size that needs
- *              to be written to the NVM.
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @param	size The required size of NVM to store all parameters.
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t NVM_WriteBlock(uint32_t id, uint32_t block_size, uint8_t const *buf);
+status_t NVM_Init(uint32_t size);
 
 /*!***************************************************************************
- * @brief   Reads a block of data from the non-volatile memory.
+ * @brief	Write a block of data to the non-volatile memory.
  *
- * @details The function is called whenever the API wants to read data from
- *          non-volatile memory, e.g. flash. The data will be previously
- *          stored using the #NVM_WriteBlock function. Otherwise, the function
- *          must return a corresponding error code, namely #ERROR_NVM_EMPTY.
+ * @details	The function is called whenever the API wants to write data into
+ * 			the previously reserved (#NVM_Init) memory block. The data shall
+ * 			be written at a given offset and with a given size.
  *
- *          The data shall be read from a specified memory block that is
- *          uniquely dedicated to each individual device. The /p id parameter
- *          is passed to the function that identifies the device. The /p id
- *          is composed of the device ID and module type, i.e. it is unique
- *          among all devices. If only a single device is used anyway, the
- *          /p id parameter can be ignored.
+ *			If no NVM module is available, the function can return with error
+ *			#ERROR_NOT_IMPLEMENTED.
  *
- *          If no NVM module is available, the function can return with error
- *          #ERROR_NOT_IMPLEMENTED and the API ignores the NVM.
+ * @note	The implementation of this function is optional for the correct
+ * 			execution of the API. If not implemented, a weak implementation
+ * 			within the API will be used that disables the NVM feature.
  *
- *          If read fails, e.g. if data has not been written previously,
- *          a negative status must be returned, e.g. #ERROR_NVM_EMPTY if no
- *          data has been written yet or any other negative error else-wise.
- *
- *          The block size is fixed for a single device. The actual block size
- *          is defined with #ARGUS_NVM_BLOCK_SIZE.
- *
- * @note    The implementation of this function is optional for the correct
- *          execution of the API. If not implemented, a weak implementation
- *          within the API will be used that disables the NVM feature.
- *
- * @param   id The 32-bit ID number to identify the corresponding memory block.
- * @param   block_size The number of bytes to be read. Note that this value
- *                     is fixed, i.e. the API always reads the same data size.
- *                     The size is defined here: #ARGUS_NVM_BLOCK_SIZE.
- * @param   buf The pointer to the data buffer of size /p block_size to copy
- *              the data to.
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @param	offset The index offset where the first byte needs to be written.
+ * @param	size The number of bytes to be written.
+ * @param	buf The pointer to the data buffer with the data to be written.
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t NVM_ReadBlock(uint32_t id, uint32_t block_size, uint8_t *buf);
+status_t NVM_Write(uint32_t offset, uint32_t size, uint8_t const *buf);
+
+/*!***************************************************************************
+ * @brief	Reads a block of data from the non-volatile memory.
+ *
+ * @details	The function is called whenever the API wants to read data from
+ * 			the previously reserved (#NVM_Init) memory block. The data shall
+ * 			be read at a given offset and with a given size.
+ *
+ *			If no NVM module is available, the function can return with error
+ *			#ERROR_NOT_IMPLEMENTED.
+ *
+ * @note	The implementation of this function is optional for the correct
+ * 			execution of the API. If not implemented, a weak implementation
+ * 			within the API will be used that disables the NVM feature.
+ *
+ * @param	offset The index offset where the first byte needs to be read.
+ * @param	size The number of bytes to be read.
+ * @param	buf The pointer to the data buffer to copy the data to.
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ *****************************************************************************/
+status_t NVM_Read(uint32_t offset, uint32_t size, uint8_t *buf);
+#ifdef __cplusplus
+}
+#endif
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif // ARGUS_NVM_H

src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file provides an interface for the optional debug module.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		This file provides an interface for the optional debug module.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,55 +36,48 @@
 
 #ifndef ARGUS_PRINT_H
 #define ARGUS_PRINT_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_log Debug: Logging Interface
- * @ingroup     argus_hal
+ * @defgroup	argus_log Debug: Logging Interface
+ * @ingroup		argus_platform
  *
- * @brief       Logging interface for the AFBR-S50 API.
+ * @brief		Logging interface for the AFBR-S50 API.
  *
- * @details     This interface provides logging utility functions.
- *              Defines a printf-like function that is used to print error and
- *              log messages.
+ * @details		This interface provides logging utility functions.
+ * 				Defines a printf-like function that is used to print error and
+ * 				log messages.
  *
- * @addtogroup  argus_log
+ * @addtogroup 	argus_log
  * @{
  *****************************************************************************/
 
 #include "api/argus_def.h"
 
 /*!***************************************************************************
- * @brief   A printf-like function to print formatted data to an debugging interface.
+ * @brief	A printf-like function to print formatted data to an debugging interface.
  *
  * @details Writes the C string pointed by fmt_t to an output. If format
- *          includes format specifiers (subsequences beginning with %), the
- *          additional arguments following fmt_t are formatted and inserted in
- *          the resulting string replacing their respective specifiers.
+ * 			includes format specifiers (subsequences beginning with %), the
+ * 			additional arguments following fmt_t are formatted and inserted in
+ * 			the resulting string replacing their respective specifiers.
  *
- *          To enable the print functionality, an implementation of the function
- *          must be provided that maps the output to an interface like UART or
- *          a debugging console, e.g. by forwarding to standard printf() method.
+ * 			To enable the print functionality, an implementation of the function
+ * 			must be provided that maps the output to an interface like UART or
+ * 			a debugging console, e.g. by forwarding to standard printf() method.
  *
- * @note    The implementation of this function is optional for the correct
- *          execution of the API. If not implemented, a weak implementation
- *          within the API will be used that does nothing. This will improve
- *          the performance but no error messages are logged.
+ * @note	The implementation of this function is optional for the correct
+ * 			execution of the API. If not implemented, a weak implementation
+ * 			within the API will be used that does nothing. This will improve
+ * 			the performance but no error messages are logged.
  *
- * @note    The naming is different from the standard printf() on purpose to
- *          prevent builtin compiler optimizations.
+ * @note	The naming is different from the standard printf() on purpose to
+ * 			prevent builtin compiler optimizations.
  *
- * @param   fmt_s The usual print() format string.
- * @param   ... The usual print() parameters.
- *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @param	fmt_s The usual print() format string.
+ * @param	... The usual print() parameters. *
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t print(const char *fmt_s, ...);
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* ARGUS_PRINT_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file provides an interface for the required S2PI module.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		This file provides an interface for the required S2PI module.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,94 +36,95 @@
 
 #ifndef ARGUS_S2PI_H
 #define ARGUS_S2PI_H
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 /*!***************************************************************************
- * @defgroup    argus_s2pi S2PI: Serial Peripheral Interface
- * @ingroup     argus_hal
+ * @defgroup	argus_s2pi S2PI: Serial Peripheral Interface
+ * @ingroup		argus_platform
  *
- * @brief       S2PI: SPI incl. GPIO Hardware Layer Module
+ * @brief		S2PI: SPI incl. GPIO Hardware Layer Module
  *
- * @details     The S2PI module consists of a standard SPI interface plus a
- *              single GPIO interrupt line. Furthermore, the SPI pins are
- *              accessible via GPIO control to allow a software emulation of
- *              additional protocols using the same pins.
+ * @details		The S2PI module consists of a standard SPI interface plus a
+ *				single GPIO interrupt line. Furthermore, the SPI pins are
+ *				accessible via GPIO control to allow a software emulation of
+ *				additional protocols using the same pins.
  *
- *              **SPI interface:**
+ *				**SPI interface:**
  *
- *              The SPI interface is based around a single functionality:
+ *				The SPI interface is based around a single functionality:
  *
- *              #S2PI_TransferFrame. This function transfers a specified number
- *              of bytes via the interfaces MOSI line and simultaneously reads
- *              the incoming data on the MOSI line. The read can also be skipped.
- *              The transfer happen asynchronously, e.g. via a DMA request. After
- *              finishing the transfer, the provided callback is invoked with
- *              the status of the transfer and the provided abstract parameter.
- *              Furthermore, the functions receives a slave parameter that can
- *              be used to connect multiple slaves, each with its individual
- *              chip select line.
+ *				#S2PI_TransferFrame. This function transfers a specified number
+ *				of bytes via the interfaces MOSI line and simultaneously reads
+ *				the incoming data on the MOSI line. The read can also be skipped.
+ *				The transfer happen asynchronously, e.g. via a DMA request. After
+ *				finishing the transfer, the provided callback is invoked with
+ *				the status of the transfer and the provided abstract parameter.
+ *				Furthermore, the functions receives a slave parameter that can
+ *				be used to connect multiple slaves, each with its individual
+ *				chip select line.
  *
- *              The interface also provides functionality to change the SPI
- *              baud rate. An additional abort method is used to cancel the
- *              ongoing transfer.
+ *				The interface also provides functionality to change the SPI
+ *				baud rate. An additional abort method is used to cancel the
+ *				ongoing transfer.
  *
- *              **GPIO interface:**
+ *				**GPIO interface:**
  *
- *              The GPIO part of the S2PI interface has two distinct concerns:
+ *				The GPIO part of the S2PI interface has two distinct concerns:
  *
- *              First, the GPIO interface handles the measurement finished interrupt
- *              from the device. When the device invokes the interrupt, it pulls
- *              the interrupt line to low. Thus the interrupt must trigger when
- *              a transition from high to low occurs on the interrupt line.
+ *				First, the GPIO interface handles the measurement finished interrupt
+ *				from the device. When the device invokes the interrupt, it pulls
+ *				the interrupt line to low. Thus the interrupt must trigger when
+ *				a transition from high to low occurs on the interrupt line.
  *
- *              The module simply invokes a callback when this interrupt occurs.
- *              The #S2PI_SetIrqCallback method is used to install the callback
- *              for a specified slave. Each slave will have its own interrupt
- *              line. An additional callback parameter can be set that would be
- *              passed to the callback function.
+ *				The module simply invokes a callback when this interrupt occurs.
+ *				The #S2PI_SetIrqCallback method is used to install the callback
+ *				for a specified slave. Each slave will have its own interrupt
+ *				line. An additional callback parameter can be set that would be
+ *				passed to the callback function.
  *
- *              In addition to the interrupt, all SPI pins need to be accessible
- *              as GPIO pins through this interface. This is required to read
- *              the EEPROM memory on the device hat is connected to the SPI
- *              pins but requires a different protocol that is not compatible
- *              to any standard SPI interface. Therefore, the interface provides
- *              the possibility to switch to GPIO control mode that allows to
- *              emulate the EEPROM protocol via software bit banging.
+ *				In addition to the interrupt, all SPI pins need to be accessible
+ *				as GPIO pins through this interface. This is required to read
+ *				the EEPROM memory on the device hat is connected to the SPI
+ *				pins but requires a different protocol that is not compatible
+ *				to any standard SPI interface. Therefore, the interface provides
+ *				the possibility to switch to GPIO control mode that allows to
+ *				emulate the EEPROM protocol via software bit banging.
  *
- *              Two methods are provided to switch forth and back between SPI
- *              and GPIO control. In GPIO mode, several functions are used to
- *              read and write the individual GPIO pins.
+ *				Two methods are provided to switch forth and back between SPI
+ *				and GPIO control. In GPIO mode, several functions are used to
+ *				read and write the individual GPIO pins.
  *
- *              Note that the GPIO mode is only required to readout the EEPROM
- *              upon initialization of the device, i.e. during execution of the
- *              #Argus_Init or #Argus_Reinit methods. The GPIO mode is not used
- *              during measurements.
+ *				Note that the GPIO mode is only required to readout the EEPROM
+ *				upon initialization of the device, i.e. during execution of the
+ *				#Argus_Init or #Argus_Reinit methods. The GPIO mode is not used
+ *				during measurements.
  *
  *
- * @addtogroup  argus_s2pi
+ * @addtogroup 	argus_s2pi
  * @{
  *****************************************************************************/
 
 #include "api/argus_def.h"
 
 /*!***************************************************************************
- * @brief   S2PI layer callback function type for the SPI transfer completed event.
+ * @brief 	S2PI layer callback function type for the SPI transfer completed event.
  *
- * @param   status The \link #status_t status\endlink of the completed
+ * @param	status The \link #status_t status\endlink of the completed
  *                   transfer (#STATUS_OK on success).
  *
- * @param   param The provided (optional, can be null) callback parameter.
+ * @param	param The provided (optional, can be null) callback parameter.
  *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 typedef status_t (*s2pi_callback_t)(status_t status, void *param);
 
 /*!***************************************************************************
- * @brief   S2PI layer callback function type for the GPIO interrupt event.
+ * @brief 	S2PI layer callback function type for the GPIO interrupt event.
  *
- * @param   param The provided (optional, can be null) callback parameter.
+ * @param	param The provided (optional, can be null) callback parameter.
  *****************************************************************************/
 typedef void (*s2pi_irq_callback_t)(void *param);
 
@@ -131,8 +132,8 @@ typedef void (*s2pi_irq_callback_t)(void *param);
  *  can be used to identify the slave within the SPI module. */
 typedef int32_t s2pi_slave_t;
 
-/*! The enumeration of S2PI pins. */
-typedef enum s2pi_pin_t {
+/*!	The enumeration of S2PI pins. */
+typedef enum {
 	/*! The SPI clock pin. */
 	S2PI_CLK,
 
@@ -152,141 +153,64 @@ typedef enum s2pi_pin_t {
 
 
 /*!***************************************************************************
- * @brief   Returns the status of the SPI module.
+ * @brief 	Returns the status of the SPI module.
  *
- * @param   slave The specified S2PI slave. Note that the slave information is
- *                only required if multiple SPI instances are used in order to
- *                map to the correct SPI instance.
- *
- * @return  Returns the \link #status_t status\endlink:
- *           - #STATUS_IDLE: No SPI transfer or GPIO access is ongoing.
- *           - #STATUS_BUSY: An SPI transfer is in progress.
- *           - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode.
+ * @return 	Returns the \link #status_t status\endlink:
+ * 			 - #STATUS_IDLE: No SPI transfer or GPIO access is ongoing.
+ *         	 - #STATUS_BUSY: An SPI transfer is in progress.
+ *         	 - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode.
  *****************************************************************************/
-status_t S2PI_GetStatus(s2pi_slave_t slave);
+status_t S2PI_GetStatus(void);
 
 /*!***************************************************************************
- * @brief   Tries to grab the SPI interface mutex for the next transfer.
- *
- * @details This mutex prevents new asynchronous SPI requests to interfere
- *          with transfers already in progress for this interface.
- *
- *          Note that this is only required if multiple device are connected to
- *          a single SPI interface. If only operating a single device per SPI,
- *          the function can simply always return #STATUS_OK.
- *
- *          There must be a dedicated mutex object per SPI interface if
- *          multiple SPI interfaces are used.
- *
- *          The mutex will be released in the #S2PI_ReleaseMutex function.
- *          See #S2PI_ReleaseMutex for additional information.
- *
- *          Here is a simple example implementation for the multiple devices on
- *          a single SPI interface case. Note that the SpiMutexBlocked must be
- *          defined per SPI interface if multiple SPI interfaces are used.
- *
- *          @code
- *          static volatile bool SpiMutexBlocked = false;
- *          status_t S2PI_TryGetMutex(s2pi_slave_t slave)
- *          {
- *              (void) slave; // not used in this implementation as all
- *                            // SPI slaves are on the same SPI interface
- *
- *              status_t status = STATUS_BUSY;
- *              IRQ_LOCK();
- *              if (!SpiMutexBlocked)
- *              {
- *                  SpiMutexBlocked = true;
- *                  status = STATUS_OK;
- *              }
- *              IRQ_UNLOCK();
- *              return status;
- *          }
- *          void S2PI_ReleaseMutex(s2pi_slave_t slave)
- *          {
- *              (void) slave; // not used in this implementation
- *              SpiMutexBlocked = false;
- *          }
- *          @endcode
- *
- * @param   slave The specified S2PI slave. Note that the slave information is
- *                only required if multiple SPI instances are used in order to
- *                map to the correct SPI instance.
- *
- * @return  Returns the \link #status_t status\endlink:
- *           - #STATUS_OK: the SPI interface was successfully reserved for the caller
- *           - #STATUS_BUSY: another transfer is ongoing, the caller must not access the bus
- *****************************************************************************/
-status_t S2PI_TryGetMutex(s2pi_slave_t slave);
-
-/*!***************************************************************************
- * @brief   Releases the SPI interface mutex.
- *
- * @details Once the mutex is captured, only a single thread (the one that
- *          captured it) will call this release function, so there is no
- *          need for any test or thread safe barriers. Also there is no
- *          side effect of calling this function when the Mutex is not
- *          taken so this function can be really simple and doesn't need
- *          to return anything.
- *
- *          See #S2PI_TryGetMutex on more information and an example
- *          implementation for the single SPI interface case.
- *
- * @param   slave The specified S2PI slave. Note that the slave information is
- *                only required if multiple SPI instances are used in order to
- *                map to the correct SPI instance.
- *****************************************************************************/
-void S2PI_ReleaseMutex(s2pi_slave_t slave);
-
-/*!***************************************************************************
- * @brief   Transfers a single SPI frame asynchronously.
+ * @brief 	Transfers a single SPI frame asynchronously.
  *
  * @details Transfers a single SPI frame in asynchronous manner. The Tx data
- *          buffer is written to the device via the MOSI line.
- *          Optionally, the data on the MISO line is written to the provided
- *          Rx data buffer. If null, the read data is dismissed. Note that
- *          Rx and Tx buffer can be identical. I.e. the same buffer is used
- *          for writing and reading data. First, a byte is transmitted and
- *          the received byte overwrites the previously send value.
+ * 			buffer is written to the device via the MOSI line.
+ * 			Optionally, the data on the MISO line is written to the provided
+ * 			Rx data buffer. If null, the read data is dismissed. Note that
+ * 			Rx and Tx buffer can be identical. I.e. the same buffer is used
+ * 			for writing and reading data. First, a byte is transmitted and
+ * 			the received byte overwrites the previously send value.
  *
- *          The transfer of a single frame requires to not toggle the chip
- *          select line to high in between the data frame. The maximum
- *          number of bytes transferred in a single SPI transfer is given by
- *          the data value register of the device, which is 396 data bytes
- *          plus a single address byte: 397 bytes.
+ * 			The transfer of a single frame requires to not toggle the chip
+ * 			select line to high in between the data frame. The maximum
+ * 			number of bytes transfered in a single SPI transfer is given by
+ * 			the data value register of the device, which is 396 data bytes
+ * 			plus a single address byte: 397 bytes.
  *
- *          An optional callback is invoked when the asynchronous transfer
- *          is finished. If the \p callback parameter is a null pointer,
- *          no callback is provided. Note that the provided buffer must not
- *          change while the transfer is ongoing.
+ * 			An optional callback is invoked when the asynchronous transfer
+ * 			is finished. If the \p callback parameter is a null pointer,
+ * 			no callback is provided. Note that the provided buffer must not
+ * 			change while the transfer is ongoing.
  *
- *          Use the slave parameter to determine the corresponding slave via the
- *          given chip select line.
+ * 			Use the slave parameter to determine the corresponding slave via the
+ * 			given chip select line.
  *
- *          Usually, two distinct interrupts are required to handle the RX and
- *          TX ready events. The callback must be invoked from whichever
- *          interrupt comes after the SPI transfer has been finished. Note
- *          that new SPI transfers are invoked from within the callback function
- *          (i.e. from within the interrupt service routine of same priority).
+ * 			Usually, two distinct interrupts are required to handle the RX and
+ * 			TX ready events. The callback must be invoked from whichever
+ * 			interrupt comes after the SPI transfer has been finished. Note
+ * 			that new SPI transfers are invoked from within the callback function
+ * 			(i.e. from within the interrupt service routine of same priority).
  *
- * @param   slave The specified S2PI slave.
- * @param   txData The 8-bit values to write to the SPI bus MOSI line.
- * @param   rxData The 8-bit values received from the SPI bus MISO line
+ * @param	slave The specified S2PI slave.
+ * @param	txData The 8-bit values to write to the SPI bus MOSI line.
+ * @param	rxData The 8-bit values received from the SPI bus MISO line
  *                   (pass a null pointer if the data don't need to be read).
- * @param   frameSize The number of 8-bit values to be sent/received.
- * @param   callback A callback function to be invoked when the transfer is
- *                     finished. Pass a null pointer if no callback is required.
- * @param   callbackData A pointer to a state that will be passed to the
+ * @param	frameSize The number of 8-bit values to be sent/received.
+ * @param	callback A callback function to be invoked when the transfer is
+ * 					   finished. Pass a null pointer if no callback is required.
+ * @param	callbackData A pointer to a state that will be passed to the
  *                         callback. Pass a null pointer if not used.
  *
- * @return  Returns the \link #status_t status\endlink:
- *           - #STATUS_OK: Successfully invoked the transfer.
- *           - #ERROR_INVALID_ARGUMENT: An invalid parameter has been passed.
- *           - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided.
- *           - #STATUS_BUSY: An SPI transfer is already in progress. The
- *                           transfer was not started.
- *           - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. The transfer
- *                                     was not started.
+ * @return 	Returns the \link #status_t status\endlink:
+ * 			 - #STATUS_OK: Successfully invoked the transfer.
+ *         	 - #ERROR_INVALID_ARGUMENT: An invalid parameter has been passed.
+ *         	 - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided.
+ *         	 - #STATUS_BUSY: An SPI transfer is already in progress. The
+ *         	                 transfer was not started.
+ *         	 - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. The transfer
+ *         	                           was not started.
  *****************************************************************************/
 status_t S2PI_TransferFrame(s2pi_slave_t slave,
 			    uint8_t const *txData,
@@ -296,158 +220,136 @@ status_t S2PI_TransferFrame(s2pi_slave_t slave,
 			    void *callbackData);
 
 /*!***************************************************************************
- * @brief   Terminates a currently ongoing asynchronous SPI transfer.
+ * @brief	Terminates a currently ongoing asynchronous SPI transfer.
  *
- * @details When a callback is set for the current ongoing activity, it is
- *          invoked with the #ERROR_ABORTED error byte.
+ * @details	When a callback is set for the current ongoing activity, it is
+ * 			invoked with the #ERROR_ABORTED error byte.
  *
- * @param   slave The specified S2PI slave. Note that the slave information is
- *                only required if multiple SPI instances are used in order to
- *                map to the correct SPI instance.
- *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t S2PI_Abort(s2pi_slave_t slave);
+status_t S2PI_Abort(void);
 
 /*!***************************************************************************
- * @brief   Set a callback for the GPIO IRQ for a specified S2PI slave.
+ * @brief 	Set a callback for the GPIO IRQ for a specified S2PI slave.
  *
- * @param   slave The specified S2PI slave.
- * @param   callback A callback function to be invoked when the specified
- *                     S2PI slave IRQ occurs. Pass a null pointer to disable
- *                     the callback.
- * @param   callbackData A pointer to a state that will be passed to the
+ * @param	slave The specified S2PI slave.
+ * @param	callback A callback function to be invoked when the specified
+ * 					   S2PI slave IRQ occurs. Pass a null pointer to disable
+ * 					   the callback.
+ * @param	callbackData A pointer to a state that will be passed to the
  *                         callback. Pass a null pointer if not used.
  *
- * @return  Returns the \link #status_t status\endlink:
- *           - #STATUS_OK: Successfully installation of the callback.
- *           - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided.
+ * @return 	Returns the \link #status_t status\endlink:
+ * 			 - #STATUS_OK: Successfully installation of the callback.
+ *         	 - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided.
  *****************************************************************************/
 status_t S2PI_SetIrqCallback(s2pi_slave_t slave,
 			     s2pi_irq_callback_t callback,
 			     void *callbackData);
 
 /*!***************************************************************************
- * @brief   Reads the current interrupt pending status of the IRQ pin.
+ * @brief	Reads the current status of the IRQ pin.
  *
  * @details In order to keep a low priority for GPIO IRQs, the state of the
- *          IRQ pin must be read in order to reliable check for chip timeouts.
+ * 			IRQ pin must be read in order to reliable check for chip timeouts.
  *
- *          The execution of the interrupt service routine for the data-ready
- *          interrupt from the corresponding GPIO pin might be delayed due to
- *          priority issues. The delayed execution might disable the timeout
- *          for the eye-safety checker too late causing false error messages.
- *          In order to overcome the issue, the interrupt state of the IRQ
- *          GPIO input pin is read before raising a timeout error in order to
- *          check if the device has already finished and the IRQ is still
- *          pending to be executed!
- *
- *          Note: an easy implementation is to simply return the state of the
- *          IRQ line, i.e. 0 if there is a low input state and 1 if there is
- *          a high input state on the IRQ input pin. However, this
- *          implementation is not fully reliable since the GPIO interrupt
- *          (triggered on the falling edge) might be missed and the callback
- *          is never invoked while the IRQ line is correctly asserted to low
- *          state by the device. In that case, the API is waiting forever
- *          until the callback is invoked which might never happen. Therefore,
- *          it is better if the implementation checks the state of the IRQ
- *          pending status register or even combines both variations.
+ * 			The execution of the interrupt service routine for the data-ready
+ * 			interrupt from the corresponding GPIO pin might be delayed due to
+ * 			priority issues. The delayed execution might disable the timeout
+ * 			for the eye-safety checker too late causing false error messages.
+ * 			In order to overcome the issue, the state of the IRQ GPIO input
+ * 			pin is read before raising a timeout error in order to check if
+ * 			the device has already finished but the IRQ is still pending to be
+ * 			executed!
 
- * @param   slave The specified S2PI slave.
- *
- * @return  Returns 1U if the IRQ is NOT pending (pin is in high state) and
- *          0U if the IRQ is pending (pin is pulled to low state by the device).
+ * @param	slave The specified S2PI slave.
+ * @return 	Returns 1U if the IRQ pin is high (IRQ not pending) and 0U if the
+ * 			devices pulls the pin to low state (IRQ pending).
  *****************************************************************************/
 uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave);
 
 /*!***************************************************************************
- * @brief   Cycles the chip select line.
+ * @brief	Cycles the chip select line.
  *
  * @details In order to cancel the integration on the ASIC, a fast toggling
- *          of the chip select pin of the corresponding SPI slave is required.
- *          Therefore, this function toggles the CS from high to low and back.
- *          The SPI instance for the specified S2PI slave must be idle,
- *          otherwise the status #STATUS_BUSY is returned.
+ * 			of the chip select pin of the corresponding SPI slave is required.
+ * 			Therefore, this function toggles the CS from high to low and back.
+ * 			The SPI instance for the specified S2PI slave must be idle,
+ * 			otherwise the status #STATUS_BUSY is returned.
  *
- * @param   slave The specified S2PI slave.
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @param	slave The specified S2PI slave.
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t S2PI_CycleCsPin(s2pi_slave_t slave);
 
-/*!*****************************************************************************
- * @brief   Captures the S2PI pins for GPIO usage.
- *
- * @details The SPI is disabled (module status: #STATUS_S2PI_GPIO_MODE) and the
- *          pins are configured for GPIO operation. The GPIO control must be
- *          release with the #S2PI_ReleaseGpioControl function in order to
- *          switch back to ordinary SPI functionality.
- *
- * @note    This function is only called during device initialization!
- *
- * @param   slave The specified S2PI slave. Note that the slave information is
- *                only required if multiple SPI instances are used in order to
- *                map to the correct SPI instance.
- *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
- *****************************************************************************/
-status_t S2PI_CaptureGpioControl(s2pi_slave_t slave);
+
 
 /*!*****************************************************************************
- * @brief   Releases the S2PI pins from GPIO usage and switches back to SPI mode.
+ * @brief	Captures the S2PI pins for GPIO usage.
  *
- * @details The GPIO pins are configured for SPI operation and the GPIO mode is
- *          left. Must be called if the pins are captured for GPIO operation via
- *          the #S2PI_CaptureGpioControl function.
+ * @details	The SPI is disabled (module status: #STATUS_S2PI_GPIO_MODE) and the
+ * 			pins are configured for GPIO operation. The GPIO control must be
+ * 			release with the #S2PI_ReleaseGpioControl function in order to
+ * 			switch back to ordinary SPI functionality.
  *
- * @note    This function is only called during device initialization!
+ * @note	This function is only called during device initialization!
  *
- * @param   slave The specified S2PI slave. Note that the slave information is
- *                only required if multiple SPI instances are used in order to
- *                map to the correct SPI instance.
- *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t S2PI_ReleaseGpioControl(s2pi_slave_t slave);
+status_t S2PI_CaptureGpioControl(void);
 
 /*!*****************************************************************************
- * @brief   Writes the output for a specified SPI pin in GPIO mode.
+ * @brief	Releases the S2PI pins from GPIO usage and switches back to SPI mode.
+ *
+ * @details	The GPIO pins are configured for SPI operation and the GPIO mode is
+ * 			left. Must be called if the pins are captured for GPIO operation via
+ * 			the #S2PI_CaptureGpioControl function.
+ *
+ * @note	This function is only called during device initialization!
+ *
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ *****************************************************************************/
+status_t S2PI_ReleaseGpioControl(void);
+
+/*!*****************************************************************************
+ * @brief	Writes the output for a specified SPI pin in GPIO mode.
  *
  * @details This function writes the value of an SPI pin if the SPI pins are
- *          captured for GPIO operation via the #S2PI_CaptureGpioControl previously.
+ * 			captured for GPIO operation via the #S2PI_CaptureGpioControl previously.
  *
- * @note    Since some GPIO peripherals switch the GPIO pins very fast a delay
- *          must be added after each GBIO access (i.e. right before returning
- *          from the #S2PI_WriteGpioPin method) in order to decrease the baud
- *          rate of the software EEPROM protocol. Increase the delay if timing
- *          issues occur while reading the EERPOM. For example:
- *          Delay = 10 µsec => Baud Rate < 100 kHz
+ * @note	Since some GPIO peripherals switch the GPIO pins very fast a delay
+ * 			must be added after each GBIO access (i.e. right before returning
+ * 			from the #S2PI_WriteGpioPin method) in order to decrease the baud
+ * 			rate of the software EEPROM protocol. Increase the delay if timing
+ * 			issues occur while reading the EERPOM. For example:
+ * 			Delay = 10 µsec => Baud Rate < 100 kHz
  *
- * @note    This function is only called during device initialization!
+ * @note	This function is only called during device initialization!
  *
- * @param   slave The specified S2PI slave.
- * @param   pin The specified S2PI pin.
- * @param   value The GPIO pin state to write (0 = low, 1 = high).
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @param	slave The specified S2PI slave.
+ * @param	pin The specified S2PI pin.
+ * @param	value The GPIO pin state to write (0 = low, 1 = high).
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value);
 
 /*!*****************************************************************************
- * @brief   Reads the input from a specified SPI pin in GPIO mode.
+ * @brief	Reads the input from a specified SPI pin in GPIO mode.
  *
  * @details This function reads the value of an SPI pin if the SPI pins are
- *          captured for GPIO operation via the #S2PI_CaptureGpioControl previously.
+ * 			captured for GPIO operation via the #S2PI_CaptureGpioControl previously.
  *
- * @note    This function is only called during device initialization!
+ * @note	This function is only called during device initialization!
  *
- * @param   slave The specified S2PI slave.
- * @param   pin The specified S2PI pin.
- * @param   value The GPIO pin state to read (0 = low, GND level, 1 = high, VCC level).
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @param	slave The specified S2PI slave.
+ * @param	pin The specified S2PI pin.
+ * @param	value The GPIO pin state to read (0 = low, GND level, 1 = high, VCC level).
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value);
+#ifdef __cplusplus
+}
+#endif
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif // ARGUS_S2PI_H

src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file provides an interface for the required timer modules.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		This file provides an interface for the required timer modules.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,117 +36,118 @@
 
 #ifndef ARGUS_TIMER_H
 #define ARGUS_TIMER_H
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 /*!***************************************************************************
- * @defgroup    argus_timer Timer: Hardware Timer Interface
- * @ingroup     argus_hal
+ * @defgroup	argus_timer Timer: Hardware Timer Interface
+ * @ingroup		argus_platform
  *
- * @brief       Timer implementations for lifetime counting as well as periodic
- *              callback.
+ * @brief 		Timer implementations for lifetime counting as well as periodic
+ * 				callback.
  *
- * @details     The module provides an interface to the timing utilities that
- *              are required by the AFBR-S50 time-of-flight sensor API.
+ * @details		The module provides an interface to the timing utilities that
+ * 				are required by the AFBR-S50 time-of-flight sensor API.
  *
- *              Two essential features have to be provided by the user code:
- *              1. Time Measurement Capability: In order to keep track of outgoing
- *                signals, the API needs to measure elapsed time. In order to
- *                provide optimum device performance, the granularity should be
- *                around 10 to 100 microseconds.
- *              2. Periodic Callback: The API provides an automatic starting of
- *                measurement cycles at a fixed frame rate via a periodic
- *                interrupt timer. If this feature is not used, implementation
- *                of the periodic interrupts can be skipped. An weak default
- *                implementation is provide in the API.
- *              .
+ *				Two essential features have to be provided by the user code:
+ *				1. Time Measurement Capability: In order to keep track of outgoing
+ *				  signals, the API needs to measure elapsed time. In order to
+ *				  provide optimum device performance, the granularity should be
+ *				  around 10 to 100 microseconds.
+ *				2. Periodic Callback: The API provides an automatic starting of
+ *				  measurement cycles at a fixed frame rate via a periodic
+ *				  interrupt timer. If this feature is not used, implementation
+ *				  of the periodic interrupts can be skipped. An weak default
+ *				  implementation is provide in the API.
+ *				.
  *
- *              The time measurement feature is simply implemented by the function
- *              #Timer_GetCounterValue. Whenever the function is called, the
- *              provided counter values must be written with the values obtained
- *              by the current time.
+ *				The time measurement feature is simply implemented by the function
+ *				#Timer_GetCounterValue. Whenever the function is called, the
+ *				provided counter values must be written with the values obtained
+ *				by the current time.
  *
- *              The periodic interrupt timer is a simple callback interface.
- *              After installing the callback function pointer via #Timer_SetCallback,
- *              the timer can be started by setting interval via #Timer_SetInterval.
- *              From then, the callback is invoked periodically as the corresponding
- *              interval may specify. The timer is stopped by setting the interval
- *              to 0 using the #Timer_SetInterval function. The interval can be
- *              updated at any time by updating the interval via the #Timer_SetInterval
- *              function. To any of these functions, an abstract parameter pointer
- *              must be passed. This parameter is passed back to the callback any
- *              time it is invoked.
+ *				The periodic interrupt timer is a simple callback interface.
+ *				After installing the callback function pointer via #Timer_SetCallback,
+ *				the timer can be started by setting interval via #Timer_SetInterval.
+ *				From then, the callback is invoked periodically as the corresponding
+ *				interval may specify. The timer is stopped by setting the interval
+ *				to 0 using the #Timer_SetInterval function. The interval can be
+ *				updated at any time by updating the interval via the #Timer_SetInterval
+ *				function. To any of these functions, an abstract parameter pointer
+ *				must be passed. This parameter is passed back to the callback any
+ *				time it is invoked.
  *
- *              In order to provide the usage of multiple devices, an mechanism is
- *              introduced to allow the installation of multiple callback interval
- *              at the same time. Therefore, the abstract parameter pointer is used
- *              to identify the corresponding callback interval. For example, there
- *              are two callbacks for two intervals, t1 and t2, required. The user
- *              can start two timers by calling the #Timer_SetInterval method twice,
- *              but with an individual parameter pointer, ptr1 and ptr2, each:
- *              \code
- *                  Timer_SetInterval(100000, ptr1); // 10 ms callback w/ parameter ptr1
- *                  Timer_SetInterval(200000, ptr2); // 20 ms callback w/ parameter ptr1
- *              \endcode
+ *				In order to provide the usage of multiple devices, an mechanism is
+ *				introduced to allow the installation of multiple callback interval
+ *				at the same time. Therefore, the abstract parameter pointer is used
+ *				to identify the corresponding callback interval. For example, there
+ *				are two callbacks for two intervals, t1 and t2, required. The user
+ *				can start two timers by calling the #Timer_SetInterval method twice,
+ *				but with an individual parameter pointer, ptr1 and ptr2, each:
+ *				\code
+ *					Timer_SetInterval(100000, ptr1); // 10 ms callback w/ parameter ptr1
+ *					Timer_SetInterval(200000, ptr2); // 20 ms callback w/ parameter ptr1
+ *				\endcode
  *
- *              Note that the implemented timer module must therefore support
- *              as many different intervals as instances of the AFBR-S50 device are
- *              used.
+ *				Note that the implemented timer module must therefore support
+ *				as many different intervals as instances of the AFBR-S50 device are
+ *				used.
  *
- * @addtogroup  argus_timer
+ * @addtogroup 	argus_timer
  * @{
  *****************************************************************************/
 
-#include "utility/status.h"
+#include "api/argus_def.h"
 
 /*******************************************************************************
  * Lifetime Counter Timer Interface
  ******************************************************************************/
 
 /*!***************************************************************************
- * @brief   Obtains the lifetime counter value from the timers.
+ * @brief	Obtains the lifetime counter value from the timers.
  *
  * @details The function is required to get the current time relative to any
- *          point in time, e.g. the startup time. The returned values \p hct and
- *          \p lct are given in seconds and microseconds respectively. The current
- *          elapsed time since the reference time is then calculated from:
+ * 			point in time, e.g. the startup time. The returned values \p hct and
+ * 			\p lct are given in seconds and microseconds respectively. The current
+ * 			elapsed time since the reference time is then calculated from:
  *
- *          t_now [µsec] = hct * 1000000 µsec + lct * 1 µsec
+ * 			t_now [µsec] = hct * 1000000 µsec + lct * 1 µsec
  *
- *          Note that the accuracy/granularity of the lifetime counter does
- *          not need to be 1 µsec. Usually, a granularity of approximately
- *          100 µsec is sufficient. However, in case of very high frame rates
- *          (above 1000 frames per second), it is recommended to implement
- *          an even lower granularity (somewhere in the 10 µsec regime).
+ * 			Note that the accuracy/granularity of the lifetime counter does
+ * 			not need to be 1 µsec. Usually, a granularity of approximately
+ * 			100 µsec is sufficient. However, in case of very high frame rates
+ * 			(above 1000 frames per second), it is recommended to implement
+ * 			an even lower granularity (somewhere in the 10 µsec regime).
  *
- *          It must be guaranteed, that each call of the #Timer_GetCounterValue
- *          function must provide a value that is greater or equal, but never lower,
- *          than the value returned from the previous call.
+ * 			It must be guaranteed, that each call of the #Timer_GetCounterValue
+ * 			function must provide a value that is greater or equal, but never lower,
+ * 			than the value returned from the previous call.
  *
- *          A hardware based implementation of the lifetime counter functionality
- *          would be to chain two distinct timers such that counter 2 increases
- *          its value when counter 1 wraps to 0. The easiest way is to setup
- *          counter 1 to wrap exactly every second. Counter 1 would than count
- *          the sub-seconds (i.e. µsec) value (\p lct) and counter 2 the seconds
- *          (\p hct) value. A 16-bit counter is sufficient in case of counter 1
- *          while counter 2 must be a 32-bit version.
+ * 			A hardware based implementation of the lifetime counter functionality
+ * 			would be to chain two distinct timers such that counter 2 increases
+ * 			its value when counter 1 wraps to 0. The easiest way is to setup
+ * 			counter 1 to wrap exactly every second. Counter 1 would than count
+ * 			the sub-seconds (i.e. µsec) value (\p lct) and counter 2 the seconds
+ * 			(\p hct) value.	A 16-bit counter is sufficient in case of counter 1
+ * 			while counter 2	must be a 32-bit version.
  *
- *          In case of a lack of available hardware timers, a software solution
- *          can be used that requires only a 16-bit timer. In a simple scenario,
- *          the timer is configured to wrap around every second and increase
- *          a software counter value in its interrupt service routine (triggered
- *          with the wrap around event) every time the wrap around occurs.
+ * 			In case of a lack of available hardware timers, a software solution
+ * 			can be used that requires only a 16-bit timer. In a simple scenario,
+ * 			the timer is configured to wrap around every second and increase
+ * 			a software counter value in its interrupt service routine (triggered
+ * 			with the wrap around event) every time the wrap around occurs.
  *
  *
- * @note    The implementation of this function is mandatory for the correct
- *          execution of the API.
+ * @note	The implementation of this function is mandatory for the correct
+ * 			execution of the API.
  *
- * @param   hct A pointer to the high counter value bits representing current
- *              time in seconds.
+ * @param	hct A pointer to the high counter value bits representing current
+ * 				time in seconds.
  *
- * @param   lct A pointer to the low counter value bits representing current
- *              time in microseconds. Range: 0, .., 999999 µsec
+ * @param	lct A pointer to the low counter value bits representing current
+ * 				time in microseconds. Range: 0, .., 999999 µsec
  *****************************************************************************/
 void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct);
 
@@ -155,70 +156,68 @@ void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct);
  ******************************************************************************/
 
 /*!***************************************************************************
- * @brief   The callback function type for periodic interrupt timer.
+ * @brief	The callback function type for periodic interrupt timer.
  *
- * @details The function that is invoked every time a specified interval elapses.
- *          An abstract parameter is passed to the function whenever it is called.
+ * @details	The function that is invoked every time a specified interval elapses.
+ * 			An abstract parameter is passed to the function whenever it is called.
  *
- * @param   param An abstract parameter to be passed to the callback. This is
- *                  also the identifier of the given interval.
+ * @param	param An abstract parameter to be passed to the callback. This is
+ * 					also the identifier of the given interval.
  *****************************************************************************/
 typedef void (*timer_cb_t)(void *param);
 
 /*!***************************************************************************
- * @brief   Installs an periodic timer callback function.
+ * @brief	Installs an periodic timer callback function.
  *
- * @details Installs an periodic timer callback function that is invoked whenever
- *          an interval elapses. The callback is the same for any interval,
- *          however, the single intervals can be identified by the passed
- *          parameter.
- *          Passing a zero-pointer removes and disables the callback.
+ * @details	Installs an periodic timer callback function that is invoked whenever
+ * 			an interval elapses. The callback is the same for any interval,
+ * 			however, the single intervals can be identified by the passed
+ * 			parameter.
+ * 			Passing a zero-pointer removes and disables the callback.
  *
- * @note    The implementation of this function is optional for the correct
- *          execution of the API. If not implemented, a weak implementation
- *          within the API will be used that disable the periodic timer callback
- *          and thus the automatic starting of measurements from the background.
+ * @note	The implementation of this function is optional for the correct
+ * 			execution of the API. If not implemented, a weak implementation
+ * 			within the API will be used that disable the periodic timer callback
+ * 			and thus the automatic starting of measurements from the background.
  *
- * @param   f The timer callback function.
+ * @param	f The timer callback function.
  *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t Timer_SetCallback(timer_cb_t f);
 
 /*!***************************************************************************
- * @brief   Sets the timer interval for a specified callback parameter.
+ * @brief	Sets the timer interval for a specified callback parameter.
  *
- * @details Sets the callback interval for the specified parameter and starts
- *          the timer with a new interval. If there is already an interval with
- *          the given parameter, the timer is restarted with the given interval.
- *          If the same time interval as already set is passed, nothing happens.
- *          Passing a interval of 0 disables the timer.
+ * @details	Sets the callback interval for the specified parameter and starts
+ * 			the timer with a new interval. If there is already an interval with
+ * 			the given parameter, the timer is restarted with the given interval.
+ * 			If the same time interval as already set is passed, nothing happens.
+ * 			Passing a interval of 0 disables the timer.
  *
- *          When enabling the timer (or resetting by applying another interval),
- *          the first timer interrupt must happen after the specified interval.
+ * 			Note that a microsecond granularity for the timer interrupt period is
+ * 			not required. Usually a microseconds granularity is sufficient.
+ * 			The required granularity depends on the targeted frame rate, e.g. in
+ * 			case of more than 1 kHz measurement rate, a granularity of less than
+ * 			a microsecond is required to achieve the given frame rate.
  *
- *          Note that a microsecond granularity for the timer interrupt period is
- *          not required. Usually a milliseconds granularity is sufficient.
- *          The required granularity depends on the targeted frame rate, e.g. in
- *          case of more than 1 kHz measurement rate, a granularity of less than
- *          a millisecond is required to achieve the given frame rate.
+ * @note	The implementation of this function is optional for the correct
+ * 			execution of the API. If not implemented, a weak implementation
+ * 			within the API will be used that disable the periodic timer callback
+ * 			and thus the automatic starting of measurements from the background.
  *
- * @note    The implementation of this function is optional for the correct
- *          execution of the API. If not implemented, a weak implementation
- *          within the API will be used that disable the periodic timer callback
- *          and thus the automatic starting of measurements from the background.
+ * @param	dt_microseconds The callback interval in microseconds.
  *
- * @param   dt_microseconds The callback interval in microseconds.
+ * @param	param An abstract parameter to be passed to the callback. This is
+ * 					also the identifier of the given interval.
  *
- * @param   param An abstract parameter to be passed to the callback. This is
- *                  also the identifier of the given interval.
- *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t Timer_SetInterval(uint32_t dt_microseconds, void *param);
 
-/*! @} */
 #ifdef __cplusplus
-} // extern "C"
+}
 #endif
+
+/*! @} */
 #endif /* ARGUS_TIMER_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Provides definitions and basic macros for fixed point data types.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		Provides definitions and basic macros for fixed point data types.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,30 +36,23 @@
 
 #ifndef FP_DEF_H
 #define FP_DEF_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_fp Fixed Point Math
- * @ingroup     argus_util
+ * @defgroup	fixedpoint Fixed Point Math
+ * @ingroup		argusutil
+ * @brief		A basic math library for fixed point number in the Qx.y fomat.
+ * @details		This module contains common fixed point type definitions as
+ * 				well as some basic math algorithms. All types are based on
+ * 				integer types. The number are defined with the Q number format.
  *
- * @brief       A basic math library for fixed point number in the Qx.y fomat.
- *
- * @details     This module contains common fixed point type definitions as
- *              well as some basic math algorithms. All types are based on
- *              integer types. The number are defined with the Q number format.
- *
- *               - For a description of the Q number format refer to:
- *                  https://en.wikipedia.org/wiki/Q_(number_format)
- *               - Another resource for fixed point math in C might be found at
- *                  http://www.eetimes.com/author.asp?section_id=36&doc_id=1287491
- *               .
- *
- * @warning     This definitions are not portable and work only with
- *              little-endian systems!
- *
- * @addtogroup  argus_fp
+ * 				 - For a description of the Q number format refer to:
+ * 					https://en.wikipedia.org/wiki/Q_(number_format)
+ * 				 - Another resource for fixed point math in C might be found at
+ * 					http://www.eetimes.com/author.asp?section_id=36&doc_id=1287491
+ * 				 .
+ * @warning		This definitions are not portable and work only with
+ * 				little-endian systems!
+ * @addtogroup 	fixedpoint
  * @{
  *****************************************************************************/
 
@@ -73,11 +66,11 @@ extern "C" {
  ***** UQ6.2
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ6.2
- * @details An unsigned fixed point number format based on the 8-bit unsigned
- *          integer type with 6 integer and 2 fractional bits.
- *          - Range: 0 .. 63.75
- *          - Granularity: 0.25
+ * @brief	Unsigned fixed point number: UQ6.2
+ * @details	An unsigned fixed point number format based on the 8-bit unsigned
+ * 			integer type with 6 integer and 2 fractional bits.
+ * 			- Range: 0 .. 63.75
+ * 			- Granularity: 0.25
  *****************************************************************************/
 typedef uint8_t uq6_2_t;
 
@@ -93,11 +86,11 @@ typedef uint8_t uq6_2_t;
  ***** UQ4.4
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ4.4
- * @details An unsigned fixed point number format based on the 8-bit unsigned
- *          integer type with 4 integer and 4 fractional bits.
- *          - Range: 0 .. 15.9375
- *          - Granularity: 0.0625
+ * @brief	Unsigned fixed point number: UQ4.4
+ * @details	An unsigned fixed point number format based on the 8-bit unsigned
+ * 			integer type with 4 integer and 4 fractional bits.
+ * 			- Range: 0 .. 15.9375
+ * 			- Granularity: 0.0625
  *****************************************************************************/
 typedef uint8_t uq4_4_t;
 
@@ -113,11 +106,11 @@ typedef uint8_t uq4_4_t;
  ***** UQ2.6
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ2.6
- * @details An unsigned fixed point number format based on the 8-bit unsigned
- *          integer type with 2 integer and 6 fractional bits.
- *          - Range: 0 .. 3.984375
- *          - Granularity: 0.015625
+ * @brief	Unsigned fixed point number: UQ2.6
+ * @details	An unsigned fixed point number format based on the 8-bit unsigned
+ * 			integer type with 2 integer and 6 fractional bits.
+ * 			- Range: 0 .. 3.984375
+ * 			- Granularity: 0.015625
  *****************************************************************************/
 typedef uint8_t uq2_6_t;
 
@@ -133,11 +126,11 @@ typedef uint8_t uq2_6_t;
  ***** UQ1.7
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ1.7
- * @details An unsigned fixed point number format based on the 8-bit unsigned
- *          integer type with 1 integer and 7 fractional bits.
- *          - Range: 0 .. 1.9921875
- *          - Granularity: 0.0078125
+ * @brief	Unsigned fixed point number: UQ1.7
+ * @details	An unsigned fixed point number format based on the 8-bit unsigned
+ * 			integer type with 1 integer and 7 fractional bits.
+ * 			- Range: 0 .. 1.9921875
+ * 			- Granularity: 0.0078125
  *****************************************************************************/
 typedef uint8_t uq1_7_t;
 
@@ -153,11 +146,11 @@ typedef uint8_t uq1_7_t;
  ***** UQ0.8
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ0.8
- * @details An unsigned fixed point number format based on the 8-bit unsigned
- *          integer type with 1 integer and 7 fractional bits.
- *          - Range: 0 .. 0.99609375
- *          - Granularity: 0.00390625
+ * @brief	Unsigned fixed point number: UQ0.8
+ * @details	An unsigned fixed point number format based on the 8-bit unsigned
+ * 			integer type with 1 integer and 7 fractional bits.
+ * 			- Range: 0 .. 0.99609375
+ * 			- Granularity: 0.00390625
  *****************************************************************************/
 typedef uint8_t uq0_8_t;
 
@@ -174,11 +167,11 @@ typedef uint8_t uq0_8_t;
  ***** Q3.4
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q3.4
- * @details A signed fixed point number format based on the 8-bit signed
- *          integer type with 3 integer and 4 fractional bits.
- *          - Range: -8 ... 7.9375
- *          - Granularity: 0.0625
+ * @brief	Signed fixed point number: Q3.4
+ * @details	A signed fixed point number format based on the 8-bit signed
+ * 			integer type with 3 integer and 4 fractional bits.
+ * 			- Range: -8 ... 7.9375
+ * 			- Granularity: 0.0625
  *****************************************************************************/
 typedef int8_t q3_4_t;
 
@@ -196,11 +189,11 @@ typedef int8_t q3_4_t;
  ***** Q1.6
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q1.6
- * @details A signed fixed point number format based on the 8-bit signed
- *          integer type with 1 integer and 6 fractional bits.
- *          - Range: -2 ... 1.984375
- *          - Granularity: 0.015625
+ * @brief	Signed fixed point number: Q1.6
+ * @details	A signed fixed point number format based on the 8-bit signed
+ * 			integer type with 1 integer and 6 fractional bits.
+ * 			- Range: -2 ... 1.984375
+ * 			- Granularity: 0.015625
  *****************************************************************************/
 typedef int8_t q1_6_t;
 
@@ -222,11 +215,11 @@ typedef int8_t q1_6_t;
  ***** UQ12.4
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ12.4
- * @details An unsigned fixed point number format based on the 16-bit unsigned
- *          integer type with 12 integer and 4 fractional bits.
- *          - Range: 0 ... 4095.9375
- *          - Granularity: 0.0625
+ * @brief	Unsigned fixed point number: UQ12.4
+ * @details	An unsigned fixed point number format based on the 16-bit unsigned
+ * 			integer type with 12 integer and 4 fractional bits.
+ * 			- Range: 0 ... 4095.9375
+ * 			- Granularity: 0.0625
  *****************************************************************************/
 typedef uint16_t uq12_4_t;
 
@@ -242,11 +235,11 @@ typedef uint16_t uq12_4_t;
  ***** UQ10.6
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ10.6
- * @details An unsigned fixed point number format based on the 16-bit unsigned
- *          integer type with 10 integer and 6 fractional bits.
- *          - Range: 0 ... 1023.984375
- *          - Granularity: 0.015625
+ * @brief	Unsigned fixed point number: UQ10.6
+ * @details	An unsigned fixed point number format based on the 16-bit unsigned
+ * 			integer type with 10 integer and 6 fractional bits.
+ * 			- Range: 0 ... 1023.984375
+ * 			- Granularity: 0.015625
  *****************************************************************************/
 typedef uint16_t uq10_6_t;
 
@@ -262,11 +255,11 @@ typedef uint16_t uq10_6_t;
  ***** UQ1.15
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ1.15
- * @details An unsigned fixed point number format based on the 16-bit unsigned
- *          integer type with 1 integer and 15 fractional bits.
- *          - Range: 0 .. 1.999969
- *          - Granularity: 0.000031
+ * @brief	Unsigned fixed point number: UQ1.15
+ * @details	An unsigned fixed point number format based on the 16-bit unsigned
+ * 			integer type with 1 integer and 15 fractional bits.
+ * 			- Range: 0 .. 1.999969
+ * 			- Granularity: 0.000031
  *****************************************************************************/
 typedef uint16_t uq1_15_t;
 
@@ -282,11 +275,11 @@ typedef uint16_t uq1_15_t;
  ***** UQ0.16
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ0.16
- * @details An unsigned fixed point number format based on the 16-bit unsigned
- *          integer type with 0 integer and 16 fractional bits.
- *          - Range: 0 .. 0.9999847412109375
- *          - Granularity: 1.52587890625e-5
+ * @brief	Unsigned fixed point number: UQ0.16
+ * @details	An unsigned fixed point number format based on the 16-bit unsigned
+ * 			integer type with 0 integer and 16 fractional bits.
+ * 			- Range: 0 .. 0.9999847412109375
+ * 			- Granularity: 1.52587890625e-5
  *****************************************************************************/
 typedef uint16_t uq0_16_t;
 
@@ -303,11 +296,11 @@ typedef uint16_t uq0_16_t;
  ***** Q11.4
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q11.4
- * @details A signed fixed point number format based on the 16-bit signed
- *          integer type with 11 integer and 4 fractional bits.
- *          - Range: -2048 ... 2047.9375
- *          - Granularity: 0.0625
+ * @brief	Signed fixed point number: Q11.4
+ * @details	A signed fixed point number format based on the 16-bit signed
+ * 			integer type with 11 integer and 4 fractional bits.
+ * 			- Range: -2048 ... 2047.9375
+ * 			- Granularity: 0.0625
  *****************************************************************************/
 typedef int16_t q11_4_t;
 
@@ -326,11 +319,11 @@ typedef int16_t q11_4_t;
  ***** Q7.8
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q7.8
- * @details A signed fixed point number format based on the 16-bit signed
- *          integer type with 7 integer and 8 fractional bits.
- *          - Range: -128 .. 127.99609375
- *          - Granularity: 0.00390625
+ * @brief	Signed fixed point number: Q7.8
+ * @details	A signed fixed point number format based on the 16-bit signed
+ * 			integer type with 7 integer and 8 fractional bits.
+ * 			- Range: -128 .. 127.99609375
+ * 			- Granularity: 0.00390625
  *****************************************************************************/
 typedef int16_t q7_8_t;
 
@@ -349,11 +342,11 @@ typedef int16_t q7_8_t;
  ***** Q3.12
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q3.12
- * @details A signed fixed point number format based on the 16-bit integer
- *          type with 3 integer and 12 fractional bits.
- *          - Range: -8 .. 7.99975586
- *          - Granularity: 0.00024414
+ * @brief	Signed fixed point number: Q3.12
+ * @details	A signed fixed point number format based on the 16-bit integer
+ * 			type with 3 integer and 12 fractional bits.
+ * 			- Range: -8 .. 7.99975586
+ * 			- Granularity: 0.00024414
  *****************************************************************************/
 typedef int16_t q3_12_t;
 
@@ -372,11 +365,11 @@ typedef int16_t q3_12_t;
  ***** Q0.15
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q0.15
- * @details A signed fixed point number format based on the 16-bit integer
- *          type with 0 integer and 15 fractional bits.
- *          - Range: -1 .. 0.999969482
- *          - Granularity: 0.000030518
+ * @brief	Signed fixed point number: Q0.15
+ * @details	A signed fixed point number format based on the 16-bit integer
+ * 			type with 0 integer and 15 fractional bits.
+ * 			- Range: -1 .. 0.999969482
+ * 			- Granularity: 0.000030518
  *****************************************************************************/
 typedef int16_t q0_15_t;
 
@@ -396,11 +389,11 @@ typedef int16_t q0_15_t;
  ***** UQ28.4
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ28.4
- * @details An unsigned fixed point number format based on the 32-bit unsigned
- *          integer type with 28 integer and 4 fractional bits.
- *          - Range: 0 ... 268435455.9375
- *          - Granularity: 0.0625
+ * @brief	Unsigned fixed point number: UQ28.4
+ * @details	An unsigned fixed point number format based on the 32-bit unsigned
+ * 			integer type with 28 integer and 4 fractional bits.
+ * 			- Range: 0 ... 268435455.9375
+ * 			- Granularity: 0.0625
  *****************************************************************************/
 typedef uint32_t uq28_4_t;
 
@@ -416,11 +409,11 @@ typedef uint32_t uq28_4_t;
  ***** UQ16.16
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ16.16
- * @details An unsigned fixed point number format based on the 32-bit unsigned
- *          integer type with 16 integer and 16 fractional bits.
- *          - Range: 0 ... 65535.999984741
- *          - Granularity: 0.000015259
+ * @brief	Unsigned fixed point number: UQ16.16
+ * @details	An unsigned fixed point number format based on the 32-bit unsigned
+ * 			integer type with 16 integer and 16 fractional bits.
+ * 			- Range: 0 ... 65535.999984741
+ * 			- Granularity: 0.000015259
  *****************************************************************************/
 typedef uint32_t uq16_16_t;
 
@@ -439,11 +432,11 @@ typedef uint32_t uq16_16_t;
  ***** UQ10.22
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Unsigned fixed point number: UQ10.22
- * @details An unsigned fixed point number format based on the 32-bit unsigned
- *          integer type with 10 integer and 22 fractional bits.
- *          - Range: 0 ... 1023.99999976158
- *          - Granularity: 2.38418579101562E-07
+ * @brief	Unsigned fixed point number: UQ10.22
+ * @details	An unsigned fixed point number format based on the 32-bit unsigned
+ * 			integer type with 10 integer and 22 fractional bits.
+ * 			- Range: 0 ... 1023.99999976158
+ * 			- Granularity: 2.38418579101562E-07
  *****************************************************************************/
 typedef uint32_t uq10_22_t;
 
@@ -463,11 +456,11 @@ typedef uint32_t uq10_22_t;
  ***** Q27.4
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q27.4
- * @details A signed fixed point number format based on the 32-bit signed
- *          integer type with 27 integer and 4 fractional bits.
- *          - Range: -134217728 ... 134217727.9375
- *          - Granularity: 0.0625
+ * @brief	Signed fixed point number: Q27.4
+ * @details	A signed fixed point number format based on the 32-bit signed
+ * 			integer type with 27 integer and 4 fractional bits.
+ * 			- Range: -134217728 ... 134217727.9375
+ * 			- Granularity: 0.0625
  *****************************************************************************/
 typedef int32_t q27_4_t;
 
@@ -482,35 +475,15 @@ typedef int32_t q27_4_t;
 
 
 
-/*******************************************************************************
- ***** Q16.15
- ******************************************************************************/
-/*!***************************************************************************
- * @brief   Signed fixed point number: Q16.15
- * @details A signed fixed point number format based on the 32-bit integer
- *          type with 16 integer and 15 fractional bits.
- *          - Range: -65536 .. 65536.999969482
- *          - Granularity: 0.000030518
- *****************************************************************************/
-typedef int32_t q16_15_t;
-
-/*! Minimum value of Q16.15 number format. */
-#define Q16_15_MIN ((q16_15_t)INT32_MIN)
-
-/*! Maximum value of Q16.15 number format. */
-#define Q16_15_MAX ((q16_15_t)INT32_MAX)
-
-
-
 /*******************************************************************************
  ***** Q15.16
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q15.16
- * @details A signed fixed point number format based on the 32-bit integer
- *          type with 15 integer and 16 fractional bits.
- *          - Range: -32768 .. 32767.99998
- *          - Granularity: 1.52588E-05
+ * @brief	Signed fixed point number: Q15.16
+ * @details	A signed fixed point number format based on the 32-bit integer
+ * 			type with 15 integer and 16 fractional bits.
+ * 			- Range: -32768 .. 32767.99998
+ * 			- Granularity: 1.52588E-05
  *****************************************************************************/
 typedef int32_t q15_16_t;
 
@@ -529,11 +502,11 @@ typedef int32_t q15_16_t;
  ***** Q9.22
  ******************************************************************************/
 /*!***************************************************************************
- * @brief   Signed fixed point number: Q9.22
- * @details A signed fixed point number format based on the 32-bit integer
- *          type with 9 integer and 22 fractional bits.
- *          - Range: -512 ... 511.9999998
- *          - Granularity: 2.38418579101562E-07
+ * @brief	Signed fixed point number: Q9.22
+ * @details	A signed fixed point number format based on the 32-bit integer
+ * 			type with 9 integer and 22 fractional bits.
+ * 			- Range: -512 ... 511.9999998
+ * 			- Granularity: 2.38418579101562E-07
  *****************************************************************************/
 typedef int32_t q9_22_t;
 
@@ -549,7 +522,4 @@ typedef int32_t q9_22_t;
 
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* FP_DEF_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h

@@ -1,173 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Provides definitions and basic macros for fixed point data types.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef FP_DIV_H
-#define FP_DIV_H
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*!***************************************************************************
- * @addtogroup  argus_fp
- * @{
- *****************************************************************************/
-
-#include "fp_def.h"
-#include "int_math.h"
-
-/*!***************************************************************************
- * Set to use hardware division (Cortex-M3/4) over software division (Cortex-M0/1).
- *****************************************************************************/
-#ifndef USE_HW_DIV
-#define USE_HW_DIV 0
-#endif
-
-/*!***************************************************************************
- * @brief   32-bit implementation of an Q15.16 division.
- *
- * @details Algorithm to evaluate a/b, where b is in Q15.16 format, on a 32-bit
- *          architecture with maximum precision.
- *          The result is correctly rounded and given as the input format.
- *          Division by 0 yields max. values determined by signa of numerator.
- *          Too high/low results are truncated to max/min values.
- *
- *          Depending on the architecture, the division is implemented with a 64-bit
- *          division and shifting (Cortex-M3/4) or as a fast software algorithm
- *          (Cortex-M0/1) wich runs fast on processors without hardware division.
- *
- *          @see https://code.google.com/archive/p/libfixmath
- *
- * @param   a Numerator in any Qx.y format
- * @param   b Denominator in Q15.16 format
- * @return  Result = a/b in the same Qx.y format as the input parameter a.
- *****************************************************************************/
-inline int32_t fp_div16(int32_t a, q15_16_t b)
-{
-	//assert(b);
-	if (b == 0) { return a < 0 ? INT32_MIN : INT32_MAX; }
-
-#if USE_HW_DIV
-	// Tested on Cortex-M4, it takes approx. 75% of the
-	// software algorithm below.
-	int64_t c = ((int64_t) a) << 30U;
-
-	if ((uint32_t)(a ^ b) & 0x80000000U) {
-		c = (((-c) / b) + (1 << 13U)) >> 14U;
-
-		if (c > 0x80000000U) { return INT32_MIN; }
-
-		return -c;
-
-	} else {
-		c = ((c / b) + (1 << 13U)) >> 14U;
-
-		if (c > (int64_t)INT32_MAX) { return INT32_MAX; }
-
-		return c;
-	}
-
-#else
-	// This uses the basic binary restoring division algorithm.
-	// It appears to be faster to do the whole division manually than
-	// trying to compose a 64-bit divide out of 32-bit divisions on
-	// platforms without hardware divide.
-	// Tested on Cortex-M0, it takes approx. 33% of the time of the
-	// 64-bit version above.
-
-	uint32_t remainder = absval(a);
-	uint32_t divider   = absval(b);
-
-	uint32_t quotient = 0;
-	uint32_t bit = 0x10000U;
-
-	/* The algorithm requires D >= R */
-	while (divider < remainder) {
-		divider <<= 1U;
-		bit <<= 1U;
-	}
-
-	if (!bit) {
-		if ((uint32_t)(a ^ b) & 0x80000000U) { // return truncated values
-			return INT32_MIN;
-
-		} else {
-			return INT32_MAX;
-		}
-	}
-
-	if (divider & 0x80000000U) {
-		// Perform one step manually to avoid overflows later.
-		// We know that divider's bottom bit is 0 here.
-		if (remainder >= divider) {
-			quotient |= bit;
-			remainder -= divider;
-		}
-
-		divider >>= 1U;
-		bit >>= 1U;
-	}
-
-	/* Main division loop */
-	while (bit && remainder) {
-		if (remainder >= divider) {
-			quotient |= bit;
-			remainder -= divider;
-		}
-
-		remainder <<= 1U;
-		bit >>= 1U;
-	}
-
-	if (remainder >= divider) {
-		quotient++;
-	}
-
-	uint32_t result = quotient;
-
-	/* Figure out the sign of result */
-	if ((uint32_t)(a ^ b) & 0x80000000U) {
-		result = -result;
-	}
-
-	return (int32_t)result;
-#endif
-}
-
-/*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
-#endif /* FP_DIV_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_ema.h

@@ -1,204 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Provides averaging algorithms for fixed point data types.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef FP_EMA_H
-#define FP_EMA_H
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*!***************************************************************************
- * @addtogroup  argus_fp
- * @{
- *****************************************************************************/
-
-#include "fp_def.h"
-
-#include "utility/fp_rnd.h"
-#include "utility/fp_mul.h"
-
-/*!***************************************************************************
- * @brief   Circular exponentially weighted moving average using UQ1.15 format.
- *
- * @details Evaluates the moving average (exponentially weighted) for circular
- *          data in UQ1.15 format.
- *          Circular data is that MAX_VALUE + 1 == MIN_VALUE. For example the
- *          usual phase information.
- *
- *          Problem: Due to circularity of phase values, i.e. 0+x and 2PI+x are
- *          the same, the usual EMA has issues with the wrap around effect.
- *          Especially for vectors with phase around 0 (or 2PI), two values
- *          like 0 + x and PI - y are averaged to something around PI instead
- *          of 0 which would be more correct.
- *
- *          Solution: Assume that phase jumps of more than PI are not allowed
- *          or possible. If a deviation of the new value to the smoothed signal
- *          occurs, it is clear that this stems from the wrap around effect and
- *          can be caught and correctly handled by the smoothing algorithm.
- *
- *          Caution: If a target comes immediately into the field of view, phase
- *          jumps of > PI are indeed possible and volitional. However, the
- *          averaging break there anyway since the smoothed signal approaches
- *          only with delay to the correct values. The error made here is, that
- *          the smoothed signal approaches from the opposite direction. However,
- *          is approaches even faster since it always takes the shortest
- *          direction.
- *
- * @param   mean The previous mean value in UQ1.15 format.
- * @param   x The current value to be added to the average UQ1.15 format.
- * @param   weight The EMA weight in UQ0.7 format.
- * @return  The new mean value in UQ1.15 format.
- *****************************************************************************/
-inline uq1_15_t fp_ema15c(uq1_15_t mean, uq1_15_t x, uq0_8_t weight)
-{
-	if (weight == 0) { return x; }
-
-	// Heeds the wrap around effect by casting dx to int16:
-	const int16_t dx = (int16_t)(x - mean);
-	const int32_t diff = weight * dx;
-	return (uq1_15_t)fp_rnds((mean << 8U) + diff, 8U);
-}
-
-/*!***************************************************************************
- * @brief   Exponentially weighted moving average using the Q11.4 format.
- *
- * @details Evaluates the moving average (exponentially weighted) for data in
- *          Q11.4 format.
- *
- * @param   mean The previous mean value in Q11.4 format.
- * @param   x The current value to be added to the average Q11.4 format.
- * @param   weight The EMA weight in UQ0.7 format.
- * @return  The new mean value in Q11.4 format.
- *****************************************************************************/
-inline q11_4_t fp_ema4(q11_4_t mean, q11_4_t x, uq0_8_t weight)
-{
-	if (weight == 0) { return x; }
-
-	const int32_t dx = x - mean;
-	const int32_t diff = weight * dx;
-	return (q11_4_t)fp_rnds((mean << 8U) + diff, 8U);
-}
-
-/*!***************************************************************************
- * @brief   Exponentially weighted moving average using the Q7.8 format.
- *
- * @details Evaluates the moving average (exponentially weighted) for data in
- *          Q7.8 format.
- *
- * @param   mean The previous mean value in Q7.8 format.
- * @param   x The current value to be added to the average Q7.8 format.
- * @param   weight The EMA weight in UQ0.7 format.
- * @return  The new mean value in Q7.8 format.
- *****************************************************************************/
-inline q7_8_t fp_ema8(q7_8_t mean, q7_8_t x, uq0_8_t weight)
-{
-	return (q7_8_t)fp_ema4(mean, x, weight);
-}
-
-/*!***************************************************************************
- * @brief   Exponentially weighted moving average using the Q15.16 format.
- *
- * @details Evaluates the moving average (exponentially weighted) for data in
- *          Q15.16 format.
- *
- * @param   mean The previous mean value in Q15.16 format.
- * @param   x The current value to be added to the average Q15.16 format.
- * @param   weight The EMA weight in UQ0.7 format.
- * @return  The new mean value in Q15.16 format.
- *****************************************************************************/
-inline uint32_t uint_ema32(uint32_t mean, uint32_t x, uq0_8_t weight)
-{
-	if (weight == 0) { return x; }
-
-	if (x > mean) {
-		const uint32_t dx = x - mean;
-		const uint32_t diff = fp_mulu(weight, dx, 8U);
-		return mean + diff;
-
-	} else {
-		const uint32_t dx = mean - x;
-		const uint32_t diff = fp_mulu(weight, dx, 8U);
-		return mean - diff;
-	}
-}
-/*!***************************************************************************
- * @brief   Exponentially weighted moving average using the Q15.16 format.
- *
- * @details Evaluates the moving average (exponentially weighted) for data in
- *          Q15.16 format.
- *
- * @param   mean The previous mean value in Q15.16 format.
- * @param   x The current value to be added to the average Q15.16 format.
- * @param   weight The EMA weight in UQ0.7 format.
- * @return  The new mean value in Q15.16 format.
- *****************************************************************************/
-inline int32_t int_ema32(int32_t mean, int32_t x, uq0_8_t weight)
-{
-	if (weight == 0) { return x; }
-
-	if (x > mean) {
-		const uint32_t dx = x - mean;
-		const uint32_t diff = fp_mulu(weight, dx, 8U);
-		return mean + diff;
-
-	} else {
-		const uint32_t dx = mean - x;
-		const uint32_t diff = fp_mulu(weight, dx, 8U);
-		return mean - diff;
-	}
-}
-
-/*!***************************************************************************
- * @brief   Exponentially weighted moving average using the Q15.16 format.
- *
- * @details Evaluates the moving average (exponentially weighted) for data in
- *          Q15.16 format.
- *
- * @param   mean The previous mean value in Q15.16 format.
- * @param   x The current value to be added to the average Q15.16 format.
- * @param   weight The EMA weight in UQ0.7 format.
- * @return  The new mean value in Q15.16 format.
- *****************************************************************************/
-inline q15_16_t fp_ema16(q15_16_t mean, q15_16_t x, uq0_8_t weight)
-{
-	return (q15_16_t)int_ema32(mean, x, weight);
-}
-
-/*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
-#endif /* FP_EMA_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_exp.h

@@ -1,69 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file provides an exponential function for fixed point type.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef FP_EXP_H
-#define FP_EXP_H
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*!***************************************************************************
- * @addtogroup  argus_fp
- * @{
- *****************************************************************************/
-
-#include "fp_def.h"
-
-/*!***************************************************************************
- * @brief   Calculates the exponential of an fixed point number Q15.16 format.
- *
- * @details Calculates y = exp(x) in fixed point representation.
- *
- *          Note that the result might not be 100 % accurate and might contain
- *          a small error!
- *
- *          @see https://www.quinapalus.com/efunc.html
- *
- * @param   x The input parameter in unsigned fixed point format Q15.16.
- * @return  Result y = exp(x) in the UQ16.16 format.
- *****************************************************************************/
-uq16_16_t fp_exp16(q15_16_t x);
-
-/*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
-#endif /* FP_DIV_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_log.h

@@ -1,69 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file provides an logarithm function for fixed point type.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef FP_LOG_H
-#define FP_LOG_H
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*!***************************************************************************
- * @addtogroup  argus_fp
- * @{
- *****************************************************************************/
-
-#include "fp_def.h"
-
-/*!***************************************************************************
- * @brief   Calculates the natural logarithm (base e) of an fixed point number.
- *
- * @details Calculates y = ln(x) = log_e(x) in fixed point representation.
- *
- *          Note that the result might not be 100 % accurate and might contain
- *          a small error!
- *
- *          @see https://www.quinapalus.com/efunc.html
- *
- * @param   x The input parameter in unsigned fixed point format Q15.16.
- * @return  Result y = ln(x) in the UQ16.16 format.
- *****************************************************************************/
-q15_16_t fp_log16(uq16_16_t x);
-
-/*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
-#endif /* FP_DIV_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h

@@ -1,235 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Provides definitions and basic macros for fixed point data types.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef FP_MUL_H
-#define FP_MUL_H
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*!***************************************************************************
- * @addtogroup  argus_fp
- * @{
- *****************************************************************************/
-
-#include "fp_def.h"
-#include "utility/fp_rnd.h"
-
-/*!***************************************************************************
- * Set to use hardware division (Cortex-M3/4) over software division (Cortex-M0/1).
- *****************************************************************************/
-#ifndef USE_64BIT_MUL
-#define USE_64BIT_MUL 0
-#endif
-
-#if !USE_64BIT_MUL
-/*!***************************************************************************
- * @brief   Long multiplication of two unsigned 32-bit into an 64-bit value on
- *          32-bit architecture.
- *
- * @details w (two words) gets the product of u and v (one word each).
- *          w[0] is the most significant word of the result, w[1] the least.
- *          (The words are in big-endian order).
- *          It is Knuth's Algorithm M from [Knu2] section 4.3.1.
- * *
- * @see     http://www.hackersdelight.org/hdcodetxt/muldwu.c.txt
- *
- * @param   w The result (u * v) value given as two unsigned 32-bit numbers:
- *              w[0] is the most significant word of the result, w[1] the least.
- *              (The words are in big-endian order).
- * @param   u Left hand side of the multiplication.
- * @param   v Right hand side of the multiplication.
- *****************************************************************************/
-inline void muldwu(uint32_t w[], uint32_t u, uint32_t v)
-{
-	const uint32_t u0 = u >> 16U;
-	const uint32_t u1 = u & 0xFFFFU;
-	const uint32_t v0 = v >> 16U;
-	const uint32_t v1 = v & 0xFFFFU;
-
-	uint32_t t = u1 * v1;
-	const uint32_t w3 = t & 0xFFFFU;
-	uint32_t k = t >> 16U;
-
-	t = u0 * v1 + k;
-	const uint32_t w2 = t & 0xFFFFU;
-	const uint32_t w1 = t >> 16U;
-
-	t = u1 * v0 + w2;
-	k = t >> 16U;
-
-	w[0] = u0 * v0 + w1 + k;
-	w[1] = (t << 16U) + w3;
-}
-#endif
-
-/*!***************************************************************************
- * @brief   64-bit implementation of an unsigned multiplication with fixed point format.
- *
- * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point
- *          number of 32-bit width. The multiplication is done in 64-bit and
- *          the result is shifted down by the passed shift parameter in order
- *          to return a 32-bit value.
- *          The shift is executed with correct rounding.
- *
- *          Note that the result must fit into the 32-bit value. An assertion
- *          error occurs otherwise (or undefined behavior of no assert available).
- *
- * @param   u The left parameter in UQx1.y1 format
- * @param   v The right parameter in UQx2.y2 format
- * @param   shift The final right shift (rounding) value.
- * @return  Result = (a*b)>>shift in UQx.(y1+y2-shift) format.
- *****************************************************************************/
-inline uint32_t fp_mulu(uint32_t u, uint32_t v, uint_fast8_t shift)
-{
-	assert(shift <= 32);
-#if USE_64BIT_MUL
-	const uint64_t w = (uint64_t)u * (uint64_t)v;
-	return (w >> shift) + ((w >> (shift - 1)) & 1U);
-#else
-	uint32_t tmp[2] = { 0 };
-	muldwu(tmp, u, v);
-
-	assert(shift ? tmp[0] <= (UINT32_MAX >> (32 - shift)) : tmp[0] == 0);
-
-	if (32 - shift) {
-		return ((tmp[0] << (32 - shift)) + fp_rndu(tmp[1], shift));
-
-	} else {
-		return tmp[1] > (UINT32_MAX >> 1) ? tmp[0] + 1 : tmp[0];
-	}
-
-#endif
-}
-
-/*!***************************************************************************
- * @brief   64-bit implementation of a signed multiplication with fixed point format.
- *
- * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point
- *          number of 32-bit width. The multiplication is done in 64-bit and
- *          the result is shifted down by the passed shift parameter in order
- *          to return a 32-bit value.
- *          The shift is executed with correct rounding.
- *
- *          Note that the result must fit into the 32-bit value. An assertion
- *          error occurs otherwise (or undefined behavior of no assert available).
- *
- * @param   u The left parameter in Qx1.y1 format
- * @param   v The right parameter in Qx2.y2 format
- * @param   shift The final right shift (rounding) value.
- * @return  Result = (a*b)>>shift in Qx.(y1+y2-shift) format.
- *****************************************************************************/
-inline int32_t fp_muls(int32_t u, int32_t v, uint_fast8_t shift)
-{
-	int_fast8_t sign = 1;
-
-	uint32_t u2, v2;
-
-	if (u < 0) { u2 = -u; sign = -sign; } else { u2 = u; }
-
-	if (v < 0) { v2 = -v; sign = -sign; } else { v2 = v; }
-
-	const uint32_t res = fp_mulu(u2, v2, shift);
-
-	assert(sign > 0 ? res <= 0x7FFFFFFFU : res <= 0x80000000U);
-
-	return sign > 0 ? res : -res;
-}
-
-
-/*!***************************************************************************
- * @brief   48-bit implementation of a unsigned multiplication with fixed point format.
- *
- * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point
- *          numbers with 32-bit unsigned and 16-bit unsigned format respectively.
- *          The multiplication is done in two 16x16-bit operations and the
- *          result is shifted down by the passed shift parameter in order to
- *          return a 32-bit value.
- *
- *          Note that the result must fit into the 32-bit value. An assertion
- *          error occurs otherwise (or undefined behavior of no assert available).
- *
- * @param   u The left parameter in Qx1.y1 format
- * @param   v The right parameter in Qx2.y2 format
- * @param   shift The final right shift (rounding) value.
- * @return  Result = (a*b)>>shift in Qx.(y1+y2-shift) format.
- *****************************************************************************/
-inline uint32_t fp_mul_u32_u16(uint32_t u, uint16_t v, uint_fast8_t shift)
-{
-	assert(shift <= 48);
-
-	if (shift > 16) {
-		uint32_t msk = 0xFFFFU;
-		uint32_t a = (u >> 16U) * v;
-		uint32_t b = (msk & u) * v;
-		return fp_rndu(a, shift - 16) + fp_rndu(b, shift);
-
-	} else {
-		uint32_t msk = ~(0xFFFFFFFFU << shift);
-		uint32_t a = (u >> shift) * v;
-		uint32_t b = fp_rndu((msk & u) * v, shift);
-		return a + b;
-	}
-}
-
-/*!***************************************************************************
- * @brief   48-bit implementation of an unsigned/signed multiplication with fixed point format.
- *
- * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point
- *          numbers with 32-bit signed and 16-bit unsigned format respectively.
- *          The multiplication is done in two 16x16-bit operations and the
- *          result is shifted down by the passed shift parameter in order to
- *          return a 32-bit value.
- *          The shift is executed with correct rounding.
- *
- *          Note that the result must fit into the 32-bit value. An assertion
- *          error occurs otherwise (or undefined behavior of no assert available).
- *
- * @param   u The left parameter in Qx1.y1 format
- * @param   v The right parameter in Qx2.y2 format
- * @param   shift The final right shift (rounding) value.
- * @return  Result = (a*b)>>shift in Qx.(y1+y2-shift) format.
- *****************************************************************************/
-inline int32_t fp_mul_s32_u16(int32_t u, uint16_t v, uint_fast8_t shift)
-{
-	return u >= 0 ? fp_mul_u32_u16(u, v, shift) : - fp_mul_u32_u16(-u, v, shift);
-}
-
-/*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
-#endif /* FP_MUL_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h

@@ -1,118 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Provides definitions and basic macros for fixed point data types.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef FP_RND_H
-#define FP_RND_H
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*!***************************************************************************
- * @addtogroup  argus_fp
- * @{
- *****************************************************************************/
-
-#include "fp_def.h"
-#include <assert.h>
-
-/*!***************************************************************************
- * @brief   Converting with rounding from UQx.n1 to UQx.n2.
- * @details Equivalent to dividing by 2^n with correct rounding to unsigned
- *          integer values.
- * @param   Q The number in (U)Qx.n1 fixed point format to be rounded.
- * @param   n The number of bits to be rounded,
- *              e.g. UQ8.8 -> UQ12.4 => n = 8 - 4 = 4.
- * @return  The rounded value in (U)Qx.n2 format.
- *****************************************************************************/
-inline uint32_t fp_rndu(uint32_t Q, uint_fast8_t n)
-{
-	if (n == 0) { return Q; }
-
-	else if (n > 32U) { return 0; }
-
-	// Shift by n>=32 yields undefined behavior! Thus, this extra first
-	// step is essential to prevent issues.
-	Q >>= n - 1;
-	return (Q >> 1) + (Q & 1U);
-}
-
-/*!***************************************************************************
- * @brief   Converting with rounding from Qx.n1 to Qx.n2.
- * @details Equivalent to dividing by 2^n with correct rounding to integer
- *          values.
- * @param   Q The number in (U)Qx.n1 fixed point format to be rounded.
- * @param   n The number of bits to be rounded,
- *              e.g. Q7.8 -> Q11.4 => n = 8 - 4 = 4.
- * @return  The rounded value in (U)Qx.n2 format.
- *****************************************************************************/
-inline int32_t fp_rnds(int32_t Q, uint_fast8_t n)
-{
-	return (Q < 0) ? -fp_rndu(-Q, n) : fp_rndu(Q, n);
-}
-
-/*!***************************************************************************
- * @brief   Converting with truncation from UQx.n1 to UQx.n2.
- * @details Equivalent to dividing by 2^n with truncating (throw away) the
- *          fractional part, resulting in an unsigned integer/fixed-point value.
- * @param   Q The number in (U)Qx.n1 fixed point format to be truncated.
- * @param   n The number of bits to be truncated,
- *              e.g. UQ8.8 -> UQ12.4 => n = 8 - 4 = 4.
- * @return  The truncated value in (U)Qx.n2 format.
- *****************************************************************************/
-inline uint32_t fp_truncu(uint32_t Q, uint_fast8_t n)
-{
-	return (n < 32U) ? (Q >> n) : 0;
-}
-
-/*!***************************************************************************
- * @brief   Converting with truncation from Qx.n1 to Qx.n2.
- * @details Equivalent to dividing by 2^n with truncating (throw away) the
- *          fractional part, resulting in a signed integer/fixed-point value.
- * @param   Q The number in (U)Qx.n1 fixed point format to be truncated.
- * @param   n The number of bits to be truncated,
- *              e.g. Q7.8 -> Q11.4 => n = 8 - 4 = 4.
- * @return  The truncated value in (U)Qx.n2 format.
- *****************************************************************************/
-inline int32_t fp_truncs(int32_t Q, uint_fast8_t n)
-{
-	return (Q < 0) ? -fp_truncu(-Q, n) : fp_truncu(Q, n);
-}
-
-/*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
-#endif /* FP_RND_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h

@@ -1,281 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     Provides algorithms applied to integer values.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef INT_MATH
-#define INT_MATH
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*!***************************************************************************
- * @addtogroup  argus_misc
- * @{
- *****************************************************************************/
-
-#include <stdint.h>
-#include <assert.h>
-
-/*! Enables the integer square root function. */
-#ifndef INT_SQRT
-#define INT_SQRT 0
-#endif
-
-/*!***************************************************************************
- * @brief   Integer Base-2 Logarithm.
- *
- * @details Calculates the base-2 logarithm for unsigned integer values. The
- *          result is the integer equivalent of floor(log2(x)).
- *
- * @param   x Input parameter.
- * @return  The floor of the base-2 logarithm.
- *****************************************************************************/
-inline uint32_t log2i(uint32_t x)
-{
-	assert(x != 0);
-#if 1
-	return 31 - __builtin_clz(x);
-#else
-#define S(k) if (x >= (1 << k)) { i += k; x >>= k; }
-	int i = 0; S(16); S(8); S(4); S(2); S(1); return i;
-#undef S
-#endif
-}
-
-/*!***************************************************************************
- * @brief   Integer Base-2 Logarithm with rounded result.
- *
- * @details Calculates the base-2 logarithm for unsigned integer values and
- *          returns the rounded result. The result is the integer equivalent
- *          of round(log2(x)).
- *
- *          It is finding the nearest power-of-two value s.t. |x - 2^n| becomes
- *          minimum for all n.
- *
- * @param   x Input parameter.
- * @return  The rounded value of the base-2 logarithm.
- *****************************************************************************/
-inline uint32_t log2_round(uint32_t x)
-{
-	assert(x != 0);
-#if 0
-	const uint32_t y = x;
-	const uint32_t i = 0;
-
-	while (y >>= 1) { i++; }
-
-#else
-	const uint32_t i = log2i(x);
-#endif
-	return (i + ((x >> (i - 1U)) == 3U));
-}
-
-/*!***************************************************************************
- * @brief   Finding the nearest power-of-two value.
- *
- * @details Implemented s.t. |x - 2^n| becomes minimum for all n.
- *          Special case 0: returns 0;
- *          Maximum input: 3037000499; higher number result in overflow! (returns 0)
- *
- * @param   x Input parameter.
- * @return  Nearest power-of-two number, i.e. 2^n.
- *****************************************************************************/
-inline uint32_t binary_round(uint32_t x)
-{
-	assert(x != 0);
-	const uint32_t shift = log2_round(x);
-	return (shift > 31U) ? 0 : 1U << shift;
-}
-
-/*!***************************************************************************
- * @brief   Counting bits set in a 32-bit unsigned integer.
- *
- * @details @see http://graphics.stanford.edu/~seander/bithacks.html
- *
- * @param   x Input parameter.
- * @return  Number of bits set in input value.
- *****************************************************************************/
-inline uint32_t popcount(uint32_t x)
-{
-	// http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
-	x = x - ((x >> 1) & 0x55555555);
-	x = (x & 0x33333333) + ((x >> 2) & 0x33333333);
-	return (((x + (x >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
-}
-
-/*!***************************************************************************
- * @brief   Determining if an integer is a power of 2
- *
- * @details @see http://graphics.stanford.edu/~seander/bithacks.html
- *
- * @param   x Input parameter.
- * @return  True if integer is power of 2.
- *****************************************************************************/
-inline uint32_t ispowoftwo(uint32_t x)
-{
-	return x && !(x & (x - 1));
-}
-
-/*!***************************************************************************
- * @brief   Calculates the absolute value.
- *
- * @param   x Input parameter.
- * @return  The absolute value of x.
- *****************************************************************************/
-inline uint32_t absval(int32_t x)
-{
-	// Note: special case of INT32_MIN must be handled correctly:
-	return x < 0 ? ((~(uint32_t)(x)) + 1) : (uint32_t)x;
-
-	/* alternative with equal performance:*/
-//  int32_t y = x >> 31;
-//  return (x ^ y) - y;
-	/* wrong implementation:
-	 * does not correctly return abs(INT32_MIN) on 32-bit platform */
-//  return x < 0 ? (uint32_t)(-x) : (uint32_t)x;
-}
-
-/*!***************************************************************************
- * @brief   Calculates the floor division by a factor of 2: floor(x / 2^n).
- *
- * @param   x Input parameter.
- * @param   n The shift value, maximum is 31.
- * @return  The floor division by 2^n result.
- *****************************************************************************/
-inline uint32_t floor2(uint32_t x, uint_fast8_t n)
-{
-	assert(n < 32);
-	return x >> n;
-}
-
-/*!***************************************************************************
- * @brief   Calculates the ceildiv division by a factor of 2: ceildiv(x / 2^n).
- *
- * @param   x Input parameter.
- * @param   n The shift value, maximum is 31.
- * @return  The ceildiv division by 2^n result.
- *****************************************************************************/
-inline uint32_t ceiling2(uint32_t x, uint_fast8_t n)
-{
-	assert(n < 32);
-	return x ? (1 + ((x - 1) >> n)) : 0;
-}
-
-/*!***************************************************************************
- * @brief   Calculates the ceildiv division: ceildiv(x / y).
- *
- * @param   x Numerator
- * @param   y Denominator
- * @return  The result of the ceildiv division ceildiv(x / y).
- *****************************************************************************/
-inline uint32_t ceildiv(uint32_t x, uint32_t y)
-{
-	assert(y != 0);
-	return x ? (1 + ((x - 1) / y)) : 0;
-}
-
-/*!***************************************************************************
- * @brief   Calculates the maximum of two values.
- *
- * @param   a Input parameter.
- * @param   b Input parameter.
- * @return  The maximum value of the input parameters.
- *****************************************************************************/
-#define MAX(a, b) ((a) > (b) ? (a) : (b))
-
-/*!***************************************************************************
- * @brief   Calculates the minimum of two values.
- *
- * @param   a Input parameter.
- * @param   b Input parameter.
- * @return  The minimum value of the input parameters.
- *****************************************************************************/
-#define MIN(a, b) ((a) < (b) ? (a) : (b))
-
-/*!***************************************************************************
- * @brief   Clamps a value between a minimum and maximum boundary.
- *
- * @details Clamps the values such that the condition min <= x <= max is true.
- *
- * @note    The condition \p min <= \p max must hold!!!
- *
- * @param   x The input parameter to be clamped.
- * @param   min The minimum or lower boundary.
- * @param   max The maximum or upper boundary.
- * @return  The clamped value of the input parameter within [min,max].
- *****************************************************************************/
-#define CLAMP(x, min, max) (MIN(MAX((x), (min)), (max)))
-
-#if INT_SQRT
-/*!***************************************************************************
- * @brief   Calculates the integer square root of x.
- *
- * @details The integer square root is defined as:
- *          isqrt(x) = (int)sqrt(x)
- *
- *          @see https://en.wikipedia.org/wiki/Integer_square_root
- *          @see https://github.com/chmike/fpsqrt/blob/master/fpsqrt.c
- *
- * @param   x Input parameter.
- * @return      isqrt(x)
- *****************************************************************************/
-inline uint32_t isqrt(uint32_t v)
-{
-	unsigned t, q, b, r;
-	r = v;           // r = v - x²
-	b = 0x40000000;  // a²
-	q = 0;           // 2ax
-
-	while (b > 0) {
-		t = q + b;   // t = 2ax + a²
-		q >>= 1;     // if a' = a/2, then q' = q/2
-
-		if (r >= t) { // if (v - x²) >= 2ax + a²
-			r -= t;  // r' = (v - x²) - (2ax + a²)
-			q += b;  // if x' = (x + a) then ax' = ax + a², thus q' = q' + b
-		}
-
-		b >>= 2;     // if a' = a/2, then b' = b / 4
-	}
-
-	return q;
-}
-#endif // INT_SQRT
-
-/*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
-#endif /* INT_MATH */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/status.h

@@ -1,49 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file contains status codes for all platform specific
- *              functions.
- *
- * @copyright
- *
- * Copyright (c) 2023, Broadcom 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 of the copyright holder 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 HOLDER 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.
- *****************************************************************************/
-
-#ifndef STATUS_H
-#define STATUS_H
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "api/argus_status.h"
-
-#ifdef __cplusplus
-} // extern "C"
-#endif
-#endif /* STATUS_H */

src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h

@@ -1,11 +1,11 @@
 /*************************************************************************//**
  * @file
- * @brief       This file is part of the AFBR-S50 API.
- * @details     This file provides utility functions for timing necessities.
+ * @brief    	This file is part of the AFBR-S50 API.
+ * @details		This file provides utility functions for timing necessities.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom Inc
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,563 +36,255 @@
 
 #ifndef TIME_H
 #define TIME_H
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 /*!***************************************************************************
- * @defgroup    argus_time Time Utility
- * @ingroup     argus_util
- *
- * @brief       Timer utilities for time measurement duties.
- *
- * @details     This module provides time measurement utility functions like
- *              delay or time measurement methods, or time math functions.
- *
- * @addtogroup  argus_time
+ * @defgroup	time Time Utility
+ * @ingroup		argusutil
+ * @brief 		Timer utilities for time measurement duties.
+ * @details		This module provides time measurement utility functions like
+ * 				delay or time measurement methods, or time math functions.
+ * @addtogroup 	time
  * @{
  *****************************************************************************/
 
-#include "platform/argus_timer.h"
-#include <assert.h>
 #include <stdint.h>
 #include <stdbool.h>
 
 /*!***************************************************************************
- * @brief   A data structure to represent current time.
+ * @brief	A data structure to represent current time.
  *
- * @details Value is obtained from the PIT time which must be configured as
- *          lifetime counter.
- *
- *          Range: [0.000000, 4294967296.999999] seconds
+ * @details	Value is obtained from the PIT time which must be configured as
+ * 			lifetime counter.
  *****************************************************************************/
-typedef struct ltc_t {
-	/*! Seconds;
-	 *  Range: [0, UINT32_MAX] seconds */
+typedef struct {
+	/*! Seconds. */
 	uint32_t sec;
 
-	/*! Microseconds;
-	 *  Range: [0, 999999] microseconds */
+	/*! Microseconds. */
 	uint32_t usec;
 
 } ltc_t;
 
 /*!***************************************************************************
- * @brief   Converts #ltc_t to microseconds (uint32_t).
- * @details The specified time value (type #ltc_t) is converted to microseconds.
- *          The value is truncated to UINT32_MAX value if the result would
- *          exceed UINT32_MAX microseconds.
- * @param   t Input #ltc_t structure.
- * @return  Time value in microseconds.
+ * @brief	Obtains the elapsed time since MCU startup.
+ * @param	t_now returned current time
  *****************************************************************************/
-inline uint32_t Time_ToUSec(ltc_t const *t)
-{
-	assert(t != 0);
-
-	// max. value to convert correctly is 4294.967295 sec (UINT32_MAX/1000000)
-	return ((t->sec < 4294U) || (t->sec == 4294U && t->usec < 967295U)) ?
-	       t->usec + t->sec * 1000000U : UINT32_MAX;
-}
+void Time_GetNow(ltc_t *t_now);
 
 /*!***************************************************************************
- * @brief   Converts #ltc_t to milliseconds (uint32_t).
- * @details The specified time value (type #ltc_t) is converted to milliseconds.
- *          The value is truncated to UINT32_MAX value if the result would
- *          exceed UINT32_MAX milliseconds.
- *          The returned value is correctly rounded to the nearest value.
- * @param   t Input #ltc_t structure.
- * @return  Time value in milliseconds.
+ * @brief	Obtains the elapsed microseconds since MCU startup.
+ * @details	Wrap around effect due to uint32_t result format!!
+ * @param 	-
+ * @return	Elapsed microseconds since MCU startup as uint32_t.
  *****************************************************************************/
-inline uint32_t Time_ToMSec(ltc_t const *t)
-{
-	assert(t != 0);
-
-	// max. value to convert correctly is 4294967.295499 sec (UINT32_MAX/1000)
-	return ((t->sec < 4294967U) || (t->sec == 4294967U && t->usec < 295500U)) ?
-	       (t->usec + 500U) / 1000U + t->sec * 1000U : UINT32_MAX;
-}
+uint32_t Time_GetNowUSec(void);
 
 /*!***************************************************************************
- * @brief   Converts #ltc_t to seconds (uint32_t).
- * @details The specified time value (type #ltc_t) is converted to seconds.
- *          The returned value is correctly rounded to the nearest value.
- * @param   t Input #ltc_t structure.
- * @return  Time value in seconds.
+ * @brief	Obtains the elapsed milliseconds since MCU startup.
+ * @details	Wrap around effect due to uint32_t result format!!
+ * @param 	-
+ * @return	Elapsed milliseconds since MCU startup as uint32_t.
  *****************************************************************************/
-inline uint32_t Time_ToSec(ltc_t const *t)
-{
-	assert(t != 0);
-
-	// max. value to convert correctly is 4294967295.499999 sec (UINT32_MAX/1000)
-	return (t->sec < 4294967295U || t->usec < 500000U) ?
-	       (t->usec + 500000U) / 1000000U + t->sec : UINT32_MAX;
-}
+uint32_t Time_GetNowMSec(void);
 
 /*!***************************************************************************
- * @brief   Converts microseconds (uint32_t) to #ltc_t.
- * @details The specified time value in microseconds is converted to type #ltc_t.
- * @param   t Output #ltc_t structure.
- * @param   t_usec Input time in microseconds.
+ * @brief	Obtains the elapsed seconds since MCU startup.
+ * @param 	-
+ * @return	Elapsed seconds since MCU startup as uint32_t.
  *****************************************************************************/
-inline void Time_FromUSec(ltc_t *t, uint32_t t_usec)
-{
-	assert(t != 0);
-	t->sec = t_usec / 1000000U;
-	t->usec = t_usec % 1000000U;
-}
+uint32_t Time_GetNowSec(void);
 
 /*!***************************************************************************
- * @brief   Converts milliseconds (uint32_t) to #ltc_t.
- * @details The specified time value in milliseconds is converted to type #ltc_t.
- * @param   t Output #ltc_t structure.
- * @param   t_msec Input time in milliseconds.
+ * @brief	Obtains the elapsed time since a given time point.
+ * @param	t_elapsed Returns the elapsed time since t_start.
+ * @param	t_start Start time point.
  *****************************************************************************/
-inline void Time_FromMSec(ltc_t *t, uint32_t t_msec)
-{
-	assert(t != 0);
-	t->sec = t_msec / 1000U;
-	t->usec = (t_msec % 1000U) * 1000U;
-}
+void Time_GetElapsed(ltc_t *t_elapsed, ltc_t const *t_start);
 
 /*!***************************************************************************
- * @brief   Converts seconds (uint32_t) to #ltc_t.
- * @details The specified time value in seconds is converted to type #ltc_t.
- * @param   t Output #ltc_t structure.
- * @param   t_sec Input time in seconds.
+ * @brief	Obtains the elapsed microseconds since a given time point.
+ * @details	Wrap around effect due to uint32_t result format!!
+ * @param	t_start Start time point.
+ * @return	Elapsed microseconds since t_start as uint32_t.
  *****************************************************************************/
-inline void Time_FromSec(ltc_t *t, uint32_t t_sec)
-{
-	assert(t != 0);
-	t->usec = 0;
-	t->sec = t_sec;
-}
-
+uint32_t Time_GetElapsedUSec(ltc_t const *t_start);
 
 /*!***************************************************************************
- * @brief   Checks if /p t1 is greater or equal that /p t2.
- * @details Handles overflow.
- * @param   t1 1st operand.
- * @param   t2 2nd operand.
- * @return  Returns (t1 >= t2);
+ * @brief	Obtains the elapsed milliseconds since a given time point.
+ * @details	Wrap around effect due to uint32_t result format!!
+ * @param	t_start Start time point.
+ * @return	Elapsed milliseconds since t_start as uint32_t.
  *****************************************************************************/
-inline bool Time_GreaterEqual(ltc_t const *t1, ltc_t const *t2)
-{
-	assert(t1 != 0);
-	assert(t2 != 0);
-	return (t1->sec == t2->sec) ? (t1->usec >= t2->usec) : (t1->sec > t2->sec);
-}
-
+uint32_t Time_GetElapsedMSec(ltc_t const *t_start);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed time since MCU startup.
- * @param   t_now returned current time
+ * @brief	Obtains the elapsed seconds since a given time point.
+ * @param	t_start Start time point.
+ * @return	Elapsed seconds since t_start as uint32_t.
  *****************************************************************************/
-inline void Time_GetNow(ltc_t *t_now)
-{
-	assert(t_now != 0);
-	Timer_GetCounterValue(&(t_now->sec), &(t_now->usec));
-	assert(t_now->usec < 1000000U);
-}
+uint32_t Time_GetElapsedSec(ltc_t const *t_start);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed time since MCU startup.
- * @return  Returns the current time.
+ * @brief	Obtains the time difference between two given time points.
+ * @details	Result is defined as t_diff = t_end - t_start.
+ * 			Note: since no negative time differences are supported, t_end has
+ * 			to be later/larger than t_start. Otherwise, the result won't be
+ * 			a negative time span but given by max value.
+ * @param	t_diff Returned time difference.
+ * @param	t_start Start time point.
+ * @param	t_end End time point.
  *****************************************************************************/
-inline ltc_t Time_Now(void)
-{
-	ltc_t t_now;
-	Time_GetNow(&t_now);
-	return t_now;
-}
+void Time_Diff(ltc_t *t_diff, ltc_t const *t_start, ltc_t const *t_end);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed microseconds since MCU startup.
- * @details Wrap around effect due to uint32_t result format!!
- * @return  Elapsed microseconds since MCU startup as uint32_t.
+ * @brief	Obtains the time difference between two given time points in
+ * 			microseconds.
+ * @details	Result is defined as t_diff = t_end - t_start.
+ * 			Refers to Time_Diff() and handles overflow such that to large
+ * 			values are limited by 0xFFFFFFFF µs.
+ * @param	t_start Start time point.
+ * @param	t_end End time point.
+ * @return	Time difference in microseconds.
  *****************************************************************************/
-inline uint32_t Time_GetNowUSec(void)
-{
-	ltc_t t_now = Time_Now();
-	return Time_ToUSec(&t_now);
-}
+uint32_t Time_DiffUSec(ltc_t const *t_start, ltc_t const *t_end);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed milliseconds (rounded) since MCU startup.
- * @details Wrap around effect due to uint32_t result format!!
- * @return  Elapsed milliseconds since MCU startup as uint32_t.
+ * @brief	Obtains the time difference between two given time points in
+ * 			milliseconds.
+ * @details	Result is defined as t_diff = t_end - t_start.
+ * 			Refers to Time_Diff() and handles overflow.
+ *			Wrap around effect due to uint32_t result format!!
+ * @param	t_start Start time point.
+ * @param	t_end End time point.
+ * @return	Time difference in milliseconds.
  *****************************************************************************/
-inline uint32_t Time_GetNowMSec(void)
-{
-	ltc_t t_now = Time_Now();
-	return Time_ToMSec(&t_now);
-}
+uint32_t Time_DiffMSec(ltc_t const *t_start, ltc_t const *t_end);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed seconds (rounded) since MCU startup.
- * @return  Elapsed seconds since MCU startup as uint32_t.
+ * @brief	Obtains the time difference between two given time points in
+ * 			seconds.
+ * @details	Result is defined as t_diff = t_end - t_start.
+ * 			Refers to Time_Diff() and handles overflow.
+ * @param	t_start Start time point.
+ * @param	t_end End time point.
+ * @return	Time difference in seconds.
  *****************************************************************************/
-inline uint32_t Time_GetNowSec(void)
-{
-	ltc_t t_now = Time_Now();
-	return Time_ToSec(&t_now);
-}
-
+uint32_t Time_DiffSec(ltc_t const *t_start, ltc_t const *t_end);
 
 /*!***************************************************************************
- * @brief   Obtains the time difference between two given time points.
- * @details Result is defined as t_diff = t_end - t_start.
- *          Note: since no negative time differences are supported, t_end has
- *          to be later/larger than t_start. Otherwise, the result is undefined!
- * @param   t_diff Returned time difference.
- * @param   t_start Start time point.
- * @param   t_end End time point.
+ * @brief	Time delay for a given time period.
+ * @param	dt Delay time.
  *****************************************************************************/
-inline void Time_Diff(ltc_t *t_diff, ltc_t const *t_start, ltc_t const *t_end)
-{
-	assert(t_diff != 0);
-	assert(t_start != 0);
-	assert(t_end != 0);
-	assert(t_diff != t_start);
-	assert(t_diff != t_end);
-	assert(Time_GreaterEqual(t_end, t_start));
-
-	if (t_start->usec <= t_end->usec) { // no carry over
-		t_diff->sec = t_end->sec - t_start->sec;
-		t_diff->usec = t_end->usec - t_start->usec;
-
-	} else { // with carry over
-		t_diff->sec = t_end->sec - 1 - t_start->sec;
-		t_diff->usec = (1000000U - t_start->usec) + t_end->usec;
-	}
-}
+void Time_Delay(ltc_t const *dt);
 
 /*!***************************************************************************
- * @brief   Obtains the time difference between two given time points in
- *          microseconds.
- * @details Result is defined as t_diff = t_end - t_start.
- *          Refers to Time_Diff() and handles overflow such that to large
- *          values are limited by 0xFFFFFFFF µs.
- * @param   t_start Start time point.
- * @param   t_end End time point.
- * @return  Time difference in microseconds.
+ * @brief	Time delay for a given time period in microseconds.
+ * @param	dt_usec Delay time in microseconds.
  *****************************************************************************/
-inline uint32_t Time_DiffUSec(ltc_t const *t_start, ltc_t const *t_end)
-{
-	ltc_t t_diff;
-	Time_Diff(&t_diff, t_start, t_end);
-	return Time_ToUSec(&t_diff);
-}
+void Time_DelayUSec(uint32_t dt_usec);
 
 /*!***************************************************************************
- * @brief   Obtains the time difference between two given time points in
- *          milliseconds.
- * @details Result is defined as t_diff = t_end - t_start.
- *          Refers to Time_Diff() and handles overflow.
- *          Wrap around effect due to uint32_t result format!!
- * @param   t_start Start time point.
- * @param   t_end End time point.
- * @return  Time difference in milliseconds.
+ * @brief	Time delay for a given time period in milliseconds.
+ * @param	dt_msec Delay time in milliseconds.
  *****************************************************************************/
-inline uint32_t Time_DiffMSec(ltc_t const *t_start, ltc_t const *t_end)
-{
-	ltc_t t_diff;
-	Time_Diff(&t_diff, t_start, t_end);
-	return Time_ToMSec(&t_diff);
-}
+void Time_DelayMSec(uint32_t dt_msec);
 
 /*!***************************************************************************
- * @brief   Obtains the time difference between two given time points in
- *          seconds.
- * @details Result is defined as t_diff = t_end - t_start.
- *          Refers to Time_Diff() and handles overflow.
- * @param   t_start Start time point.
- * @param   t_end End time point.
- * @return  Time difference in seconds.
+ * @brief	Time delay for a given time period in seconds.
+ * @param	dt_sec Delay time in seconds.
  *****************************************************************************/
-inline uint32_t Time_DiffSec(ltc_t const *t_start, ltc_t const *t_end)
-{
-	ltc_t t_diff;
-	Time_Diff(&t_diff, t_start, t_end);
-	return Time_ToSec(&t_diff);
-}
-
+void Time_DelaySec(uint32_t dt_sec);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed time since a given time point.
- * @details Calculates the currently elapsed time since a specified start time
- *          (/p t_start).
- *
- *          Note that /p t_start must be in the past! Otherwise, the behavior is
- *          undefined!
- *
- * @param   t_elapsed Returns the elapsed time since /p t_start.
- * @param   t_start Start time point.
+ * @brief	Checks if timeout is reached from a given starting time.
+ * @details	Handles overflow.
+ * @param	t_start Start time.
+ * @param	t_timeout Timeout period.
+ * @return	Timeout elapsed? True/False (boolean value)
  *****************************************************************************/
-inline void Time_GetElapsed(ltc_t *t_elapsed, ltc_t const *t_start)
-{
-	assert(t_elapsed != 0);
-	assert(t_start != 0);
-	assert(t_elapsed != t_start);
-	ltc_t t_now = Time_Now();
-	Time_Diff(t_elapsed, t_start, &t_now);
-}
+bool Time_CheckTimeout(ltc_t const *t_start, ltc_t const *t_timeout);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed microseconds since a given time point.
- * @details Wrap around effect due to uint32_t result format!!
- * @param   t_start Start time point.
- * @return  Elapsed microseconds since t_start as uint32_t.
+ * @brief	Checks if timeout is reached from a given starting time.
+ * @details	Handles overflow.
+ * @param	t_start Start time.
+ * @param	t_timeout_usec Timeout period in microseconds.
+ * @return	Timeout elapsed? True/False (boolean value)
  *****************************************************************************/
-inline uint32_t Time_GetElapsedUSec(ltc_t const *t_start)
-{
-	assert(t_start != 0);
-	ltc_t t_now = Time_Now();
-	return Time_DiffUSec(t_start, &t_now);
-}
+bool Time_CheckTimeoutUSec(ltc_t const *t_start, uint32_t const t_timeout_usec);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed milliseconds since a given time point.
- * @details Wrap around effect due to uint32_t result format!!
- * @param   t_start Start time point.
- * @return  Elapsed milliseconds since t_start as uint32_t.
+ * @brief	Checks if timeout is reached from a given starting time.
+ * @details	Handles overflow.
+ * @param	t_start Start time.
+ * @param	t_timeout_msec Timeout period in milliseconds.
+ * @return	Timeout elapsed? True/False (boolean value)
  *****************************************************************************/
-inline uint32_t Time_GetElapsedMSec(ltc_t const *t_start)
-{
-	assert(t_start != 0);
-	ltc_t t_now = Time_Now();
-	return Time_DiffMSec(t_start, &t_now);
-}
+bool Time_CheckTimeoutMSec(ltc_t const *t_start, uint32_t const t_timeout_msec);
 
 /*!***************************************************************************
- * @brief   Obtains the elapsed seconds since a given time point.
- * @param   t_start Start time point.
- * @return  Elapsed seconds since t_start as uint32_t.
+ * @brief	Checks if timeout is reached from a given starting time.
+ * @details	Handles overflow.
+ * @param	t_start Start time.
+ * @param	t_timeout_sec Timeout period in seconds.
+ * @return	Timeout elapsed? True/False (boolean value)
  *****************************************************************************/
-inline uint32_t Time_GetElapsedSec(ltc_t const *t_start)
-{
-	assert(t_start != 0);
-	ltc_t t_now = Time_Now();
-	return Time_DiffSec(t_start, &t_now);
-}
-
+bool Time_CheckTimeoutSec(ltc_t const *t_start, uint32_t const t_timeout_sec);
 
 /*!***************************************************************************
- * @brief   Adds two #ltc_t values.
- * @details Result is defined as t = t1 + t2.
- *          The results are wrapped around at maximum values just like integers.
- *          The references for t, t1 and t2 may point to the same instance(s).
- *
- * @param   t Return value: t = t1 + t2.
- * @param   t1 1st operand.
- * @param   t2 2nd operand.
+ * @brief	Adds two ltc_t values.
+ * @details	Result is defined as t = t1 + t2. Results are wrapped around at
+ * 			maximum values just like integers.
+ * @param	t Return value: t = t1 + t2.
+ * @param	t1 1st operand.
+ * @param	t2 2nd operand.
  *****************************************************************************/
-inline void Time_Add(ltc_t *t, ltc_t const *t1, ltc_t const *t2)
-{
-	assert(t != 0);
-	assert(t1 != 0);
-	assert(t2 != 0);
-
-	t->sec = t1->sec + t2->sec;
-	t->usec = t1->usec + t2->usec;
-
-	if (t->usec > 999999U) {
-		t->sec += 1U;
-		t->usec -= 1000000U;
-	}
-}
+void Time_Add(ltc_t *t, ltc_t const *t1, ltc_t const *t2);
 
 /*!***************************************************************************
- * @brief   Adds a given time in microseconds to an #ltc_t value.
- * @details Result is defined as t = t1 + t2.
- *          The results are wrapped around at maximum values just like integers.
- *          The references for t and t1 may point to the same instance.
- *
- * @param   t Return value: t = t1 + t2.
- * @param   t1 1st operand.
- * @param   t2_usec 2nd operand in microseconds.
+ * @brief	Adds a given time in microseconds to an ltc_t value.
+ * @param	t Return value: t = t1 + t2.
+ * @param	t1 1st operand.
+ * @param	t2_usec 2nd operand in microseconds.
  *****************************************************************************/
-inline void Time_AddUSec(ltc_t *t, ltc_t const *t1, uint32_t t2_usec)
-{
-	assert(t != 0);
-	assert(t1 != 0);
-	ltc_t t2;
-	Time_FromUSec(&t2, t2_usec);
-	Time_Add(t, t1, &t2);
-}
+void Time_AddUSec(ltc_t *t, ltc_t const *t1, uint32_t t2_usec);
 
 /*!***************************************************************************
- * @brief   Adds a given time in milliseconds to an #ltc_t value.
- * @details Result is defined as t = t1 + t2.
- *          The results are wrapped around at maximum values just like integers.
- *          The references for t and t1 may point to the same instance.
- *
- * @param   t Return value: t = t1 + t2.
- * @param   t1 1st operand.
- * @param   t2_msec 2nd operand in milliseconds.
+ * @brief	Adds a given time in milliseconds to an ltc_t value.
+ * @param	t Return value: t = t1 + t2.
+ * @param	t1 1st operand.
+ * @param	t2_msec 2nd operand in milliseconds.
  *****************************************************************************/
-inline void Time_AddMSec(ltc_t *t, ltc_t const *t1, uint32_t t2_msec)
-{
-	assert(t != 0);
-	assert(t1 != 0);
-	ltc_t t2;
-	Time_FromMSec(&t2, t2_msec);
-	Time_Add(t, t1, &t2);
-}
+void Time_AddMSec(ltc_t *t, ltc_t const *t1, uint32_t t2_msec);
 
 /*!***************************************************************************
- * @brief   Adds a given time in seconds to an #ltc_t value.
- * @details Result is defined as t = t1 + t2.
- *          The results are wrapped around at maximum values just like integers.
- *          The references for t and t1 may point to the same instance.
- *
- * @param   t Return value: t = t1 + t2.
- * @param   t1 1st operand.
- * @param   t2_sec 2nd operand in seconds.
+ * @brief	Adds a given time in seconds to an ltc_t value.
+ * @param	t Return value: t = t1 + t2.
+ * @param	t1 1st operand.
+ * @param	t2_sec 2nd operand in seconds.
  *****************************************************************************/
-inline void Time_AddSec(ltc_t *t, ltc_t const *t1, uint32_t t2_sec)
-{
-	assert(t != 0);
-	assert(t1 != 0);
-	ltc_t t2;
-	Time_FromSec(&t2, t2_sec);
-	Time_Add(t, t1, &t2);
-}
-
+void Time_AddSec(ltc_t *t, ltc_t const *t1, uint32_t t2_sec);
 
 /*!***************************************************************************
- * @brief   Checks if /p t is within the time interval /p t_start and /p t_end.
- * @details The interval is from /p t_start to /p t_end.
- *          The function returns true if /p t >= /p t_start AND /p t < /p t_end.
- *          If /p t_end is before /p t_start, /p t_end is consider to be wrapped
- *          around and the condition inverts (i.e. the function returns true if
- *          /p < /p t_end OR /p t >= t_start.
- * @param   t_start The start of the time interval.
- * @param   t_end The end of the time interval.
- * @param   t The time to be checked if it is with the interval.
- * @return  True if t is within t_start and t_stop.
+ * @brief	Converts ltc_t to microseconds (uint32_t).
+ * @param	t Input ltc_t struct.
+ * @return	Time value in microseconds.
  *****************************************************************************/
-inline bool Time_CheckWithin(ltc_t const *t_start, ltc_t const *t_end, ltc_t const *t)
-{
-	if (Time_GreaterEqual(t_end, t_start)) {
-		return Time_GreaterEqual(t, t_start) && !Time_GreaterEqual(t, t_end);
-
-	} else {
-		return Time_GreaterEqual(t, t_start) || !Time_GreaterEqual(t, t_end);
-	}
-}
-
+uint32_t Time_ToUSec(ltc_t const *t);
 
 /*!***************************************************************************
- * @brief   Checks if timeout is reached from a given starting time.
- * @details Checks if a specified time (/p t_timeout) has elapsed since a
- *          specified start time (/p t_start).
- *          Handles overflow/wraparound of time values at the maximum value.
- * @param   t_start Start time.
- * @param   t_timeout Timeout period.
- * @return  Timeout elapsed? True/False (boolean value)
+ * @brief	Converts ltc_t to milliseconds (uint32_t).
+ * @param	t Input ltc_t struct.
+ * @return	Time value in milliseconds.
  *****************************************************************************/
-inline bool Time_CheckTimeout(ltc_t const *t_start, ltc_t const *t_timeout)
-{
-	assert(t_start != 0);
-	assert(t_timeout != 0);
-
-	ltc_t t_end;
-	ltc_t t_now = Time_Now();
-	Time_Add(&t_end, t_start, t_timeout);
-	return !Time_CheckWithin(t_start, &t_end, &t_now);
-}
+uint32_t Time_ToMSec(ltc_t const *t);
 
 /*!***************************************************************************
- * @brief   Checks if timeout is reached from a given starting time.
- * @details Handles overflow.
- * @param   t_start Start time.
- * @param   t_timeout_usec Timeout period in microseconds.
- * @return  Timeout elapsed? True/False (boolean value)
+ * @brief	Converts ltc_t to seconds (uint32_t).
+ * @param	t Input ltc_t struct.
+ * @return	Time value in seconds.
  *****************************************************************************/
-inline bool Time_CheckTimeoutUSec(ltc_t const *t_start, uint32_t const t_timeout_usec)
-{
-	ltc_t t_timeout;
-	Time_FromUSec(&t_timeout, t_timeout_usec);
-	return Time_CheckTimeout(t_start, &t_timeout);
-}
-
-/*!***************************************************************************
- * @brief   Checks if timeout is reached from a given starting time.
- * @details Handles overflow.
- * @param   t_start Start time.
- * @param   t_timeout_msec Timeout period in milliseconds.
- * @return  Timeout elapsed? True/False (boolean value)
- *****************************************************************************/
-inline bool Time_CheckTimeoutMSec(ltc_t const *t_start, uint32_t const t_timeout_msec)
-{
-	ltc_t t_timeout;
-	Time_FromMSec(&t_timeout, t_timeout_msec);
-	return Time_CheckTimeout(t_start, &t_timeout);
-}
-
-/*!***************************************************************************
- * @brief   Checks if timeout is reached from a given starting time.
- * @details Handles overflow.
- * @param   t_start Start time.
- * @param   t_timeout_sec Timeout period in seconds.
- * @return  Timeout elapsed? True/False (boolean value)
- *****************************************************************************/
-inline bool Time_CheckTimeoutSec(ltc_t const *t_start, uint32_t const t_timeout_sec)
-{
-	ltc_t t_timeout;
-	Time_FromSec(&t_timeout, t_timeout_sec);
-	return Time_CheckTimeout(t_start, &t_timeout);
-}
-
-
-/*!***************************************************************************
- * @brief   Time delay for a given time period.
- * @param   dt Delay time.
- *****************************************************************************/
-inline void Time_Delay(ltc_t const *dt)
-{
-	assert(dt != 0);
-	ltc_t t_start = Time_Now();
-
-	while (!Time_CheckTimeout(&t_start, dt));
-}
-
-/*!***************************************************************************
- * @brief   Time delay for a given time period in microseconds.
- * @param   dt_usec Delay time in microseconds.
- *****************************************************************************/
-inline void Time_DelayUSec(uint32_t dt_usec)
-{
-	ltc_t t_start = Time_Now();
-
-	while (!Time_CheckTimeoutUSec(&t_start, dt_usec));
-}
-
-/*!***************************************************************************
- * @brief   Time delay for a given time period in milliseconds.
- * @param   dt_msec Delay time in milliseconds.
- *****************************************************************************/
-inline void Time_DelayMSec(uint32_t dt_msec)
-{
-	ltc_t t_start = Time_Now();
-
-	while (!Time_CheckTimeoutMSec(&t_start, dt_msec));
-}
-
-/*!***************************************************************************
- * @brief   Time delay for a given time period in seconds.
- * @param   dt_sec Delay time in seconds.
- *****************************************************************************/
-inline void Time_DelaySec(uint32_t dt_sec)
-{
-	ltc_t t_start = Time_Now();
-
-	while (!Time_CheckTimeoutSec(&t_start, dt_sec));
-}
-
+uint32_t Time_ToSec(ltc_t const *t);
 
 /*! @} */
-#ifdef __cplusplus
-} // extern "C"
-#endif
 #endif /* TIME_H */

src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h

@@ -1,10 +1,10 @@
 /*************************************************************************//**
- * @file
- * @brief   Tests for the AFBR-S50 API hardware abstraction layer.
+ * @file	argus_hal_test.c
+ * @brief	Tests for the AFBR-S50 API hardware abstraction layer.
  *
  * @copyright
  *
- * Copyright (c) 2023, Broadcom Inc.
+ * Copyright (c) 2021, Broadcom, Inc.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -41,192 +41,147 @@
 extern "C" {
 #endif
 
+
 /*!***************************************************************************
- * @defgroup    argus_test HAL Self Test
- * @ingroup     argus
+ * @defgroup	argustest HAL Self Test
  *
- * @brief       A test module to verify implementation of the HAL.
+ * @brief		A test module to verify implementation of the HAL.
  *
- * @details     A series of automated tests that can be executed on the target
- *              platform in order to verify the implementation of the HAL that
- *              are required by the API.
+ * @details 	A series of automated tests that can be executed on the target
+ * 				platform in order to verify the	implementation of the HAL that
+ * 				are required by the API.
  *
- *              See #Argus_VerifyHALImplementation for a detailed documentation.
- *
- * @addtogroup  argus_test
+ * @addtogroup 	argustest
  * @{
  *****************************************************************************/
 
 #include "argus.h"
 
 /*!***************************************************************************
- * @brief   Version number of the HAL Self Test.
+ * @brief	Version number of the HAL Self Test.
  *
  * @details Changes:
- *          * v1.0:
- *              - Initial release.
- *          * v1.1:
- *              - Added additional print output.
- *              - Increased tolerance for timer test to 3%.
- *              - Fixed callback issue by disabling it after IRQ test.
- *          * v1.2:
- *              - Added PIT test cases.
- *          * v1.3:
- *              - Added test case for SPI maximum data transfer size.
- *              - Added tests for SPI transfers invoked from all IRQ callbacks.
- *              - Added verification of first PIT event occurrence.
- *              - Relaxed PIT pass conditions (0.1% -> 0.5%)
- *          * v1.4:
- *              - Adopted to new multi-device HAL interface of API v1.4.4 release.
- *              - Added verification of SPI callback invocation.
- *              - Updated GPIO interrupt test to verify if delayed interrupt
- *                pending states can be detected via #S2PI_ReadIrqPin.
- *
+ *  		* v1.0:
+ *  			- Initial release.
+ *  		* v1.1:
+ *  			- Added additional print output.
+ *  			- Increased tolerance for timer test to 3%.
+ *  			- Fixed callback issue by disabling it after IRQ test.
+ *  		* v1.1:
+ *  			- Added PIT test cases.
  *****************************************************************************/
-#define HAL_TEST_VERSION "v1.4"
+#define HAL_TEST_VERSION "v1.2"
 
 /*!***************************************************************************
- * @brief   Executes a series of tests in order to verify the HAL implementation.
+ * @brief	Executes a series of tests in order to verify the HAL implementation.
  *
  * @details A series of automated tests are executed on the target platform in
- *          order to verify the implementation of the HAL that are required by
- *          the API.
+ *			order to verify the	implementation of the HAL that are required by
+ *			the API.
  *
- *          Each test will write an error description via the print (i.e. UART)
- *          function that shows what went wrong. Also an corresponding status is
- *          returned in case no print functionality is available.
+ *			Each test will write an error description via the print (i.e. UART)
+ *			function that shows what went wrong. Also an corresponding status is
+ *			returned in case no print functionality is available.
  *
- *          The following tests are executed:
+ *			The following tests are executed:
  *
- *          **1) Timer Plausibility Test:**
+ *			**1) Timer Plausibility Test:**
  *
- *          Rudimentary tests of the lifetime counter (LTC) implementation.
- *          This verifies that the LTC is running by checking if the returned
- *          values of two consecutive calls to the #Timer_GetCounterValue
- *          function are ascending. An artificial delay using the NOP operation
- *          is induced such that the timer is not read to fast.
+ *			Rudimentary tests of the lifetime counter (LTC) implementation.
+ *			This verifies that the LTC is running by checking if the returned
+ *			values of two consecutive calls to the #Timer_GetCounterValue
+ *			function are ascending. An artificial delay using the NOP operation
+ *			is induced such that the timer is not read to fast.
  *
- *          **2) Timer Wraparound Test:**
+ *			**2) Timer Wraparound Test:**
  *
- *          The LTC values must wrap from 999999 µs to 0 µs and increase the
- *          seconds counter accordingly. This test verifies the correct wrapping
- *          by consecutively calling the #Timer_GetCounterValue function until
- *          at least 2 wraparound events have been occurred.
+ *			The LTC values must wrap from 999999 µs to 0 µs and increase the
+ *			seconds counter accordingly. This test verifies the correct wrapping
+ *			by consecutively calling the #Timer_GetCounterValue function until
+ *			at least 2 wraparound events have been occurred.
  *
- *          **3) SPI Connection Test:**
+ *			**3) SPI Connection Test:**
  *
- *          This test verifies the basic functionality of the SPI interface.
- *          The test utilizes the devices laser pattern register, which can
- *          be freely programmed by any 128-bit pattern. Thus, it writes a byte
- *          sequence and reads back the written values on the consecutive SPI
- *          access.
+ *			This test verifies the basic functionality of the SPI interface.
+ *			The test utilizes the devices laser pattern register, which can
+ *			be freely programmed by any 128-bit pattern. Thus, it writes a byte
+ *			sequence and reads back the written values on the consecutive SPI
+ *			access.
  *
- *          **4) SPI Maximum Data Length Test**:
+ *			**4) SPI Interrupt Test:**
  *
- *          This test verifies the maximum data transfer length of the SPI
- *          interface. The test sends and receives up to 396 data bytes plus
- *          a single address byte over the SPI interface and verifies that no
- *          data get lost.
+ *			This test verifies the correct implementation of the device
+ *			integration finished interrupt callback. Therefore it configures
+ *			the device with a minimal setup to run a pseudo measurement that
+ *			does not emit any laser light.
  *
- *          **5) SPI Interrupt Test:**
+ *			Note that this test does verify the GPIO interrupt that occurs
+ *			whenever the device has finished the integration/measurement and
+ *			new data is waiting to be read from the device. This does not test
+ *			the interrupt that is triggered when the SPI transfer has finished.
  *
- *          This test verifies the correct implementation of the device
- *          integration finished interrupt callback. Therefore it configures
- *          the device with a minimal setup to run a pseudo measurement that
- *          does not emit any laser light.
+ *			The data ready interrupt implies two S2PI layer functions that
+ *			are tested in this test: The #S2PI_SetIrqCallback function installs
+ *			a callback function that is invoked whenever the IRQ occurs.
+ *			The IRQ can be delayed due to higher priority task, e.g. from the
+ *			user code. It is essential for the laser safety timeout algorithm
+ *			to determine the device ready signal as fast as possible, another
+ *			method is implemented to read if the IRQ is pending but the
+ *			callback has not been reset yet. This is what the #S2PI_ReadIrqPin
+ *			function is for.
  *
- *          Note that this test does verify the GPIO interrupt that occurs
- *          whenever the device has finished the integration/measurement and
- *          new data is waiting to be read from the device. This does not test
- *          the interrupt that is triggered when the SPI transfer has finished.
+ *			**5) GPIO Mode Test:**
  *
- *          The data ready interrupt implies two S2PI layer functions that
- *          are tested in this test: The #S2PI_SetIrqCallback function installs
- *          a callback function that is invoked whenever the IRQ occurs.
- *          The IRQ can be delayed due to higher priority task, e.g. from the
- *          user code. It is essential for the laser safety timeout algorithm
- *          to determine the device ready signal as fast as possible, another
- *          method is implemented to read if the IRQ is pending but the
- *          callback has not been reset yet. This is what the #S2PI_ReadIrqPin
- *          function is for.
+ *			This test verifies the GPIO mode of the S2PI HAL module. This is
+ *			done by leveraging the EEPROM readout sequence that accesses the
+ *			devices EEPROM via a software protocol that depends on the GPIO
+ *			mode.
  *
- *          **6) GPIO Mode Test:**
+ *			This the requires several steps, most of them are already verified
+ *			in previous tests:
  *
- *          This test verifies the GPIO mode of the S2PI HAL module. This is
- *          done by leveraging the EEPROM readout sequence that accesses the
- *          devices EEPROM via a software protocol that depends on the GPIO
- *          mode.
+ *			- Basic device configuration and enable EEPROM.
+ *			- Read EERPOM via GPIO mode and apply Hamming weight.
+ *			- Repeat several times (to eliminate random readout issues).
+ *			- Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c).
+ *			- Check if Module Number and Chip ID is not 0.
  *
- *          This the requires several steps, most of them are already verified
- *          in previous tests:
+ *			**6) Timer Test for Lifetime Counter:**
  *
- *          - Basic device configuration and enable EEPROM.
- *          - Read EERPOM via GPIO mode and apply Hamming weight.
- *          - Repeat several times (to eliminate random readout issues).
- *          - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c).
- *          - Check if Module Number and Chip ID is not 0.
+ *			The test verifies the lifetime counter timer HAL implementation by
+ *			comparing the timings to the AFBR-S50 device as a reference.
+ *			Therefore several measurement are executed on the device, each with
+ *			a different averaging sample count. The elapsed time increases
+ *			linearly with the number of averaging samples. In order to remove
+ *			the time for software/setup, a linear regression fit is applied to
+ *			the measurement results and only the slope is considered for the
+ *			result. A delta of 102.4 microseconds per sample is expected.
+ *			If the measured delta per sample is within an specified error range,
+ *			the timer implementation is considered correct.
  *
- *          **7) Timer Test for Lifetime Counter:**
+ *			**7) Timer Test for Periodic Interrupt Timer:**
  *
- *          The test verifies the lifetime counter timer HAL implementation by
- *          comparing the timings to the AFBR-S50 device as a reference.
- *          Therefore several measurement are executed on the device, each with
- *          a different averaging sample count. The elapsed time increases
- *          linearly with the number of averaging samples. In order to remove
- *          the time for software/setup, a linear regression fit is applied to
- *          the measurement results and only the slope is considered for the
- *          result. A delta of 102.4 microseconds per sample is expected.
- *          If the measured delta per sample is within an specified error range,
- *          the timer implementation is considered correct.
+ *			The test verifies the correct implementation of the periodic
+ *			interrupt timer (PIT). It sets different intervals and waits for
+ *			a certain number of interrupts to happen. Each interrupt event
+ *			is counted and the time between the first and the last interrupt
+ *			is measured. Finally, the measured interval is compared to the
+ *			expectations.
  *
- *          **8) Timer Test for Periodic Interrupt Timer (optional):**
  *
- *          The test verifies the correct implementation of the periodic
- *          interrupt timer (PIT). It sets different intervals and waits for
- *          a certain number of interrupts to happen. Each interrupt event
- *          is counted and the time between the first and the last interrupt
- *          is measured. Finally, the measured interval is compared to the
- *          expectations.
+ * @param	spi_slave The SPI hardware slave, i.e. the specified CS and IRQ
+ *						lines. This is actually just a number that is passed
+ *						to the SPI interface to distinct for multiple SPI slave
+ *						devices. Note that the slave must be not equal to 0,
+ *						since is reserved for error handling.
  *
- *          Note that this test is only executed if the PIT is actually
- *          implemented. Otherwise, the test is skipped.
- *
- *          **9) SPI Transfer from Interrupt Callback Test:**
- *
- *          The test verifies that the #S2PI_TransferFrame method of the
- *          S2PI layer can be invoked from a interrupt callback function too.
- *          Thus, it repeats the S2PI Connection Test but this time from
- *          different interrupt callback functions:
- *
- *          - SPI Callback: The first transfer is invoked from thread level,
- *            the second transfer is invoke from the SPI interrupt callback
- *            function.
- *
- *          - GPIO Callback: The device is setup to trigger an GPIO interrupt
- *            (see also the SPI Interrupt Test). The corresponding GPIO
- *            interrupt callback function will trigger the first transfer while
- *            the second one is triggered from the SPI callback function.
- *
- *          - PIT Callback (optional): This test is only executed optional if
- *            the PIT interface is implemented. The test sequence is the same
- *            as for the GPIO callback, but the first transfer is triggered
- *            from the PIT callback function.
- *
- * @note    See #HAL_TEST_VERSION for a version history and change log of
- *          the HAL self tests.
- *
- * @param   spi_slave The SPI hardware slave, i.e. the specified CS and IRQ
- *                    lines. This is actually just a number that is passed
- *                    to the SPI interface to distinct for multiple SPI slave
- *                    devices. Note that the slave must be not equal to 0,
- *                    since is reserved for error handling.
- *
- * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave);
 
-/*! @} */
 #ifdef __cplusplus
-} // extern "C"
+}
 #endif
-#endif /* ARGUS_HAL_TEST_H */
+
+/*! @} */
+#endif /* ARGUS_CAL_API_H */

src/drivers/distance_sensor/broadcom/afbrs50/parameters.c

@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * AFBR Rangefinder Mode
- *
- * This parameter defines the mode of the AFBR Rangefinder.
- *
- * @reboot_required true
- * @min 0
- * @max 3
- * @group Sensors
- *
- * @value 0 Short Range Mode
- * @value 1 Long Range Mode
- * @value 2 High Speed Short Range Mode
- * @value 3 High Speed Long Range Mode
- */
-PARAM_DEFINE_INT32(SENS_AFBR_MODE, 1);
-
-/**
- * AFBR Rangefinder Short Range Rate
- *
- * This parameter defines measurement rate of the AFBR Rangefinder in short range mode.
- *
- * @min 1
- * @max 100
- * @group Sensors
- *
- */
-PARAM_DEFINE_INT32(SENS_AFBR_S_RATE, 50);
-
-/**
- * AFBR Rangefinder Long Range Rate
- *
- * This parameter defines measurement rate of the AFBR Rangefinder in long range mode.
- *
- * @min 1
- * @max 100
- * @group Sensors
- *
- */
-PARAM_DEFINE_INT32(SENS_AFBR_L_RATE, 25);
-
-/**
- * AFBR Rangefinder Short/Long Range Threshold
- *
- * This parameter defines the threshold for switching between short and long range mode.
- * The mode will switch from short to long range when the distance is greater than the threshold plus the hysteresis.
- * The mode will switch from long to short range when the distance is less than the threshold minus the hysteresis.
- *
- * @unit m
- * @min 1
- * @max 50
- * @group Sensors
- *
- */
-PARAM_DEFINE_INT32(SENS_AFBR_THRESH, 5);
-
-
-/**
- * AFBR Rangefinder Short/Long Range Threshold Hysteresis
- *
- * This parameter defines the hysteresis for switching between short and long range mode.
- *
- * @unit m
- * @min 1
- * @max 10
- * @group Sensors
- *
- */
-PARAM_DEFINE_INT32(SENS_AFBR_HYSTER, 1);

src/drivers/ins/vectornav/VectorNav.cpp

@@ -38,15 +38,9 @@
 
 #include <fcntl.h>
 
-using matrix::Vector2f;
-
 VectorNav::VectorNav(const char *port) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_attitude_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_attitude) : ORB_ID(vehicle_attitude)),
-	_local_position_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_local_position) : ORB_ID(vehicle_local_position)),
-	_global_position_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_global_position) : ORB_ID(
-				     vehicle_global_position))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port))
 {
 	// store port name
 	strncpy(_port, port, sizeof(_port) - 1);
@@ -54,27 +48,6 @@ VectorNav::VectorNav(const char *port) :
 	// enforce null termination
 	_port[sizeof(_port) - 1] = '\0';
 
-	// VN_MODE 1 full INS
-	if (_param_vn_mode.get() == 1) {
-		int32_t v = 0;
-
-		// EKF2_EN 0 (disabled)
-		v = 0;
-		param_set(param_find("EKF2_EN"), &v);
-
-		// SYS_MC_EST_GROUP 0 (disabled)
-		v = 0;
-		param_set(param_find("SYS_MC_EST_GROUP"), &v);
-
-		// SENS_IMU_MODE (VN handles sensor selection)
-		v = 0;
-		param_set(param_find("SENS_IMU_MODE"), &v);
-
-		// SENS_MAG_MODE (VN handles sensor selection)
-		v = 0;
-		param_set(param_find("SENS_MAG_MODE"), &v);
-	}
-
 	device::Device::DeviceId device_id{};
 	device_id.devid_s.devtype = DRV_INS_DEVTYPE_VN300;
 	device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
@@ -102,16 +75,6 @@ VectorNav::~VectorNav()
 
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
-
-	perf_free(_accel_pub_interval_perf);
-	perf_free(_gyro_pub_interval_perf);
-	perf_free(_mag_pub_interval_perf);
-	perf_free(_gnss_pub_interval_perf);
-	perf_free(_baro_pub_interval_perf);
-
-	perf_free(_attitude_pub_interval_perf);
-	perf_free(_local_position_pub_interval_perf);
-	perf_free(_global_position_pub_interval_perf);
 }
 
 void VectorNav::binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex)
@@ -145,7 +108,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 	if (VnUartPacket_isCompatible(packet,
 				      COMMONGROUP_NONE,
 				      TIMEGROUP_TIMESTARTUP,
-				      (ImuGroup)(IMUGROUP_ACCEL | IMUGROUP_ANGULARRATE),
+				      (ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO),
 				      GPSGROUP_NONE,
 				      ATTITUDEGROUP_NONE,
 				      INSGROUP_NONE,
@@ -155,21 +118,17 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 		uint64_t time_startup = VnUartPacket_extractUint64(packet);
 		(void)time_startup;
 
-		// IMUGROUP_ACCEL
+		// IMUGROUP_UNCOMPACCEL
 		vec3f accel = VnUartPacket_extractVec3f(packet);
 
-		// IMUGROUP_ANGULARRATE
+		// IMUGROUP_UNCOMPGYRO
 		vec3f angular_rate = VnUartPacket_extractVec3f(packet);
 
 		// publish sensor_accel
 		_px4_accel.update(time_now_us, accel.c[0], accel.c[1], accel.c[2]);
-		perf_count(_accel_pub_interval_perf);
 
 		// publish sensor_gyro
 		_px4_gyro.update(time_now_us, angular_rate.c[0], angular_rate.c[1], angular_rate.c[2]);
-		perf_count(_gyro_pub_interval_perf);
-
-		_time_last_valid_imu_us.store(hrt_absolute_time());
 	}
 
 	// binary output 2
@@ -229,11 +188,9 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 		sensor_baro.temperature = temperature;
 		sensor_baro.timestamp = hrt_absolute_time();
 		_sensor_baro_pub.publish(sensor_baro);
-		perf_count(_baro_pub_interval_perf);
 
 		// publish sensor_mag
 		_px4_mag.update(time_now_us, mag.c[0], mag.c[1], mag.c[2]);
-		perf_count(_mag_pub_interval_perf);
 
 		// publish attitude
 		vehicle_attitude_s attitude{};
@@ -244,14 +201,13 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 		attitude.q[3] = quaternion.c[2];
 		attitude.timestamp = hrt_absolute_time();
 		_attitude_pub.publish(attitude);
-		perf_count(_attitude_pub_interval_perf);
 
 		// mode
 		const uint16_t mode = (insStatus & 0b11);
 		//const bool mode_initializing = (mode == 0b00);
 		const bool mode_aligning     = (mode == 0b01);
 		const bool mode_tracking     = (mode == 0b10);
-		//const bool mode_loss_gnss    = (mode == 0b11);
+		const bool mode_loss_gnss    = (mode == 0b11);
 
 
 		// mode_initializing
@@ -272,88 +228,42 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 		//  - attitude is good
 		//  - treat as mode 0
 
-		if (mode_aligning || mode_tracking) {
+
+		if ((mode_aligning || mode_tracking) && !mode_loss_gnss) {
 			// publish local_position
 
-			const double lat = positionEstimatedLla.c[0];
-			const double lon = positionEstimatedLla.c[1];
-			const float alt = positionEstimatedLla.c[2];
-
-			if (!_pos_ref.isInitialized()) {
-				_pos_ref.initReference(lat, lon, time_now_us);
-				_gps_alt_ref = alt;
-			}
-
-			const Vector2f pos_ned = _pos_ref.project(lat, lon);
-
+			// TODO: projection
 			vehicle_local_position_s local_position{};
 			local_position.timestamp_sample = time_now_us;
-
-			local_position.xy_valid = true;
-			local_position.z_valid = true;
-			local_position.v_xy_valid = true;
-			local_position.v_z_valid = true;
-
-			local_position.x = pos_ned(0);
-			local_position.y = pos_ned(1);
-			local_position.z = -(alt - _gps_alt_ref);
-
-			local_position.vx = velocityEstimatedNed.c[0];
-			local_position.vy = velocityEstimatedNed.c[1];
-			local_position.vz = velocityEstimatedNed.c[2];
-			local_position.z_deriv = velocityEstimatedNed.c[2]; // TODO
-
 			local_position.ax = accelerationLinearNed.c[0];
 			local_position.ay = accelerationLinearNed.c[1];
 			local_position.az = accelerationLinearNed.c[2];
-
-			matrix::Quatf q{attitude.q};
-			local_position.heading = matrix::Eulerf{q}.psi();
-			local_position.heading_good_for_control = mode_tracking;
-
-			if (_pos_ref.isInitialized()) {
-				local_position.xy_global = true;
-				local_position.z_global = true;
-				local_position.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
-				local_position.ref_lat = _pos_ref.getProjectionReferenceLat();
-				local_position.ref_lon = _pos_ref.getProjectionReferenceLon();
-				local_position.ref_alt = _gps_alt_ref;
-			}
-
-			local_position.dist_bottom_valid = false;
-
+			local_position.x = positionEstimatedLla.c[0]; // TODO
+			local_position.y = positionEstimatedLla.c[1]; // TODO
+			local_position.z = positionEstimatedLla.c[2]; // TODO
+			local_position.vx = velocityEstimatedNed.c[0];
+			local_position.vy = velocityEstimatedNed.c[1];
+			local_position.vz = velocityEstimatedNed.c[2];
 			local_position.eph = positionUncertaintyEstimated;
 			local_position.epv = positionUncertaintyEstimated;
 			local_position.evh = velocityUncertaintyEstimated;
 			local_position.evv = velocityUncertaintyEstimated;
-
-			local_position.dead_reckoning = false;
-
-			local_position.vxy_max = INFINITY;
-			local_position.vz_max = INFINITY;
-			local_position.hagl_min = INFINITY;
-			local_position.hagl_max = INFINITY;
-
+			local_position.xy_valid = true;
+			local_position.heading_good_for_control = mode_tracking;
 			local_position.unaided_heading = NAN;
 			local_position.timestamp = hrt_absolute_time();
 			_local_position_pub.publish(local_position);
-			perf_count(_local_position_pub_interval_perf);
+
 
 
 			// publish global_position
 			vehicle_global_position_s global_position{};
 			global_position.timestamp_sample = time_now_us;
-			global_position.lat = lat;
-			global_position.lon = lon;
-			global_position.alt = alt;
-			global_position.alt = alt;
-
-			global_position.eph = positionUncertaintyEstimated;
-			global_position.epv = positionUncertaintyEstimated;
-
+			global_position.lat = positionEstimatedLla.c[0];
+			global_position.lon = positionEstimatedLla.c[1];
+			global_position.alt = positionEstimatedLla.c[2];
 			global_position.timestamp = hrt_absolute_time();
 			_global_position_pub.publish(global_position);
-			perf_count(_global_position_pub_interval_perf);
 		}
 	}
 
@@ -424,14 +334,13 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			sensor_gps.hdop = dop.hDOP;
 			sensor_gps.vdop = dop.vDOP;
 
-			sensor_gps.eph = sqrtf(sq(positionUncertaintyGpsNed.c[0]) + sq(positionUncertaintyGpsNed.c[1]));
+			sensor_gps.eph = positionUncertaintyGpsNed.c[0];
 			sensor_gps.epv = positionUncertaintyGpsNed.c[2];
 
 			sensor_gps.s_variance_m_s = velocityUncertaintyGps;
 
 			sensor_gps.timestamp = hrt_absolute_time();
 			_sensor_gps_pub.publish(sensor_gps);
-			perf_count(_gnss_pub_interval_perf);
 		}
 	}
 }
@@ -585,7 +494,7 @@ bool VectorNav::configure()
 		1, // divider
 		COMMONGROUP_NONE,
 		TIMEGROUP_TIMESTARTUP,
-		(ImuGroup)(IMUGROUP_ACCEL | IMUGROUP_ANGULARRATE),
+		(ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO),
 		GPSGROUP_NONE,
 		ATTITUDEGROUP_NONE,
 		INSGROUP_NONE,
@@ -643,8 +552,6 @@ bool VectorNav::configure()
 	VnSensor_registerAsyncPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this);
 	VnSensor_registerErrorPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this);
 
-	_time_configured_us.store(hrt_absolute_time());
-
 	return true;
 }
 
@@ -676,42 +583,11 @@ void VectorNav::Run()
 			_initialized = true;
 
 		} else {
-			ScheduleDelayed(1_s);
+			ScheduleDelayed(3_s);
 			return;
 		}
 	}
 
-	if (_connected && _configured && _initialized) {
-
-		// check for timeout
-		const hrt_abstime time_configured_us = _time_configured_us.load();
-		const hrt_abstime time_last_valid_imu_us = _time_last_valid_imu_us.load();
-
-		if (_param_vn_mode.get() == 1) {
-			if ((time_last_valid_imu_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) < 3_s))
-
-				// update sensor_selection if configured in INS mode
-				if ((_px4_accel.get_device_id() != 0) && (_px4_gyro.get_device_id() != 0)) {
-					sensor_selection_s sensor_selection{};
-					sensor_selection.accel_device_id = _px4_accel.get_device_id();
-					sensor_selection.gyro_device_id = _px4_gyro.get_device_id();
-					sensor_selection.timestamp = hrt_absolute_time();
-					_sensor_selection_pub.publish(sensor_selection);
-				}
-		}
-
-		if ((time_configured_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) > 5_s)
-		    && (time_last_valid_imu_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) > 1_s)
-		   ) {
-			PX4_ERR("timeout, reinitializing");
-			VnSensor_unregisterAsyncPacketReceivedHandler(&_vs);
-			VnSensor_disconnect(&_vs);
-			_connected = false;
-			_configured = false;
-			_initialized = false;
-		}
-	}
-
 	ScheduleDelayed(100_ms);
 }
 

src/drivers/ins/vectornav/VectorNav.hpp

@@ -56,7 +56,6 @@ extern "C" {
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-#include <lib/geo/geo.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/module.h>
@@ -66,7 +65,6 @@ extern "C" {
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
-#include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -101,9 +99,6 @@ private:
 
 	static void binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex);
 
-	// return the square of two floating point numbers
-	static constexpr float sq(float var) { return var * var; }
-
 	void sensorCallback(VnUartPacket *packet);
 
 	char _port[20] {};
@@ -113,8 +108,7 @@ private:
 	bool _connected{false};
 	bool _configured{false};
 
-	px4::atomic<hrt_abstime> _time_configured_us{false};
-	px4::atomic<hrt_abstime> _time_last_valid_imu_us{false};
+	hrt_abstime _last_read{0};
 
 	VnSensor _vs{};
 
@@ -128,31 +122,16 @@ private:
 	PX4Gyroscope     _px4_gyro{0};
 	PX4Magnetometer  _px4_mag{0};
 
-	MapProjection _pos_ref{};
-	float _gps_alt_ref{NAN};		///< WGS-84 height (m)
-
 	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
 
-	uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)};
-
-	uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
-	uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
-	uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub;
+	uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub{ORB_ID(external_ins_attitude)};
+	uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub{ORB_ID(external_ins_local_position)};
+	uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub{ORB_ID(external_ins_global_position)};
 
 	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
 	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
 
-	perf_counter_t _accel_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel publish interval")};
-	perf_counter_t _gyro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro publish interval")};
-	perf_counter_t _mag_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": mag publish interval")};
-	perf_counter_t _gnss_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": GNSS publish interval")};
-	perf_counter_t _baro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": baro publish interval")};
-
-	perf_counter_t _attitude_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": attitude publish interval")};
-	perf_counter_t _local_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": local position publish interval")};
-	perf_counter_t _global_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": global position publish interval")};
-
 
 	// TODO: params for GNSS antenna offsets
 	// A
@@ -175,8 +154,4 @@ private:
 	// VN_GNSS_ANTB_POS_X
 
 	// Uncertainty in the X-axis position measurement.
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::VN_MODE>) _param_vn_mode
-	)
 };

src/drivers/ins/vectornav/module.yaml

@@ -1,22 +1,7 @@
 module_name: VectorNav (VN-100, VN-200, VN-300)
-
 serial_config:
     - command: vectornav start -d ${SERIAL_DEV}
       port_config_param:
         name: SENS_VN_CFG
         group: Sensors
 
-parameters:
-    - group: Sensors
-      definitions:
-
-        VN_MODE:
-            description:
-                short: VectorNav driver mode
-                long: INS or sensors
-            category: System
-            type: enum
-            values:
-                0: Sensors Only (default)
-                1: INS
-            default: 0

src/drivers/optical_flow/paa3905/PAA3905.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -194,13 +194,13 @@ void PAA3905::RunImpl()
 			_state = STATE::READ;
 
 			if (DataReadyInterruptConfigure()) {
-				_motion_interrupt_enabled = true;
+				_data_ready_interrupt_enabled = true;
 
 				// backup schedule as a watchdog timeout
 				ScheduleDelayed(1_s);
 
 			} else {
-				_motion_interrupt_enabled = false;
+				_data_ready_interrupt_enabled = false;
 				ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
 			}
 
@@ -222,7 +222,7 @@ void PAA3905::RunImpl()
 	case STATE::READ: {
 			hrt_abstime timestamp_sample = now;
 
-			if (_motion_interrupt_enabled) {
+			if (_data_ready_interrupt_enabled) {
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
@@ -258,6 +258,12 @@ void PAA3905::RunImpl()
 					PX4_ERR("invalid RawData_Sum > 0x98");
 				}
 
+				// Number of Features = SQUAL * 4
+				// RawData_Sum maximum register value is 0x98
+				bool data_valid = (buffer.data.SQUAL > 0)
+						  && (buffer.data.RawData_Sum <= 0x98)
+						  && (_discard_reading == 0);
+
 				// Bit [5:0] check if chip is working correctly
 				//  0x3F: chip is working correctly
 				if ((buffer.data.Observation & 0x3F) != 0x3F) {
@@ -307,7 +313,7 @@ void PAA3905::RunImpl()
 
 					if (prev_mode != _mode) {
 						// update scheduling on mode change
-						if (!_motion_interrupt_enabled) {
+						if (!_data_ready_interrupt_enabled) {
 							ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
 						}
 					}
@@ -342,14 +348,6 @@ void PAA3905::RunImpl()
 
 				const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower;
 
-				// Number of Features = SQUAL * 4
-				// RawData_Sum maximum register value is 0x98
-				bool data_valid = (buffer.data.SQUAL > 0)
-						  && (buffer.data.RawData_Sum > 0)
-						  && (buffer.data.RawData_Sum <= 0x98)
-						  && (shutter > 0)
-						  && (_discard_reading == 0);
-
 				switch (_mode) {
 				case Mode::Bright:
 					sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
@@ -394,17 +392,12 @@ void PAA3905::RunImpl()
 				// motion in burst transfer
 				const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred);
 
-				const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
-				const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
-
 				if (data_valid) {
-
-					const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0);
-					const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
-
-					bool publish = false;
-
 					if (motion_reported) {
+						// only populate flow if data valid (motion and quality > 0)
+						const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
+						const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
+
 						// rotate measurements in yaw from sensor frame to body frame
 						const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
 
@@ -419,53 +412,15 @@ void PAA3905::RunImpl()
 						sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
 
 						sensor_optical_flow.quality = buffer.data.SQUAL;
-
-						publish = true;
-
-						_last_motion = timestamp_sample;
-
-					} else if (zero_flow && (timestamp_sample > _last_motion)) {
-						// no motion, but burst read looks valid and we should have seen new data by now if there was any motion
-						const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
-										|| (shutter != _shutter_prev)
-										|| (buffer.data.RawData_Sum != _raw_data_sum_prev)
-										|| (buffer.data.SQUAL != _quality_prev);
-
-						if (burst_read_changed) {
-
-							sensor_optical_flow.pixel_flow[0] = 0;
-							sensor_optical_flow.pixel_flow[1] = 0;
-
-							sensor_optical_flow.quality = buffer.data.SQUAL;
-
-							publish = true;
-						}
 					}
 
-					// only publish when there's valid data or on timeout
-					if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
+					// only publish when there's motion or at least every second
+					if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
 
 						sensor_optical_flow.timestamp = hrt_absolute_time();
 						_sensor_optical_flow_pub.publish(sensor_optical_flow);
 
-						_last_publish = sensor_optical_flow.timestamp_sample;
-					}
-
-					// backup schedule if we're reliant on the motion interrupt and there's very little flow
-					if (_motion_interrupt_enabled && little_to_no_flow) {
-						switch (_mode) {
-						case Mode::Bright:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
-							break;
-
-						case Mode::LowLight:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
-							break;
-
-						case Mode::SuperLowLight:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
-							break;
-						}
+						_last_publish = sensor_optical_flow.timestamp;
 					}
 
 					success = true;
@@ -475,12 +430,6 @@ void PAA3905::RunImpl()
 					}
 				}
 
-				_delta_x_raw_prev = delta_x_raw;
-				_delta_y_raw_prev = delta_y_raw;
-				_shutter_prev = shutter;
-				_raw_data_sum_prev = buffer.data.RawData_Sum;
-				_quality_prev = buffer.data.SQUAL;
-
 			} else {
 				perf_count(_bad_transfer_perf);
 			}

src/drivers/optical_flow/paa3905/PAA3905.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -114,22 +114,14 @@ private:
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_publish{0};
-	hrt_abstime _last_motion{0};
-
-	int16_t _delta_x_raw_prev{0};
-	int16_t _delta_y_raw_prev{0};
-	uint32_t _shutter_prev{0};
-	uint8_t _quality_prev{0};
-	uint8_t _raw_data_sum_prev{0};
-
 	int _failure_count{0};
 	int _discard_reading{0};
 
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-	bool _motion_interrupt_enabled{false};
+	bool _data_ready_interrupt_enabled{false};
 
 	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
-	static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
+	static constexpr uint32_t kBackupScheduleIntervalUs{200_ms};
 
 	Mode _mode{Mode::LowLight};
 

src/drivers/optical_flow/paw3902/PAW3902.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -199,13 +199,13 @@ void PAW3902::RunImpl()
 			_state = STATE::READ;
 
 			if (DataReadyInterruptConfigure()) {
-				_motion_interrupt_enabled = true;
+				_data_ready_interrupt_enabled = true;
 
 				// backup schedule as a watchdog timeout
 				ScheduleDelayed(1_s);
 
 			} else {
-				_motion_interrupt_enabled = false;
+				_data_ready_interrupt_enabled = false;
 				ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
 			}
 
@@ -227,7 +227,7 @@ void PAW3902::RunImpl()
 	case STATE::READ: {
 			hrt_abstime timestamp_sample = now;
 
-			if (_motion_interrupt_enabled) {
+			if (_data_ready_interrupt_enabled) {
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
@@ -263,6 +263,12 @@ void PAW3902::RunImpl()
 					PX4_ERR("invalid RawData_Sum > 0x98");
 				}
 
+				// Number of Features = SQUAL * 4
+				// RawData_Sum maximum register value is 0x98
+				bool data_valid = (buffer.data.SQUAL > 0)
+						  && (buffer.data.RawData_Sum <= 0x98)
+						  && (_discard_reading == 0);
+
 				// publish sensor_optical_flow
 				sensor_optical_flow_s sensor_optical_flow{};
 				sensor_optical_flow.timestamp_sample = timestamp_sample;
@@ -287,14 +293,6 @@ void PAW3902::RunImpl()
 
 				const uint16_t shutter = (shutter_upper << 8) | shutter_lower;
 
-				// Number of Features = SQUAL * 4
-				// RawData_Sum maximum register value is 0x98
-				bool data_valid = (buffer.data.SQUAL > 0)
-						  && (buffer.data.RawData_Sum > 0)
-						  && (buffer.data.RawData_Sum <= 0x98)
-						  && (shutter > 0)
-						  && (_discard_reading == 0);
-
 				switch (_mode) {
 				case Mode::Bright:
 					sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
@@ -397,17 +395,12 @@ void PAW3902::RunImpl()
 				// motion in burst transfer
 				const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT);
 
-				const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
-				const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
-
 				if (data_valid) {
-
-					const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0);
-					const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
-
-					bool publish = false;
-
 					if (motion_reported) {
+						// only populate flow if data valid (motion and quality > 0)
+						const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
+						const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
+
 						// rotate measurements in yaw from sensor frame to body frame
 						const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
 
@@ -422,53 +415,15 @@ void PAW3902::RunImpl()
 						sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
 
 						sensor_optical_flow.quality = buffer.data.SQUAL;
-
-						publish = true;
-
-						_last_motion = timestamp_sample;
-
-					} else if (zero_flow && (timestamp_sample > _last_motion)) {
-						// no motion, but burst read looks valid and we should have seen new data by now if there was any motion
-						const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
-										|| (shutter != _shutter_prev)
-										|| (buffer.data.RawData_Sum != _raw_data_sum_prev)
-										|| (buffer.data.SQUAL != _quality_prev);
-
-						if (burst_read_changed) {
-
-							sensor_optical_flow.pixel_flow[0] = 0;
-							sensor_optical_flow.pixel_flow[1] = 0;
-
-							sensor_optical_flow.quality = buffer.data.SQUAL;
-
-							publish = true;
-						}
 					}
 
-					// only publish when there's valid data or on timeout
-					if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
+					// only publish when there's motion or at least every second
+					if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
 
 						sensor_optical_flow.timestamp = hrt_absolute_time();
 						_sensor_optical_flow_pub.publish(sensor_optical_flow);
 
-						_last_publish = sensor_optical_flow.timestamp_sample;
-					}
-
-					// backup schedule if we're reliant on the motion interrupt and there's very little flow
-					if (_motion_interrupt_enabled && little_to_no_flow) {
-						switch (_mode) {
-						case Mode::Bright:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
-							break;
-
-						case Mode::LowLight:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
-							break;
-
-						case Mode::SuperLowLight:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
-							break;
-						}
+						_last_publish = sensor_optical_flow.timestamp;
 					}
 
 					success = true;
@@ -478,12 +433,6 @@ void PAW3902::RunImpl()
 					}
 				}
 
-				_delta_x_raw_prev = delta_x_raw;
-				_delta_y_raw_prev = delta_y_raw;
-				_shutter_prev = shutter;
-				_raw_data_sum_prev = buffer.data.RawData_Sum;
-				_quality_prev = buffer.data.SQUAL;
-
 			} else {
 				perf_count(_bad_transfer_perf);
 			}

src/drivers/optical_flow/paw3902/PAW3902.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -114,22 +114,14 @@ private:
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_publish{0};
-	hrt_abstime _last_motion{0};
-
-	int16_t _delta_x_raw_prev{0};
-	int16_t _delta_y_raw_prev{0};
-	uint16_t _shutter_prev{0};
-	uint8_t _quality_prev{0};
-	uint8_t _raw_data_sum_prev{0};
-
 	int _failure_count{0};
 	int _discard_reading{0};
 
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-	bool _motion_interrupt_enabled{false};
+	bool _data_ready_interrupt_enabled{false};
 
 	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
-	static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
+	static constexpr uint32_t kBackupScheduleIntervalUs{200_ms};
 
 	Mode _mode{Mode::LowLight};
 

src/drivers/power_monitor/ina228/ina228.h

@@ -214,7 +214,7 @@ using namespace time_literals;
 #define INA228_ENERGY_SHIFTS                  (0)
 #define INA228_ENERGY_MASK                    (UINT64_C(0xffffffffff) << INA228_ENERGY_SHIFTS)
 
-/* INA228 Charge Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */
+/* INA228 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */
 
 #define INA228_CHARGE_SHIFTS                 (0)
 #define INA228_CHARGE_MASK                   (UINT64_C(0xffffffffff) << INA228_CHARGE_SHIFTS)

src/drivers/rc_input/RCInput.cpp

@@ -124,7 +124,6 @@ RCInput::task_spawn(int argc, char *argv[])
 	int ch;
 	const char *myoptarg = nullptr;
 	const char *device_name = nullptr;
-	bool silent = false;
 #if defined(RC_SERIAL_PORT)
 	device_name = RC_SERIAL_PORT;
 #endif // RC_SERIAL_PORT
@@ -134,7 +133,6 @@ RCInput::task_spawn(int argc, char *argv[])
 	// if RC_SERIAL_PORT == PX4IO_SERIAL_DEVICE then don't use it by default if the px4io is running
 	if ((strcmp(RC_SERIAL_PORT, PX4IO_SERIAL_DEVICE) == 0) && (access("/dev/px4io", R_OK) == 0)) {
 		device_name = nullptr;
-		silent = true;
 	}
 
 #endif // RC_SERIAL_PORT && PX4IO_SERIAL_DEVICE
@@ -143,7 +141,6 @@ RCInput::task_spawn(int argc, char *argv[])
 		switch (ch) {
 		case 'd':
 			device_name = myoptarg;
-			silent = false;
 			break;
 
 		case '?':
@@ -176,9 +173,6 @@ RCInput::task_spawn(int argc, char *argv[])
 
 		return PX4_OK;
 
-	} else if (silent) {
-		return PX4_OK;
-
 	} else {
 		if (device_name) {
 			PX4_ERR("invalid device (-d) %s", device_name);

src/drivers/uavcan/sensors/rangefinder.cpp

@@ -108,13 +108,14 @@ void UavcanRangefinderBridge::range_sub_cb(const
 		_inited = true;
 	}
 
-	int8_t quality = -1;
-
-	if (msg.reading_type == uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE) {
-		quality = 100;
-	}
-
-	rangefinder->update(hrt_absolute_time(), msg.range, quality);
+	/*
+	 * FIXME HACK
+	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	rangefinder->update(hrt_absolute_time(), msg.range);
 }
 
 int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel)

src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp

@@ -798,7 +798,6 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
 	 * factor of 4 necessary in the address relative to the SA register values.
 	 */
 
-
 // Location of this interface's message RAM - address in CPU memory address
 // and relative address (in words) used for configuration
 	const uint32_t iface_ram_base = (2560 / 2) * self_index_;
@@ -810,16 +809,14 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
 	message_ram_.StdIdFilterSA = gl_ram_base + ram_offset * WORD_LENGTH;
 	can_->SIDFC = ((n_stdid << FDCAN_SIDFC_LSS_Pos)
 		       | ram_offset << FDCAN_SIDFC_FLSSA_Pos);
-	memset((void *)message_ram_.StdIdFilterSA, 0, WORD_LENGTH * n_stdid); // make sure filters are disabled
 	ram_offset += n_stdid;
 
-	// Extended ID Filters: Allow space for 64 filters (128 words)
-	const uint8_t n_extid = 64;
+	// Extended ID Filters: Allow space for 128 filters (128 words)
+	const uint8_t n_extid = 128;
 	message_ram_.ExtIdFilterSA = gl_ram_base + ram_offset * WORD_LENGTH;
 	can_->XIDFC = ((n_extid << FDCAN_XIDFC_LSE_Pos)
 		       | ram_offset << FDCAN_XIDFC_FLESA_Pos);
-	memset((void *)message_ram_.ExtIdFilterSA, 0, (2 * WORD_LENGTH) * n_extid); // make sure filters are disabled
-	ram_offset += 2 * n_extid;
+	ram_offset += n_extid;
 
 	// Set size of each element in the Rx/Tx buffers and FIFOs
 	can_->RXESC = 0; // 8 byte space for every element (Rx buffer, FIFO1, FIFO0)

src/drivers/uwb/uwb_sr150/module.yaml

@@ -4,6 +4,7 @@ serial_config:
     port_config_param:
       name: UWB_PORT_CFG
       group: UWB
+      default: "TEL2"
 
 parameters:
     - group: UWB

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -126,43 +126,18 @@ public:
 		return res;
 	}
 
-	template <size_t Width>
-	Type trace(size_t first) const
+	Type trace() const
 	{
-		static_assert(Width <= M, "Width bigger than matrix");
-		assert(first + Width <= M);
-
 		Type res = 0;
 		const SquareMatrix<Type, M> &self = *this;
 
-		for (size_t i = first; i < (first + Width); i++) {
+		for (size_t i = 0; i < M; i++) {
 			res += self(i, i);
 		}
 
 		return res;
 	}
 
-	Type trace() const
-	{
-		const SquareMatrix<Type, M> &self = *this;
-		return self.trace<M>(0);
-	}
-
-	// keep the sub covariance matrix and zero all covariance elements related
-	// to the rest of the matrix
-	template <size_t Width>
-	void uncorrelateCovarianceBlock(size_t first)
-	{
-		static_assert(Width <= M, "Width bigger than matrix");
-		assert(first + Width <= M);
-
-		SquareMatrix<Type, M> &self = *this;
-		SquareMatrix<Type, Width> cov = self.slice<Width, Width>(first, first);
-		self.slice<M, Width>(0, first) = 0.f;
-		self.slice<Width, M>(first, 0) = 0.f;
-		self.slice<Width, Width>(first, first) = cov;
-	}
-
 	// zero all offdiagonal elements and keep corresponding diagonal elements
 	template <size_t Width>
 	void uncorrelateCovariance(size_t first)

src/lib/matrix/test/MatrixSquareTest.cpp

@@ -47,7 +47,6 @@ TEST(MatrixSquareTest, Square)
 
 	EXPECT_EQ(A.diag(), diag_check);
 	EXPECT_FLOAT_EQ(A.trace(), 16);
-	EXPECT_FLOAT_EQ(A.trace<2>(1), 15);
 
 	float data_check[9] = {
 		1.01158503f,  0.02190432f,  0.03238144f,
@@ -118,16 +117,6 @@ TEST(MatrixSquareTest, Square)
 	SquareMatrix<float, 4> E_check(data_E_check);
 	EXPECT_EQ(E, E_check);
 
-	SquareMatrix<float, 4> A_block(data_4x4);
-	A_block.uncorrelateCovarianceBlock<2>(1);
-	float data_A_block_check[16] = {1, 0, 0, 4,
-					0, 6, 7, 0,
-					0, 10, 11, 0,
-					13, 0, 0, 16
-				       };
-	SquareMatrix<float, 4> A_block_check(data_A_block_check);
-	EXPECT_EQ(A_block, A_block_check);
-
 	// test symmetric functions
 	SquareMatrix<float, 4> F(data_4x4);
 	F.makeBlockSymmetric<2>(1);

src/lib/npfg/npfg.cpp

@@ -59,7 +59,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g
 	const float wind_speed = wind_vel.norm();
 
 	const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path};
-	signed_track_error_ = unit_path_tangent.cross(path_pos_to_vehicle);
+	const float signed_track_error = unit_path_tangent.cross(path_pos_to_vehicle);
 
 	// on-track wind triangle projections
 	const float wind_cross_upt = wind_vel.cross(unit_path_tangent);
@@ -68,7 +68,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g
 	// calculate the bearing feasibility on the track at the current closest point
 	feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
 
-	const float track_error = fabsf(signed_track_error_);
+	const float track_error = fabsf(signed_track_error);
 
 	// update control parameters considering upper and lower stability bounds (if enabled)
 	// must be called before trackErrorBound() as it updates time_const_
@@ -86,7 +86,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g
 
 	track_proximity_ = trackProximity(look_ahead_ang);
 
-	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_);
+	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
 
 	// wind triangle projections
 	const float wind_cross_bearing = wind_vel.cross(bearing_vec_);
@@ -112,7 +112,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g
 
 	// lateral acceleration needed to stay on curved track (assuming no heading error)
 	lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt,
-					   wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature);
+					   wind_cross_upt, airspeed, wind_speed, signed_track_error, path_curvature);
 
 	// total lateral acceleration to drive aircaft towards track as well as account
 	// for path curvature. The full effect of the feed-forward acceleration is smoothly

src/lib/parameters/px4params/srcparser.py

@@ -357,7 +357,7 @@ class SourceParser(object):
                                 'bit/s', 'B/s',
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'gauss^2',
-                                'hPa', 'kg', 'kg/m^2', 'kg m^2',
+                                'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
                                 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad',
                                 'Ohm', 'V', 'A',
                                 'us', 'ms', 's',

src/lib/tecs/TECS.cpp

@@ -156,10 +156,6 @@ void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceS
 	_alt_control_traj_generator.setMaxAccel(param.vert_accel_limit);
 	_alt_control_traj_generator.setMaxVel(fmax(param.max_climb_rate, param.max_sink_rate));
 
-	// XXX: this is a bit risky.. .alt_rate here could be NAN (by interface design) - and is only ok to input to the
-	// setVelSpFeedback() method because it calls the reset in the logic below when it is NAN.
-	// TODO: stop it with the NAN interfaces, make sure to take care of this when refactoring and separating altitude
-	// and height rate control loops.
 	_velocity_control_traj_generator.setVelSpFeedback(setpoint.alt_rate);
 
 	bool control_altitude = true;

src/modules/commander/Commander.cpp

@@ -719,14 +719,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			// to not require navigator and command to receive / process
 			// the data at the exact same time.
 
-			const uint32_t change_mode_flags = uint32_t(cmd.param2);
-			const bool mode_switch_not_requested = (change_mode_flags & 1) == 0;
-			const bool unsupported_bits_set = (change_mode_flags & ~1) != 0;
-
-			if (mode_switch_not_requested || unsupported_bits_set) {
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
-
-			} else {
+			// Check if a mode switch had been requested
+			if ((((uint32_t)cmd.param2) & 1) > 0) {
 				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
 					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -734,6 +728,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
 					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
+
+			} else {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 		}
 		break;

src/modules/commander/failsafe/failsafe.cpp

@@ -404,8 +404,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 	CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());
 
 	// Battery
-	CHECK_FAILSAFE(status_flags, battery_low_remaining_time,
-		       ActionOptions(Action::RTL).causedBy(Cause::BatteryLow).clearOn(ClearCondition::OnModeChangeOrDisarm));
+	CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow));
 	CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
 
 	switch (status_flags.battery_warning) {

src/modules/dataman/dataman.cpp

@@ -529,8 +529,6 @@ _file_clear(dm_item_t item)
 static int
 _file_initialize(unsigned max_offset)
 {
-	const bool file_existed = (access(k_data_manager_device_path, F_OK) == 0);
-
 	/* Open or create the data manager file */
 	dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666);
 
@@ -555,7 +553,7 @@ _file_initialize(unsigned max_offset)
 
 	dm_operations_data.silence = false;
 
-	if (!file_existed || (compat_state.key != DM_COMPAT_KEY)) {
+	if (compat_state.key != DM_COMPAT_KEY) {
 
 		/* Write current compat info */
 		compat_state.key = DM_COMPAT_KEY;

src/modules/ekf2/CMakeLists.txt

@@ -68,6 +68,7 @@ add_subdirectory(Utility)
 
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
+	EKF/baro_height_control.cpp
 	EKF/bias_estimator.cpp
 	EKF/control.cpp
 	EKF/covariance.cpp
@@ -83,9 +84,12 @@ list(APPEND EKF_SRCS
 	EKF/gravity_fusion.cpp
 	EKF/height_control.cpp
 	EKF/imu_down_sampler.cpp
+	EKF/mag_3d_control.cpp
+	EKF/mag_control.cpp
+	EKF/mag_fusion.cpp
+	EKF/mag_heading_control.cpp
 	EKF/output_predictor.cpp
 	EKF/vel_pos_fusion.cpp
-	EKF/yaw_fusion.cpp
 	EKF/zero_innovation_heading_update.cpp
 	EKF/zero_gyro_update.cpp
 	EKF/zero_velocity_update.cpp
@@ -99,12 +103,6 @@ if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
 endif()
 
-if(CONFIG_EKF2_BAROMETER)
-	list(APPEND EKF_SRCS
-		EKF/baro_height_control.cpp
-	)
-endif()
-
 if(CONFIG_EKF2_DRAG_FUSION)
 	list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
 endif()
@@ -123,15 +121,6 @@ if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
 endif()
 
-if(CONFIG_EKF2_MAGNETOMETER)
-	list(APPEND EKF_SRCS
-		EKF/mag_3d_control.cpp
-		EKF/mag_control.cpp
-		EKF/mag_fusion.cpp
-		EKF/mag_heading_control.cpp
-	)
-endif()
-
 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
 		EKF/optical_flow_control.cpp
@@ -144,6 +133,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
 		EKF/range_finder_consistency_check.cpp
 		EKF/range_height_control.cpp
 		EKF/sensor_range_finder.cpp
+		EKF/terrain_estimator.cpp
 	)
 endif()
 
@@ -151,10 +141,6 @@ if(CONFIG_EKF2_SIDESLIP)
 	list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
 endif()
 
-if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS EKF/terrain_estimator.cpp)
-endif()
-
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2

src/modules/ekf2/EKF/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-2023 ECL Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
 #    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
+# 3. Neither the name ECL nor the names of its contributors may be
 #    used to endorse or promote products derived from this software
 #    without specific prior written permission.
 #
@@ -33,6 +33,7 @@
 
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
+	baro_height_control.cpp
 	bias_estimator.cpp
 	control.cpp
 	covariance.cpp
@@ -48,9 +49,12 @@ list(APPEND EKF_SRCS
 	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
+	mag_3d_control.cpp
+	mag_control.cpp
+	mag_fusion.cpp
+	mag_heading_control.cpp
 	output_predictor.cpp
 	vel_pos_fusion.cpp
-	yaw_fusion.cpp
 	zero_innovation_heading_update.cpp
 	zero_gyro_update.cpp
 	zero_velocity_update.cpp
@@ -64,12 +68,6 @@ if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS auxvel_fusion.cpp)
 endif()
 
-if(CONFIG_EKF2_BAROMETER)
-	list(APPEND EKF_SRCS
-		baro_height_control.cpp
-	)
-endif()
-
 if(CONFIG_EKF2_DRAG_FUSION)
 	list(APPEND EKF_SRCS drag_fusion.cpp)
 endif()
@@ -88,15 +86,6 @@ if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
 endif()
 
-if(CONFIG_EKF2_MAGNETOMETER)
-	list(APPEND EKF_SRCS
-		mag_3d_control.cpp
-		mag_control.cpp
-		mag_fusion.cpp
-		mag_heading_control.cpp
-	)
-endif()
-
 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
 		optical_flow_control.cpp
@@ -109,6 +98,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
 		range_finder_consistency_check.cpp
 		range_height_control.cpp
 		sensor_range_finder.cpp
+		terrain_estimator.cpp
 	)
 endif()
 
@@ -116,10 +106,6 @@ if(CONFIG_EKF2_SIDESLIP)
 	list(APPEND EKF_SRCS sideslip_fusion.cpp)
 endif()
 
-if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS terrain_estimator.cpp)
-endif()
-
 add_library(ecl_EKF
 	${EKF_SRCS}
 )

src/modules/ekf2/EKF/Sensor.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *

src/modules/ekf2/EKF/baro_height_control.cpp

@@ -51,12 +51,7 @@ void Ekf::controlBaroHeightFusion()
 
 	if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) {
 
-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 		const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
-#else
-		const float measurement = baro_sample.hgt;
-#endif
-
 		const float measurement_var = sq(_params.baro_noise);
 
 		const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
@@ -108,7 +103,7 @@ void Ekf::controlBaroHeightFusion()
 		if (measurement_valid) {
 			bias_est.setMaxStateNoise(sqrtf(measurement_var));
 			bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9));
 		}
 
 		// determine if we should use height aiding

src/modules/ekf2/EKF/bias_estimator.hpp

@@ -47,8 +47,7 @@
  * @author Mathieu Bresciani 	<mathieu@auterion.com>
  */
 
-#ifndef EKF_BIAS_ESTIMATOR_HPP
-#define EKF_BIAS_ESTIMATOR_HPP
+#pragma once
 
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -134,5 +133,3 @@ private:
 	static constexpr float _innov_sequence_monitnoring_time_constant{10.f}; ///< in seconds
 	static constexpr float _process_var_boost_gain{1.0e3f};
 };
-
-#endif // !EKF_BIAS_ESTIMATOR_HPP

src/modules/ekf2/EKF/common.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -105,12 +105,12 @@ enum MagFuseType : uint8_t {
 	NONE    = 5    	///< Do not use magnetometer under any circumstance..
 };
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 enum TerrainFusionMask : uint8_t {
 	TerrainFuseRangeFinder = (1 << 0),
 	TerrainFuseOpticalFlow = (1 << 1)
 };
-#endif // CONFIG_EKF2_TERRAIN
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 enum HeightSensor : uint8_t {
 	BARO  = 0,
@@ -362,27 +362,18 @@ struct parameters {
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)
 #endif // CONFIG_EKF2_SIDESLIP
 
-#if defined(CONFIG_EKF2_TERRAIN)
-	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
-				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
-
-	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
-	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
-	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
-#endif // CONFIG_EKF2_TERRAIN
-
-#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
-	float rng_gnd_clearance{0.1f};          ///< minimum valid value for range when on ground (m)
-#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
-
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range finder fusion
 	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
 
+	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
+				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
+
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
 	float rng_hgt_bias_nsd{0.13f};          ///< process noise for range height bias estimation (m/s/sqrt(Hz))
+	float rng_gnd_clearance{0.1f};          ///< minimum valid value for range when on ground (m)
 	float rng_sens_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
 	float range_noise_scaler{0.0f};         ///< scaling from range measurement to noise (m/m)
 	const float vehicle_variance_scaler{0.0f};      ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
@@ -394,6 +385,11 @@ struct parameters {
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
 
 	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
+
+	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
+	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
+	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
+
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -629,7 +625,7 @@ union ekf_solution_status_u {
 	uint16_t value;
 };
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 union terrain_fusion_status_u {
 	struct {
 		bool range_finder : 1;  ///< 0 - true if we are fusing range finder data
@@ -637,7 +633,7 @@ union terrain_fusion_status_u {
 	} flags;
 	uint8_t value;
 };
-#endif // CONFIG_EKF2_TERRAIN
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 // define structure used to communicate information events
 union information_event_status_u {

src/modules/ekf2/EKF/control.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -102,10 +102,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 		}
 	}
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control use of observations for aiding
 	controlMagFusion();
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	controlOpticalFlowFusion(imu_delayed);

src/modules/ekf2/EKF/covariance.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -57,53 +57,56 @@ void Ekf::initialiseCovariance()
 	resetQuatCov();
 
 	// velocity
-	const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f));
-	P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var));
+	P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f));
+	P(5,5) = P(4,4);
+	P(6,6) = sq(1.5f) * P(4,4);
 
 	// position
-	const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
-	float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
+	P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f));
+	P(8,8) = P(7,7);
+	P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
 
 	if (_control_status.flags.gps_hgt) {
-		z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
+		P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
 	}
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
-		z_pos_var = sq(fmaxf(_params.range_noise, 0.01f));
+		P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
 	}
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	P.uncorrelateCovarianceSetVariance<State::pos.dof>(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var));
-
 	// gyro bias
-	const float gyro_bias_var = sq(_params.switch_on_gyro_bias);
-	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, gyro_bias_var);
-	_prev_gyro_bias_var.setAll(gyro_bias_var);
+	_prev_gyro_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias);
+	_prev_gyro_bias_var(1) = P(11,11) = P(10,10);
+	_prev_gyro_bias_var(2) = P(12,12) = P(10,10);
 
 	// accel bias
-	const float accel_bias_var = sq(_params.switch_on_accel_bias);
-	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, accel_bias_var);
-	_prev_accel_bias_var.setAll(accel_bias_var);
+	_prev_accel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias);
+	_prev_accel_bias_var(1) = P(14,14) = P(13,13);
+	_prev_accel_bias_var(2) = P(15,15) = P(13,13);
 
 	resetMagCov();
 
 	// wind
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
+	P(22,22) = sq(_params.initial_wind_uncertainty);
+	P(23,23) = P(22,22);
+
 }
 
 void Ekf::predictCovariance(const imuSample &imu_delayed)
 {
 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
 	const float dt = _dt_ekf_avg;
+	const float dt_inv = 1.f / dt;
 
 	// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
 	// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
 	const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
 	const float beta = 1.0f - alpha;
-	_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt);
-	_accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt);
-	_accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt;
+	_ang_rate_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
+	_accel_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
+	_accel_vec_filt = alpha * dt_inv * imu_delayed.delta_vel + beta * _accel_vec_filt;
 
 	const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
 					     || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
@@ -111,8 +114,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	// gyro bias inhibit
 	const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
 
-	for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
-		const unsigned stateIndex = State::gyro_bias.idx + index;
+	for (unsigned stateIndex = 10; stateIndex <= 12; stateIndex++) {
+		const unsigned index = stateIndex - 10;
 
 		bool is_bias_observable = true;
 
@@ -141,8 +144,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 					 || is_manoeuvre_level_high
 					 || _fault_status.flags.bad_acc_vertical;
 
-	for (unsigned index = 0; index < State::accel_bias.dof; index++) {
-		const unsigned stateIndex = State::accel_bias.idx + index;
+	for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) {
+		const unsigned index = stateIndex - 13;
 
 		bool is_bias_observable = true;
 
@@ -176,21 +179,23 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 
 	// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
-	float mag_I_sig = 0.0f;
+	float mag_I_sig;
 
-	if (_control_status.flags.mag && P.trace<State::mag_I.dof>(State::mag_I.idx) < 0.1f) {
-#if defined(CONFIG_EKF2_MAGNETOMETER)
+	if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
 		mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
-#endif // CONFIG_EKF2_MAGNETOMETER
+
+	} else {
+		mag_I_sig = 0.0f;
 	}
 
 	// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
-	float mag_B_sig = 0.0f;
+	float mag_B_sig;
 
-	if (_control_status.flags.mag && P.trace<State::mag_B.dof>(State::mag_B.idx) < 0.1f) {
-#if defined(CONFIG_EKF2_MAGNETOMETER)
+	if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
 		mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
-#endif // CONFIG_EKF2_MAGNETOMETER
+
+	} else {
+		mag_B_sig = 0.0f;
 	}
 
 	float wind_vel_nsd_scaled;
@@ -200,29 +205,30 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 
 	// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
-	if (_control_status.flags.wind && P.trace<State::wind_vel.dof>(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) {
+	if (_control_status.flags.wind && (P(22,22) + P(23,23)) < sq(_params.initial_wind_uncertainty)) {
 		wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.0f, 1.0f) * (1.0f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
 
 	} else {
 		wind_vel_nsd_scaled = 0.0f;
 	}
 
+
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities
 	float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
-	const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise);
+	const float d_ang_var = sq(dt * gyro_noise);
 
 	float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
 
 	Vector3f d_vel_var;
 
-	for (unsigned i = 0; i < 3; i++) {
+	for (int i = 0; i <= 2; i++) {
 		if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) {
 			// Increase accelerometer process noise if bad accel data is detected
-			d_vel_var(i) = sq(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE);
+			d_vel_var(i) = sq(dt * BADACC_BIAS_PNOISE);
 
 		} else {
-			d_vel_var(i) = sq(imu_delayed.delta_vel_dt * accel_noise);
+			d_vel_var(i) = sq(dt * accel_noise);
 		}
 	}
 
@@ -230,92 +236,56 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	SquareMatrix24f nextP;
 
 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
-	sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P,
-		imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var,
-		imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var,
-		&nextP);
+	sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, imu_delayed.delta_vel, d_vel_var, imu_delayed.delta_ang, d_ang_var, dt, &nextP);
+
+	// compute noise variance for stationary processes
+	Vector24f process_noise;
 
 	// Construct the process noise variance diagonal for those states with a stationary process model
 	// These are kinematic states and their error growth is controlled separately by the IMU noise variances
 
+	// earth frame magnetic field states
+	process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig);
+	// body frame magnetic field states
+	process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
+	// wind velocity states
+	process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt;
+
+	// add process noise that is not from the IMU
+	for (unsigned i = 16; i <= 23; i++) {
+		nextP(i, i) += process_noise(i);
+	}
+
 	// gyro bias: add process noise, or restore previous gyro bias var if state inhibited
 	const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
 	const float gyro_bias_process_noise = sq(gyro_bias_sig);
-	for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
-		const unsigned i = State::gyro_bias.idx + index;
+	for (unsigned i = 10; i <= 12; i++) {
+		const int axis_index = i - 10;
 
-		if (!_gyro_bias_inhibit[index]) {
+		if (!_gyro_bias_inhibit[axis_index]) {
 			nextP(i, i) += gyro_bias_process_noise;
 
 		} else {
-			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index));
+			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(axis_index));
 		}
 	}
 
 	// accel bias: add process noise, or restore previous accel bias var if state inhibited
 	const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
 	const float accel_bias_process_noise = sq(accel_bias_sig);
-	for (unsigned index = 0; index < State::accel_bias.dof; index++) {
-		const unsigned i = State::accel_bias.idx + index;
+	for (int i = 13; i <= 15; i++) {
+		const int axis_index = i - 13;
 
-		if (!_accel_bias_inhibit[index]) {
+		if (!_accel_bias_inhibit[axis_index]) {
 			nextP(i, i) += accel_bias_process_noise;
 
 		} else {
-			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index));
-		}
-	}
-
-	if (_control_status.flags.mag) {
-		const float mag_I_process_noise = sq(mag_I_sig);
-		for (unsigned index = 0; index < State::mag_I.dof; index++) {
-			unsigned i = State::mag_I.idx + index;
-			nextP(i, i) += mag_I_process_noise;
-		}
-
-		const float mag_B_process_noise = sq(mag_B_sig);
-		for (unsigned index = 0; index < State::mag_B.dof; index++) {
-			unsigned i = State::mag_B.idx + index;
-			nextP(i, i) += mag_B_process_noise;
-		}
-
-	} else {
-		// keep previous covariance
-		for (unsigned i = 0; i < State::mag_I.dof; i++) {
-			unsigned row = State::mag_I.idx + i;
-			for (unsigned col = 0; col < State::size; col++) {
-				nextP(row, col) = nextP(col, row) = P(row, col);
-			}
-		}
-
-		for (unsigned i = 0; i < State::mag_B.dof; i++) {
-			unsigned row = State::mag_B.idx + i;
-			for (unsigned col = 0; col < State::size; col++) {
-				nextP(row, col) = nextP(col, row) = P(row, col);
-			}
-		}
-	}
-
-	if (_control_status.flags.wind) {
-		const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
-;
-		for (unsigned index = 0; index < State::wind_vel.dof; index++) {
-			unsigned i = State::wind_vel.idx + index;
-			nextP(i, i) += wind_vel_process_noise;
-		}
-
-	} else {
-		// keep previous covariance
-		for (unsigned i = 0; i < State::wind_vel.dof; i++) {
-			unsigned row = State::wind_vel.idx + i;
-			for (unsigned col = 0 ; col < State::size; col++) {
-				nextP(row, col) = nextP(col, row) = P(row, col);
-			}
+			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(axis_index));
 		}
 	}
 
 	// covariance matrix is symmetrical, so copy upper half to lower half
-	for (unsigned row = 0; row < State::size; row++) {
+	for (unsigned row = 0; row <= 15; row++) {
 		for (unsigned column = 0 ; column < row; column++) {
 			P(row, column) = P(column, row) = nextP(column, row);
 		}
@@ -323,9 +293,30 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		P(row, row) = nextP(row, row);
 	}
 
+	if (_control_status.flags.mag) {
+		for (unsigned row = 16; row <= 21; row++) {
+			for (unsigned column = 0 ; column < row; column++) {
+				P(row, column) = P(column, row) = nextP(column, row);
+			}
+
+			P(row, row) = nextP(row, row);
+		}
+	}
+
+	if (_control_status.flags.wind) {
+		for (unsigned row = 22; row <= 23; row++) {
+			for (unsigned column = 0 ; column < row; column++) {
+				P(row, column) = P(column, row) = nextP(column, row);
+			}
+
+			P(row, row) = nextP(row, row);
+		}
+	}
+
 	// fix gross errors in the covariance matrix and ensure rows and
 	// columns for un-used states are zero
 	fixCovarianceErrors(false);
+
 }
 
 void Ekf::fixCovarianceErrors(bool force_symmetry)
@@ -335,25 +326,39 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	// and set corresponding entries in Q to zero when states exceed 50% of the limit
 	// Covariance diagonal limits. Use same values for states which
 	// belong to the same group (e.g. vel_x, vel_y, vel_z)
-	const float quat_var_max = 1.0f;
-	const float vel_var_max = 1e6f;
-	const float pos_var_max = 1e6f;
-	const float gyro_bias_var_max = 1.0f;
-	const float mag_I_var_max = 1.0f;
-	const float mag_B_var_max = 1.0f;
-	const float wind_vel_var_max = 1e6f;
+	float P_lim[8] = {};
+	P_lim[0] = 1.0f;		// quaternion max var
+	P_lim[1] = 1e6f;		// velocity max var
+	P_lim[2] = 1e6f;		// position max var
+	P_lim[3] = 1.0f;		// gyro bias max var
+	P_lim[4] = 1.0f;		// delta velocity z bias max var
+	P_lim[5] = 1.0f;		// earth mag field max var
+	P_lim[6] = 1.0f;		// body mag field max var
+	P_lim[7] = 1e6f;		// wind max var
 
-	constrainStateVar(State::quat_nominal, 0.f, quat_var_max);
-	constrainStateVar(State::vel, 1e-6f, vel_var_max);
-	constrainStateVar(State::pos, 1e-6f, pos_var_max);
-	constrainStateVar(State::gyro_bias, 0.f, gyro_bias_var_max);
+	for (int i = 0; i <= 3; i++) {
+		// quaternion states
+		P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[0]);
+	}
+
+	for (int i = 4; i <= 6; i++) {
+		// NED velocity states
+		P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[1]);
+	}
+
+	for (int i = 7; i <= 9; i++) {
+		// NED position states
+		P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[2]);
+	}
+
+	for (int i = 10; i <= 12; i++) {
+		// gyro bias states
+		P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[3]);
+	}
 
 	// force symmetry on the quaternion, velocity and position state covariances
 	if (force_symmetry) {
-		P.makeRowColSymmetric<State::quat_nominal.dof>(State::quat_nominal.idx);
-		P.makeRowColSymmetric<State::vel.dof>(State::vel.idx);
-		P.makeRowColSymmetric<State::pos.dof>(State::pos.idx);
-		P.makeRowColSymmetric<State::gyro_bias.dof>(State::gyro_bias.idx); //TODO: needed?
+		P.makeRowColSymmetric<13>(0);
 	}
 
 	// the following states are optional and are deactivated when not required
@@ -366,10 +371,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		float maxStateVar = minSafeStateVar;
 		bool resetRequired = false;
 
-		for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) {
-			const unsigned stateIndex = State::accel_bias.idx + axis;
-
-			if (_accel_bias_inhibit[axis]) {
+		for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
+			if (_accel_bias_inhibit[stateIndex - 13]) {
 				// Skip the check for the inhibited axis
 				continue;
 			}
@@ -388,10 +391,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		const float minStateVarTarget = 5E-8f / sq(_dt_ekf_avg);
 		float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
 
-		for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) {
-			const unsigned stateIndex = State::accel_bias.idx + axis;
-
-			if (_accel_bias_inhibit[axis]) {
+		for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
+			if (_accel_bias_inhibit[stateIndex - 13]) {
 				// Skip the check for the inhibited axis
 				continue;
 			}
@@ -401,7 +402,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 		// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
 		if (resetRequired) {
-			P.uncorrelateCovariance<State::accel_bias.dof>(State::accel_bias.idx);
+			P.uncorrelateCovariance<3>(13);
 		}
 
 		// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
@@ -422,11 +423,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 			if (bad_vz_gps || bad_vz_ev) {
-#if defined(CONFIG_EKF2_BAROMETER)
 				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
-#else
-				bool bad_z_baro = false;
-#endif
 				bool bad_z_gps  = _control_status.flags.gps_hgt  && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -460,7 +457,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
 		if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) {
 
-			P.uncorrelateCovariance<State::accel_bias.dof>(State::accel_bias.idx);
+			P.uncorrelateCovariance<3>(13);
 
 			_time_acc_bias_check = _time_delayed_us;
 			_fault_status.flags.bad_acc_bias = false;
@@ -469,7 +466,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 		} else if (force_symmetry) {
 			// ensure the covariance values are symmetrical
-			P.makeRowColSymmetric<State::accel_bias.dof>(State::accel_bias.idx);
+			P.makeRowColSymmetric<3>(13);
 		}
 
 	}
@@ -480,32 +477,37 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
 
 	} else {
-		constrainStateVar(State::mag_I, 0.f, mag_I_var_max);
-		constrainStateVar(State::mag_B, 0.f, mag_B_var_max);
-
-		if (force_symmetry) {
-			P.makeRowColSymmetric<State::mag_I.dof>(State::mag_I.idx);
-			P.makeRowColSymmetric<State::mag_B.dof>(State::mag_B.idx);
+		// constrain variances
+		for (int i = 16; i <= 18; i++) {
+			P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[5]);
 		}
+
+		for (int i = 19; i <= 21; i++) {
+			P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[6]);
+		}
+
+		// force symmetry
+		if (force_symmetry) {
+			P.makeRowColSymmetric<3>(16);
+			P.makeRowColSymmetric<3>(19);
+		}
+
 	}
 
 	// wind velocity states
 	if (!_control_status.flags.wind) {
-		P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);
+		P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
 
 	} else {
-		constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max);
-
-		if (force_symmetry) {
-			P.makeRowColSymmetric<State::wind_vel.dof>(State::wind_vel.idx);
+		// constrain variances
+		for (int i = 22; i <= 23; i++) {
+			P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[7]);
 		}
-	}
-}
 
-void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
-{
-	for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
-		P(i, i) = math::constrain(P(i, i), min, max);
+		// force symmetry
+		if (force_symmetry) {
+			P.makeRowColSymmetric<2>(22);
+		}
 	}
 }
 
@@ -515,7 +517,7 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
 {
 	bool healthy = true;
 
-	for (int i = 0; i < State::size; i++) {
+	for (int i = 0; i < _k_num_states; i++) {
 		if (P(i, i) < KHP(i, i)) {
 			P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
 			healthy = false;
@@ -537,38 +539,31 @@ void Ekf::resetQuatCov(const float yaw_noise)
 	}
 
 	// clear existing quaternion covariance
-	// Optimization: avoid the creation of a <4> function
-	P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx, 0.0f);
-	P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx + 2, 0.0f);
+	P.uncorrelateCovarianceSetVariance<2>(0, 0.0f);
+	P.uncorrelateCovarianceSetVariance<2>(2, 0.0f);
 
-	matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov;
+	matrix::SquareMatrix<float, 4> q_cov;
 	sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov);
 	q_cov.copyLowerToUpperTriangle();
-	P.slice<State::quat_nominal.dof, State::quat_nominal.dof>(State::quat_nominal.idx, State::quat_nominal.idx) = q_cov;
+	P.slice<4, 4>(0, 0) = q_cov;
 }
 
 void Ekf::resetMagCov()
 {
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_mag_decl_cov_reset) {
 		ECL_INFO("reset mag covariance");
 		_mag_decl_cov_reset = false;
 	}
 
-	P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.mag_noise));
-	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
+	P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise));
+	P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
 
 	saveMagCovData();
-#else
-	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
-	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
-
-#endif
 }
 
 void Ekf::resetGyroBiasZCov()
 {
 	const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias);
 
-	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, init_gyro_bias_var);
+	P.uncorrelateCovarianceSetVariance<1>(12, init_gyro_bias_var);
 }

src/modules/ekf2/EKF/ekf.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -109,21 +109,14 @@ void Ekf::reset()
 	_gps_checks_passed = false;
 	_gps_alt_ref = NAN;
 
-#if defined(CONFIG_EKF2_BAROMETER)
 	_baro_counter = 0;
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_mag_counter = 0;
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 	_time_bad_vert_accel = 0;
 	_time_good_vert_accel = 0;
 	_clip_counter = 0;
 
-#if defined(CONFIG_EKF2_BAROMETER)
 	resetEstimatorAidStatus(_aid_src_baro_hgt);
-#endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_AIRSPEED)
 	resetEstimatorAidStatus(_aid_src_airspeed);
 #endif // CONFIG_EKF2_AIRSPEED
@@ -149,10 +142,8 @@ void Ekf::reset()
 	resetEstimatorAidStatus(_aid_src_gnss_yaw);
 #endif // CONFIG_EKF2_GNSS_YAW
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	resetEstimatorAidStatus(_aid_src_mag_heading);
 	resetEstimatorAidStatus(_aid_src_mag);
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	resetEstimatorAidStatus(_aid_src_aux_vel);
@@ -193,10 +184,10 @@ bool Ekf::update()
 		// control fusion of observation data
 		controlFusionModes(imu_sample_delayed);
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 		// run a separate filter for terrain estimation
 		runTerrainEstimator(imu_sample_delayed);
-#endif // CONFIG_EKF2_TERRAIN
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);
 
@@ -233,10 +224,10 @@ bool Ekf::initialiseFilter()
 	// initialise the state covariance matrix now we have starting values for all the states
 	initialiseCovariance();
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Initialise the terrain estimator
 	initHagl();
-#endif // CONFIG_EKF2_TERRAIN
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	// reset the output predictor state history to match the EKF initial values
 	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);

src/modules/ekf2/EKF/ekf.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -49,7 +49,6 @@
 #include "bias_estimator.hpp"
 #include "height_bias_estimator.hpp"
 #include "position_bias_estimator.hpp"
-#include "python/ekf_derivation/generated/state.h"
 
 #include <uORB/topics/estimator_aid_source1d.h>
 #include <uORB/topics/estimator_aid_source2d.h>
@@ -60,12 +59,14 @@ enum class Likelihood { LOW, MEDIUM, HIGH };
 class Ekf final : public EstimatorInterface
 {
 public:
-	typedef matrix::Vector<float, State::size> Vector24f;
-	typedef matrix::SquareMatrix<float, State::size> SquareMatrix24f;
+	static constexpr uint8_t _k_num_states{24};		///< number of EKF states
+
+	typedef matrix::Vector<float, _k_num_states> Vector24f;
+	typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
 	typedef matrix::SquareMatrix<float, 2> Matrix2f;
 	template<int ... Idxs>
 
-	using SparseVector24f = matrix::SparseVectorf<State::size, Idxs...>;
+	using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
 
 	Ekf()
 	{
@@ -80,8 +81,6 @@ public:
 	// should be called every time new data is pushed into the filter
 	bool update();
 
-	static uint8_t getNumberOfStates() { return State::size; }
-
 	void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
@@ -92,16 +91,27 @@ public:
 	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-#if defined(CONFIG_EKF2_BAROMETER)
-	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
-	const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
-
 	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
 	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
 	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
-#endif // CONFIG_EKF2_BAROMETER
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// range height
+	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
+	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
+
+	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
+	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
+	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
+
+	void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
+	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
+	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
+
+	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
+	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
+	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
+
 	// terrain estimate
 	bool isTerrainEstimateValid() const;
 
@@ -115,64 +125,13 @@ public:
 
 	// get the terrain variance
 	float get_terrain_var() const { return _terrain_var; }
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; }
-
-	void getHaglInnov(float &hagl_innov) const { hagl_innov = _aid_src_terrain_range_finder.innovation; }
-	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; }
-	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _aid_src_terrain_range_finder.test_ratio; }
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
-
-	void getTerrainFlowInnov(float flow_innov[2]) const
-	{
-		flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0];
-		flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1];
-	}
-
-	void getTerrainFlowInnovVar(float flow_innov_var[2]) const
-	{
-		flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
-		flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
-	}
-
-	void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#endif // CONFIG_EKF2_TERRAIN
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	// range height
-	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
-	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
-
-	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
-	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
-	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
-
-	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
-	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
-	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
 
-	void getFlowInnov(float flow_innov[2]) const
-	{
-		flow_innov[0] = _aid_src_optical_flow.innovation[0];
-		flow_innov[1] = _aid_src_optical_flow.innovation[1];
-	}
-
-	void getFlowInnovVar(float flow_innov_var[2]) const
-	{
-		flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0];
-		flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
-	}
-
+	void getFlowInnov(float flow_innov[2]) const;
+	void getFlowInnovVar(float flow_innov_var[2]) const;
 	void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
 
 	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
@@ -183,6 +142,12 @@ public:
 
 	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
 	const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
+
+	void getTerrainFlowInnov(float flow_innov[2]) const;
+	void getTerrainFlowInnovVar(float flow_innov_var[2]) const;
+	void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
+
+	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_AUXVEL)
@@ -193,7 +158,6 @@ public:
 
 	void getHeadingInnov(float &heading_innov) const
 	{
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov = _aid_src_mag_heading.innovation;
 			return;
@@ -202,7 +166,6 @@ public:
 			heading_innov = Vector3f(_aid_src_mag.innovation).max();
 			return;
 		}
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -221,7 +184,6 @@ public:
 
 	void getHeadingInnovVar(float &heading_innov_var) const
 	{
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_var = _aid_src_mag_heading.innovation_variance;
 			return;
@@ -230,7 +192,6 @@ public:
 			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
 			return;
 		}
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -249,7 +210,6 @@ public:
 
 	void getHeadingInnovRatio(float &heading_innov_ratio) const
 	{
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
 			return;
@@ -258,7 +218,6 @@ public:
 			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
 			return;
 		}
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -275,11 +234,9 @@ public:
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
 	void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
 	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
@@ -394,29 +351,24 @@ public:
 
 	bool isYawFinalAlignComplete() const
 	{
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 		const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
 		const bool is_mag_alignment_in_flight_complete = is_using_mag
 				&& _control_status.flags.mag_aligned_in_flight
 				&& ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6);
 		return _control_status.flags.yaw_align
 		       && (is_mag_alignment_in_flight_complete || !is_using_mag);
-#else
-		return _control_status.flags.yaw_align;
-#endif
 	}
 
 	// gyro bias (states 10, 11, 12)
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
-	Vector3f getGyroBiasVariance() const { return P.slice<State::gyro_bias.dof, State::gyro_bias.dof>(State::gyro_bias.idx, State::gyro_bias.idx).diag(); } // get the gyroscope bias variance in rad/s
+	Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)}; } // get the gyroscope bias variance in rad/s
 	float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
 
 	// accel bias (states 13, 14, 15)
 	const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2
-	Vector3f getAccelBiasVariance() const { return P.slice<State::accel_bias.dof, State::accel_bias.dof>(State::accel_bias.idx, State::accel_bias.idx).diag(); } // get the accelerometer bias variance in m/s**2
+	Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2
 	float getAccelBiasLimit() const { return _params.acc_bias_lim; }
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	const Vector3f &getMagEarthField() const { return _state.mag_I; }
 
 	// mag bias (states 19, 20, 21)
@@ -424,13 +376,12 @@ public:
 	Vector3f getMagBiasVariance() const
 	{
 		if (_control_status.flags.mag) {
-			return P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx).diag();
+			return Vector3f{P(19, 19), P(20, 20), P(21, 21)};
 		}
 
 		return _saved_mag_bf_covmat.diag();
 	}
 	float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 	bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
 	bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; }
@@ -509,7 +460,7 @@ public:
 	bool isYawEmergencyEstimateAvailable() const;
 
 	uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
-
+	const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
 	const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -526,6 +477,8 @@ public:
 	const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
 #endif // CONFIG_EKF2_SIDESLIP
 
+	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
+
 	const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
 
@@ -544,10 +497,8 @@ public:
 	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
 #endif // CONFIG_EKF2_GNSS_YAW
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
 	const auto &aid_src_mag() const { return _aid_src_mag; }
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 	const auto &aid_src_gravity() const { return _aid_src_gravity; }
 
@@ -594,6 +545,9 @@ private:
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
 
+	float _mag_heading_prev{};                 ///< previous value of mag heading (rad)
+	float _mag_heading_pred_prev{};            ///< previous value of yaw state used by mag heading fusion (rad)
+
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 
@@ -620,9 +574,19 @@ private:
 	Vector3f _zgup_delta_ang{};
 	float _zgup_delta_ang_dt{0.f};
 
+	// used by magnetometer fusion mode selection
 	Vector2f _accel_lpf_NE{};			///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
 	float _yaw_delta_ef{0.0f};		///< Recent change in yaw angle measured about the earth frame D axis (rad)
 	float _yaw_rate_lpf_ef{0.0f};		///< Filtered angular rate about earth frame D axis (rad/sec)
+	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
+	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
+	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
+	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
+	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
+	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
+	uint8_t _nb_mag_heading_reset_available{0};
+	uint8_t _nb_mag_3d_reset_available{0};
+	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
 
 	SquareMatrix24f P{};	///< state covariance matrix
 
@@ -631,33 +595,30 @@ private:
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
 #endif // CONFIG_EKF2_DRAG_FUSION
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	estimator_aid_source1d_s _aid_src_rng_hgt{};
+
+	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
+
+	float _hagl_innov{0.0f};		///< innovation of the last height above terrain measurement (m)
+	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
+	float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
+
+	uint64_t _time_last_healthy_rng_data{0};
+
 	// Terrain height state estimation
 	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
 	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
 	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
-
+	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
+
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	estimator_aid_source1d_s _aid_src_terrain_range_finder{};
-	uint64_t _time_last_healthy_rng_data{0};
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#endif // CONFIG_EKF2_TERRAIN
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	estimator_aid_source1d_s _aid_src_rng_hgt{};
-	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	estimator_aid_source2d_s _aid_src_optical_flow{};
+	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
 
 	// optical flow processing
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
@@ -671,8 +632,10 @@ private:
 	Vector2f _flow_compensated_XY_rad{};	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
+	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+	estimator_aid_source1d_s _aid_src_baro_hgt{};
 #if defined(CONFIG_EKF2_AIRSPEED)
 	estimator_aid_source1d_s _aid_src_airspeed{};
 #endif // CONFIG_EKF2_AIRSPEED
@@ -705,6 +668,9 @@ private:
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 #endif // CONFIG_EKF2_GNSS_YAW
 
+	estimator_aid_source1d_s _aid_src_mag_heading{};
+	estimator_aid_source3d_s _aid_src_mag{};
+
 	estimator_aid_source3d_s _aid_src_gravity{};
 
 #if defined(CONFIG_EKF2_AUXVEL)
@@ -727,41 +693,14 @@ private:
 
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
+	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
+	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
 	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
 
-#if defined(CONFIG_EKF2_BAROMETER)
-	estimator_aid_source1d_s _aid_src_baro_hgt{};
-
 	// Variables used to perform in flight resets and switch between height sources
-	AlphaFilter<float> _baro_lpf{0.1f};	///< filtered barometric height measurement (m)
-	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
-
-	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
-
-	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	float _mag_heading_prev{};                 ///< previous value of mag heading (rad)
-	float _mag_heading_pred_prev{};            ///< previous value of yaw state used by mag heading fusion (rad)
-
-	// used by magnetometer fusion mode selection
-	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
-	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
-	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
-	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
-	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
-	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
-	uint8_t _nb_mag_heading_reset_available{0};
-	uint8_t _nb_mag_3d_reset_available{0};
-	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
-
-	estimator_aid_source1d_s _aid_src_mag_heading{};
-	estimator_aid_source3d_s _aid_src_mag{};
-
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
-	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
+	AlphaFilter<float> _baro_lpf{0.1f};	///< filtered barometric height measurement (m)
 
 	// Variables used to control activation of post takeoff functionality
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
@@ -769,7 +708,6 @@ private:
 	uint64_t _time_last_mag_check_failing{0};
 	Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
 	Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2)
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
@@ -784,6 +722,7 @@ private:
 	Vector3f _prev_accel_bias_var{};        ///< saved accel XYZ bias variances
 
 	// height sensor status
+	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
 	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
 
 	// imu fault status
@@ -805,6 +744,9 @@ private:
 	// predict ekf covariance
 	void predictCovariance(const imuSample &imu_delayed);
 
+	// ekf sequential fusion of magnetometer measurements
+	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
+
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
@@ -825,17 +767,12 @@ private:
 #endif // CONFIG_EKF2_GNSS_YAW
 	void stopGpsYawFusion();
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	// ekf sequential fusion of magnetometer measurements
-	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
-
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
 	bool fuseDeclination(float decl_sigma);
 
 	// apply sensible limits to the declination and length of the NE mag field states estimates
 	void limitDeclination();
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	// control fusion of air data observations
@@ -871,7 +808,7 @@ private:
 #endif // CONFIG_EKF2_DRAG_FUSION
 
 	// fuse single velocity and position measurement
-	bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index);
+	bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
 
 	void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var);
 
@@ -908,7 +845,12 @@ private:
 	void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
 	void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// range height
+	void controlRangeHeightFusion();
+	bool isConditionalRangeAidSuitable();
+	void stopRngHgtFusion();
+
 	// terrain vertical position estimator
 	void initHagl();
 	void runTerrainEstimator(const imuSample &imu_delayed);
@@ -916,33 +858,16 @@ private:
 
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
 
-	void controlHaglFakeFusion();
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	void controlHaglRngFusion();
-	void updateHaglRng(estimator_aid_source1d_s &aid_src) const;
-	void fuseHaglRng(estimator_aid_source1d_s &aid_src);
+	void fuseHaglRng();
+	void startHaglRngFusion();
+	void resetHaglRngIfNeeded();
 	void resetHaglRng();
 	void stopHaglRngFusion();
-	float getRngVar() const;
-# endif // CONFIG_EKF2_RANGE_FINDER
+	float getRngVar();
 
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// update the terrain vertical position estimate using an optical flow measurement
-	void controlHaglFlowFusion();
-	void resetHaglFlow();
-	void stopHaglFlowFusion();
-	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#endif // CONFIG_EKF2_TERRAIN
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	// range height
-	void controlRangeHeightFusion();
-	bool isConditionalRangeAidSuitable();
-	void stopRngHgtFusion();
+	void controlHaglFakeFusion();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -965,12 +890,17 @@ private:
 	void fuseOptFlow();
 	float predictFlowRange();
 	Vector2f predictFlowVelBody();
+
+	// update the terrain vertical position estimate using an optical flow measurement
+	void controlHaglFlowFusion();
+	void startHaglFlowFusion();
+	void resetHaglFlow();
+	void stopHaglFlowFusion();
+	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 	void clearInhibitedStateKalmanGains(Vector24f &K) const
 	{
@@ -1016,8 +946,8 @@ private:
 		const Vector24f KS = K * innovation_variance;
 		SquareMatrix24f KHP;
 
-		for (unsigned row = 0; row < State::size; row++) {
-			for (unsigned col = 0; col < State::size; col++) {
+		for (unsigned row = 0; row < _k_num_states; row++) {
+			for (unsigned col = 0; col < _k_num_states; col++) {
 				// Instad of literally computing KHP, use an equvalent
 				// equation involving less mathematical operations
 				KHP(row, col) = KS(row) * K(col);
@@ -1047,8 +977,6 @@ private:
 	// force symmetry when the argument is true
 	void fixCovarianceErrors(bool force_symmetry);
 
-	void constrainStateVar(const IdxDof &state, float min, float max);
-
 	// constrain the ekf states
 	void constrainStates();
 
@@ -1056,9 +984,7 @@ private:
 	// and a scalar innovation value
 	void fuse(const Vector24f &K, float innovation);
 
-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
-#endif // CONFIG_EKF2_BARO_COMPENSATION
 
 	// calculate the earth rotation vector from a given latitude
 	Vector3f calcEarthRateNED(float lat_rad) const;
@@ -1091,7 +1017,6 @@ private:
 	bool shouldResetGpsFusion() const;
 	bool isYawFailure() const;
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 	void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src);
@@ -1110,19 +1035,6 @@ private:
 	bool checkMagField(const Vector3f &mag);
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
 
-	void stopMagHdgFusion();
-	void stopMagFusion();
-
-	// load and save mag field state covariance data for re-use
-	void loadMagCovData();
-	void saveMagCovData();
-
-	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
-	// sensor measurement
-	float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
-
-#endif // CONFIG_EKF2_MAGNETOMETER
-
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
 
@@ -1149,16 +1061,16 @@ private:
 	// control for combined height fusion mode (implemented for switching between baro and range height)
 	void controlHeightFusion(const imuSample &imu_delayed);
 	void checkHeightSensorRefFallback();
+	void controlBaroHeightFusion();
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
 
-#if defined(CONFIG_EKF2_BAROMETER)
-	void controlBaroHeightFusion();
+	void stopMagHdgFusion();
+	void stopMagFusion();
+
 	void stopBaroHgtFusion();
+	void stopGpsHgtFusion();
 
 	void updateGroundEffect();
-#endif // CONFIG_EKF2_BAROMETER
-
-	void stopGpsHgtFusion();
 
 	// gravity fusion: heuristically enable / disable gravity fusion
 	void controlGravityFusion(const imuSample &imu_delayed);
@@ -1175,11 +1087,19 @@ private:
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);
 
+	// load and save mag field state covariance data for re-use
+	void loadMagCovData();
+	void saveMagCovData();
+
 	void resetGyroBiasZCov();
 
 	// uncorrelate quaternion states from other states
 	void uncorrelateQuatFromOtherStates();
 
+	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
+	// sensor measurement
+	float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
+
 	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
 	{
 		return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us);
@@ -1200,7 +1120,7 @@ private:
 	void resetFakePosFusion();
 	void stopFakePosFusion();
 
-	void setVelPosStatus(const int state_index, const bool healthy);
+	void setVelPosStatus(const int index, const bool healthy);
 
 	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
 	// yaw : Euler yaw angle (rad)
@@ -1215,6 +1135,7 @@ private:
 	uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
 	uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};
 
+	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
 	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -67,11 +67,11 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
 	_state.vel.xy() = new_horz_vel;
 
 	if (PX4_ISFINITE(new_horz_vel_var(0))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
+		P.uncorrelateCovarianceSetVariance<1>(4, math::max(sq(0.01f), new_horz_vel_var(0)));
 	}
 
 	if (PX4_ISFINITE(new_horz_vel_var(1))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
+		P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1)));
 	}
 
 	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
@@ -97,7 +97,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 	_state.vel(2) = new_vert_vel;
 
 	if (PX4_ISFINITE(new_vert_vel_var)) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
+		P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var));
 	}
 
 	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
@@ -133,11 +133,11 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 	_state.pos.xy() = new_horz_pos;
 
 	if (PX4_ISFINITE(new_horz_pos_var(0))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
+		P.uncorrelateCovarianceSetVariance<1>(7, math::max(sq(0.01f), new_horz_pos_var(0)));
 	}
 
 	if (PX4_ISFINITE(new_horz_pos_var(1))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
+		P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1)));
 	}
 
 	_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
@@ -180,7 +180,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 
 	if (PX4_ISFINITE(new_vert_pos_var)) {
 		// the state variance is the same as the observation
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
+		P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var));
 	}
 
 	const float delta_z = new_vert_pos - old_vert_pos;
@@ -200,9 +200,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 
 	_state_reset_status.reset_count.posD++;
 
-#if defined(CONFIG_EKF2_BAROMETER)
 	_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -236,16 +234,13 @@ void Ekf::constrainStates()
 	_state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit);
 
 	_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
-#endif // CONFIG_EKF2_MAGNETOMETER
-
 	_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
 }
 
-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
 {
+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 	if (_control_status.flags.wind && local_position_is_valid()) {
 		// calculate static pressure error = Pmeas - Ptruth
 		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
@@ -273,11 +268,11 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
 		// correct baro measurement using pressure error estimate and assuming sea level gravity
 		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
 	}
+#endif // CONFIG_EKF2_BARO_COMPENSATION
 
 	// otherwise return the uncorrected baro measurement
 	return baro_alt_uncompensated;
 }
-#endif // CONFIG_EKF2_BARO_COMPENSATION
 
 // calculate the earth rotation vector
 Vector3f Ekf::calcEarthRateNED(float lat_rad) const
@@ -365,18 +360,44 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
 }
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+void Ekf::getFlowInnov(float flow_innov[2]) const
+{
+	flow_innov[0] = _aid_src_optical_flow.innovation[0];
+	flow_innov[1] = _aid_src_optical_flow.innovation[1];
+}
+
+void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
+{
+	flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0];
+	flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
+}
+
+void Ekf::getTerrainFlowInnov(float flow_innov[2]) const
+{
+	flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0];
+	flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1];
+}
+
+void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const
+{
+	flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
+	flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
+}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 // get the state vector at the delayed time horizon
 matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 {
 	matrix::Vector<float, 24> state;
-	state.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) = _state.quat_nominal;
-	state.slice<State::vel.dof, 1>(State::vel.idx, 0) = _state.vel;
-	state.slice<State::pos.dof, 1>(State::pos.idx, 0) = _state.pos;
-	state.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) = _state.gyro_bias;
-	state.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) = _state.accel_bias;
-	state.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = _state.mag_I;
-	state.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = _state.mag_B;
-	state.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = _state.wind_vel;
+	state.slice<4, 1>(0, 0) = _state.quat_nominal;
+	state.slice<3, 1>(4, 0) = _state.vel;
+	state.slice<3, 1>(7, 0) = _state.pos;
+	state.slice<3, 1>(10, 0) = _state.gyro_bias;
+	state.slice<3, 1>(13, 0) = _state.accel_bias;
+	state.slice<3, 1>(16, 0) = _state.mag_I;
+	state.slice<3, 1>(19, 0) = _state.mag_B;
+	state.slice<2, 1>(22, 0) = _state.wind_vel;
 	return state;
 }
 
@@ -466,7 +487,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// report absolute accuracy taking into account the uncertainty in location of the origin
 	// If not aiding, return 0 for horizontal position estimate as no estimate is available
 	// TODO - allow for baro drift in vertical position error
-	float hpos_err = sqrtf(P.trace<2>(State::pos.idx) + sq(_gpos_origin_eph));
+	float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gpos_origin_eph));
 
 	// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
@@ -484,14 +505,14 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	}
 
 	*ekf_eph = hpos_err;
-	*ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2) + sq(_gpos_origin_epv));
+	*ekf_epv = sqrtf(P(9, 9) + sq(_gpos_origin_epv));
 }
 
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
 void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
 	// TODO - allow for baro drift in vertical position error
-	float hpos_err = sqrtf(P.trace<2>(State::pos.idx));
+	float hpos_err = sqrtf(P(7, 7) + P(8, 8));
 
 	// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
@@ -509,13 +530,13 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	}
 
 	*ekf_eph = hpos_err;
-	*ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2));
+	*ekf_epv = sqrtf(P(9, 9));
 }
 
 // get the 1-sigma horizontal and vertical velocity uncertainty
 void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 {
-	float hvel_err = sqrtf(P.trace<2>(State::vel.idx));
+	float hvel_err = sqrtf(P(4, 4) + P(5, 5));
 
 	// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
@@ -548,7 +569,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	}
 
 	*ekf_evh = hvel_err;
-	*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
+	*ekf_evv = sqrtf(P(6, 6));
 }
 
 /*
@@ -581,8 +602,9 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		*hagl_min = rangefinder_hagl_min;
 		*hagl_max = rangefinder_hagl_max;
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// Keep within flow AND range sensor limits when exclusively using optical flow
 	const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
 
@@ -600,9 +622,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		*hagl_min = flow_hagl_min;
 		*hagl_max = flow_hagl_max;
 	}
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#endif // CONFIG_EKF2_RANGE_FINDER
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 }
 
 void Ekf::resetImuBias()
@@ -618,10 +638,10 @@ void Ekf::resetGyroBias()
 
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
+	P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias));
 
 	// Set previous frame values
-	_prev_gyro_bias_var = P.slice<State::gyro_bias.dof, State::gyro_bias.dof>(State::gyro_bias.idx, State::gyro_bias.idx).diag();
+	_prev_gyro_bias_var = P.slice<3, 3>(10, 10).diag();
 }
 
 void Ekf::resetAccelBias()
@@ -631,10 +651,10 @@ void Ekf::resetAccelBias()
 
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
+	P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias));
 
 	// Set previous frame values
-	_prev_accel_bias_var = P.slice<State::accel_bias.dof, State::accel_bias.dof>(State::accel_bias.idx, State::accel_bias.idx).diag();
+	_prev_accel_bias_var = P.slice<3, 3>(13, 13).diag();
 }
 
 // get EKF innovation consistency check status information comprising of:
@@ -651,7 +671,6 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	// return the largest magnetometer innovation test ratio
 	mag = 0.f;
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_control_status.flags.mag_hdg) {
 		mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
 	}
@@ -659,7 +678,6 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	if (_control_status.flags.mag_3D) {
 		mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
 	}
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 	if (_control_status.flags.gps_yaw) {
@@ -708,12 +726,10 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	float hgt_sum = 0.f;
 	int n_hgt_sources = 0;
 
-#if defined(CONFIG_EKF2_BAROMETER)
 	if (_control_status.flags.baro_hgt) {
 		hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio);
 		n_hgt_sources++;
 	}
-#endif // CONFIG_EKF2_BAROMETER
 
 	if (_control_status.flags.gps_hgt) {
 		hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio);
@@ -746,23 +762,11 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	tas = sqrtf(_aid_src_airspeed.test_ratio);
 #endif // CONFIG_EKF2_AIRSPEED
 
-	hagl = NAN;
-#if defined(CONFIG_EKF2_TERRAIN)
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	if (_hagl_sensor_status.flags.range_finder) {
-		// return the terrain height innovation test ratio
-		hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio);
-	}
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// return the terrain height innovation test ratio
+	hagl = sqrtf(_hagl_test_ratio);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	if (_hagl_sensor_status.flags.flow) {
-		// return the terrain height innovation test ratio
-		hagl = sqrtf(math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]));
-	}
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-#endif // CONFIG_EKF2_TERRAIN
-
 #if defined(CONFIG_EKF2_SIDESLIP)
 	// return the synthetic sideslip innovation test ratio
 	beta = sqrtf(_aid_src_sideslip.test_ratio);
@@ -780,16 +784,13 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
 	soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
-#if defined(CONFIG_EKF2_TERRAIN)
 	soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
-#endif // CONFIG_EKF2_TERRAIN
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
 
 	bool mag_innov_good = true;
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_control_status.flags.mag_hdg) {
 		if (_aid_src_mag_heading.test_ratio < 1.f) {
 			mag_innov_good = false;
@@ -800,7 +801,6 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 			mag_innov_good = false;
 		}
 	}
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 	const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
 	const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
@@ -812,22 +812,23 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 
 void Ekf::fuse(const Vector24f &K, float innovation)
 {
-	_state.quat_nominal -= K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * innovation;
+	_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
 	_state.quat_nominal.normalize();
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
-	_state.vel -= K.slice<State::vel.dof, 1>(State::vel.idx, 0) * innovation;
-	_state.pos -= K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation;
-	_state.gyro_bias -= K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) * innovation;
-	_state.accel_bias -= K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) * innovation;
-	_state.mag_I -= K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation;
-	_state.mag_B -= K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation;
-	_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
+	_state.vel        -= K.slice<3, 1>(4, 0)  * innovation;
+	_state.pos        -= K.slice<3, 1>(7, 0)  * innovation;
+	_state.gyro_bias  -= K.slice<3, 1>(10, 0) * innovation;
+	_state.accel_bias -= K.slice<3, 1>(13, 0) * innovation;
+	_state.mag_I      -= K.slice<3, 1>(16, 0) * innovation;
+	_state.mag_B      -= K.slice<3, 1>(19, 0) * innovation;
+	_state.wind_vel   -= K.slice<2, 1>(22, 0) * innovation;
 }
 
 void Ekf::uncorrelateQuatFromOtherStates()
 {
-	P.uncorrelateCovarianceBlock<State::quat_nominal.dof>(State::quat_nominal.idx);
+	P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;
+	P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
 }
 
 void Ekf::updateDeadReckoningStatus()
@@ -916,19 +917,15 @@ float Ekf::getYawVar() const
 	return yaw_var;
 }
 
-#if defined(CONFIG_EKF2_BAROMETER)
 void Ekf::updateGroundEffect()
 {
 	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
-#if defined(CONFIG_EKF2_TERRAIN)
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
 			float height = _terrain_vpos - _state.pos(2);
 			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
 
-		} else
-#endif // CONFIG_EKF2_TERRAIN
-		if (_control_status.flags.gnd_effect) {
+		} else if (_control_status.flags.gnd_effect) {
 			// Turn off ground effect compensation if it times out
 			if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
 				_control_status.flags.gnd_effect = false;
@@ -939,7 +936,6 @@ void Ekf::updateGroundEffect()
 		_control_status.flags.gnd_effect = false;
 	}
 }
-#endif // CONFIG_EKF2_BAROMETER
 
 void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 {
@@ -949,6 +945,26 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 	P.slice<4, 4>(0, 0) += q_cov;
 }
 
+void Ekf::saveMagCovData()
+{
+	// save the NED axis covariance sub-matrix
+	_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
+
+	// save the XYZ body covariance sub-matrix
+	_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
+}
+
+void Ekf::loadMagCovData()
+{
+	// re-instate the NED axis covariance sub-matrix
+	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
+	P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
+
+	// re-instate the XYZ body axis covariance sub-matrix
+	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
+	P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
+}
+
 void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 {
 	// save a copy of the quaternion state for later use in calculating the amount of reset change
@@ -1069,5 +1085,5 @@ void Ekf::resetWindToZero()
 	_state.wind_vel.setZero();
 
 	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, _params.initial_wind_uncertainty);
+	P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
 }

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -47,12 +47,8 @@
 EstimatorInterface::~EstimatorInterface()
 {
 	delete _gps_buffer;
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	delete _mag_buffer;
-#endif // CONFIG_EKF2_MAGNETOMETER
-#if defined(CONFIG_EKF2_BAROMETER)
 	delete _baro_buffer;
-#endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	delete _range_buffer;
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -106,7 +102,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 #endif // CONFIG_EKF2_DRAG_FUSION
 }
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 void EstimatorInterface::setMagData(const magSample &mag_sample)
 {
 	if (!_initialised) {
@@ -142,7 +137,6 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 		ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 void EstimatorInterface::setGpsData(const gpsMessage &gps)
 {
@@ -223,7 +217,6 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 	}
 }
 
-#if defined(CONFIG_EKF2_BAROMETER)
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 {
 	if (!_initialised) {
@@ -259,7 +252,6 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 		ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
-#endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
@@ -715,17 +707,13 @@ void EstimatorInterface::print_status()
 		printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
 	}
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_mag_buffer) {
 		printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
 	}
-#endif // CONFIG_EKF2_MAGNETOMETER
 
-#if defined(CONFIG_EKF2_BAROMETER)
 	if (_baro_buffer) {
 		printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
 	}
-#endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_range_buffer) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -86,15 +86,11 @@ public:
 
 	void setIMUData(const imuSample &imu_sample);
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	void setMagData(const magSample &mag_sample);
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 	void setGpsData(const gpsMessage &gps);
 
-#if defined(CONFIG_EKF2_BAROMETER)
 	void setBaroData(const baroSample &baro_sample);
-#endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	void setAirspeedData(const airspeedSample &airspeed_sample);
@@ -235,7 +231,6 @@ public:
 	Vector3f getPosition() const { return _output_predictor.getPosition(); }
 	const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
 	// At the next startup, set param.mag_declination_deg to the value saved
@@ -268,7 +263,6 @@ public:
 		strength_gs = _mag_strength;
 		strength_ref_gs = _mag_strength_gps;
 	}
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 	// get EKF mode status
 	const filter_control_status_u &control_status() const { return _control_status; }
@@ -418,11 +412,8 @@ protected:
 	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
 	RingBuffer<magSample> *_mag_buffer{nullptr};
-	uint64_t _time_last_mag_buffer_push{0};
-#endif // CONFIG_EKF2_MAGNETOMETER
+	RingBuffer<baroSample> *_baro_buffer{nullptr};
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
@@ -438,11 +429,8 @@ protected:
 	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
 	uint64_t _time_last_gps_buffer_push{0};
-
-#if defined(CONFIG_EKF2_BAROMETER)
-	RingBuffer<baroSample> *_baro_buffer{nullptr};
+	uint64_t _time_last_mag_buffer_push{0};
 	uint64_t _time_last_baro_buffer_push{0};
-#endif // CONFIG_EKF2_BAROMETER
 
 	uint64_t _time_last_gnd_effect_on{0};
 

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -94,7 +94,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 	if (measurement_valid && quality_sufficient) {
 		bias_est.setMaxStateNoise(sqrtf(measurement_var));
 		bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
-		bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+		bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
 	}
 
 	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -164,7 +164,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 	if (measurement_valid && quality_sufficient) {
 		_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
 		_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
-		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + P.slice<2, 2>(State::pos.idx, State::pos.idx).diag());
+		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(P(7, 7), P(8, 8)));
 	}
 
 	if (!measurement_valid) {

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -158,6 +158,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 
 			} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
 				// turn on fusion of external vision yaw measurements and disable all other heading fusion
+				stopMagFusion();
 				stopGpsYawFusion();
 				stopGpsFusion();
 

src/modules/ekf2/EKF/gnss_height_control.cpp

@@ -82,7 +82,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 		if (measurement_valid && gps_checks_passing && !gps_checks_failing) {
 			bias_est.setMaxStateNoise(sqrtf(measurement_var));
 			bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9));
 		}
 
 		// determine if we should use height aiding

src/modules/ekf2/EKF/gps_checks.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *

src/modules/ekf2/EKF/height_bias_estimator.hpp

@@ -35,8 +35,7 @@
  * @file height_bias_estimator.hpp
  */
 
-#ifndef EKF_HEIGHT_BIAS_ESTIMATOR_HPP
-#define EKF_HEIGHT_BIAS_ESTIMATOR_HPP
+#pragma once
 
 #include "bias_estimator.hpp"
 
@@ -73,5 +72,3 @@ private:
 
 	bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool
 };
-
-#endif // !EKF_HEIGHT_BIAS_ESTIMATOR_HPP