WIP: pmw3901 rewrite

- remove internal accumulation and publish every valid raw sample synchronized with sensor
 - store timestamp_sample from motion interrupt
 - improve timing requirements from datasheet (minimum delays after register read/write)

ROMFS/cannode/init.d/rcS

@@ -86,10 +86,17 @@ unset BOARD_RC_SENSORS
 . ${R}etc/init.d/rc.serial
 
 # Check for flow sensor
-if param compare SENS_EN_PX4FLOW 1
+if param compare -s SENS_EN_PX4FLOW 1
 then
 	px4flow start -X &
 fi
 
+if param compare -s IMU_GYRO_CAL_EN 1
+then
+	gyro_calibration start
+fi
+
+sensors start
+
 uavcannode start
 unset R

ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup

@@ -9,7 +9,6 @@
 
 # EKF2
 param set-default EKF2_AID_MASK 2
-param set-default SENS_FLOW_ROT 0
 
 # LPE: Flow-only mode
 param set-default LPE_FUSION 242

ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision

@@ -104,7 +104,6 @@ param set-default SDLOG_PROFILE 131
 param set-default SENS_CM8JL65_CFG 104
 param set-default SENS_FLOW_MAXHGT 25
 param set-default SENS_FLOW_MINHGT 0.5
-param set-default SENS_FLOW_ROT 0
 param set-default IMU_GYRO_CUTOFF 100
 param set-default SENS_EN_PMW3901 1
 

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -174,7 +174,6 @@ param set-default RC1_TRIM          1000
 param set-default SENS_FLOW_MAXR   7.4
 param set-default SENS_FLOW_MINHGT 0.15
 param set-default SENS_FLOW_MAXHGT 5.0
-param set-default SENS_FLOW_ROT    0
 
 # ignore the SD card errors and use normal startup sound
 set STARTUP_TUNE "1"

Tools/kconfig/board_cmake_to_kconfig.py

@@ -1,72 +0,0 @@
-#!/bin/python3
-
-import parse_cmake.parsing as cmp
-import glob
-import pprint
-import re
-import os
-
-__location__ = os.path.realpath(
-    os.path.join(os.getcwd(), os.path.dirname(__file__)))
-
-serial_regex = r"(\D\D\D\d):(/dev/ttyS\d+)"
-io_regex = r"IO (.*)"
-romfs_regex = r"ROMFSROOT (.*)"
-arch_regex = r"ARCHITECTURE (.*)"
-toolchain_regex = r"TOOLCHAIN (.*)"
-
-
-
-def stripComments(code):
-    code = str(code)
-    return re.sub(r'(?m) *#.*\n?', '', code)
-
-lut = {}
-with open(os.path.join(__location__, "cmake_kconfig_lut.txt"),'r') as lookup:
-    for line in lookup:
-        if ',' in line:
-            key, value = line.strip().split(',')
-            lut[key] = value
-
-#for name in glob.glob('boards/*/*/*.cmake'):
-px4_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '../../'))
-
-for name in glob.glob(px4_dir + '/boards/*/*/*.cmake'):
-    print(name)
-    with open(name, 'r') as f:
-        romfs_set = False
-        w = open(name.replace(".cmake",".px4board"), "w")
-        for line in f:
-            clean_line = stripComments(line.strip())
-            value = lut.get(clean_line)
-            if value is not None:
-                print(value, file=w)
-                print(value)
-            else:
-                matches = re.finditer(serial_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_SERIAL_" + match.groups()[0] + "=\"" + match.groups()[1] + "\"")
-                    print("CONFIG_BOARD_SERIAL_" + match.groups()[0] + "=\"" + match.groups()[1] + "\"", file=w)
-                matches = re.finditer(io_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_IO=\"" + match.groups()[0] + "\"")
-                    print("CONFIG_BOARD_IO=\"" + match.groups()[0] + "\"", file=w)
-                matches = re.finditer(romfs_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_ROMFSROOT=\"" + match.groups()[0] + "\"")
-                    print("CONFIG_BOARD_ROMFSROOT=\"" + match.groups()[0] + "\"", file=w)
-                    romfs_set = True
-                matches = re.finditer(arch_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_ARCHITECTURE=\"" + match.groups()[0] + "\"")
-                    print("CONFIG_BOARD_ARCHITECTURE=\"" + match.groups()[0] + "\"", file=w)
-                matches = re.finditer(toolchain_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_TOOLCHAIN=\"" + match.groups()[0] + "\"")
-                    print("CONFIG_BOARD_TOOLCHAIN=\"" + match.groups()[0] + "\"", file=w)
-
-    if(romfs_set == False):
-        print("CONFIG_BOARD_ROMFSROOT=\"\"", file=w)
-
-
-    w.close()

Tools/kconfig/cmake_kconfig_lut.txt

@@ -1,204 +0,0 @@
-PLATFORM nuttx,CONFIG_PLATFORM_NUTTX=y
-PLATFORM posix,CONFIG_PLATFORM_POSIX=y
-CONSTRAINED_MEMORY,CONFIG_BOARD_CONSTRAINED_MEMORY=y
-CONSTRAINED_FLASH,CONFIG_BOARD_CONSTRAINED_FLASH=y
-NO_HELP,CONFIG_BOARD_NO_HELP=y
-EXTERNAL_METADATA,CONFIG_BOARD_EXTERNAL_METADATA=y
-BUILD_BOOTLOADER,CONFIG_BOARD_BUILD_BOOTLOADER=y
-UAVCAN_INTERFACES 2,CONFIG_BOARD_UAVCAN_INTERFACES=2
-UAVCAN_INTERFACES 1,CONFIG_BOARD_UAVCAN_INTERFACES=1
-UAVCAN_TIMER_OVERRIDE 2,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
-UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=1
-UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=0
-TESTING,CONFIG_BOARD_TESTING=y
-ETHERNET,CONFIG_BOARD_ETHERNET=y
-adc/ads1115,CONFIG_DRIVERS_ADC_ADS1115=y
-adc/board_adc,CONFIG_DRIVERS_ADC_BOARD_ADC=y
-barometer,CONFIG_COMMON_BAROMETERS=y
-barometer/bmp280,CONFIG_DRIVERS_BAROMETER_BMP280=y
-barometer/bmp388,CONFIG_DRIVERS_BAROMETER_BMP388=y
-barometer/dps310,CONFIG_DRIVERS_BAROMETER_DPS310=y
-barometer/lps22hb,CONFIG_DRIVERS_BAROMETER_LPS22HB=y
-barometer/lps25h,CONFIG_DRIVERS_BAROMETER_LPS25H=y
-barometer/lps33hw,CONFIG_DRIVERS_BAROMETER_LPS33HW=y
-barometer/mpl3115a2,CONFIG_DRIVERS_BAROMETER_MPL3115A2=y
-barometer/ms5611,CONFIG_DRIVERS_BAROMETER_MS5611=y
-barometer/tcbp001ta,CONFIG_DRIVERS_BAROMETER_TCBP001TA=y
-batt_smbus,CONFIG_DRIVERS_BATT_SMBUS=y
-bootloaders,CONFIG_DRIVERS_BOOTLOADERS=y
-camera_capture,CONFIG_DRIVERS_CAMERA_CAPTURE=y
-camera_trigger,CONFIG_DRIVERS_CAMERA_TRIGGER=y
-differential_pressure,CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE=y
-distance_sensor,CONFIG_COMMON_DISTANCE_SENSOR=y
-distance_sensor/ll40ls,CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
-distance_sensor/lightware_laser_serial,CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
-distance_sensor/broadcom/afbrs50,CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
-distance_sensor/vl53l0x,CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
-distance_sensor/vl53l1x,CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
-distance_sensor/srf05,CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y
-dshot,CONFIG_DRIVERS_DSHOT=y
-gps,CONFIG_DRIVERS_GPS=y
-heater,CONFIG_DRIVERS_HEATER=y
-imu,CONFIG_COMMON_IMU=y
-imu/adis16477,CONFIG_DRIVERS_IMU_ADIS16477=y
-imu/adis16497,CONFIG_DRIVERS_IMU_ADIS16497=y
-imu/analog_devices/adis16448,CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
-imu/analog_devices/adis16470,CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
-imu/bosch/bmi055,CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
-imu/bosch/bmi088,CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
-imu/fxas21002c,CONFIG_DRIVERS_IMU_FXAS21002C=y
-imu/fxos8701cq,CONFIG_DRIVERS_IMU_FXOS8701CQ=y
-imu/invensense/icm20602,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
-imu/invensense/icm20608g,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y
-imu/invensense/icm20649,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
-imu/invensense/icm20689,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
-imu/invensense/icm20948,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
-imu/invensense/icm40609d,CONFIG_DRIVERS_IMU_INVENSENSE_ICM40609D=y
-imu/invensense/icm42605,CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
-imu/invensense/icm42688p,CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
-imu/invensense/mpu6000,CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
-imu/invensense/mpu6500,CONFIG_DRIVERS_IMU_INVENSENSE_MPU6500=y
-imu/invensense/mpu9250,CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
-imu/l3gd20,CONFIG_DRIVERS_IMU_L3GD20=y
-imu/lsm303d,CONFIG_DRIVERS_IMU_LSM303D=y
-imu/st,CONFIG_DRIVERS_IMU_ST=y
-irlock,CONFIG_DRIVERS_IRLOCK=y
-lights,CONFIG_COMMON_LIGHT=y
-lights/neopixel,CONFIG_DRIVERS_LIGHTS_NEOPIXEL=y
-lights/rgbled,CONFIG_DRIVERS_LIGHTS_RGBLED=y
-lights/rgbled_ncp5623c,CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
-lights/rgbled_pwm,CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
-magnetometer,CONFIG_COMMON_MAGNETOMETER=y
-magnetometer/akm/ak09916,CONFIG_DRIVERS_MAGNETOMETER_AKM_AK09916=y
-magnetometer/akm/ak8963,CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y
-magnetometer/bosch/bmm150,CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
-magnetometer/hmc5883,CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
-magnetometer/isentek/ist8308,CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
-magnetometer/isentek/ist8310,CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
-magnetometer/lis2mdl,CONFIG_DRIVERS_MAGNETOMETER_LIS2MDL=y
-magnetometer/lis3mdl,CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
-magnetometer/lsm303agr,CONFIG_DRIVERS_MAGNETOMETER_LSM303AGR=y
-magnetometer/lsm9ds1_mag,CONFIG_DRIVERS_MAGNETOMETER_LSM9DS1_MAG=y
-magnetometer/qmc5883l,CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
-magnetometer/rm3100,CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
-magnetometer/vtrantech/vcm1193l,CONFIG_DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L=y
-optical_flow,CONFIG_COMMON_OPTICAL_FLOW=y
-optical_flow/paw3902,CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
-optical_flow/paw3901,CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y
-optical_flow/px4flow,CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW=y
-optical_flow/thoneflow,CONFIG_DRIVERS_OPTICAL_FLOW_THONEFLOW=y
-osd,CONFIG_DRIVERS_OSD=y
-pca9685,CONFIG_DRIVERS_PCA9685=y
-pca9685_pwm_out,CONFIG_DRIVERS_PCA9685_PWM_OUT=y
-power_monitor/ina226,CONFIG_DRIVERS_POWER_MONITOR_INA226=y
-power_monitor/voxlpm,CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
-pps_capture,CONFIG_DRIVERS_PPS_CAPTURE=y
-protocol_splitter,CONFIG_DRIVERS_PROTOCOL_SPLITTER=y
-pwm_input,CONFIG_DRIVERS_PWM_INPUT=y
-pwm_out_sim,CONFIG_DRIVERS_PWM_OUT_SIM=y
-pwm_out,CONFIG_DRIVERS_PWM_OUT=y
-px4io,CONFIG_DRIVERS_PX4IO=y
-rc_input,CONFIG_DRIVERS_RC_INPUT=y
-roboclaw,CONFIG_DRIVERS_ROBOCLAW=y
-rpi_rc_in,CONFIG_DRIVERS_RPI_RC_IN=y
-rpm,CONFIG_DRIVERS_RPM=y
-safety_button,CONFIG_DRIVERS_SAFETY_BUTTON=y
-smart_battery/batmon,CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
-spektrum_rc,CONFIG_DRIVERS_SPEKTRUM_RC=y
-telemetry,CONFIG_DRIVERS_TELEMETRY=y
-test_ppm,CONFIG_DRIVERS_TEST_PPM=y
-tone_alarm,CONFIG_DRIVERS_TONE_ALARM=y
-uavcan,CONFIG_DRIVERS_UAVCAN=y
-uavcannode,CONFIG_DRIVERS_UAVCANNODE=y
-uavcannode_gps_demo,CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=y
-airship_att_control,CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y
-airspeed_selector,CONFIG_MODULES_AIRSPEED_SELECTOR=y
-velocity_controller,CONFIG_MODULES_ANGULAR_VELOCITY_CONTROLLER=y
-attitude_estimator_q,CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
-battery_status,CONFIG_MODULES_BATTERY_STATUS=y
-camera_feedback,CONFIG_MODULES_CAMERA_FEEDBACK=y
-commander,CONFIG_MODULES_COMMANDER=y
-control_allocator,CONFIG_MODULES_CONTROL_ALLOCATOR=y
-dataman,CONFIG_MODULES_DATAMAN=y
-ekf2,CONFIG_MODULES_EKF2=y
-esc_battery,CONFIG_MODULES_ESC_BATTERY=y
-events,CONFIG_MODULES_EVENTS=y
-flight_mode_manager,CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
-fw_att_control,CONFIG_MODULES_FW_ATT_CONTROL=y
-fw_pos_control_l1,CONFIG_MODULES_FW_POS_CONTROL_L1=y
-gyro_calibration,CONFIG_MODULES_GYRO_CALIBRATION=y
-gyro_fft,CONFIG_MODULES_GYRO_FFT=y
-land_detector,CONFIG_MODULES_LAND_DETECTOR=y
-landing_target_estimator,CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
-load_mon,CONFIG_MODULES_LOAD_MON=y
-local_position_estimator,CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
-logger,CONFIG_MODULES_LOGGER=y
-mavlink,CONFIG_MODULES_MAVLINK=y
-mc_att_control,CONFIG_MODULES_MC_ATT_CONTROL=y
-mc_hover_thrust_estimator,CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
-mc_pos_control,CONFIG_MODULES_MC_POS_CONTROL=y
-mc_rate_control,CONFIG_MODULES_MC_RATE_CONTROL=y
-micrortps_bridge,CONFIG_MODULES_MICRORTPS_BRIDGE=y
-microdds_client,CONFIG_MODULES_MICRODDS_CLIENT=y
-navigator,CONFIG_MODULES_NAVIGATOR=y
-px4iofirmware,CONFIG_MODULES_PX4IOFIRMWARE=y
-rc_update,CONFIG_MODULES_RC_UPDATE=y
-replay,CONFIG_MODULES_REPLAY=y
-rover_pos_control,CONFIG_MODULES_ROVER_POS_CONTROL=y
-sensors,CONFIG_MODULES_SENSORS=y
-sih,CONFIG_MODULES_SIH=y
-simulator,CONFIG_MODULES_SIMULATOR=y
-temperature_compensation,CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-uuv_att_control,CONFIG_MODULES_UUV_ATT_CONTROL=y
-uuv_pos_control,CONFIG_MODULES_UUV_POS_CONTROL=y
-gimbal,CONFIG_MODULES_GIMBAL=y
-vtol_att_control,CONFIG_MODULES_VTOL_ATT_CONTROL=y
-bl_update,CONFIG_SYSTEMCMDS_BL_UPDATE=y
-dmesg,CONFIG_SYSTEMCMDS_DMESG=y
-dumpfile,CONFIG_SYSTEMCMDS_DUMPFILE=y
-dyn,CONFIG_SYSTEMCMDS_DYN=y
-failure,CONFIG_SYSTEMCMDS_FAILURE=y
-gpio,CONFIG_SYSTEMCMDS_GPIO=y
-hardfault_log,CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
-i2cdetect,CONFIG_SYSTEMCMDS_I2CDETECT=y
-led_control,CONFIG_SYSTEMCMDS_LED_CONTROL=y
-mft,CONFIG_SYSTEMCMDS_MFT=y
-microbench,CONFIG_SYSTEMCMDS_MICROBENCH=y
-mixer,CONFIG_SYSTEMCMDS_MIXER=y
-motor_test,CONFIG_SYSTEMCMDS_MOTOR_TEST=y
-mtd,CONFIG_SYSTEMCMDS_MTD=y
-netman,CONFIG_SYSTEMCMDS_NETMAN=y
-nshterm,CONFIG_SYSTEMCMDS_NSHTERM=y
-param,CONFIG_SYSTEMCMDS_PARAM=y
-perf,CONFIG_SYSTEMCMDS_PERF=y
-pwm,CONFIG_SYSTEMCMDS_PWM=y
-reboot,CONFIG_SYSTEMCMDS_REBOOT=y
-reflect,CONFIG_SYSTEMCMDS_REFLECT=y
-sd_bench,CONFIG_SYSTEMCMDS_SD_BENCH=y
-serial_tet,CONFIG_SYSTEMCMDS_SERIAL_TEST=y
-shutdown,CONFIG_SYSTEMCMDS_SHUTDOWN=y
-system_time,CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-tests,CONFIG_SYSTEMCMDS_TESTS=y
-top,CONFIG_SYSTEMCMDS_TOP=y
-topic_listener,CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-tune_control,CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
-uorb,CONFIG_SYSTEMCMDS_UORB=y
-usb_connected,CONFIG_SYSTEMCMDS_USB_CONNECTED=y
-ver,CONFIG_SYSTEMCMDS_VER=y
-work_queue,CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-dyn_hello,CONFIG_EXAMPLES_DYN_HELLO=y
-fake_gps,CONFIG_EXAMPLES_FAKE_GPS=y
-fake_gyro,CONFIG_EXAMPLES_FAKE_GYRO=y
-fake_imu,CONFIG_EXAMPLES_FAKE_IMU=y
-fake_magnetometer,CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y
-fixedwing_control,CONFIG_EXAMPLES_FIXEDWING_CONTROL=y
-hello,CONFIG_EXAMPLES_HELLO=y
-hwtest,CONFIG_EXAMPLES_HWTEST=y
-matlab_csv_serial,CONFIG_EXAMPLES_MATLAB_CSV_SERIAL=y
-px4_mavlink_debug,CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
-px4_simple_app,CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
-rover_steering_control,CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y
-uuv_example_app,CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y
-work_item,CONFIG_EXAMPLES_WORK_ITEM=y
-add_compile_options(-Wno-narrowing),CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing"
--D__PX4_LINUX,CONFIG_BOARD_LINUX=y

boards/ark/can-flow/default.px4board

@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
@@ -10,7 +11,16 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
+# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/ark/can-flow/init/rc.board_sensors

@@ -3,6 +3,8 @@
 # board sensors init
 #------------------------------------------------------------------------------
 
+param set-default IMU_GYRO_RATEMAX 1000
+
 # Internal SPI
 paw3902 -s start -Y 180
 

boards/ark/can-gps/default.px4board

@@ -4,7 +4,6 @@ CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
-CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -14,5 +13,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/ark/can-rtk-gps/default.px4board

@@ -4,7 +4,6 @@ CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
-CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -14,5 +13,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/bitcraze/crazyflie/default.px4board

@@ -30,6 +30,7 @@ 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_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MFT=y

boards/bitcraze/crazyflie21/default.px4board

@@ -30,6 +30,7 @@ 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_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y

boards/holybro/kakutef7/default.px4board

@@ -32,6 +32,7 @@ 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_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PWM=y

boards/holybro/kakuteh7/default.px4board

@@ -15,10 +15,10 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
+CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
@@ -28,8 +28,8 @@ CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -42,6 +42,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -60,12 +61,12 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y

boards/omnibus/f4sd/default.px4board

@@ -32,6 +32,7 @@ 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_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y

boards/spracing/h7extreme/default.px4board

@@ -41,6 +41,7 @@ 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_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y

msg/CMakeLists.txt

@@ -65,26 +65,25 @@ set(msg_files
 	differential_pressure.msg
 	distance_sensor.msg
 	ekf2_timestamps.msg
-	estimator_gps_status.msg
 	esc_report.msg
 	esc_status.msg
+	estimator_aid_source_1d.msg
+	estimator_aid_source_2d.msg
+	estimator_aid_source_3d.msg
 	estimator_baro_bias.msg
 	estimator_event_flags.msg
+	estimator_gps_status.msg
 	estimator_innovations.msg
-	estimator_optical_flow_vel.msg
 	estimator_selector_status.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg
 	estimator_status.msg
 	estimator_status_flags.msg
-	estimator_aid_source_1d.msg
-	estimator_aid_source_2d.msg
-	estimator_aid_source_3d.msg
 	event.msg
-	follow_target_status.msg
+	failure_detector_status.msg
 	follow_target.msg
 	follow_target_estimator.msg
-	failure_detector_status.msg
+	follow_target_status.msg
 	generator_status.msg
 	geofence_result.msg
 	gimbal_device_attitude_status.msg
@@ -99,8 +98,8 @@ set(msg_files
 	heater_status.msg
 	home_position.msg
 	hover_thrust_estimate.msg
-	internal_combustion_engine_status.msg
 	input_rc.msg
+	internal_combustion_engine_status.msg
 	iridiumsbd_status.msg
 	irlock_report.msg
 	landing_gear.msg
@@ -123,17 +122,16 @@ set(msg_files
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg
-	optical_flow.msg
 	orbit_status.msg
 	parameter_update.msg
 	ping.msg
-	pps_capture.msg
 	position_controller_landing_status.msg
 	position_controller_status.msg
 	position_setpoint.msg
 	position_setpoint_triplet.msg
 	power_button_state.msg
 	power_monitor.msg
+	pps_capture.msg
 	pwm_input.msg
 	px4io_status.msg
 	radio_status.msg
@@ -148,17 +146,18 @@ set(msg_files
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
-	sensor_gps.msg
 	sensor_gnss_relative.msg
+	sensor_gps.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
 	sensor_gyro_fifo.msg
 	sensor_hygrometer.msg
 	sensor_mag.msg
+	sensor_optical_flow.msg
 	sensor_preflight_mag.msg
 	sensor_selection.msg
-	sensors_status_imu.msg
 	sensors_status.msg
+	sensors_status_imu.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg
@@ -176,8 +175,8 @@ set(msg_files
 	uavcan_parameter_value.msg
 	ulog_stream.msg
 	ulog_stream_ack.msg
-	uwb_grid.msg
 	uwb_distance.msg
+	uwb_grid.msg
 	vehicle_acceleration.msg
 	vehicle_air_data.msg
 	vehicle_angular_acceleration.msg
@@ -198,6 +197,8 @@ set(msg_files
 	vehicle_local_position_setpoint.msg
 	vehicle_magnetometer.msg
 	vehicle_odometry.msg
+	vehicle_optical_flow.msg
+	vehicle_optical_flow_vel.msg
 	vehicle_rates_setpoint.msg
 	vehicle_roi.msg
 	vehicle_status.msg

msg/estimator_optical_flow_vel.msg

@@ -1,8 +0,0 @@
-uint64 timestamp			# time since system start (microseconds)
-uint64 timestamp_sample			# the timestamp of the raw data (microseconds)
-
-float32[2] vel_body			# velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
-float32[2] vel_ne			# same as vel_body but in local frame (m/s)
-float32[2] flow_uncompensated_integral	# integrated optical flow measurement (rad)
-float32[2] flow_compensated_integral	# integrated optical flow measurement compensated for angular motion (rad)
-float32[3] gyro_rate_integral		# gyro measurement integrated to flow rate and synchronized with flow measurements (rad)

msg/optical_flow.msg

@@ -1,29 +0,0 @@
-# Optical flow in XYZ body frame in SI units.
-# http://en.wikipedia.org/wiki/International_System_of_Units
-
-uint64 timestamp		# time since system start (microseconds)
-
-uint8 sensor_id			# id of the sensor emitting the flow value
-float32 pixel_flow_x_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis
-float32 pixel_flow_y_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis
-float32 gyro_x_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 gyro_y_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 gyro_z_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 ground_distance_m	# Altitude / distance to ground in meters
-uint32 integration_timespan	# accumulation timespan in microseconds
-uint32 time_since_last_sonar_update	# time since last sonar update in microseconds
-uint16 frame_count_since_last_readout	# number of accumulated frames in timespan
-int16 gyro_temperature	# Temperature * 100 in centi-degrees Celsius
-uint8 quality	# Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
-
-float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably
-float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably
-float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably
-
-
-uint8 MODE_UNKNOWN        = 0
-uint8 MODE_BRIGHT         = 1
-uint8 MODE_LOWLIGHT       = 2
-uint8 MODE_SUPER_LOWLIGHT = 3
-
-uint8 mode

msg/sensor_optical_flow.msg

@@ -0,0 +1,27 @@
+uint64 timestamp               # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id               # unique device ID for the sensor that does not change between power cycles
+
+float32[2] pixel_flow          # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis
+
+float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+bool delta_angle_available
+
+uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
+
+uint8 quality                  # quality, 0: bad quality, 255: maximum quality
+
+uint32 error_count
+
+float32 max_flow_rate          # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
+
+float32 min_ground_distance    # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance    # (meters) Maximum distance from ground at which the optical flow sensor operates reliably
+
+uint8 MODE_UNKNOWN        = 0
+uint8 MODE_BRIGHT         = 1
+uint8 MODE_LOWLIGHT       = 2
+uint8 MODE_SUPER_LOWLIGHT = 3
+
+uint8 mode

msg/tools/urtps_bridge_topics.yaml

@@ -40,7 +40,7 @@ rtps:
     receive: true
   - msg:     offboard_control_mode
     receive: true
-  - msg:     optical_flow
+  - msg:     sensor_optical_flow
     receive: true
   - msg:     position_setpoint
     receive: true

msg/vehicle_optical_flow.msg

@@ -0,0 +1,19 @@
+# Optical flow in XYZ body frame in SI units.
+
+uint64 timestamp               # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id               # unique device ID for the sensor that does not change between power cycles
+
+float32[2] pixel_flow          # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis
+
+float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+
+uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
+
+uint8 quality                  # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
+
+float32 max_flow_rate          # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
+
+float32 min_ground_distance    # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance    # (meters) Maximum distance from ground at which the optical flow sensor operates reliably

msg/vehicle_optical_flow_vel.msg

@@ -0,0 +1,13 @@
+uint64 timestamp                       # time since system start (microseconds)
+uint64 timestamp_sample                # the timestamp of the raw data (microseconds)
+
+float32[2] vel_body                    # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
+float32[2] vel_ne                      # same as vel_body but in local frame (m/s)
+
+float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad)
+float32[2] flow_compensated_integral   # integrated optical flow measurement compensated for angular motion (rad)
+
+float32[3] gyro_rate                   # gyro measurement synchronized with flow measurements (rad/s)
+float32[3] gyro_rate_integral          # gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
+
+# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel

src/drivers/optical_flow/paa3905/PAA3905.cpp

@@ -43,6 +43,10 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio)
 {
+	if (_drdy_gpio != 0) {
+		_no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt");
+	}
+
 	float yaw_rotation_degrees = (float)config.custom1;
 
 	if (yaw_rotation_degrees >= 0.f) {
@@ -52,27 +56,21 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
 		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};
 
 	} else {
-		// otherwise use the parameter SENS_FLOW_ROT
-		param_t rot = param_find("SENS_FLOW_ROT");
-		int32_t val = 0;
-
-		if (param_get(rot, &val) == PX4_OK) {
-			_rotation = get_rot_matrix((enum Rotation)val);
-
-		} else {
-			_rotation.identity();
-		}
+		_rotation.identity();
 	}
 }
 
 PAA3905::~PAA3905()
 {
 	// free perf counters
-	perf_free(_sample_perf);
+	perf_free(_cycle_perf);
 	perf_free(_interval_perf);
-	perf_free(_comms_errors);
+	perf_free(_reset_perf);
 	perf_free(_false_motion_perf);
-	perf_free(_register_write_fail_perf);
+	perf_free(_mode_change_bright_perf);
+	perf_free(_mode_change_low_light_perf);
+	perf_free(_mode_change_super_low_light_perf);
+	perf_free(_no_motion_interrupt_perf);
 }
 
 int PAA3905::init()
@@ -84,35 +82,35 @@ int PAA3905::init()
 
 	Configure();
 
-	_previous_collect_timestamp = hrt_absolute_time();
-
 	return PX4_OK;
 }
 
 int PAA3905::probe()
 {
-	const uint8_t Product_ID = RegisterRead(Register::Product_ID);
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t Product_ID = RegisterRead(Register::Product_ID);
+		const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
+		const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
 
-	if (Product_ID != PRODUCT_ID) {
-		PX4_ERR("Product_ID: %X", Product_ID);
-		return PX4_ERROR;
+		if (Product_ID != PRODUCT_ID) {
+			PX4_ERR("Product_ID: %X", Product_ID);
+			break;
+		}
+
+		if (Revision_ID != REVISION_ID) {
+			PX4_ERR("Revision_ID: %X", Revision_ID);
+			break;
+		}
+
+		if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
+			PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
+			break;
+		}
+
+		return PX4_OK;
 	}
 
-	const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
-
-	if (Revision_ID != REVISION_ID) {
-		PX4_ERR("Revision_ID: %X", Revision_ID);
-		return PX4_ERROR;
-	}
-
-	const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
-
-	if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
-		PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
@@ -123,12 +121,14 @@ int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void PAA3905::DataReady()
 {
+	_drdy_timestamp_sample.store(hrt_absolute_time());
 	ScheduleNow();
 }
 
 bool PAA3905::DataReadyInterruptConfigure()
 {
 	if (_drdy_gpio == 0) {
+		_data_ready_interrupt_enabled = false;
 		return false;
 	}
 
@@ -144,12 +144,12 @@ bool PAA3905::DataReadyInterruptConfigure()
 
 bool PAA3905::DataReadyInterruptDisable()
 {
+	_data_ready_interrupt_enabled = false;
+
 	if (_drdy_gpio == 0) {
 		return false;
 	}
 
-	_data_ready_interrupt_enabled = false;
-
 	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }
 
@@ -159,8 +159,10 @@ void PAA3905::exit_and_cleanup()
 	I2CSPIDriverBase::exit_and_cleanup();
 }
 
-void PAA3905::Configure()
+void PAA3905::Reset()
 {
+	perf_count(_reset_perf);
+
 	DataReadyInterruptDisable();
 	ScheduleClear();
 
@@ -169,274 +171,312 @@ void PAA3905::Configure()
 	px4_usleep(1000);
 	_last_reset = hrt_absolute_time();
 
-	StandardDetectionSetting();
-	ModeAuto012();
+	_discard_reading = 3;
 
-	CheckMode();
+	// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
+	RegisterRead(0x02);
+	RegisterRead(0x03);
+	RegisterRead(0x04);
+	RegisterRead(0x05);
+	RegisterRead(0x06);
+}
 
-	switch (_mode) {
-	case Mode::Bright:
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
-		break;
+void PAA3905::Configure()
+{
+	Reset();
 
-	case Mode::LowLight:
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;
-		break;
+	ConfigureStandardDetectionSetting();
 
-	case Mode::SuperLowLight:
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
-		break;
-	}
+	ConfigureAutomaticModeSwitching();
 
 	EnableLed();
 
-	_discard_reading = 3;
-	_valid_count = 0;
+	// Read Register 0x15. Check Bit [7:6] for AMS mode
+	const uint8_t Observation = RegisterRead(Register::Observation);
+	UpdateMode(Observation);
 
 	if (DataReadyInterruptConfigure()) {
-		// backup schedule as a watchdog timeout
-		ScheduleDelayed(_scheduled_interval_us * 2);
+		// backup schedule
+		ScheduleDelayed(500_ms);
 
 	} else {
-		ScheduleOnInterval(_scheduled_interval_us);
+		ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
 	}
 }
 
-void PAA3905::CheckMode()
+void PAA3905::ConfigureStandardDetectionSetting()
 {
-	// Read Register 0x15. Check Bit [7:6] for AMS mode and store it into a variable.
-	const uint8_t Observation = RegisterRead(Register::Observation);
+	// Standard Detection Setting is recommended for general tracking operations. In this mode, the chip can detect
+	// when it is operating over striped, checkerboard, and glossy tile surfaces where tracking performance is
+	// compromised.
 
-	// Bit [7:6] AMS mode
-	const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5;
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x51, 0xFF);
+	RegisterWrite(0x4E, 0x2A);
+	RegisterWrite(0x66, 0x3E);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x7E, 0x71);
+	RegisterWrite(0x55, 0x00);
+	RegisterWrite(0x59, 0x00);
+	RegisterWrite(0x6F, 0x2C);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x4D, 0xAC);
+	RegisterWrite(0x4E, 0x32);
+	RegisterWrite(0x7F, 0x09);
+	RegisterWrite(0x5C, 0xAF);
+	RegisterWrite(0x5F, 0xAF);
+	RegisterWrite(0x70, 0x08);
+	RegisterWrite(0x71, 0x04);
+	RegisterWrite(0x72, 0x06);
+	RegisterWrite(0x74, 0x3C);
+	RegisterWrite(0x75, 0x28);
+	RegisterWrite(0x76, 0x20);
+	RegisterWrite(0x4E, 0xBF);
+	RegisterWrite(0x7F, 0x03);
+	RegisterWrite(0x64, 0x14);
+	RegisterWrite(0x65, 0x0A);
+	RegisterWrite(0x66, 0x10);
+	RegisterWrite(0x55, 0x3C);
+	RegisterWrite(0x56, 0x28);
+	RegisterWrite(0x57, 0x20);
+	RegisterWrite(0x4A, 0x2D);
 
-	if (ams_mode == 0x0) {
-		// Mode 0
-		_mode = Mode::SuperLowLight;
-
-	} else if (ams_mode == 0x1) {
-		// Mode 1
-		_mode = Mode::LowLight;
-
-	} else if (ams_mode == 0x2) {
-		// Mode 2
-		_mode = Mode::SuperLowLight;
-	}
+	RegisterWrite(0x4B, 0x2D);
+	RegisterWrite(0x4E, 0x4B);
+	RegisterWrite(0x69, 0xFA);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x69, 0x1F);
+	RegisterWrite(0x47, 0x1F);
+	RegisterWrite(0x48, 0x0C);
+	RegisterWrite(0x5A, 0x20);
+	RegisterWrite(0x75, 0x0F);
+	RegisterWrite(0x4A, 0x0F);
+	RegisterWrite(0x42, 0x02);
+	RegisterWrite(0x45, 0x03);
+	RegisterWrite(0x65, 0x00);
+	RegisterWrite(0x67, 0x76);
+	RegisterWrite(0x68, 0x76);
+	RegisterWrite(0x6A, 0xC5);
+	RegisterWrite(0x43, 0x00);
+	RegisterWrite(0x7F, 0x06);
+	RegisterWrite(0x4A, 0x18);
+	RegisterWrite(0x4B, 0x0C);
+	RegisterWrite(0x4C, 0x0C);
+	RegisterWrite(0x4D, 0x0C);
+	RegisterWrite(0x46, 0x0A);
+	RegisterWrite(0x59, 0xCD);
+	RegisterWrite(0x7F, 0x0A);
+	RegisterWrite(0x4A, 0x2A);
+	RegisterWrite(0x48, 0x96);
+	RegisterWrite(0x52, 0xB4);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x5B, 0xA0);
 }
 
-void PAA3905::StandardDetectionSetting()
+void PAA3905::ConfigureEnhancedDetectionMode()
 {
-	RegisterWriteVerified(0x7F, 0x00);
-	RegisterWriteVerified(0x51, 0xFF);
-	RegisterWriteVerified(0x4E, 0x2A);
-	RegisterWriteVerified(0x66, 0x3E);
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x7E, 0x71);
-	RegisterWriteVerified(0x55, 0x00);
-	RegisterWriteVerified(0x59, 0x00);
-	RegisterWriteVerified(0x6F, 0x2C);
-	RegisterWriteVerified(0x7F, 0x05);
-	RegisterWriteVerified(0x4D, 0xAC);
-	RegisterWriteVerified(0x4E, 0x32);
-	RegisterWriteVerified(0x7F, 0x09);
-	RegisterWriteVerified(0x5C, 0xAF);
-	RegisterWriteVerified(0x5F, 0xAF);
-	RegisterWriteVerified(0x70, 0x08);
-	RegisterWriteVerified(0x71, 0x04);
-	RegisterWriteVerified(0x72, 0x06);
-	RegisterWriteVerified(0x74, 0x3C);
-	RegisterWriteVerified(0x75, 0x28);
-	RegisterWriteVerified(0x76, 0x20);
-	RegisterWriteVerified(0x4E, 0xBF);
-	RegisterWriteVerified(0x7F, 0x03);
-	RegisterWriteVerified(0x64, 0x14);
-	RegisterWriteVerified(0x65, 0x0A);
-	RegisterWriteVerified(0x66, 0x10);
-	RegisterWriteVerified(0x55, 0x3C);
-	RegisterWriteVerified(0x56, 0x28);
-	RegisterWriteVerified(0x57, 0x20);
-	RegisterWriteVerified(0x4A, 0x2D);
+	// Enhance Detection Setting relatively has better detection sensitivity, it is recommended where yaw motion
+	// detection is required, and also where more sensitive challenging surface detection is required. The recommended
+	// operating height must be greater than 15 cm to avoid false detection on challenging surfaces due to increasing of
+	// sensitivity.
 
-	RegisterWriteVerified(0x4B, 0x2D);
-	RegisterWriteVerified(0x4E, 0x4B);
-	RegisterWriteVerified(0x69, 0xFA);
-	RegisterWriteVerified(0x7F, 0x05);
-	RegisterWriteVerified(0x69, 0x1F);
-	RegisterWriteVerified(0x47, 0x1F);
-	RegisterWriteVerified(0x48, 0x0C);
-	RegisterWriteVerified(0x5A, 0x20);
-	RegisterWriteVerified(0x75, 0x0F);
-	RegisterWriteVerified(0x4A, 0x0F);
-	RegisterWriteVerified(0x42, 0x02);
-	RegisterWriteVerified(0x45, 0x03);
-	RegisterWriteVerified(0x65, 0x00);
-	RegisterWriteVerified(0x67, 0x76);
-	RegisterWriteVerified(0x68, 0x76);
-	RegisterWriteVerified(0x6A, 0xC5);
-	RegisterWriteVerified(0x43, 0x00);
-	RegisterWriteVerified(0x7F, 0x06);
-	RegisterWriteVerified(0x4A, 0x18);
-	RegisterWriteVerified(0x4B, 0x0C);
-	RegisterWriteVerified(0x4C, 0x0C);
-	RegisterWriteVerified(0x4D, 0x0C);
-	RegisterWriteVerified(0x46, 0x0A);
-	RegisterWriteVerified(0x59, 0xCD);
-	RegisterWriteVerified(0x7F, 0x0A);
-	RegisterWriteVerified(0x4A, 0x2A);
-	RegisterWriteVerified(0x48, 0x96);
-	RegisterWriteVerified(0x52, 0xB4);
-	RegisterWriteVerified(0x7F, 0x00);
-	RegisterWriteVerified(0x5B, 0xA0);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x51, 0xFF);
+	RegisterWrite(0x4E, 0x2A);
+	RegisterWrite(0x66, 0x26);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x7E, 0x71);
+	RegisterWrite(0x55, 0x00);
+	RegisterWrite(0x59, 0x00);
+	RegisterWrite(0x6F, 0x2C);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x4D, 0xAC);
+	RegisterWrite(0x4E, 0x65);
+	RegisterWrite(0x7F, 0x09);
+	RegisterWrite(0x5C, 0xAF);
+	RegisterWrite(0x5F, 0xAF);
+	RegisterWrite(0x70, 0x00);
+	RegisterWrite(0x71, 0x00);
+	RegisterWrite(0x72, 0x00);
+	RegisterWrite(0x74, 0x14);
+	RegisterWrite(0x75, 0x14);
+	RegisterWrite(0x76, 0x06);
+	RegisterWrite(0x4E, 0x8F);
+	RegisterWrite(0x7F, 0x03);
+	RegisterWrite(0x64, 0x00);
+	RegisterWrite(0x65, 0x00);
+	RegisterWrite(0x66, 0x00);
+	RegisterWrite(0x55, 0x14);
+	RegisterWrite(0x56, 0x14);
+	RegisterWrite(0x57, 0x06);
+	RegisterWrite(0x4A, 0x20);
+
+	RegisterWrite(0x4B, 0x20);
+	RegisterWrite(0x4E, 0x32);
+	RegisterWrite(0x69, 0xFE);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x69, 0x14);
+	RegisterWrite(0x47, 0x14);
+	RegisterWrite(0x48, 0x1C);
+	RegisterWrite(0x5A, 0x20);
+	RegisterWrite(0x75, 0xE5);
+	RegisterWrite(0x4A, 0x05);
+	RegisterWrite(0x42, 0x04);
+	RegisterWrite(0x45, 0x03);
+	RegisterWrite(0x65, 0x00);
+	RegisterWrite(0x67, 0x50);
+	RegisterWrite(0x68, 0x50);
+	RegisterWrite(0x6A, 0xC5);
+	RegisterWrite(0x43, 0x00);
+	RegisterWrite(0x7F, 0x06);
+	RegisterWrite(0x4A, 0x1E);
+	RegisterWrite(0x4B, 0x1E);
+	RegisterWrite(0x4C, 0x34);
+	RegisterWrite(0x4D, 0x34);
+	RegisterWrite(0x46, 0x32);
+	RegisterWrite(0x59, 0x0D);
+	RegisterWrite(0x7F, 0x0A);
+	RegisterWrite(0x4A, 0x2A);
+	RegisterWrite(0x48, 0x96);
+	RegisterWrite(0x52, 0xB4);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x5B, 0xA0);
 }
 
-void PAA3905::EnhancedDetectionMode()
-{
-	RegisterWriteVerified(0x7F, 0x00);
-	RegisterWriteVerified(0x51, 0xFF);
-	RegisterWriteVerified(0x4E, 0x2A);
-	RegisterWriteVerified(0x66, 0x26);
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x7E, 0x71);
-	RegisterWriteVerified(0x55, 0x00);
-	RegisterWriteVerified(0x59, 0x00);
-	RegisterWriteVerified(0x6F, 0x2C);
-	RegisterWriteVerified(0x7F, 0x05);
-	RegisterWriteVerified(0x4D, 0xAC);
-	RegisterWriteVerified(0x4E, 0x65);
-	RegisterWriteVerified(0x7F, 0x09);
-	RegisterWriteVerified(0x5C, 0xAF);
-	RegisterWriteVerified(0x5F, 0xAF);
-	RegisterWriteVerified(0x70, 0x00);
-	RegisterWriteVerified(0x71, 0x00);
-	RegisterWriteVerified(0x72, 0x00);
-	RegisterWriteVerified(0x74, 0x14);
-	RegisterWriteVerified(0x75, 0x14);
-	RegisterWriteVerified(0x76, 0x06);
-	RegisterWriteVerified(0x4E, 0x8F);
-	RegisterWriteVerified(0x7F, 0x03);
-	RegisterWriteVerified(0x64, 0x00);
-	RegisterWriteVerified(0x65, 0x00);
-	RegisterWriteVerified(0x66, 0x00);
-	RegisterWriteVerified(0x55, 0x14);
-	RegisterWriteVerified(0x56, 0x14);
-	RegisterWriteVerified(0x57, 0x06);
-	RegisterWriteVerified(0x4A, 0x20);
-
-	RegisterWriteVerified(0x4B, 0x20);
-	RegisterWriteVerified(0x4E, 0x32);
-	RegisterWriteVerified(0x69, 0xFE);
-	RegisterWriteVerified(0x7F, 0x05);
-	RegisterWriteVerified(0x69, 0x14);
-	RegisterWriteVerified(0x47, 0x14);
-	RegisterWriteVerified(0x48, 0x1C);
-	RegisterWriteVerified(0x5A, 0x20);
-	RegisterWriteVerified(0x75, 0xE5);
-	RegisterWriteVerified(0x4A, 0x05);
-	RegisterWriteVerified(0x42, 0x04);
-	RegisterWriteVerified(0x45, 0x03);
-	RegisterWriteVerified(0x65, 0x00);
-	RegisterWriteVerified(0x67, 0x50);
-	RegisterWriteVerified(0x68, 0x50);
-	RegisterWriteVerified(0x6A, 0xC5);
-	RegisterWriteVerified(0x43, 0x00);
-	RegisterWriteVerified(0x7F, 0x06);
-	RegisterWriteVerified(0x4A, 0x1E);
-	RegisterWriteVerified(0x4B, 0x1E);
-	RegisterWriteVerified(0x4C, 0x34);
-	RegisterWriteVerified(0x4D, 0x34);
-	RegisterWriteVerified(0x46, 0x32);
-	RegisterWriteVerified(0x59, 0x0D);
-	RegisterWriteVerified(0x7F, 0x0A);
-	RegisterWriteVerified(0x4A, 0x2A);
-	RegisterWriteVerified(0x48, 0x96);
-	RegisterWriteVerified(0x52, 0xB4);
-	RegisterWriteVerified(0x7F, 0x00);
-	RegisterWriteVerified(0x5B, 0xA0);
-}
-
-void PAA3905::ModeAuto012()
+void PAA3905::ConfigureAutomaticModeSwitching()
 {
 	// Automatic switching between Mode 0, 1 and 2:
-	RegisterWriteVerified(0x7F, 0x08);
-	RegisterWriteVerified(0x68, 0x02);
-	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWrite(0x7F, 0x08);
+	RegisterWrite(0x68, 0x02);
+	RegisterWrite(0x7F, 0x00);
+
+	// TODO: for mode 0 and 1 only
+	// Automatic switching between Mode 0 and 1 only:
+	// RegisterWrite(0x7F, 0x08);
+	// RegisterWrite(0x68, 0x01); // different than mode 0,1,2
+	// RegisterWrite(0x7F, 0x00);
 }
 
 void PAA3905::EnableLed()
 {
 	// Enable LED_N controls
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x6F, 0x0C);
-	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x6F, 0x0C);
+	RegisterWrite(0x7F, 0x00);
 }
 
-uint8_t PAA3905::RegisterRead(uint8_t reg, int retries)
+bool PAA3905::UpdateMode(const uint8_t observation)
 {
-	for (int i = 0; i < retries; i++) {
-		px4_udelay(TIME_us_TSRAD);
-		uint8_t cmd[2] {reg, 0};
+	bool mode_changed = false;
 
-		if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) {
-			return cmd[1];
+	// Bit [7:6] AMS mode
+	const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5;
+
+	if (ams_mode == 0x0) {
+		// Mode 0 (Bright)
+		if (_mode != Mode::Bright) {
+			mode_changed = true;
+			perf_count(_mode_change_bright_perf);
 		}
+
+		_mode = Mode::Bright;
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
+
+	} else if (ams_mode == 0x1) {
+		// Mode 1 (LowLight)
+		if (_mode != Mode::LowLight) {
+			mode_changed = true;
+			perf_count(_mode_change_low_light_perf);
+		}
+
+		_mode = Mode::LowLight;
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;
+
+	} else if (ams_mode == 0x2) {
+		// Mode 2 (SuperLowLight)
+		if (_mode != Mode::SuperLowLight) {
+			mode_changed = true;
+			perf_count(_mode_change_super_low_light_perf);
+		}
+
+		_mode = Mode::SuperLowLight;
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
 	}
 
-	perf_count(_comms_errors);
-	return 0;
+	return mode_changed;
+}
+
+uint8_t PAA3905::RegisterRead(uint8_t reg)
+{
+	// tSWR SPI Time Between Write And Read Commands
+	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
+
+	if (elapsed_last_write < TIME_TSWR_us) {
+		px4_udelay(TIME_TSWR_us - elapsed_last_write);
+	}
+
+	// tSRW/tSRR SPI Time Between Read And Subsequent Commands
+	const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time);
+
+	if (elapsed_last_write < TIME_TSRW_TSRR_us) {
+		px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read);
+	}
+
+	uint8_t cmd[2];
+	cmd[0] = DIR_READ(reg);
+	cmd[1] = 0;
+	transfer(&cmd[0], &cmd[0], sizeof(cmd));
+	hrt_store_absolute_time(&_last_read_time);
+
+	return cmd[1];
 }
 
 void PAA3905::RegisterWrite(uint8_t reg, uint8_t data)
 {
+	// tSWW SPI Time Between Write Commands
+	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
+
+	if (elapsed_last_write < TIME_TSWW_us) {
+		px4_udelay(TIME_TSWW_us - elapsed_last_write);
+	}
+
 	uint8_t cmd[2];
 	cmd[0] = DIR_WRITE(reg);
 	cmd[1] = data;
-
-	if (transfer(&cmd[0], nullptr, sizeof(cmd)) != 0) {
-		perf_count(_comms_errors);
-	}
-}
-
-bool PAA3905::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries)
-{
-	for (int i = 0; i < retries; i++) {
-		uint8_t cmd[2];
-		cmd[0] = DIR_WRITE(reg);
-		cmd[1] = data;
-		transfer(&cmd[0], nullptr, sizeof(cmd));
-		px4_udelay(TIME_us_TSWW);
-
-		// read back to verify
-		uint8_t data_read = RegisterRead(reg);
-
-		if (data_read == data) {
-			return true;
-		}
-
-		PX4_DEBUG("Register write failed 0x%02hhX: 0x%02hhX (actual value 0x%02hhX)", reg, data, data_read);
-	}
-
-	perf_count(_register_write_fail_perf);
-
-	return false;
+	transfer(&cmd[0], nullptr, sizeof(cmd));
+	hrt_store_absolute_time(&_last_write_time);
 }
 
 void PAA3905::RunImpl()
 {
-	// backup schedule
-	if (_data_ready_interrupt_enabled) {
-		ScheduleDelayed(_scheduled_interval_us * 2);
-	}
+	perf_begin(_cycle_perf);
+	perf_count(_interval_perf);
 
-	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
-	static constexpr hrt_abstime RESET_TIMEOUT_US = 5_s;
+	const hrt_abstime now = hrt_absolute_time();
 
-	if ((hrt_elapsed_time(&_last_good_publish) > RESET_TIMEOUT_US) && (hrt_elapsed_time(&_last_reset) > RESET_TIMEOUT_US)) {
+	// force reconfigure if we haven't received valid data for quite some time
+	if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) {
 		Configure();
+		perf_end(_cycle_perf);
 		return;
 	}
 
-	perf_begin(_sample_perf);
-	perf_count(_interval_perf);
+	hrt_abstime timestamp_sample = now;
+
+	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);
+
+		if (now < drdy_timestamp_sample + _scheduled_interval_us) {
+			timestamp_sample = drdy_timestamp_sample;
+
+		} else {
+			perf_count(_no_motion_interrupt_perf);
+		}
+
+		// push backup schedule back
+		ScheduleDelayed(500_ms);
+	}
 
 	struct TransferBuffer {
 		uint8_t cmd = Register::Motion_Burst;
@@ -444,37 +484,52 @@ void PAA3905::RunImpl()
 	} buf{};
 	static_assert(sizeof(buf) == (14 + 1));
 
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-
-	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != PX4_OK) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
+	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) {
+		perf_end(_cycle_perf);
 		return;
 	}
 
-	perf_end(_sample_perf);
-
-	const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp;
-
-	// update for next iteration
-	_previous_collect_timestamp = timestamp_sample;
+	hrt_store_absolute_time(&_last_read_time);
 
 	if (_discard_reading > 0) {
 		_discard_reading--;
-		ResetAccumulatedData();
-		_valid_count = 0;
+		perf_end(_cycle_perf);
 		return;
 	}
 
-	CheckMode(); // update _mode variable
+	// Bit [5:0] check if chip is working correctly
+	//  0x3F: chip is working correctly
+	if ((buf.data.Observation & (Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0)) != 0x3F) {
+		// Other value: recommend to issue a software reset
+		Configure();
+		perf_end(_cycle_perf);
+		return;
+	}
+
+	if (UpdateMode(buf.data.Observation)) {
+		// update scheduling if mode changed
+		if (!_data_ready_interrupt_enabled) {
+			ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
+		}
+	}
 
 	// check SQUAL & Shutter values
 	// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
 	// Bright Mode,          SQUAL < 0x19, Shutter ≥ 0x00FF80
 	// Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x00FF80
 	// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x025998
-	const uint32_t shutter = (buf.data.Shutter_Upper << 16) | (buf.data.Shutter_Middle << 8) | buf.data.Shutter_Lower;
 
+	// 23-bit Shutter register
+	const uint8_t Shutter_Lower = buf.data.Shutter_Lower;
+	const uint8_t Shutter_Middle = buf.data.Shutter_Middle;
+	const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
+
+	const uint32_t shutter = (Shutter_Upper << 16) | (Shutter_Middle << 8) | Shutter_Lower;
+
+	// Motion since last report and Surface quality non-zero
+	const bool motion_detected = buf.data.Motion & Motion_Bit::MotionOccurred;
+
+	// Number of Features = SQUAL * 4
 	bool data_valid = (buf.data.SQUAL > 0);
 
 	switch (_mode) {
@@ -513,97 +568,66 @@ void PAA3905::RunImpl()
 	}
 
 	if (data_valid) {
-		const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
-		const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L);
+		// publish sensor_optical_flow
+		sensor_optical_flow_s report{};
+		report.timestamp_sample = timestamp_sample;
+		report.device_id = get_device_id();
 
-		_flow_dt_sum_usec += dt_flow;
-		_flow_sum_x += delta_x_raw;
-		_flow_sum_y += delta_y_raw;
-		_flow_sample_counter++;
-		_flow_quality_sum += buf.data.SQUAL;
+		report.integration_timespan_us = _scheduled_interval_us;
+		report.quality = buf.data.SQUAL;
 
-		_valid_count++;
+		// set specs according to datasheet
+		report.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
+		report.min_ground_distance = 0.08f;    // Datasheet: 80mm
+		report.max_ground_distance = INFINITY; // Datasheet: infinity
 
-	} else {
-		_valid_count = 0;
-		ResetAccumulatedData();
-		return;
+		switch (_mode) {
+		case Mode::Bright:
+			report.mode = sensor_optical_flow_s::MODE_BRIGHT;
+			break;
+
+		case Mode::LowLight:
+			report.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
+			break;
+
+		case Mode::SuperLowLight:
+			report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
+			break;
+		}
+
+		if (motion_detected) {
+			// only populate flow if data valid (motion and quality > 0)
+			const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
+			const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.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};
+
+			report.pixel_flow[0] = pixel_flow_rotated(0) / 500.0f; // proportional factor + convert from pixels to radians
+			report.pixel_flow[1] = pixel_flow_rotated(1) / 500.0f; // proportional factor + convert from pixels to radians
+		}
+
+		report.timestamp = hrt_absolute_time();
+		_sensor_optical_flow_pub.publish(report);
+
+		if (report.quality >= 1) {
+			_last_good_data = report.timestamp_sample;
+		}
 	}
 
-	// returns if the collect time has not been reached
-	if (_flow_dt_sum_usec < COLLECT_TIME) {
-		return;
-	}
-
-	optical_flow_s report{};
-	report.timestamp = timestamp_sample;
-	//report.device_id = get_device_id();
-
-	float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f;	// proportional factor + convert from pixels to radians
-	float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f;	// proportional factor + convert from pixels to radians
-
-	// rotate measurements in yaw from sensor frame to body frame
-	const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f};
-	report.pixel_flow_x_integral = pixel_flow_rotated(0);
-	report.pixel_flow_y_integral = pixel_flow_rotated(1);
-
-	report.frame_count_since_last_readout = _flow_sample_counter; // number of frames
-	report.integration_timespan = _flow_dt_sum_usec;              // microseconds
-
-	report.quality = _flow_quality_sum / _flow_sample_counter;
-
-	// No gyro on this board
-	report.gyro_x_rate_integral = NAN;
-	report.gyro_y_rate_integral = NAN;
-	report.gyro_z_rate_integral = NAN;
-
-	// set (conservative) specs according to datasheet
-	report.max_flow_rate = 7.4f;        // Datasheet: 7.4 rad/s
-	report.min_ground_distance = 0.08f; // Datasheet: 80mm
-	report.max_ground_distance = 30.0f; // Datasheet: infinity
-
-
-	switch (_mode) {
-	case Mode::Bright:
-		report.mode = optical_flow_s::MODE_BRIGHT;
-		break;
-
-	case Mode::LowLight:
-		report.mode = optical_flow_s::MODE_LOWLIGHT;
-		break;
-
-	case Mode::SuperLowLight:
-		report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT;
-		break;
-	}
-
-	report.timestamp = hrt_absolute_time();
-	_optical_flow_pub.publish(report);
-
-	if (report.quality > 10) {
-		_last_good_publish = report.timestamp;
-	}
-
-	ResetAccumulatedData();
-}
-
-void PAA3905::ResetAccumulatedData()
-{
-	// reset
-	_flow_dt_sum_usec = 0;
-	_flow_sum_x = 0;
-	_flow_sum_y = 0;
-	_flow_sample_counter = 0;
-	_flow_quality_sum = 0;
+	perf_end(_cycle_perf);
 }
 
 void PAA3905::print_status()
 {
 	I2CSPIDriverBase::print_status();
 
-	perf_print_counter(_sample_perf);
+	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
-	perf_print_counter(_comms_errors);
+	perf_print_counter(_reset_perf);
 	perf_print_counter(_false_motion_perf);
-	perf_print_counter(_register_write_fail_perf);
+	perf_print_counter(_mode_change_bright_perf);
+	perf_print_counter(_mode_change_low_light_perf);
+	perf_print_counter(_mode_change_super_low_light_perf);
+	perf_print_counter(_no_motion_interrupt_perf);
 }

src/drivers/optical_flow/paa3905/PAA3905.hpp

@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file paa3905.hpp
+ * @file PAA3905.hpp
  *
- * Driver for the Pixart paa3905 optical flow sensors connected via SPI.
+ * Driver for the PAA3905E1-Q: Optical Motion Tracking Chip
  */
 
 #pragma once
@@ -48,16 +48,15 @@
 #include <drivers/device/spi.h>
 #include <conversion/rotation.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 using namespace time_literals;
 using namespace PixArt_PAA3905;
 
-#define DIR_WRITE(a) ((a) | (1 << 7))
-#define DIR_READ(a) ((a) & 0x7f)
+#define DIR_WRITE(a) ((a) | Bit7)
+#define DIR_READ(a) ((a) & 0x7F)
 
 class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905>
 {
@@ -78,59 +77,61 @@ private:
 
 	int probe() override;
 
+	void Reset();
+
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
 	void DataReady();
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();
 
-	uint8_t	RegisterRead(uint8_t reg, int retries = 2);
+	uint8_t RegisterRead(uint8_t reg);
 	void RegisterWrite(uint8_t reg, uint8_t data);
-	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);
-
-	void EnableLed();
-
-	void StandardDetectionSetting();
-	void EnhancedDetectionMode();
-	void ModeAuto012();
-
-	void CheckMode();
 
 	void Configure();
 
-	void ResetAccumulatedData();
+	void ConfigureAutomaticModeSwitching();
 
-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	void ConfigureModeBright();
+	void ConfigureModeLowLight();
+	void ConfigureModeSuperLowLight();
 
-	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-	perf_counter_t	_comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
-	perf_counter_t	_false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
-	perf_counter_t	_register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
+	void ConfigureStandardDetectionSetting();
+	void ConfigureEnhancedDetectionMode();
 
-	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
+	void EnableLed();
+
+	bool UpdateMode(const uint8_t observation);
+
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
+	perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
+	perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
+	perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")};
+	perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
+	perf_counter_t _no_motion_interrupt_perf{nullptr};
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	uint64_t _previous_collect_timestamp{0};
-	uint64_t _flow_dt_sum_usec{0};
-	uint8_t _flow_sample_counter{0};
-	uint16_t _flow_quality_sum{0};
+	matrix::Dcmf _rotation;
 
-	matrix::Dcmf	_rotation;
+	int _discard_reading{3};
 
-	int             _discard_reading{3};
+	Mode _mode{Mode::LowLight};
 
-	int		_flow_sum_x{0};
-	int		_flow_sum_y{0};
-
-	Mode		_mode{Mode::LowLight};
-
-	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
-
-	int _valid_count{0};
+	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};
 
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
-	hrt_abstime _last_good_publish{0};
+	hrt_abstime _last_write_time{0};
+	hrt_abstime _last_read_time{0};
+
+	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
+	static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s;
+
+	hrt_abstime _last_good_data{0};
 	hrt_abstime _last_reset{0};
 };

src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp

@@ -33,8 +33,8 @@
 
 #pragma once
 
-namespace PixArt_PAA3905
-{
+#include <cstdint>
+
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
 static constexpr uint8_t Bit1 = (1 << 1);
@@ -45,19 +45,24 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-static constexpr uint8_t PRODUCT_ID = 0xA2;
-static constexpr uint8_t REVISION_ID = 0x00;
+namespace PixArt_PAA3905
+{
+
+static constexpr uint8_t PRODUCT_ID         = 0xA2;
+static constexpr uint8_t REVISION_ID        = 0x00;
 static constexpr uint8_t PRODUCT_ID_INVERSE = 0x5D;
 
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126};	// 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126};	// 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};		// 50 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};  // 50 fps
 
 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
 
-// Various time delay needed for paa3905
-static constexpr uint32_t TIME_us_TSWW  = 11; // actually 10.5us
-static constexpr uint32_t TIME_us_TSRAD = 2;
+// Various time delays
+static constexpr uint32_t TIME_TSWW_us      = 11; // SPI Time Between Write Commands (actually 10.5us)
+static constexpr uint32_t TIME_TSWR_us      = 6;  // SPI Time Between Write and Read Commands
+static constexpr uint32_t TIME_TSRW_TSRR_us = 2;  // SPI Time Between Read And Subsequent Commands (actually 1.5us)
+static constexpr uint32_t TIME_TSRAD_us     = 2;  // SPI Read Address-Data Delay
 
 enum Register : uint8_t {
 	Product_ID         = 0x00,
@@ -86,9 +91,20 @@ enum Register : uint8_t {
 	Inverse_Product_ID = 0x5F,
 };
 
-// Observation
+enum Motion_Bit : uint8_t {
+	MotionOccurred     = Bit7, // Motion since last report
+
+	ChallengingSurface = Bit0, // Challenging surface is detected
+};
+
 enum Observation_Bit : uint8_t {
-	Reset = 0x5A,
+	// Bit [7:6]
+	AMS_mode_0 = 0,
+	AMS_mode_1 = Bit6,
+	AMS_mode_2 = Bit7,
+
+	// Bit [5:0]
+	WorkingCorrectly = 0x3F,
 };
 
 enum class Mode {

src/drivers/optical_flow/paa3905/paa3905_main.cpp

@@ -49,7 +49,7 @@ extern "C" __EXPORT int paa3905_main(int argc, char *argv[])
 	using ThisDriver = PAA3905;
 	BusCLIArguments cli{false, true};
 	cli.custom1 = -1;
-	cli.spi_mode = SPIDEV_MODE0;
+	cli.spi_mode = SPIDEV_MODE3;
 	cli.default_spi_frequency = SPI_SPEED;
 
 	while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {

src/drivers/optical_flow/paa3905/parameters.c

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * paa3905 Optical Flow
+ * PAA3905 Optical Flow
  *
  * @reboot_required true
  *

src/drivers/optical_flow/paw3902/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2021 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
@@ -38,7 +38,7 @@ px4_add_module(
 		paw3902_main.cpp
 		PAW3902.cpp
 		PAW3902.hpp
-		PixArt_PAW3902JF_Registers.hpp
+		PixArt_PAW3902_Registers.hpp
 	DEPENDS
 		conversion
 		drivers__device

src/drivers/optical_flow/paw3902/PAW3902.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 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
@@ -34,12 +34,12 @@
 /**
  * @file PAW3902.hpp
  *
- * Driver for the Pixart PAW3902 & PAW3903 optical flow sensors connected via SPI.
+ * Driver for the PAW3902JF-TXQT: Optical Motion Tracking Chip
  */
 
 #pragma once
 
-#include "PixArt_PAW3902JF_Registers.hpp"
+#include "PixArt_PAW3902_Registers.hpp"
 
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -48,16 +48,15 @@
 #include <drivers/device/spi.h>
 #include <conversion/rotation.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 using namespace time_literals;
-using namespace PixArt_PAW3902JF;
+using namespace PixArt_PAW3902;
 
-#define DIR_WRITE(a) ((a) | (1 << 7))
-#define DIR_READ(a) ((a) & 0x7f)
+#define DIR_WRITE(a) ((a) | Bit7)
+#define DIR_READ(a) ((a) & 0x7F)
 
 class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
 {
@@ -78,65 +77,61 @@ private:
 
 	int probe() override;
 
+	void Reset();
+
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
 	void DataReady();
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();
 
-	uint8_t	RegisterRead(uint8_t reg, int retries = 2);
+	uint8_t RegisterRead(uint8_t reg);
 	void RegisterWrite(uint8_t reg, uint8_t data);
-	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);
 
-	void EnableLed();
-
-	void ModeBright();
-	void ModeLowLight();
-	void ModeSuperLowLight();
+	void Configure();
 
 	bool ChangeMode(Mode newMode, bool force = false);
 
-	void ResetAccumulatedData();
+	void ConfigureModeBright();
+	void ConfigureModeLowLight();
+	void ConfigureModeSuperLowLight();
 
-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	void EnableLed();
 
-	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-	perf_counter_t	_comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
-	perf_counter_t	_false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
-	perf_counter_t	_register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
-	perf_counter_t	_mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
-	perf_counter_t	_mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")};
-	perf_counter_t	_mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
-	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
+	perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
+	perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
+	perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")};
+	perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
+	perf_counter_t _no_motion_interrupt_perf{nullptr};
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	uint64_t _previous_collect_timestamp{0};
-	uint64_t _flow_dt_sum_usec{0};
-	uint8_t _flow_sample_counter{0};
-	uint16_t _flow_quality_sum{0};
+	matrix::Dcmf _rotation;
 
-	matrix::Dcmf	_rotation;
+	int _discard_reading{3};
 
-	int             _discard_reading{3};
+	Mode _mode{Mode::LowLight};
 
-	int		_flow_sum_x{0};
-	int		_flow_sum_y{0};
-
-	Mode		_mode{Mode::LowLight};
-
-	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
+	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};
 
 	int _bright_to_low_counter{0};
 	int _low_to_superlow_counter{0};
 	int _low_to_bright_counter{0};
 	int _superlow_to_low_counter{0};
 
-	int _valid_count{0};
-
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
-	hrt_abstime _last_good_publish{0};
+	hrt_abstime _last_write_time{0};
+	hrt_abstime _last_read_time{0};
+
+	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
+	static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s;
+
+	hrt_abstime _last_good_data{0};
 	hrt_abstime _last_reset{0};
 };

src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 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
@@ -33,22 +33,36 @@
 
 #pragma once
 
-namespace PixArt_PAW3902JF
+#include <cstdint>
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+namespace PixArt_PAW3902
 {
 
-static constexpr uint8_t PRODUCT_ID = 0x49;
-static constexpr uint8_t REVISION_ID = 0x01;
+static constexpr uint8_t PRODUCT_ID         = 0x49;
+static constexpr uint8_t REVISION_ID        = 0x01;
 static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6;
 
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126};	// 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126};	// 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};		// 50 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};  // 50 fps
 
 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
 
-// Various time delay needed for PAW3902
-static constexpr uint32_t TIME_us_TSWW  = 11; // actually 10.5us
-static constexpr uint32_t TIME_us_TSRAD = 2;
+// Various time delays
+static constexpr uint32_t TIME_TSWW_us      = 11; // SPI Time Between Write Commands (actually 10.5us)
+static constexpr uint32_t TIME_TSWR_us      = 6;  // SPI Time Between Write and Read Commands
+static constexpr uint32_t TIME_TSRW_TSRR_us = 2;  // SPI Time Between Read And Subsequent Commands (actually 1.5us)
+static constexpr uint32_t TIME_TSRAD_us     = 2;  // SPI Read Address-Data Delay
 
 enum Register : uint8_t {
 	Product_ID         = 0x00,
@@ -75,8 +89,8 @@ enum Register : uint8_t {
 	Inverse_Product_ID = 0x5F,
 };
 
-enum Product_ID_Bit : uint8_t {
-	Reset = 0x5A,
+enum Motion_Bit : uint8_t {
+	MOT = Bit7, // Motion since last report
 };
 
 enum class Mode {

src/drivers/optical_flow/paw3902/parameters.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 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
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * PAW3902 & PAW3903 Optical Flow
+ * PAW3902/PAW3903 Optical Flow
  *
  * @reboot_required true
  *

src/drivers/optical_flow/paw3902/paw3902_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 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
@@ -71,13 +71,11 @@ extern "C" __EXPORT int paw3902_main(int argc, char *argv[])
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);
-	}
 
-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}
 
-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}
 

src/drivers/optical_flow/pmw3901/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-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
@@ -38,4 +38,9 @@ px4_add_module(
 		pmw3901_main.cpp
 		PMW3901.cpp
 		PMW3901.hpp
+		PixArt_PMW3901_Registers.hpp
+	DEPENDS
+		conversion
+		drivers__device
+		px4_work_queue
 	)

src/drivers/optical_flow/pmw3901/PMW3901.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -33,74 +33,200 @@
 
 #include "PMW3901.hpp"
 
-static constexpr uint32_t TIME_us_TSWW = 11; //  - actually 10.5us
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
 
 PMW3901::PMW3901(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
-	_sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")),
-	_comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")),
-	_yaw_rotation(config.rotation)
+	_drdy_gpio(config.drdy_gpio)
 {
+	if (_drdy_gpio != 0) {
+		_no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt");
+	}
+
+	float yaw_rotation_degrees = (float)config.custom1;
+
+	if (yaw_rotation_degrees >= 0.f) {
+		PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)",
+			 (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees));
+
+		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};
+
+	} else {
+		_rotation.identity();
+	}
 }
 
 PMW3901::~PMW3901()
 {
 	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
+	perf_free(_cycle_perf);
+	perf_free(_interval_perf);
+	perf_free(_reset_perf);
+	perf_free(_false_motion_perf);
+	perf_free(_no_motion_interrupt_perf);
 }
 
-int
-PMW3901::sensorInit()
+int PMW3901::init()
 {
-	uint8_t data[5] {};
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK) {
+		return PX4_ERROR;
+	}
 
-	// Power on reset
-	writeRegister(0x3A, 0x5A);
-	usleep(5000);
+	Configure();
 
-	// Reading the motion registers one time
-	readRegister(0x02, &data[0], 1);
-	readRegister(0x03, &data[1], 1);
-	readRegister(0x04, &data[2], 1);
-	readRegister(0x05, &data[3], 1);
-	readRegister(0x06, &data[4], 1);
+	return PX4_OK;
+}
 
-	usleep(1000);
+int PMW3901::probe()
+{
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t Product_ID = RegisterRead(Register::Product_ID);
+		const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
+		const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
 
+		if (Product_ID != PRODUCT_ID) {
+			PX4_ERR("Product_ID: %X", Product_ID);
+			break;
+		}
+
+		if (Revision_ID != REVISION_ID) {
+			PX4_ERR("Revision_ID: %X", Revision_ID);
+			break;
+		}
+
+		if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
+			PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
+			break;
+		}
+
+		return PX4_OK;
+	}
+
+	return PX4_ERROR;
+}
+
+int PMW3901::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<PMW3901 *>(arg)->DataReady();
+	return 0;
+}
+
+void PMW3901::DataReady()
+{
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
+}
+
+bool PMW3901::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		_data_ready_interrupt_enabled = false;
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	if (px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0) {
+		_data_ready_interrupt_enabled = true;
+		return true;
+	}
+
+	_data_ready_interrupt_enabled = false;
+	return false;
+}
+
+bool PMW3901::DataReadyInterruptDisable()
+{
+	_data_ready_interrupt_enabled = false;
+
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+void PMW3901::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void PMW3901::Reset()
+{
+	perf_count(_reset_perf);
+
+	DataReadyInterruptDisable();
+	ScheduleClear();
+
+	// Issue a soft reset
+	RegisterWrite(Register::Power_Up_Reset, 0x5A);
+	px4_usleep(1000);
+	_last_reset = hrt_absolute_time();
+
+	_discard_reading = 3;
+
+	// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
+	RegisterRead(0x02);
+	RegisterRead(0x03);
+	RegisterRead(0x04);
+	RegisterRead(0x05);
+	RegisterRead(0x06);
+}
+
+void PMW3901::Configure()
+{
+	Reset();
+
+	SetPerformanceOptimizationRegisters();
+
+	if (DataReadyInterruptConfigure()) {
+		// backup schedule
+		ScheduleDelayed(500_ms);
+
+	} else {
+		ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
+	}
+}
+
+void PMW3901::SetPerformanceOptimizationRegisters()
+{
 	// set performance optimization registers
 	// from PixArt PMW3901MB Optical Motion Tracking chip demo kit V3.20 (21 Aug 2018)
 	unsigned char v = 0;
 	unsigned char c1 = 0;
 	unsigned char c2 = 0;
 
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x55, 0x01);
-	writeRegister(0x50, 0x07);
-	writeRegister(0x7f, 0x0e);
-	writeRegister(0x43, 0x10);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x55, 0x01);
+	RegisterWrite(0x50, 0x07);
+	RegisterWrite(0x7f, 0x0e);
+	RegisterWrite(0x43, 0x10);
 
-	readRegister(0x67, &v, 1);
+	v = RegisterRead(0x67);
 
 	// if bit7 is set
 	if (v & (1 << 7)) {
-		writeRegister(0x48, 0x04);
+		RegisterWrite(0x48, 0x04);
 
 	} else {
-		writeRegister(0x48, 0x02);
+		RegisterWrite(0x48, 0x02);
 	}
 
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x51, 0x7b);
-	writeRegister(0x50, 0x00);
-	writeRegister(0x55, 0x00);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x51, 0x7b);
+	RegisterWrite(0x50, 0x00);
+	RegisterWrite(0x55, 0x00);
 
-	writeRegister(0x7F, 0x0e);
-	readRegister(0x73, &v, 1);
+	RegisterWrite(0x7F, 0x0e);
+	v = RegisterRead(0x73);
 
 	if (v == 0) {
-		readRegister(0x70, &c1, 1);
+		c1 = RegisterRead(0x70);
 
 		if (c1 <= 28) {
 			c1 = c1 + 14;
@@ -113,307 +239,257 @@ PMW3901::sensorInit()
 			c1 = 0x3F;
 		}
 
-		readRegister(0x71, &c2, 1);
+		c2 = RegisterRead(0x71);
 		c2 = ((unsigned short)c2 * 45) / 100;
 
-		writeRegister(0x7f, 0x00);
-		writeRegister(0x61, 0xAD);
-		writeRegister(0x51, 0x70);
-		writeRegister(0x7f, 0x0e);
-		writeRegister(0x70, c1);
-		writeRegister(0x71, c2);
+		RegisterWrite(0x7f, 0x00);
+		RegisterWrite(0x61, 0xAD);
+		RegisterWrite(0x51, 0x70);
+		RegisterWrite(0x7f, 0x0e);
+		RegisterWrite(0x70, c1);
+		RegisterWrite(0x71, c2);
 	}
 
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x61, 0xAD);
-	writeRegister(0x7F, 0x03);
-	writeRegister(0x40, 0x00);
-	writeRegister(0x7F, 0x05);
-	writeRegister(0x41, 0xB3);
-	writeRegister(0x43, 0xF1);
-	writeRegister(0x45, 0x14);
-	writeRegister(0x5B, 0x32);
-	writeRegister(0x5F, 0x34);
-	writeRegister(0x7B, 0x08);
-	writeRegister(0x7F, 0x06);
-	writeRegister(0x44, 0x1B);
-	writeRegister(0x40, 0xBF);
-	writeRegister(0x4E, 0x3F);
-	writeRegister(0x7F, 0x08);
-	writeRegister(0x65, 0x20);
-	writeRegister(0x6A, 0x18);
-	writeRegister(0x7F, 0x09);
-	writeRegister(0x4F, 0xAF);
-	writeRegister(0x5F, 0x40);
-	writeRegister(0x48, 0x80);
-	writeRegister(0x49, 0x80);
-	writeRegister(0x57, 0x77);
-	writeRegister(0x60, 0x78);
-	writeRegister(0x61, 0x78);
-	writeRegister(0x62, 0x08);
-	writeRegister(0x63, 0x50);
-	writeRegister(0x7F, 0x0A);
-	writeRegister(0x45, 0x60);
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x4D, 0x11);
-	writeRegister(0x55, 0x80);
-	writeRegister(0x74, 0x21);
-	writeRegister(0x75, 0x1F);
-	writeRegister(0x4A, 0x78);
-	writeRegister(0x4B, 0x78);
-	writeRegister(0x44, 0x08);
-	writeRegister(0x45, 0x50);
-	writeRegister(0x64, 0xFF);
-	writeRegister(0x65, 0x1F);
-	writeRegister(0x7F, 0x14);
-	writeRegister(0x65, 0x67);
-	writeRegister(0x66, 0x08);
-	writeRegister(0x63, 0x70);
-	writeRegister(0x7F, 0x15);
-	writeRegister(0x48, 0x48);
-	writeRegister(0x7F, 0x07);
-	writeRegister(0x41, 0x0D);
-	writeRegister(0x43, 0x14);
-	writeRegister(0x4B, 0x0E);
-	writeRegister(0x45, 0x0F);
-	writeRegister(0x44, 0x42);
-	writeRegister(0x4C, 0x80);
-	writeRegister(0x7F, 0x10);
-	writeRegister(0x5B, 0x02);
-	writeRegister(0x7F, 0x07);
-	writeRegister(0x40, 0x41);
-	writeRegister(0x70, 0x00);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x61, 0xAD);
+	RegisterWrite(0x7F, 0x03);
+	RegisterWrite(0x40, 0x00);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x41, 0xB3);
+	RegisterWrite(0x43, 0xF1);
+	RegisterWrite(0x45, 0x14);
+	RegisterWrite(0x5B, 0x32);
+	RegisterWrite(0x5F, 0x34);
+	RegisterWrite(0x7B, 0x08);
+	RegisterWrite(0x7F, 0x06);
+	RegisterWrite(0x44, 0x1B);
+	RegisterWrite(0x40, 0xBF);
+	RegisterWrite(0x4E, 0x3F);
+	RegisterWrite(0x7F, 0x08);
+	RegisterWrite(0x65, 0x20);
+	RegisterWrite(0x6A, 0x18);
+	RegisterWrite(0x7F, 0x09);
+	RegisterWrite(0x4F, 0xAF);
+	RegisterWrite(0x5F, 0x40);
+	RegisterWrite(0x48, 0x80);
+	RegisterWrite(0x49, 0x80);
+	RegisterWrite(0x57, 0x77);
+	RegisterWrite(0x60, 0x78);
+	RegisterWrite(0x61, 0x78);
+	RegisterWrite(0x62, 0x08);
+	RegisterWrite(0x63, 0x50);
+	RegisterWrite(0x7F, 0x0A);
+	RegisterWrite(0x45, 0x60);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x4D, 0x11);
+	RegisterWrite(0x55, 0x80);
+	RegisterWrite(0x74, 0x21);
+	RegisterWrite(0x75, 0x1F);
+	RegisterWrite(0x4A, 0x78);
+	RegisterWrite(0x4B, 0x78);
+	RegisterWrite(0x44, 0x08);
+	RegisterWrite(0x45, 0x50);
+	RegisterWrite(0x64, 0xFF);
+	RegisterWrite(0x65, 0x1F);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x65, 0x67);
+	RegisterWrite(0x66, 0x08);
+	RegisterWrite(0x63, 0x70);
+	RegisterWrite(0x7F, 0x15);
+	RegisterWrite(0x48, 0x48);
+	RegisterWrite(0x7F, 0x07);
+	RegisterWrite(0x41, 0x0D);
+	RegisterWrite(0x43, 0x14);
+	RegisterWrite(0x4B, 0x0E);
+	RegisterWrite(0x45, 0x0F);
+	RegisterWrite(0x44, 0x42);
+	RegisterWrite(0x4C, 0x80);
+	RegisterWrite(0x7F, 0x10);
+	RegisterWrite(0x5B, 0x02);
+	RegisterWrite(0x7F, 0x07);
+	RegisterWrite(0x40, 0x41);
+	RegisterWrite(0x70, 0x00);
 
-	px4_usleep(10000); // delay 10ms
+	px4_usleep(10'000); // delay 10ms
 
-	writeRegister(0x32, 0x44);
-	writeRegister(0x7F, 0x07);
-	writeRegister(0x40, 0x40);
-	writeRegister(0x7F, 0x06);
-	writeRegister(0x62, 0xF0);
-	writeRegister(0x63, 0x00);
-	writeRegister(0x7F, 0x0D);
-	writeRegister(0x48, 0xC0);
-	writeRegister(0x6F, 0xD5);
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x5B, 0xA0);
-	writeRegister(0x4E, 0xA8);
-	writeRegister(0x5A, 0x50);
-	writeRegister(0x40, 0x80);
-
-	return PX4_OK;
+	RegisterWrite(0x32, 0x44);
+	RegisterWrite(0x7F, 0x07);
+	RegisterWrite(0x40, 0x40);
+	RegisterWrite(0x7F, 0x06);
+	RegisterWrite(0x62, 0xF0);
+	RegisterWrite(0x63, 0x00);
+	RegisterWrite(0x7F, 0x0D);
+	RegisterWrite(0x48, 0xC0);
+	RegisterWrite(0x6F, 0xD5);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x5B, 0xA0);
+	RegisterWrite(0x4E, 0xA8);
+	RegisterWrite(0x5A, 0x50);
+	RegisterWrite(0x40, 0x80);
 }
 
-int
-PMW3901::init()
+uint8_t PMW3901::RegisterRead(uint8_t reg)
 {
-	// get yaw rotation from sensor frame to body frame
-	param_t rot = param_find("SENS_FLOW_ROT");
+	// tSWR SPI Time Between Write And Read Commands
+	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
 
-	if (rot != PARAM_INVALID) {
-		int32_t val = 0;
-		param_get(rot, &val);
-
-		_yaw_rotation = (enum Rotation)val;
+	if (elapsed_last_write < TIME_TSWR_us) {
+		px4_udelay(TIME_TSWR_us - elapsed_last_write);
 	}
 
-	/* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */
-	SPI::set_lockmode(LOCK_THREADS);
+	// tSRW/tSRR SPI Time Between Read And Subsequent Commands
+	const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time);
 
-	/* do SPI init (and probe) first */
-	if (SPI::init() != OK) {
-		return PX4_ERROR;
+	if (elapsed_last_write < TIME_TSRW_TSRR_us) {
+		px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read);
 	}
 
-	sensorInit();
-
-	_previous_collect_timestamp = hrt_absolute_time();
-
-	start();
-
-	return PX4_OK;
-}
-
-int
-PMW3901::probe()
-{
-	uint8_t data[2] {};
-
-	readRegister(0x00, &data[0], 1); // chip id
-
-	// Test the SPI communication, checking chipId and inverse chipId
-	if (data[0] == 0x49) {
-		return OK;
-	}
-
-	// not found on any address
-	return -EIO;
-}
-
-int
-PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count)
-{
-	uint8_t cmd[5];	// read up to 4 bytes
-
+	uint8_t cmd[2];
 	cmd[0] = DIR_READ(reg);
+	cmd[1] = 0;
+	transfer(&cmd[0], &cmd[0], sizeof(cmd));
+	hrt_store_absolute_time(&_last_read_time);
 
-	int ret = transfer(&cmd[0], &cmd[0], count + 1);
-
-	if (OK != ret) {
-		perf_count(_comms_errors);
-		DEVICE_LOG("spi::transfer returned %d", ret);
-		return ret;
-	}
-
-	memcpy(&data[0], &cmd[1], count);
-
-	return ret;
+	return cmd[1];
 }
 
-int
-PMW3901::writeRegister(unsigned reg, uint8_t data)
+void PMW3901::RegisterWrite(uint8_t reg, uint8_t data)
 {
-	uint8_t cmd[2]; 						// write 1 byte
-	int ret;
+	// tSWW SPI Time Between Write Commands
+	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
 
+	if (elapsed_last_write < TIME_TSWW_us) {
+		px4_udelay(TIME_TSWW_us - elapsed_last_write);
+	}
+
+	uint8_t cmd[2];
 	cmd[0] = DIR_WRITE(reg);
 	cmd[1] = data;
-
-	ret = transfer(&cmd[0], nullptr, 2);
-
-	if (OK != ret) {
-		perf_count(_comms_errors);
-		DEVICE_LOG("spi::transfer returned %d", ret);
-		return ret;
-	}
-
-	px4_usleep(TIME_us_TSWW);
-
-	return ret;
+	transfer(&cmd[0], nullptr, sizeof(cmd));
+	hrt_store_absolute_time(&_last_write_time);
 }
 
-void
-PMW3901::RunImpl()
+void PMW3901::RunImpl()
 {
-	perf_begin(_sample_perf);
+	const hrt_abstime now = hrt_absolute_time();
 
-	int16_t delta_x_raw = 0;
-	int16_t delta_y_raw = 0;
-	uint8_t qual = 0;
-	float delta_x = 0.0f;
-	float delta_y = 0.0f;
+	perf_begin(_cycle_perf);
+	perf_count(_interval_perf);
 
-	uint64_t timestamp = hrt_absolute_time();
-	uint64_t dt_flow = timestamp - _previous_collect_timestamp;
-	_previous_collect_timestamp = timestamp;
-
-	_flow_dt_sum_usec += dt_flow;
-
-	readMotionCount(delta_x_raw, delta_y_raw, qual);
-
-	if (qual > 0) {
-		_flow_sum_x += delta_x_raw;
-		_flow_sum_y += delta_y_raw;
-		_flow_sample_counter ++;
-		_flow_quality_sum += qual;
-	}
-
-	// returns if the collect time has not been reached
-	if (_flow_dt_sum_usec < _collect_time) {
+	// force reconfigure if we haven't received valid data for quite some time
+	if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) {
+		Configure();
+		perf_end(_cycle_perf);
 		return;
 	}
 
-	delta_x = (float)_flow_sum_x / 385.0f;		// proportional factor + convert from pixels to radians
-	delta_y = (float)_flow_sum_y / 385.0f;		// proportional factor + convert from pixels to radians
+	hrt_abstime timestamp_sample = 0;
 
-	optical_flow_s report{};
-	report.timestamp = timestamp;
+	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);
 
-	report.pixel_flow_x_integral = static_cast<float>(delta_x);
-	report.pixel_flow_y_integral = static_cast<float>(delta_y);
+		if (now < drdy_timestamp_sample + _scheduled_interval_us) {
+			timestamp_sample = drdy_timestamp_sample;
 
-	// rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT
-	float zeroval = 0.0f;
-	rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
-	rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
+		} else {
+			perf_count(_no_motion_interrupt_perf);
+		}
 
-	report.frame_count_since_last_readout = _flow_sample_counter;	// number of frames
-	report.integration_timespan = _flow_dt_sum_usec; 	// microseconds
-
-	report.sensor_id = 0;
-	report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0;
-
-
-	/* No gyro on this board */
-	report.gyro_x_rate_integral = NAN;
-	report.gyro_y_rate_integral = NAN;
-	report.gyro_z_rate_integral = NAN;
-
-	// set (conservative) specs according to datasheet
-	report.max_flow_rate = 5.0f;       // Datasheet: 7.4 rad/s
-	report.min_ground_distance = 0.1f; // Datasheet: 80mm
-	report.max_ground_distance = 30.0f; // Datasheet: infinity
-
-	_flow_dt_sum_usec = 0;
-	_flow_sum_x = 0;
-	_flow_sum_y = 0;
-	_flow_sample_counter = 0;
-	_flow_quality_sum = 0;
-
-	_optical_flow_pub.publish(report);
-
-	perf_end(_sample_perf);
-}
-
-int
-PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual)
-{
-	uint8_t data[12] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,
-			     DIR_READ(0x05), 0, DIR_READ(0x06), 0, DIR_READ(0x07), 0
-			   };
-
-	int ret = transfer(&data[0], &data[0], 12);
-
-	if (OK != ret) {
-		qual = 0;
-		perf_count(_comms_errors);
-		DEVICE_LOG("spi::transfer returned %d", ret);
-		return ret;
+		// push backup schedule back
+		ScheduleDelayed(500_ms);
 	}
 
-	deltaX = ((int16_t)data[5] << 8) | data[3];
-	deltaY = ((int16_t)data[9] << 8) | data[7];
+	struct TransferBuffer {
+		uint8_t cmd = Register::Motion_Burst;
+		BURST_TRANSFER data{};
+	} buf{};
+	static_assert(sizeof(buf) == (12 + 1));
 
-	// If the reported flow is impossibly large, we just got garbage from the SPI
-	if (deltaX > 240 || deltaY > 240 || deltaX < -240 || deltaY < -240) {
-		qual = 0;
-
-	} else {
-		qual = data[11];
+	if (timestamp_sample == 0) {
+		timestamp_sample = hrt_absolute_time();
 	}
 
-	ret = OK;
+	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) {
+		perf_end(_cycle_perf);
+		return;
+	}
 
-	return ret;
+	hrt_store_absolute_time(&_last_read_time);
+
+	if (_discard_reading > 0) {
+		_discard_reading--;
+		perf_end(_cycle_perf);
+		return;
+	}
+
+	// check SQUAL & Shutter values
+	// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
+	//                  SQUAL < 25, Shutter ≥ 0x1F00
+
+	// 13-bit Shutter register
+	const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
+	const uint8_t Shutter_Lower = buf.data.Shutter_Lower;
+
+	const uint16_t shutter = (Shutter_Upper << 8) | Shutter_Lower;
+
+	// Motion since last report and Surface quality non-zero
+	const bool motion_detected = buf.data.Motion & Motion_Bit::MOT;
+
+	// Number of Features = SQUAL * 4
+	bool data_valid = (buf.data.SQUAL > 0);
+
+	// quality < 25 (0x19) and shutter >= 7936 (0x1F00)
+	if ((buf.data.SQUAL < 0x19) && (shutter >= 0x1F00)) {
+		// false motion report, discarding
+		perf_count(_false_motion_perf);
+		data_valid = false;
+	}
+
+	if (data_valid) {
+		// publish sensor_optical_flow
+		sensor_optical_flow_s report{};
+		report.timestamp_sample = timestamp_sample;
+		report.device_id = get_device_id();
+
+		report.integration_timespan_us = _scheduled_interval_us;
+		report.quality = buf.data.SQUAL;
+
+		// set specs according to datasheet
+		report.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
+		report.min_ground_distance = 0.08f;    // Datasheet: 80mm
+		report.max_ground_distance = INFINITY; // Datasheet: infinity
+
+		if (motion_detected) {
+			// only populate flow if data valid (motion and quality > 0)
+			const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
+			const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.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};
+
+			report.pixel_flow[0] = pixel_flow_rotated(0) / 385.0f; // proportional factor + convert from pixels to radians
+			report.pixel_flow[1] = pixel_flow_rotated(1) / 385.0f; // proportional factor + convert from pixels to radians
+		}
+
+		report.timestamp = hrt_absolute_time();
+		_sensor_optical_flow_pub.publish(report);
+
+		if (report.quality >= 1) {
+			_last_good_data = report.timestamp_sample;
+		}
+	}
+
+	perf_end(_cycle_perf);
 }
 
-void
-PMW3901::start()
-{
-	// schedule a cycle to start things
-	ScheduleOnInterval(PMW3901_SAMPLE_INTERVAL, PMW3901_US);
-}
-
-void
-PMW3901::stop()
-{
-	ScheduleClear();
-}
-
-void
-PMW3901::print_status()
+void PMW3901::print_status()
 {
 	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_interval_perf);
+	perf_print_counter(_reset_perf);
+	perf_print_counter(_false_motion_perf);
+	perf_print_counter(_no_motion_interrupt_perf);
 }

src/drivers/optical_flow/pmw3901/PMW3901.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -33,89 +33,89 @@
 
 /**
  * @file PMW3901.hpp
- * @author Daniele Pettenuzzo
  *
- * Driver for the pmw3901 optical flow sensor connected via SPI.
+ * Driver for the PMW3901MB-TXQT: Optical Motion Tracking Chip
  */
 
+#pragma once
+
+#include "PixArt_PMW3901_Registers.hpp"
+
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/getopt.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/i2c_spi_buses.h>
 #include <drivers/device/spi.h>
 #include <conversion/rotation.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
-#include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
-/* Configuration Constants */
+using namespace time_literals;
+using namespace PixArt_PMW3901;
 
-#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz
-
-#define DIR_WRITE(a) ((a) | (1 << 7))
-#define DIR_READ(a) ((a) & 0x7f)
-
-/* PMW3901 Registers addresses */
-#define PMW3901_US 1000 /*   1 ms */
-#define PMW3901_SAMPLE_INTERVAL 10000 /*  10 ms */
+#define DIR_WRITE(a) ((a) | Bit7)
+#define DIR_READ(a) ((a) & 0x7F)
 
 class PMW3901 : public device::SPI, public I2CSPIDriver<PMW3901>
 {
 public:
 	PMW3901(const I2CSPIDriverConfig &config);
-
 	virtual ~PMW3901();
 
 	static void print_usage();
 
-	virtual int init();
+	int init() override;
 
-	void print_status();
+	void print_status() override;
 
 	void RunImpl();
 
-protected:
-	virtual int probe();
-
 private:
+	void exit_and_cleanup() override;
 
-	const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz)
+	int probe() override;
 
-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	void Reset();
 
-	perf_counter_t _sample_perf;
-	perf_counter_t _comms_errors;
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
 
-	uint64_t _previous_collect_timestamp{0};
+	uint8_t RegisterRead(uint8_t reg);
+	void RegisterWrite(uint8_t reg, uint8_t data);
 
-	enum Rotation _yaw_rotation;
+	void Configure();
 
-	int _flow_sum_x{0};
-	int _flow_sum_y{0};
-	uint64_t _flow_dt_sum_usec{0};
-	uint16_t _flow_quality_sum{0};
-	uint8_t _flow_sample_counter{0};
+	void SetPerformanceOptimizationRegisters();
 
-	/**
-	* Initialise the automatic measurement state machine and start it.
-	*
-	* @note This function is called at open and error time.  It might make sense
-	*       to make it more aggressive about resetting the bus in case of errors.
-	*/
-	void start();
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
-	/**
-	* Stop the automatic measurement state machine.
-	*/
-	void stop();
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
+	perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
+	perf_counter_t _no_motion_interrupt_perf{nullptr};
 
+	const spi_drdy_gpio_t _drdy_gpio;
 
-	int readRegister(unsigned reg, uint8_t *data, unsigned count);
-	int writeRegister(unsigned reg, uint8_t data);
+	matrix::Dcmf _rotation;
 
-	int sensorInit();
-	int readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual);
+	int _discard_reading{3};
+
+	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE};
+
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	bool _data_ready_interrupt_enabled{false};
+
+	hrt_abstime _last_write_time{0};
+	hrt_abstime _last_read_time{0};
+
+	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
+	static constexpr hrt_abstime RESET_TIMEOUT_US = 5_s;
+
+	hrt_abstime _last_good_data{0};
+	hrt_abstime _last_reset{0};
 };

src/drivers/optical_flow/pmw3901/PixArt_PMW3901_Registers.hpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <cstdint>
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+namespace PixArt_PMW3901
+{
+
+static constexpr uint8_t PRODUCT_ID         = 0x49;
+static constexpr uint8_t REVISION_ID        = 0x00;
+static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6;
+
+static constexpr uint32_t SAMPLE_INTERVAL_MODE{1000000 / 126}; // 126 fps
+
+static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
+
+// Various time delays
+static constexpr uint32_t TIME_TSWW_us      = 45; // SPI Time Between Write Commands
+static constexpr uint32_t TIME_TSWR_us      = 45; // SPI Time Between Write and Read Commands
+static constexpr uint32_t TIME_TSRW_TSRR_us = 20; // SPI Time Between Read And Subsequent Commands
+static constexpr uint32_t TIME_TSRAD_us     = 35; // SPI Read Address-Data Delay
+
+enum Register : uint8_t {
+	Product_ID         = 0x00,
+	Revision_ID        = 0x01,
+	Motion             = 0x02,
+	Delta_X_L          = 0x03,
+	Delta_X_H          = 0x04,
+	Delta_Y_L          = 0x05,
+	Delta_Y_H          = 0x06,
+	Squal              = 0x07,
+	RawData_Sum        = 0x08,
+	Maximum_RawData    = 0x09,
+	Minimum_RawData    = 0x0A,
+	Shutter_Lower      = 0x0B,
+	Shutter_Upper      = 0x0C,
+
+	Observation        = 0x15,
+	Motion_Burst       = 0x16,
+
+	Power_Up_Reset     = 0x3A,
+
+	Inverse_Product_ID = 0x5F,
+};
+
+enum Motion_Bit : uint8_t {
+	MOT = Bit7, // Motion since last report
+};
+
+struct BURST_TRANSFER {
+	uint8_t Motion;
+	uint8_t Observation;
+	uint8_t Delta_X_L;
+	uint8_t Delta_X_H;
+	uint8_t Delta_Y_L;
+	uint8_t Delta_Y_H;
+	uint8_t SQUAL;
+	uint8_t RawData_Sum;
+	uint8_t Maximum_RawData;
+	uint8_t Minimum_RawData;
+	uint8_t Shutter_Upper;
+	uint8_t Shutter_Lower;
+};
+
+}

src/drivers/optical_flow/pmw3901/parameters.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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

src/drivers/optical_flow/pmw3901/pmw3901_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -34,31 +34,28 @@
 #include "PMW3901.hpp"
 #include <px4_platform_common/module.h>
 
-extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]);
-
-void
-PMW3901::print_usage()
+void PMW3901::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("pmw3901", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-int
-pmw3901_main(int argc, char *argv[])
+extern "C" __EXPORT int pmw3901_main(int argc, char *argv[])
 {
-	int ch;
+	int ch = 0;
 	using ThisDriver = PMW3901;
 	BusCLIArguments cli{false, true};
+	cli.custom1 = -1;
 	cli.spi_mode = SPIDEV_MODE0;
-	cli.default_spi_frequency = PMW3901_SPI_BUS_SPEED;
+	cli.default_spi_frequency = SPI_SPEED;
 
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
+	while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
 		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
+		case 'Y':
+			cli.custom1 = atoi(cli.optArg());
 			break;
 		}
 	}
@@ -74,13 +71,11 @@ pmw3901_main(int argc, char *argv[])
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);
-	}
 
-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}
 
-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}
 

src/drivers/optical_flow/px4flow/px4flow.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -41,8 +41,6 @@
 
 #include <drivers/device/i2c.h>
 #include <drivers/drv_hrt.h>
-#include <lib/conversion/rotation.h>
-#include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -51,7 +49,7 @@
 #include <px4_platform_common/module.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/distance_sensor.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 /* Configuration Constants */
 #define I2C_FLOW_ADDRESS_DEFAULT    0x42	///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
@@ -98,8 +96,8 @@ private:
 	bool				_sensor_ok{false};
 	bool				_collect_phase{false};
 
-	uORB::PublicationMulti<optical_flow_s>		_px4flow_topic{ORB_ID(optical_flow)};
-	uORB::PublicationMulti<distance_sensor_s>	_distance_sensor_topic{ORB_ID(distance_sensor)};
+	uORB::PublicationMulti<distance_sensor_s> _distance_sensor_topic{ORB_ID(distance_sensor)};
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -166,44 +164,6 @@ PX4FLOW::init()
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
 
-	/* get yaw rotation from sensor frame to body frame */
-	param_t rot = param_find("SENS_FLOW_ROT");
-
-	if (rot != PARAM_INVALID) {
-		int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward
-		param_get(rot, &val);
-
-		_sensor_rotation = (enum Rotation)val;
-	}
-
-	/* get operational limits of the sensor */
-	param_t hmin = param_find("SENS_FLOW_MINHGT");
-
-	if (hmin != PARAM_INVALID) {
-		float val = 0.7;
-		param_get(hmin, &val);
-
-		_sensor_min_range = val;
-	}
-
-	param_t hmax = param_find("SENS_FLOW_MAXHGT");
-
-	if (hmax != PARAM_INVALID) {
-		float val = 3.0;
-		param_get(hmax, &val);
-
-		_sensor_max_range = val;
-	}
-
-	param_t ratemax = param_find("SENS_FLOW_MAXR");
-
-	if (ratemax != PARAM_INVALID) {
-		float val = 2.5;
-		param_get(ratemax, &val);
-
-		_sensor_max_flow_rate = val;
-	}
-
 	start();
 
 	return ret;
@@ -279,49 +239,55 @@ PX4FLOW::collect()
 	}
 
 
-	optical_flow_s report{};
+	DeviceId device_id;
+	device_id.devid = get_device_id();
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
+	device_id.devid_s.address = get_i2c_address();
 
-	report.timestamp = hrt_absolute_time();
-	report.pixel_flow_x_integral = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
-	report.pixel_flow_y_integral = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
-	report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout;
-	report.ground_distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.0f;//convert to meters
-	report.quality = _frame_integral.qual; //0:bad ; 255 max quality
-	report.gyro_x_rate_integral = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
-	report.gyro_y_rate_integral = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
-	report.gyro_z_rate_integral = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
-	report.integration_timespan = _frame_integral.integration_timespan; //microseconds
-	report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds
-	report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
-	report.sensor_id = 0;
-	report.max_flow_rate = _sensor_max_flow_rate;
-	report.min_ground_distance = _sensor_min_range;
-	report.max_ground_distance = _sensor_max_range;
+	sensor_optical_flow_s report{};
+
+	report.timestamp_sample = hrt_absolute_time();
+	report.device_id = device_id.devid;
+
+	report.pixel_flow[0] = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.f; //convert to radians
+	report.pixel_flow[1] = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.f; //convert to radians
+
+	// report.ground_distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.f; //convert to meters
+
+	report.integration_timespan_us = _frame_integral.integration_timespan; // microseconds
+
+	report.quality = _frame_integral.qual; // 0:bad ; 255 max quality
+
+	report.delta_angle_available = true;
+	report.delta_angle[0] = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians
+	report.delta_angle[1] = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians
+	report.delta_angle[2] = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians
 
 	/* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */
 	float zeroval = 0.0f;
 
-	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
-	rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
+	rotate_3f(_sensor_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
+	rotate_3f(_sensor_rotation, report.delta_angle[0], report.delta_angle[1], report.delta_angle[2]);
 
-	_px4flow_topic.publish(report);
+	report.max_flow_rate = 2.5f;
+	report.min_ground_distance = 0.7f;
+	report.max_ground_distance = 3.f;
+
+	report.timestamp = hrt_absolute_time();
+	_sensor_optical_flow_pub.publish(report);
 
 	/* publish to the distance_sensor topic as well */
 	if (_distance_sensor_topic.get_instance() == 0) {
 		distance_sensor_s distance_report{};
-		DeviceId device_id;
-		device_id.devid = get_device_id();
-		device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
-
-		distance_report.timestamp = report.timestamp;
+		distance_report.device_id = device_id.devid;
 		distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
 		distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
-		distance_report.current_distance = report.ground_distance_m;
+		distance_report.current_distance = static_cast<float>(_frame_integral.ground_distance) / 1000.f; // convert to meters
 		distance_report.variance = 0.0f;
 		distance_report.signal_quality = -1;
 		distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
-		distance_report.device_id = device_id.devid;
 		distance_report.orientation = _sonar_rotation;
+		distance_report.timestamp = hrt_absolute_time();
 
 		_distance_sensor_topic.publish(distance_report);
 	}
@@ -415,13 +381,11 @@ px4flow_main(int argc, char *argv[])
 		}
 
 		return ThisDriver::module_start(cli, iterator);
-	}
 
-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}
 
-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}
 

src/drivers/optical_flow/thoneflow/thoneflow.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 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
@@ -50,10 +50,10 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
-#include <perf/perf_counter.h>
-#include <systemlib/err.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/uORB.h>
+#include <lib/perf/perf_counter.h>
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_optical_flow.h>
 
 #include "thoneflow_parser.h"
 
@@ -71,21 +71,20 @@ public:
 	void print_info();
 
 private:
-	char                     _port[20];
-	Rotation            	 _rotation;
-	int                      _cycle_interval;
-	int                      _fd;
-	char                     _linebuf[5];
-	unsigned                 _linebuf_index;
-	THONEFLOW_PARSE_STATE    _parse_state;
+	char                     _port[20] {};
+	Rotation            	 _rotation{ROTATION_NONE};
+	int                      _cycle_interval{10526};
+	int                      _fd{-1};
+	char                     _linebuf[5] {};
+	unsigned                 _linebuf_index{0};
+	THONEFLOW_PARSE_STATE    _parse_state{THONEFLOW_PARSE_STATE0_UNSYNC};
 
-	hrt_abstime              _last_read;
+	hrt_abstime              _last_read{0};
 
-	optical_flow_s           _report;
-	orb_advert_t             _optical_flow_pub;
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
-	perf_counter_t           _sample_perf;
-	perf_counter_t           _comms_errors;
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
 
 	/**
 	* Initialise the automatic measurement state machine and start it.
@@ -106,23 +105,8 @@ private:
 
 };
 
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]);
-
 Thoneflow::Thoneflow(const char *port) :
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_rotation(Rotation(0)),
-	_cycle_interval(10526),
-	_fd(-1),
-	_linebuf_index(0),
-	_parse_state(THONEFLOW_PARSE_STATE0_UNSYNC),
-	_last_read(0),
-	_report(),
-	_optical_flow_pub(nullptr),
-	_sample_perf(perf_alloc(PC_ELAPSED, "thoneflow_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "thoneflow_com_err"))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port))
 {
 	/* store port name */
 	strncpy(_port, port, sizeof(_port) - 1);
@@ -206,41 +190,6 @@ Thoneflow::init()
 			ret = PX4_ERROR;
 			break;
 		}
-
-		/* get yaw rotation from sensor frame to body frame */
-		param_t rot = param_find("SENS_FLOW_ROT");
-
-		if (rot != PARAM_INVALID) {
-			int32_t val = 0;
-			param_get(rot, &val);
-
-			_rotation = Rotation(val);
-		}
-
-		/* Initialise report structure */
-		/* No gyro on this board */
-		_report.gyro_x_rate_integral = NAN;
-		_report.gyro_y_rate_integral = NAN;
-		_report.gyro_z_rate_integral = NAN;
-
-		/* Conservative specs according to datasheet */
-		_report.max_flow_rate = 5.0f;           // Datasheet: 7.4 rad/s
-		_report.min_ground_distance = 0.1f;     // Datasheet: 80mm
-		_report.max_ground_distance = 30.0f;    // Datasheet: infinity
-
-		/* Integrated flow is sent at 66fps */
-		_report.frame_count_since_last_readout = 1;
-		_report.integration_timespan = 10526;	// microseconds
-
-		/* Get a publish handle on the optical flow topic */
-		_optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report);
-
-		if (_optical_flow_pub == nullptr) {
-			PX4_ERR("Failed to create optical_flow object");
-			ret = PX4_ERROR;
-			break;
-		}
-
 	} while (0);
 
 	/* Close the fd */
@@ -299,20 +248,33 @@ Thoneflow::collect()
 
 		_last_read = hrt_absolute_time();
 
+		// publish sensor_optical_flow
+		sensor_optical_flow_s report{};
+		report.timestamp_sample = hrt_absolute_time();
+
 		/* Parse each byte of read buffer */
 		for (int i = 0; i < ret; i++) {
-			valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &_report);
+			valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &report);
 		}
 
 		/* Publish most recent valid measurement */
 		if (valid) {
-			_report.timestamp = hrt_absolute_time();
+
+			report.device_id = 0; // TODO get_device_id();
+			report.integration_timespan_us = 10526; // microseconds
 
 			/* Rotate measurements from sensor frame to body frame */
 			float zeroval = 0.0f;
-			rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval);
+			rotate_3f(_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
 
-			orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report);
+			// Conservative specs according to datasheet
+			report.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
+			report.min_ground_distance = 0.08f;    // Datasheet: 80mm
+			report.max_ground_distance = INFINITY; // Datasheet: infinity
+
+			report.timestamp = hrt_absolute_time();
+
+			_sensor_optical_flow_pub.publish(report);
 		}
 
 		/* Bytes left to parse */
@@ -478,16 +440,15 @@ $ thoneflow stop
 
     PRINT_MODULE_USAGE_NAME("thoneflow", "driver");
     PRINT_MODULE_USAGE_SUBCATEGORY("optical_flow");
-    PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
     PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
-    PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
-    PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Print driver information");
 }
 
 } // namespace
 
-int
-thoneflow_main(int argc, char *argv[])
+extern "C" __EXPORT int thoneflow_main(int argc, char *argv[])
 {
     int ch;
     const char *device_path = "";
@@ -522,19 +483,11 @@ thoneflow_main(int argc, char *argv[])
             thoneflow::usage();
             return -1;
         }
-    }
 
-    /*
-     * Stop the driver
-     */
-    if (!strcmp(argv[myoptind], "stop")) {
+    } else if (!strcmp(argv[myoptind], "stop")) {
         return thoneflow::stop();
-    }
 
-    /*
-     * Print driver information.
-     */
-    if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
+    } else if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
         thoneflow::info();
         return 0;
     }

src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp

@@ -61,7 +61,7 @@ const char *parser_state[] = {
 #endif
 
 bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
-		     optical_flow_s *flow)
+		     sensor_optical_flow_s *flow)
 {
 	bool parsed_packet = false;
 
@@ -133,8 +133,8 @@ bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TH
 				// Checksum valid, populate sensor report
 				int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0];
 				int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2];
-				flow->pixel_flow_x_integral = static_cast<float>(delta_x) * (3.52e-3f);
-				flow->pixel_flow_y_integral = static_cast<float>(delta_y) * (3.52e-3f);
+				flow->pixel_flow[0] = static_cast<float>(delta_x) * (3.52e-3f);
+				flow->pixel_flow[1] = static_cast<float>(delta_y) * (3.52e-3f);
 				*state = THONEFLOW_PARSE_STATE7_CHECKSUM;
 
 			} else {

src/drivers/optical_flow/thoneflow/thoneflow_parser.h

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 // Data Format for ThoneFlow 3901U
 // ===============================
@@ -62,4 +62,4 @@ enum THONEFLOW_PARSE_STATE {
 };
 
 bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
-		     optical_flow_s *report);
+		     sensor_optical_flow_s *report);

src/drivers/uavcan/sensors/flow.cpp

@@ -38,7 +38,7 @@
 const char *const UavcanFlowBridge::NAME = "flow";
 
 UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) :
-	UavcanSensorBridgeBase("uavcan_flow", ORB_ID(optical_flow)),
+	UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow)),
 	_sub_flow(node)
 {
 }
@@ -58,23 +58,41 @@ UavcanFlowBridge::init()
 
 void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex::equipment::flow::Measurement> &msg)
 {
-	optical_flow_s flow{};
+	sensor_optical_flow_s flow{};
+	flow.timestamp_sample = hrt_absolute_time(); // TODO
 
-	// We're only given an 8 bit field for sensor ID; just use the UAVCAN node ID
-	flow.sensor_id = msg.getSrcNodeID().get();
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN;
+	device_id.devid_s.bus = 0;
+	device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_UAVCAN;
+	device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
 
-	flow.timestamp = hrt_absolute_time();
-	flow.integration_timespan = 1.e6f * msg.integration_interval; // s -> micros
-	flow.pixel_flow_x_integral = msg.flow_integral[0];
-	flow.pixel_flow_y_integral = msg.flow_integral[1];
+	flow.device_id = device_id.devid;
 
-	flow.gyro_x_rate_integral = msg.rate_gyro_integral[0];
-	flow.gyro_y_rate_integral = msg.rate_gyro_integral[1];
+	flow.pixel_flow[0] = msg.flow_integral[0];
+	flow.pixel_flow[1] = msg.flow_integral[1];
+
+	flow.integration_timespan_us = 1.e6f * msg.integration_interval; // s -> us
 
 	flow.quality = msg.quality;
-	flow.max_flow_rate = 5.0f;       // Datasheet: 7.4 rad/s
-	flow.min_ground_distance = 0.1f; // Datasheet: 80mm
-	flow.max_ground_distance = 30.0f; // Datasheet: infinity
+
+	if (PX4_ISFINITE(msg.rate_gyro_integral[0]) && PX4_ISFINITE(msg.rate_gyro_integral[1])) {
+		flow.delta_angle[0] = msg.rate_gyro_integral[0];
+		flow.delta_angle[1] = msg.rate_gyro_integral[1];
+		flow.delta_angle[2] = 0.f;
+		flow.delta_angle_available = true;
+
+	} else {
+		flow.delta_angle[0] = NAN;
+		flow.delta_angle[1] = NAN;
+		flow.delta_angle[2] = NAN;
+	}
+
+	flow.max_flow_rate = NAN;
+	flow.min_ground_distance = NAN;
+	flow.max_ground_distance = NAN;
+
+	flow.timestamp = hrt_absolute_time();
 
 	publish(msg.getSrcNodeID().get(), &flow);
 }

src/drivers/uavcan/sensors/flow.hpp

@@ -37,7 +37,7 @@
 
 #include <stdint.h>
 
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 
 #include <com/hex/equipment/flow/Measurement.hpp>
 

src/drivers/uavcannode/Publishers/FlowMeasurement.hpp

@@ -39,7 +39,7 @@
 #include <conversion/rotation.h>
 
 #include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow.h>
 
 namespace uavcannode
 {
@@ -52,18 +52,9 @@ class FlowMeasurement :
 public:
 	FlowMeasurement(px4::WorkItem *work_item, uavcan::INode &node) :
 		UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID),
-		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_optical_flow)),
 		uavcan::Publisher<com::hex::equipment::flow::Measurement>(node)
 	{
-		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, 0.f}};
-
-		param_t rot = param_find("CANNODE_FLOW_ROT");
-		int32_t val = 0;
-
-		if (param_get(rot, &val) == PX4_OK) {
-			_rotation = get_rot_matrix((enum Rotation)val);
-		}
-
 		this->setPriority(uavcan::TransferPriority::Default);
 	}
 
@@ -80,20 +71,18 @@ public:
 	void BroadcastAnyUpdates() override
 	{
 		// optical_flow -> com::hex::equipment::flow::Measurement
-		optical_flow_s optical_flow;
+		vehicle_optical_flow_s optical_flow;
 
 		if (uORB::SubscriptionCallbackWorkItem::update(&optical_flow)) {
 			com::hex::equipment::flow::Measurement measurement{};
-			measurement.integration_interval  = optical_flow.integration_timespan * 1e-6f; // us -> s
+			measurement.integration_interval  = optical_flow.integration_timespan_us * 1e-6f; // us -> s
 
 			// rotate measurements in yaw from sensor frame to body frame
-			const matrix::Vector3f gyro_flow_rotated = _rotation * matrix::Vector3f{optical_flow.gyro_x_rate_integral, optical_flow.gyro_y_rate_integral, 0.f};
-			const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{optical_flow.pixel_flow_x_integral, optical_flow.pixel_flow_y_integral, 0.f};
+			measurement.rate_gyro_integral[0] = optical_flow.delta_angle[0];
+			measurement.rate_gyro_integral[1] = optical_flow.delta_angle[1];
 
-			measurement.rate_gyro_integral[0] = gyro_flow_rotated(0);
-			measurement.rate_gyro_integral[1] = gyro_flow_rotated(1);
-			measurement.flow_integral[0] = pixel_flow_rotated(0);
-			measurement.flow_integral[1] = pixel_flow_rotated(1);
+			measurement.flow_integral[0] = optical_flow.pixel_flow[0];
+			measurement.flow_integral[1] = optical_flow.pixel_flow[1];
 
 			measurement.quality = optical_flow.quality;
 

src/drivers/uavcannode/uavcannode_params.c

@@ -60,27 +60,6 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000);
  */
 PARAM_DEFINE_INT32(CANNODE_TERM, 0);
 
-/**
- * Cannode flow board rotation
- *
- * This parameter defines the yaw rotation of the Cannode flow board relative to the vehicle body frame.
- * Zero rotation is defined as X on flow board pointing towards front of vehicle.
- *
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- *
- * @reboot_required true
- *
- * @group UAVCAN
- */
-PARAM_DEFINE_INT32(CANNODE_FLOW_ROT, 0);
-
 /**
  * Enable RTCM pub/sub
  *

src/lib/sensor_calibration/Gyroscope.cpp

@@ -110,7 +110,7 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
 
 bool Gyroscope::set_offset(const Vector3f &offset)
 {
-	if (Vector3f(_offset - offset).longerThan(0.01f)) {
+	if (Vector3f(_offset - offset).longerThan(0.01f) || (_calibration_count == 0)) {
 		if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) {
 			_offset = offset;
 			_calibration_count++;

src/modules/ekf2/EKF/ekf.h

@@ -104,9 +104,12 @@ public:
 
 	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
 	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
+
 	const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; }
 	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; }
-	const Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; }
+
+	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 getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
 	void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }

src/modules/ekf2/EKF2.cpp

@@ -1426,14 +1426,18 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 {
 	if (_ekf.getFlowCompensated().longerThan(0.f)) {
-		estimator_optical_flow_vel_s flow_vel{};
+		vehicle_optical_flow_vel_s flow_vel{};
 		flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
 
 		_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
 		_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
+
 		_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
 		_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
-		_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
+
+		_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate);
+		_ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral);
+
 		flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_estimator_optical_flow_vel_pub.publish(flow_vel);
@@ -1711,29 +1715,29 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF flow sample
 	bool new_optical_flow = false;
-	const unsigned last_generation = _optical_flow_sub.get_last_generation();
-	optical_flow_s optical_flow;
+	const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation();
+	vehicle_optical_flow_s optical_flow;
 
-	if (_optical_flow_sub.update(&optical_flow)) {
+	if (_vehicle_optical_flow_sub.update(&optical_flow)) {
 		if (_msg_missed_optical_flow_perf == nullptr) {
 			_msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed");
 
-		} else if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
+		} else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) {
 			perf_count(_msg_missed_optical_flow_perf);
 		}
 
 		flowSample flow {
-			.time_us = optical_flow.timestamp,
+			.time_us = optical_flow.timestamp_sample,
 			// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
 			// is produced by a RH rotation of the image about the sensor axis.
-			.flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral},
-			.gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral},
-			.dt = 1e-6f * (float)optical_flow.integration_timespan,
+			.flow_xy_rad = Vector2f{-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]},
+			.gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]},
+			.dt = 1e-6f * (float)optical_flow.integration_timespan_us,
 			.quality = optical_flow.quality,
 		};
 
-		if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
-		    PX4_ISFINITE(optical_flow.pixel_flow_x_integral) &&
+		if (PX4_ISFINITE(optical_flow.pixel_flow[0]) &&
+		    PX4_ISFINITE(optical_flow.pixel_flow[1]) &&
 		    flow.dt < 1) {
 
 			// Save sensor limits reported by the optical flow sensor

src/modules/ekf2/EKF2.hpp

@@ -71,13 +71,11 @@
 #include <uORB/topics/estimator_event_flags.h>
 #include <uORB/topics/estimator_gps_status.h>
 #include <uORB/topics/estimator_innovations.h>
-#include <uORB/topics/estimator_optical_flow_vel.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/estimator_status_flags.h>
 #include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/optical_flow.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_selection.h>
@@ -91,11 +89,12 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/vehicle_optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow_vel.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
 #include <uORB/topics/yaw_estimator_status.h>
 
-
 extern pthread_mutex_t ekf2_module_mutex;
 
 class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
@@ -276,12 +275,12 @@ private:
 	uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
 	uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
-	uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
@@ -314,12 +313,12 @@ private:
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovations_pub{ORB_ID(estimator_innovations)};
-	uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
 	uORB::PublicationMulti<estimator_sensor_bias_s>      _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
 	uORB::PublicationMulti<estimator_states_s>           _estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::PublicationMulti<estimator_status_flags_s>     _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
 	uORB::PublicationMulti<estimator_status_s>           _estimator_status_pub{ORB_ID(estimator_status)};
 	uORB::PublicationMulti<vehicle_odometry_s>           _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
+	uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
 	uORB::PublicationMulti<yaw_estimator_status_s>       _yaw_est_pub{ORB_ID(yaw_estimator_status)};
 
 	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};

src/modules/gyro_calibration/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2021-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
@@ -41,4 +41,5 @@ px4_add_module(
 		GyroCalibration.hpp
 	DEPENDS
 		px4_work_queue
+		sensor_calibration
 	)

src/modules/gyro_calibration/GyroCalibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -245,7 +245,7 @@ void GyroCalibration::Run()
 
 				const Vector3f old_offset{_gyro_calibration[gyro].offset()};
 
-				if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) {
+				if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean()) || !_gyro_calibration[gyro].calibrated()) {
 
 					calibration_updated = true;
 

src/modules/gyro_calibration/GyroCalibration.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -20,7 +20,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/parameter_update.h>
@@ -270,7 +270,7 @@ private:
 	uORB::SubscriptionData<vehicle_land_detected_s> _sub_land{ORB_ID(vehicle_land_detected)};
 	uORB::SubscriptionData<vehicle_attitude_s> _sub_att{ORB_ID(vehicle_attitude)};
 	uORB::SubscriptionData<vehicle_angular_velocity_s> _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)};
-	uORB::SubscriptionData<optical_flow_s> _sub_flow{ORB_ID(optical_flow)};
+	uORB::SubscriptionData<vehicle_optical_flow_s> _sub_flow{ORB_ID(vehicle_optical_flow)};
 	uORB::SubscriptionData<vehicle_gps_position_s> _sub_gps{ORB_ID(vehicle_gps_position)};
 	uORB::SubscriptionData<vehicle_odometry_s> _sub_visual_odom{ORB_ID(vehicle_visual_odometry)};
 	uORB::SubscriptionData<vehicle_odometry_s> _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)};

src/modules/local_position_estimator/sensors/flow.cpp

@@ -61,9 +61,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 
 	// optical flow in x, y axis
 	// TODO consider making flow scale a states of the kalman filter
-	float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _param_lpe_flw_scale.get();
-	float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _param_lpe_flw_scale.get();
-	float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
+	float flow_x_rad = _sub_flow.get().pixel_flow[0] * _param_lpe_flw_scale.get();
+	float flow_y_rad = _sub_flow.get().pixel_flow[1] * _param_lpe_flw_scale.get();
+	float dt_flow = _sub_flow.get().integration_timespan_us / 1.0e6f;
 
 	if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
 		return -1;
@@ -74,10 +74,8 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	float gyro_y_rad = 0;
 
 	if (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) {
-		gyro_x_rad = _flow_gyro_x_high_pass.update(
-				     _sub_flow.get().gyro_x_rate_integral);
-		gyro_y_rad = _flow_gyro_y_high_pass.update(
-				     _sub_flow.get().gyro_y_rate_integral);
+		gyro_x_rad = _flow_gyro_x_high_pass.update(_sub_flow.get().delta_angle[0]);
+		gyro_y_rad = _flow_gyro_y_high_pass.update(_sub_flow.get().delta_angle[1]);
 	}
 
 	//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",

src/modules/logger/logged_topics.cpp

@@ -162,7 +162,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES);
-	add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_optical_flow_vel", 0, MAX_ESTIMATOR_INSTANCES); // TODO: restore
 	add_optional_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES);
@@ -174,7 +174,6 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("battery_status", 200, 2);
 	add_topic_multi("differential_pressure", 1000, 2);
 	add_topic_multi("distance_sensor", 1000, 2);
-	add_topic_multi("optical_flow", 1000, 1);
 	add_optional_topic_multi("sensor_accel", 1000, 4);
 	add_optional_topic_multi("sensor_baro", 1000, 4);
 	add_topic_multi("sensor_gps", 1000, 2);
@@ -182,9 +181,13 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("pps_capture", 1000);
 	add_optional_topic_multi("sensor_gyro", 1000, 4);
 	add_optional_topic_multi("sensor_mag", 1000, 4);
+	add_topic_multi("sensor_optical_flow", 0, 1); // TODO: 0 -> 1000 and optional
+
 	add_topic_multi("vehicle_imu", 500, 4);
 	add_topic_multi("vehicle_imu_status", 1000, 4);
 	add_optional_topic_multi("vehicle_magnetometer", 500, 4);
+	add_topic_multi("vehicle_optical_flow", 0, 1); // TODO: 0 -> 1000 and optional
+	add_topic_multi("vehicle_optical_flow_vel", 0, 1); // // TODO: 0 -> 1000 and optional
 
 	// SYS_CTRL_ALLOC: additional dynamic control allocation logging when enabled
 	int32_t sys_ctrl_alloc = 0;

src/modules/mavlink/mavlink_receiver.cpp

@@ -89,10 +89,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_parameters_manager(parent),
 	_mavlink_timesync(parent)
 {
-	_handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT");
-	_handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR");
-	_handle_sens_flow_minhgt = param_find("SENS_FLOW_MINHGT");
-	_handle_sens_flow_rot = param_find("SENS_FLOW_ROT");
 	_handle_ekf2_min_rng = param_find("EKF2_MIN_RNG");
 	_handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX");
 }
@@ -799,45 +795,43 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 	mavlink_optical_flow_rad_t flow;
 	mavlink_msg_optical_flow_rad_decode(msg, &flow);
 
-	optical_flow_s f{};
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
+	device_id.devid_s.address = msg->sysid;
+
+	sensor_optical_flow_s f{};
+
+	f.timestamp_sample = hrt_absolute_time();
+	f.device_id = device_id.devid;
+
+	f.pixel_flow[0] = flow.integrated_x;
+	f.pixel_flow[1] = flow.integrated_y;
+
+	f.integration_timespan_us = flow.integration_time_us;
+	f.quality = flow.quality;
+
+	if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
+		f.delta_angle[0] = flow.integrated_xgyro;
+		f.delta_angle[1] = flow.integrated_ygyro;
+		f.delta_angle[2] = flow.integrated_zgyro;
+		f.delta_angle_available = true;
+	}
+
+	//f.ground_distance_m = flow.distance;
+
+	f.max_flow_rate       = NAN;
+	f.min_ground_distance = NAN;
+	f.max_ground_distance = NAN;
 
 	f.timestamp = hrt_absolute_time();
-	f.time_since_last_sonar_update = flow.time_delta_distance_us;
-	f.integration_timespan  = flow.integration_time_us;
-	f.pixel_flow_x_integral = flow.integrated_x;
-	f.pixel_flow_y_integral = flow.integrated_y;
-	f.gyro_x_rate_integral  = flow.integrated_xgyro;
-	f.gyro_y_rate_integral  = flow.integrated_ygyro;
-	f.gyro_z_rate_integral  = flow.integrated_zgyro;
-	f.gyro_temperature      = flow.temperature;
-	f.ground_distance_m     = flow.distance;
-	f.quality               = flow.quality;
-	f.sensor_id             = flow.sensor_id;
-	f.max_flow_rate         = _param_sens_flow_maxr;
-	f.min_ground_distance   = _param_sens_flow_minhgt;
-	f.max_ground_distance   = _param_sens_flow_maxhgt;
 
-	/* read flow sensor parameters */
-	const Rotation flow_rot = (Rotation)_param_sens_flow_rot;
-
-	/* rotate measurements according to parameter */
-	float zero_val = 0.0f;
-	rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val);
-	rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);
-
-	_flow_pub.publish(f);
+	_sensor_optical_flow_pub.publish(f);
 
 	/* Use distance value for distance sensor topic */
-	if (flow.distance > 0.0f) { // negative values signal invalid data
+	if (PX4_ISFINITE(flow.distance) && (flow.distance > 0.0f)) { // negative values signal invalid data
 
 		distance_sensor_s d{};
-
-		device::Device::DeviceId device_id;
-		device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
-		device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
-		device_id.devid_s.address = msg->sysid;
-
-		d.timestamp = f.timestamp;
 		d.min_distance = _param_ekf2_min_rng;
 		d.max_distance = _param_ekf2_rng_a_hmax;
 		d.current_distance = flow.distance; /* both are in m */
@@ -846,6 +840,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 		d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 		d.variance = 0.01;
 		d.signal_quality = -1;
+		d.timestamp = f.timestamp;
 
 		_flow_distance_sensor_pub.publish(d);
 	}
@@ -858,32 +853,42 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	mavlink_hil_optical_flow_t flow;
 	mavlink_msg_hil_optical_flow_decode(msg, &flow);
 
-	optical_flow_s f{};
-
-	f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
-	f.integration_timespan = flow.integration_time_us;
-	f.pixel_flow_x_integral = flow.integrated_x;
-	f.pixel_flow_y_integral = flow.integrated_y;
-	f.gyro_x_rate_integral = flow.integrated_xgyro;
-	f.gyro_y_rate_integral = flow.integrated_ygyro;
-	f.gyro_z_rate_integral = flow.integrated_zgyro;
-	f.time_since_last_sonar_update = flow.time_delta_distance_us;
-	f.ground_distance_m = flow.distance;
-	f.quality = flow.quality;
-	f.sensor_id = flow.sensor_id;
-	f.gyro_temperature = flow.temperature;
-
-	_flow_pub.publish(f);
-
-	/* Use distance value for distance sensor topic */
-	distance_sensor_s d{};
-
 	device::Device::DeviceId device_id;
 	device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
 	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
 	device_id.devid_s.address = msg->sysid;
 
-	d.timestamp = hrt_absolute_time();
+	sensor_optical_flow_s f{};
+
+	f.timestamp_sample = hrt_absolute_time();
+	f.device_id = device_id.devid;
+
+	f.pixel_flow[0] = flow.integrated_x;
+	f.pixel_flow[1] = flow.integrated_y;
+
+	f.integration_timespan_us = flow.integration_time_us;
+	f.quality = flow.quality;
+
+	if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
+		f.delta_angle[0] = flow.integrated_xgyro;
+		f.delta_angle[1] = flow.integrated_ygyro;
+		f.delta_angle[2] = flow.integrated_zgyro;
+		f.delta_angle_available = true;
+	}
+
+	//f.ground_distance_m = flow.distance;
+
+	f.max_flow_rate       = NAN;
+	f.min_ground_distance = NAN;
+	f.max_ground_distance = NAN;
+
+	f.timestamp = hrt_absolute_time();
+
+	_sensor_optical_flow_pub.publish(f);
+
+
+	/* Use distance value for distance sensor topic */
+	distance_sensor_s d{};
 	d.min_distance = 0.3f;
 	d.max_distance = 5.0f;
 	d.current_distance = flow.distance; /* both are in m */
@@ -892,6 +897,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	d.variance = 0.01;
 	d.signal_quality = -1;
+	d.timestamp = hrt_absolute_time();
 
 	_flow_distance_sensor_pub.publish(d);
 }
@@ -3476,22 +3482,6 @@ MavlinkReceiver::updateParams()
 	// update parameters from storage
 	ModuleParams::updateParams();
 
-	if (_handle_sens_flow_maxhgt != PARAM_INVALID) {
-		param_get(_handle_sens_flow_maxhgt, &_param_sens_flow_maxhgt);
-	}
-
-	if (_handle_sens_flow_maxr != PARAM_INVALID) {
-		param_get(_handle_sens_flow_maxr, &_param_sens_flow_maxr);
-	}
-
-	if (_handle_sens_flow_minhgt != PARAM_INVALID) {
-		param_get(_handle_sens_flow_minhgt, &_param_sens_flow_minhgt);
-	}
-
-	if (_handle_sens_flow_rot != PARAM_INVALID) {
-		param_get(_handle_sens_flow_rot, &_param_sens_flow_rot);
-	}
-
 	if (_handle_ekf2_min_rng != PARAM_INVALID) {
 		param_get(_handle_ekf2_min_rng, &_param_ekf2_min_rng);
 	}

src/modules/mavlink/mavlink_receiver.h

@@ -86,13 +86,13 @@
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/onboard_computer_status.h>
-#include <uORB/topics/optical_flow.h>
 #include <uORB/topics/ping.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/trajectory_setpoint.h>
@@ -308,7 +308,6 @@ private:
 	uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
-	uORB::Publication<optical_flow_s>			_flow_pub{ORB_ID(optical_flow)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
@@ -338,6 +337,7 @@ private:
 	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status)};
 	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::PublicationMulti<sensor_optical_flow_s>         _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	// ORB publications (queue length > 1)
 	uORB::Publication<gps_inject_data_s>     _gps_inject_data_pub{ORB_ID(gps_inject_data)};
@@ -396,17 +396,9 @@ private:
 	hrt_abstime _heartbeat_component_udp_bridge{0};
 	hrt_abstime _heartbeat_component_uart_bridge{0};
 
-	param_t _handle_sens_flow_maxhgt{PARAM_INVALID};
-	param_t _handle_sens_flow_maxr{PARAM_INVALID};
-	param_t _handle_sens_flow_minhgt{PARAM_INVALID};
-	param_t _handle_sens_flow_rot{PARAM_INVALID};
 	param_t _handle_ekf2_min_rng{PARAM_INVALID};
 	param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID};
 
-	float _param_sens_flow_maxhgt{-1.0f};
-	float _param_sens_flow_maxr{-1.0f};
-	float _param_sens_flow_minhgt{-1.0f};
-	int32_t _param_sens_flow_rot{0};
 	float _param_ekf2_min_rng{NAN};
 	float _param_ekf2_rng_a_hmax{NAN};
 

src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp

@@ -34,7 +34,7 @@
 #ifndef OPTICAL_FLOW_RAD_HPP
 #define OPTICAL_FLOW_RAD_HPP
 
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow.h>
 
 class MavlinkStreamOpticalFlowRad : public MavlinkStream
 {
@@ -49,34 +49,34 @@ public:
 
 	unsigned get_size() override
 	{
-		return _optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _vehicle_optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) :
+		       0;
 	}
 
 private:
 	explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
-	uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
+	uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
 
 	bool send() override
 	{
-		optical_flow_s flow;
+		vehicle_optical_flow_s flow;
 
-		if (_optical_flow_sub.update(&flow)) {
+		if (_vehicle_optical_flow_sub.update(&flow)) {
 			mavlink_optical_flow_rad_t msg{};
 
 			msg.time_usec = flow.timestamp;
-			msg.sensor_id = flow.sensor_id;
-			msg.integrated_x = flow.pixel_flow_x_integral;
-			msg.integrated_y = flow.pixel_flow_y_integral;
-			msg.integrated_xgyro = flow.gyro_x_rate_integral;
-			msg.integrated_ygyro = flow.gyro_y_rate_integral;
-			msg.integrated_zgyro = flow.gyro_z_rate_integral;
-			msg.distance = flow.ground_distance_m;
+			msg.sensor_id = _vehicle_optical_flow_sub.get_instance();
+			msg.integrated_x = flow.pixel_flow[0];
+			msg.integrated_y = flow.pixel_flow[1];
+			msg.integrated_xgyro = flow.delta_angle[0];
+			msg.integrated_ygyro = flow.delta_angle[1];
+			msg.integrated_zgyro = flow.delta_angle[2];
+			//msg.distance = flow.ground_distance_m;
 			msg.quality = flow.quality;
-			msg.integration_time_us = flow.integration_timespan;
-			msg.sensor_id = flow.sensor_id;
-			msg.time_delta_distance_us = flow.time_since_last_sonar_update;
-			msg.temperature = flow.gyro_temperature;
+			msg.integration_time_us = flow.integration_timespan_us;
+			//msg.time_delta_distance_us = flow.time_since_last_sonar_update;
+			//msg.temperature = flow.gyro_temperature;
 
 			mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/replay/ReplayEkf2.cpp

@@ -39,7 +39,6 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/optical_flow.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -47,6 +46,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
+#include <uORB/topics/vehicle_optical_flow.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_odometry.h>
 
@@ -91,7 +91,7 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
 	} else if (sub.orb_meta == ORB_ID(distance_sensor)) {
 		_distance_sensor_msg_id = msg_id;
 
-	} else if (sub.orb_meta == ORB_ID(optical_flow)) {
+	} else if (sub.orb_meta == ORB_ID(vehicle_optical_flow)) {
 		_optical_flow_msg_id = msg_id;
 
 	} else if (sub.orb_meta == ORB_ID(vehicle_air_data)) {
@@ -204,7 +204,7 @@ ReplayEkf2::onExitMainLoop()
 
 	print_sensor_statistics(_airspeed_msg_id, "airspeed");
 	print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor");
-	print_sensor_statistics(_optical_flow_msg_id, "optical_flow");
+	print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow");
 	print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined");
 	print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
 	print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");

src/modules/sensors/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-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
@@ -34,32 +34,65 @@
 add_subdirectory(data_validator)
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
-add_subdirectory(vehicle_air_data)
-add_subdirectory(vehicle_gps_position)
 add_subdirectory(vehicle_imu)
-add_subdirectory(vehicle_magnetometer)
+
+if(CONFIG_SENSORS_VEHICLE_AIR_DATA)
+	add_subdirectory(vehicle_air_data)
+endif()
+
+if(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+	add_subdirectory(vehicle_gps_position)
+endif()
+
+if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
+	add_subdirectory(vehicle_magnetometer)
+endif()
+
+if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+	add_subdirectory(vehicle_optical_flow)
+endif()
 
 px4_add_module(
 	MODULE modules__sensors
 	MAIN sensors
+	INCLUDES
+		${CMAKE_CURRENT_SOURCE_DIR}
 	SRCS
 		sensors.cpp
 		voted_sensors_update.cpp
 		voted_sensors_update.h
+		Integrator.hpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
-		airspeed
 		conversion
 		data_validator
 		mathlib
 		sensor_calibration
 		vehicle_acceleration
 		vehicle_angular_velocity
-		vehicle_air_data
-		vehicle_gps_position
 		vehicle_imu
-		vehicle_magnetometer
 	)
+
+if(CONFIG_SENSORS_VEHICLE_AIRSPEED)
+	target_link_libraries(modules__sensors PRIVATE airspeed)
+endif()
+
+if(CONFIG_SENSORS_VEHICLE_AIR_DATA)
+	target_link_libraries(modules__sensors PRIVATE vehicle_air_data)
+endif()
+
+if(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+	target_link_libraries(modules__sensors PRIVATE vehicle_gps_position)
+endif()
+
+if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
+	target_link_libraries(modules__sensors PRIVATE vehicle_magnetometer)
+endif()
+
+if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+	target_link_libraries(modules__sensors PRIVATE vehicle_optical_flow)
+endif()

src/modules/sensors/Integrator.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-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
@@ -45,6 +45,9 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 
+namespace sensors
+{
+
 class Integrator
 {
 public:
@@ -94,6 +97,8 @@ public:
 	 */
 	inline bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); }
 
+	float integral_dt() const { return _integral_dt; }
+
 	void reset()
 	{
 		_alpha.zero();
@@ -137,7 +142,7 @@ protected:
 	matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */
 	float _integral_dt{0};
 
-	float _reset_interval_min{0.005f}; /**< the interval after which the content will be published and the integrator reset */
+	float _reset_interval_min{0.001f}; /**< the interval after which the content will be published and the integrator reset */
 	uint8_t _reset_samples_min{1};
 
 	uint8_t _integrated_samples{0};
@@ -214,3 +219,5 @@ private:
 	matrix::Vector3f _last_alpha{0.f, 0.f, 0.f};       /**< previous value of _alpha */
 
 };
+
+}; // namespace sensors

src/modules/sensors/Kconfig

@@ -10,3 +10,26 @@ menuconfig USER_SENSORS
 	depends on BOARD_PROTECTED && MODULES_SENSORS
 	---help---
 		Put sensors in userspace memory
+
+if MODULES_SENSORS
+    config SENSORS_VEHICLE_AIRSPEED
+        bool "Include vehicle airspeed"
+        default y
+
+    config SENSORS_VEHICLE_AIR_DATA
+        bool "Include vehicle air data"
+        default y
+
+    config SENSORS_VEHICLE_GPS_POSITION
+        bool "Include vehicle gps position"
+        default y
+
+    config SENSORS_VEHICLE_MAGNETOMETER
+        bool "Include vehicle magnetometer"
+        default y
+
+    config SENSORS_VEHICLE_OPTICAL_FLOW
+        bool "Include vehicle optical flow"
+        default y
+
+endif #MODULES_SENSORS

src/modules/sensors/sensor_params_flow.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -32,11 +32,10 @@
  ****************************************************************************/
 
 /**
- * PX4Flow board rotation
+ * Optical flow rotation
  *
- * This parameter defines the yaw rotation of the PX4FLOW board relative to the vehicle body frame.
+ * This parameter defines the yaw rotation of the optical flow relative to the vehicle body frame.
  * Zero rotation is defined as X on flow board pointing towards front of vehicle.
- * The recommneded installation default for the PX4FLOW board is with the Y axis forward (270 deg yaw).
  *
  * @value 0 No rotation
  * @value 1 Yaw 45°
@@ -47,11 +46,9 @@
  * @value 6 Yaw 270°
  * @value 7 Yaw 315°
  *
- * @reboot_required true
- *
  * @group Sensors
  */
-PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6);
+PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
 
 /**
  * Minimum height above ground when reliant on optical flow.
@@ -66,7 +63,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6);
  * @decimal 1
  * @group Sensor Calibration
  */
-PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f);
+PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.08f);
 
 /**
  * Maximum height above ground when reliant on optical flow.
@@ -78,12 +75,12 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f);
  *
  * @unit m
  * @min 1.0
- * @max 25.0
+ * @max 100.0
  * @increment 0.1
  * @decimal 1
  * @group Sensor Calibration
  */
-PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f);
+PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 100.f);
 
 /**
  * Magnitude of maximum angular flow rate reliably measurable by the optical flow sensor.
@@ -97,4 +94,20 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f);
  * @decimal 2
  * @group Sensor Calibration
  */
-PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 2.5f);
+PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 8.f);
+
+/**
+ * Optical flow max rate.
+ *
+ * Optical flow data maximum publication rate. This is an upper bound,
+ * actual optical flow data rate is still dependant on the sensor.
+ *
+ * @min 1
+ * @max 200
+ * @group Sensors
+ * @unit Hz
+ *
+ * @reboot_required true
+ *
+ */
+PARAM_DEFINE_FLOAT(SENS_FLOW_RATE, 70.0f);

src/modules/sensors/sensors.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -41,13 +41,11 @@
  * @author Beat Küng <beat-kueng@gmx.net>
  */
 
-#include <drivers/drv_adc.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_sensor.h>
-#include <lib/airspeed/airspeed.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
+
 #include <lib/sensor_calibration/Utilities.hpp>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
@@ -61,23 +59,43 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensors_status_imu.h>
-#include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu.h>
 
 #include "voted_sensors_update.h"
 #include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
-#include "vehicle_air_data/VehicleAirData.hpp"
-#include "vehicle_gps_position/VehicleGPSPosition.hpp"
 #include "vehicle_imu/VehicleIMU.hpp"
-#include "vehicle_magnetometer/VehicleMagnetometer.hpp"
+
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
+# include <drivers/drv_sensor.h>
+# include <drivers/drv_adc.h>
+# include <lib/airspeed/airspeed.h>
+# include <uORB/topics/airspeed.h>
+# include <uORB/topics/differential_pressure.h>
+# include <uORB/topics/vehicle_air_data.h>
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
+
+#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
+# include <uORB/topics/sensor_baro.h>
+# include "vehicle_air_data/VehicleAirData.hpp"
+#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
+
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+# include "vehicle_gps_position/VehicleGPSPosition.hpp"
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
+
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
+# include "vehicle_magnetometer/VehicleMagnetometer.hpp"
+# include <lib/sensor_calibration/Magnetometer.hpp>
+# include <uORB/topics/sensor_mag.h>
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+# include "vehicle_optical_flow/VehicleOpticalFlow.hpp"
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
 
 using namespace sensors;
 using namespace time_literals;
@@ -127,16 +145,18 @@ private:
 	};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
 	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 
-	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<sensor_combined_s>      _sensor_pub{ORB_ID(sensor_combined)};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
+	uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+
+	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
+
 	DataValidator	_airspeed_validator;		/**< data validator to monitor airspeed */
 
 	uint64_t _airspeed_last_publish{0};
@@ -146,11 +166,10 @@ private:
 	float _baro_pressure_sum{0.f};
 	int _diff_pres_count{0};
 
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
+# ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 	uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};
 	uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)};
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
+# endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
 
 	struct Parameters {
 		float diff_pres_offset_pa;
@@ -173,28 +192,44 @@ private:
 		param_t air_tube_length;
 		param_t air_tube_diameter_mm;
 	} _parameter_handles{};		/**< handles for interesting parameters */
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
 	VotedSensorsUpdate _voted_sensors_update;
 
 	VehicleAcceleration	_vehicle_acceleration;
 	VehicleAngularVelocity	_vehicle_angular_velocity;
-	VehicleAirData          *_vehicle_air_data{nullptr};
-	VehicleMagnetometer     *_vehicle_magnetometer{nullptr};
-	VehicleGPSPosition	*_vehicle_gps_position{nullptr};
 
-	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
+#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
+	VehicleAirData *_vehicle_air_data {nullptr};
+	uint8_t _n_baro{0};
+#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
+
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
+	VehicleMagnetometer *_vehicle_magnetometer {nullptr};
+	uint8_t _n_mag{0};
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+	VehicleGPSPosition *_vehicle_gps_position {nullptr};
+	uint8_t _n_gps{0};
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
+
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+	VehicleOpticalFlow *_vehicle_optical_flow {nullptr};
+	uint8_t _n_optical_flow{0};
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
+	VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
 
 	uint8_t _n_accel{0};
-	uint8_t _n_baro{0};
-	uint8_t _n_gps{0};
 	uint8_t _n_gyro{0};
-	uint8_t _n_mag{0};
 
 	/**
 	 * Update our local parameter cache.
 	 */
 	int		parameters_update();
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	/**
 	 * Poll the differential pressure sensor for updated data.
 	 *
@@ -203,11 +238,6 @@ private:
 	 */
 	void		diff_pres_poll();
 
-	/**
-	 * Check for changes in parameters.
-	 */
-	void 		parameter_update_poll(bool forced = false);
-
 	/**
 	 * Poll the ADC and update readings to suit.
 	 *
@@ -215,16 +245,24 @@ private:
 	 *				data should be returned.
 	 */
 	void		adc_poll();
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
 	void		InitializeVehicleAirData();
 	void		InitializeVehicleGPSPosition();
 	void		InitializeVehicleIMU();
 	void		InitializeVehicleMagnetometer();
+	void		InitializeVehicleOpticalFlow();
 
 	DEFINE_PARAMETERS(
+#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
 		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
+#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
 		(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
 };
@@ -241,6 +279,7 @@ Sensors::Sensors(bool hil_enabled) :
 	_vehicle_angular_velocity.Start();
 	_vehicle_acceleration.Start();
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
@@ -251,6 +290,10 @@ Sensors::Sensors(bool hil_enabled) :
 	_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
 	_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
 
+	_airspeed_validator.set_timeout(300000);
+	_airspeed_validator.set_equal_value_threshold(100);
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
+
 	param_find("SYS_FAC_CAL_MODE");
 
 	// Parameters controlling the on-board sensor thermal calibrator
@@ -260,15 +303,9 @@ Sensors::Sensors(bool hil_enabled) :
 
 	_sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
 
-	_airspeed_validator.set_timeout(300000);
-	_airspeed_validator.set_equal_value_threshold(100);
-
 	parameters_update();
 
-	InitializeVehicleAirData();
-	InitializeVehicleGPSPosition();
 	InitializeVehicleIMU();
-	InitializeVehicleMagnetometer();
 }
 
 Sensors::~Sensors()
@@ -281,21 +318,42 @@ Sensors::~Sensors()
 	_vehicle_acceleration.Stop();
 	_vehicle_angular_velocity.Stop();
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
+
 	if (_vehicle_air_data) {
 		_vehicle_air_data->Stop();
 		delete _vehicle_air_data;
 	}
 
+#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
+
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+
 	if (_vehicle_gps_position) {
 		_vehicle_gps_position->Stop();
 		delete _vehicle_gps_position;
 	}
 
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
+
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
+
 	if (_vehicle_magnetometer) {
 		_vehicle_magnetometer->Stop();
 		delete _vehicle_magnetometer;
 	}
 
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+
+	if (_vehicle_optical_flow) {
+		_vehicle_optical_flow->Stop();
+		delete _vehicle_optical_flow;
+	}
+
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
 	for (auto &vehicle_imu : _vehicle_imu_list) {
 		if (vehicle_imu) {
 			vehicle_imu->Stop();
@@ -319,6 +377,7 @@ int Sensors::parameters_update()
 		return 0;
 	}
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	/* Airspeed offset */
 	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
@@ -328,68 +387,91 @@ int Sensors::parameters_update()
 	param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel);
 	param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length);
 	param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm);
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
 	_voted_sensors_update.parametersUpdate();
 
-	// mark all existing sensor calibrations active even if sensor is missing
-	// this preserves the calibration in the event of a parameter export while the sensor is missing
-	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-		uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC",  "ID", i);
-		uint32_t device_id_gyro  = calibration::GetCalibrationParamInt32("GYRO", "ID", i);
-		uint32_t device_id_mag   = calibration::GetCalibrationParamInt32("MAG",  "ID", i);
-
-		if (device_id_accel != 0) {
-			calibration::Accelerometer accel_cal(device_id_accel);
-		}
-
-		if (device_id_gyro != 0) {
-			calibration::Gyroscope gyro_cal(device_id_gyro);
-		}
-
-		if (device_id_mag != 0) {
-			calibration::Magnetometer mag_cal(device_id_mag);
-		}
-	}
-
-	// ensure calibration slots are active for the number of sensors currently available
-	// this to done to eliminate differences in the active set of parameters before and after sensor calibration
+	// 1. mark all existing sensor calibrations active even if sensor is missing
+	//    this preserves the calibration in the event of a parameter export while the sensor is missing
+	// 2. ensure calibration slots are active for the number of sensors currently available
+	//    this to done to eliminate differences in the active set of parameters before and after sensor calibration
 	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-
 		// sensor_accel
-		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
+		{
+			const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC",  "ID", i);
 
-		if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) {
-			calibration::Accelerometer cal;
-			cal.set_calibration_index(i);
-			cal.ParametersLoad();
+			if (device_id_accel != 0) {
+				calibration::Accelerometer accel_cal(device_id_accel);
+			}
+
+			uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
+
+			if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) {
+				calibration::Accelerometer cal;
+				cal.set_calibration_index(i);
+				cal.ParametersLoad();
+			}
 		}
 
+
 		// sensor_gyro
-		uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
+		{
+			const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i);
 
-		if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) {
-			calibration::Gyroscope cal;
-			cal.set_calibration_index(i);
-			cal.ParametersLoad();
+			if (device_id_gyro != 0) {
+				calibration::Gyroscope gyro_cal(device_id_gyro);
+			}
+
+			uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
+
+			if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) {
+				calibration::Gyroscope cal;
+				cal.set_calibration_index(i);
+				cal.ParametersLoad();
+			}
 		}
 
+
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 		// sensor_mag
-		uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
+		{
+			uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG",  "ID", i);
 
-		if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) {
-			calibration::Magnetometer cal;
-			cal.set_calibration_index(i);
-			cal.ParametersLoad();
+			if (device_id_mag != 0) {
+				calibration::Magnetometer mag_cal(device_id_mag);
+			}
+
+			uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
+
+			if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) {
+				calibration::Magnetometer cal;
+				cal.set_calibration_index(i);
+				cal.ParametersLoad();
+			}
 		}
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 	}
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
 	InitializeVehicleAirData();
+#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
+
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
 	InitializeVehicleGPSPosition();
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
+
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 	InitializeVehicleMagnetometer();
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+	InitializeVehicleOpticalFlow();
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
 
 	return PX4_OK;
 }
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 void Sensors::diff_pres_poll()
 {
 	differential_pressure_s diff_pres{};
@@ -497,21 +579,6 @@ void Sensors::diff_pres_poll()
 	}
 }
 
-void
-Sensors::parameter_update_poll(bool forced)
-{
-	// check for parameter updates
-	if (_parameter_update_sub.updated() || forced) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		parameters_update();
-		updateParams();
-	}
-}
-
 void Sensors::adc_poll()
 {
 	/* only read if not in HIL mode */
@@ -560,9 +627,11 @@ void Sensors::adc_poll()
 		}
 	}
 
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
+#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
 }
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED)
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
 void Sensors::InitializeVehicleAirData()
 {
 	if (_param_sys_has_baro.get()) {
@@ -575,7 +644,9 @@ void Sensors::InitializeVehicleAirData()
 		}
 	}
 }
+#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
 
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
 void Sensors::InitializeVehicleGPSPosition()
 {
 	if (_param_sys_has_gps.get()) {
@@ -588,6 +659,7 @@ void Sensors::InitializeVehicleGPSPosition()
 		}
 	}
 }
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 
 void Sensors::InitializeVehicleIMU()
 {
@@ -624,6 +696,7 @@ void Sensors::InitializeVehicleIMU()
 	}
 }
 
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 void Sensors::InitializeVehicleMagnetometer()
 {
 	if (_param_sys_has_mag.get()) {
@@ -636,6 +709,24 @@ void Sensors::InitializeVehicleMagnetometer()
 		}
 	}
 }
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+void Sensors::InitializeVehicleOpticalFlow()
+{
+	if (_vehicle_optical_flow == nullptr) {
+		uORB::Subscription sensor_optical_flow_sub{ORB_ID(sensor_optical_flow)};
+
+		if (sensor_optical_flow_sub.advertised()) {
+			_vehicle_optical_flow = new VehicleOpticalFlow();
+
+			if (_vehicle_optical_flow) {
+				_vehicle_optical_flow->Start();
+			}
+		}
+	}
+}
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
 
 void Sensors::Run()
 {
@@ -664,18 +755,55 @@ void Sensors::Run()
 	// when not adding sensors poll for param updates
 	if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) {
 
-		const int n_accel = orb_group_count(ORB_ID(sensor_accel));
-		const int n_baro  = orb_group_count(ORB_ID(sensor_baro));
-		const int n_gps   = orb_group_count(ORB_ID(sensor_gps));
-		const int n_gyro  = orb_group_count(ORB_ID(sensor_gyro));
-		const int n_mag   = orb_group_count(ORB_ID(sensor_mag));
+		bool updated = false;
 
-		if ((n_accel != _n_accel) || (n_baro != _n_baro) || (n_gps != _n_gps) || (n_gyro != _n_gyro) || (n_mag != _n_mag)) {
-			_n_accel = n_accel;
+#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
+		const int n_baro = orb_group_count(ORB_ID(sensor_baro));
+
+		if (n_baro != _n_baro) {
 			_n_baro = n_baro;
+			updated = true;
+		}
+
+#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
+
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+		const int n_gps = orb_group_count(ORB_ID(sensor_gps));
+
+		if (n_gps != _n_gps) {
 			_n_gps = n_gps;
-			_n_gyro = n_gyro;
+			updated = true;
+		}
+
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
+
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
+		const int n_mag = orb_group_count(ORB_ID(sensor_mag));
+
+		if (n_mag != _n_mag) {
 			_n_mag = n_mag;
+			updated = true;
+		}
+
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+		const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow));
+
+		if (n_optical_flow != _n_optical_flow) {
+			_n_optical_flow = n_optical_flow;
+			updated = true;
+		}
+
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
+
+
+		const int n_accel = orb_group_count(ORB_ID(sensor_accel));
+		const int n_gyro  = orb_group_count(ORB_ID(sensor_gyro));
+
+		if ((n_accel != _n_accel) || (n_gyro != _n_gyro) || updated) {
+			_n_accel = n_accel;
+			_n_gyro = n_gyro;
 
 			parameters_update();
 		}
@@ -687,8 +815,16 @@ void Sensors::Run()
 		_last_config_update = hrt_absolute_time();
 
 	} else {
-		// check parameters for updates
-		parameter_update_poll();
+		// check for parameter updates
+		if (_parameter_update_sub.updated()) {
+			// clear update
+			parameter_update_s pupdate;
+			_parameter_update_sub.copy(&pupdate);
+
+			// update parameters from storage
+			parameters_update();
+			updateParams();
+		}
 	}
 
 	_voted_sensors_update.sensorsPoll(_sensor_combined);
@@ -700,10 +836,11 @@ void Sensors::Run()
 		_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
 	}
 
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	// check analog airspeed
 	adc_poll();
-
 	diff_pres_poll();
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
 	// backup schedule as a watchdog timeout
 	ScheduleDelayed(10_ms);
@@ -766,19 +903,38 @@ int Sensors::print_status()
 {
 	_voted_sensors_update.printStatus();
 
+#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
+
 	if (_vehicle_magnetometer) {
 		PX4_INFO_RAW("\n");
 		_vehicle_magnetometer->PrintStatus();
 	}
 
+#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+
+#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
+
 	if (_vehicle_air_data) {
 		PX4_INFO_RAW("\n");
 		_vehicle_air_data->PrintStatus();
 	}
 
+#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
+
+#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	PX4_INFO_RAW("\n");
 	PX4_INFO_RAW("Airspeed status:\n");
 	_airspeed_validator.print();
+#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
+
+#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
+
+	if (_vehicle_optical_flow) {
+		PX4_INFO_RAW("\n");
+		_vehicle_optical_flow->PrintStatus();
+	}
+
+#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
 
 	PX4_INFO_RAW("\n");
 	_vehicle_acceleration.PrintStatus();
@@ -786,11 +942,15 @@ int Sensors::print_status()
 	PX4_INFO_RAW("\n");
 	_vehicle_angular_velocity.PrintStatus();
 
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+
 	if (_vehicle_gps_position) {
 		PX4_INFO_RAW("\n");
 		_vehicle_gps_position->PrintStatus();
 	}
 
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
+
 	PX4_INFO_RAW("\n");
 
 	for (auto &i : _vehicle_imu_list) {

src/modules/sensors/vehicle_air_data/CMakeLists.txt

@@ -37,6 +37,7 @@ px4_add_library(vehicle_air_data
 )
 target_link_libraries(vehicle_air_data
 	PRIVATE
+		data_validator
 		px4_work_queue
 		sensor_calibration
 )

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -32,7 +32,6 @@
 ############################################################################
 
 px4_add_library(vehicle_imu
-	Integrator.hpp
 	VehicleIMU.cpp
 	VehicleIMU.hpp
 )

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "Integrator.hpp"
+#include <Integrator.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/mathlib/math/WelfordMean.hpp>
@@ -108,8 +108,8 @@ private:
 	calibration::Accelerometer _accel_calibration{};
 	calibration::Gyroscope _gyro_calibration{};
 
-	Integrator       _accel_integrator{};
-	IntegratorConing _gyro_integrator{};
+	sensors::Integrator       _accel_integrator{};
+	sensors::IntegratorConing _gyro_integrator{};
 
 	uint32_t _imu_integration_interval_us{5000};
 

src/modules/sensors/vehicle_magnetometer/CMakeLists.txt

@@ -38,6 +38,7 @@ px4_add_library(vehicle_magnetometer
 
 target_link_libraries(vehicle_magnetometer
 	PRIVATE
+		data_validator
 		px4_work_queue
 		sensor_calibration
 )

src/modules/sensors/vehicle_optical_flow/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,7 +31,8 @@
 #
 ############################################################################
 
-add_subdirectory(paw3902)
-add_subdirectory(pmw3901)
-add_subdirectory(px4flow)
-add_subdirectory(thoneflow)
+px4_add_library(vehicle_optical_flow
+	VehicleOpticalFlow.cpp
+	VehicleOpticalFlow.hpp
+)
+target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue)

src/modules/sensors/vehicle_optical_flow/RingBuffer.h

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RingBuffer.h
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * Template RingBuffer.
+ */
+
+#include <inttypes.h>
+#include <cstdio>
+#include <cstring>
+
+template <typename data_type, size_t SIZE>
+class RingBuffer
+{
+public:
+	void push(const data_type &sample)
+	{
+		uint8_t head_new = _head;
+
+		if (!_first_write) {
+			head_new = (_head + 1) % SIZE;
+		}
+
+		_buffer[head_new] = sample;
+		_head = head_new;
+
+		// move tail if we overwrite it
+		if (_head == _tail && !_first_write) {
+			_tail = (_tail + 1) % SIZE;
+
+		} else {
+			_first_write = false;
+		}
+	}
+
+	uint8_t get_length() const { return SIZE; }
+
+	data_type &operator[](const uint8_t index) { return _buffer[index]; }
+
+	const data_type &get_newest() const { return _buffer[_head]; }
+	const data_type &get_oldest() const { return _buffer[_tail]; }
+
+	uint8_t get_oldest_index() const { return _tail; }
+
+	bool pop_first_older_than(const uint64_t &timestamp, data_type *sample)
+	{
+		// start looking from newest observation data
+		for (uint8_t i = 0; i < SIZE; i++) {
+			int index = (_head - i);
+			index = index < 0 ? SIZE + index : index;
+
+			if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) {
+				*sample = _buffer[index];
+
+				// Now we can set the tail to the item which
+				// comes after the one we removed since we don't
+				// want to have any older data in the buffer
+				if (index == _head) {
+					_tail = _head;
+					_first_write = true;
+
+				} else {
+					_tail = (index + 1) % SIZE;
+				}
+
+				_buffer[index].time_us = 0;
+
+				return true;
+			}
+
+			if (index == _tail) {
+				// we have reached the tail and haven't got a
+				// match
+				return false;
+			}
+		}
+
+		return false;
+	}
+
+	bool pop_oldest(const uint64_t &timestamp_oldest, const uint64_t &timestamp_newest, data_type *sample)
+	{
+		if (timestamp_oldest >= timestamp_newest) {
+			return false;
+		}
+
+		for (uint8_t i = 0; i < SIZE; i++) {
+
+			uint8_t index = (_tail + i) % SIZE;
+
+			if (_buffer[index].time_us >= timestamp_oldest && _buffer[index].time_us <= timestamp_newest) {
+				*sample = _buffer[index];
+
+				// Now we can set the tail to the item which
+				// comes after the one we removed since we don't
+				// want to have any older data in the buffer
+				if (index == _head) {
+					_tail = _head;
+					_first_write = true;
+
+				} else {
+					_tail = (index + 1) % SIZE;
+				}
+
+				_buffer[index] = {};
+
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	bool pop_oldest(data_type *sample)
+	{
+		if (_tail == _head) {
+			return false;
+		}
+
+		*sample = _buffer[_tail];
+		_buffer[_tail].time_us = 0;
+
+		_tail = (_tail + 1) % SIZE;
+
+		return true;
+	}
+
+	bool pop_newest(data_type *sample)
+	{
+		if (_tail == _head) {
+			return false;
+		}
+
+		*sample = _buffer[_head];
+		_buffer[_head].time_us = 0;
+
+		_head = (_head - 1) % SIZE;
+
+		return true;
+	}
+
+	int get_totalSIZE() const { return sizeof(*this) + sizeof(data_type) * SIZE; }
+
+	int entries() const
+	{
+		int count = 0;
+
+		for (uint8_t i = 0; i < SIZE; i++) {
+			if (_buffer[i].time_us != 0) {
+				count++;
+			}
+		}
+
+		return count;
+	}
+
+private:
+	data_type _buffer[SIZE] {};
+
+	uint8_t _head{0};
+	uint8_t _tail{0};
+
+	bool _first_write{true};
+};

src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp

@@ -0,0 +1,396 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "VehicleOpticalFlow.hpp"
+
+#include <px4_platform_common/log.h>
+
+namespace sensors
+{
+
+using namespace matrix;
+using namespace time_literals;
+
+static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
+
+VehicleOpticalFlow::VehicleOpticalFlow() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+	_vehicle_optical_flow_pub.advertise();
+
+	_gyro_integrator.set_reset_samples(1);
+}
+
+VehicleOpticalFlow::~VehicleOpticalFlow()
+{
+	Stop();
+	perf_free(_cycle_perf);
+}
+
+bool VehicleOpticalFlow::Start()
+{
+	_sensor_flow_sub.registerCallback();
+
+	_sensor_gyro_sub.registerCallback();
+	_sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2);
+
+	_sensor_selection_sub.registerCallback();
+
+	ScheduleNow();
+	return true;
+}
+
+void VehicleOpticalFlow::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	_sensor_flow_sub.unregisterCallback();
+	_sensor_gyro_sub.unregisterCallback();
+	_sensor_selection_sub.unregisterCallback();
+}
+
+void VehicleOpticalFlow::ParametersUpdate()
+{
+	// Check if parameters have changed
+	if (_params_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_params_sub.copy(&param_update);
+
+		updateParams();
+	}
+}
+
+void VehicleOpticalFlow::Run()
+{
+	perf_begin(_cycle_perf);
+
+	ParametersUpdate();
+
+	if (_sensor_selection_sub.updated()) {
+
+		sensor_selection_s sensor_selection{};
+		_sensor_selection_sub.copy(&sensor_selection);
+
+		for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+			uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
+
+			if (sensor_gyro_sub.advertised()
+			    && (sensor_gyro_sub.get().timestamp != 0)
+			    && (sensor_gyro_sub.get().device_id != 0)
+			    && (hrt_elapsed_time(&sensor_gyro_sub.get().timestamp) < 1_s)) {
+
+				if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) {
+					if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) {
+
+						_gyro_calibration.set_device_id(sensor_gyro_sub.get().device_id);
+						PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
+						break;
+
+					} else {
+						PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
+					}
+				}
+			}
+		}
+	}
+
+	while (_sensor_gyro_sub.updated()) {
+		const unsigned last_generation = _sensor_gyro_sub.get_last_generation();
+
+		sensor_gyro_s sensor_gyro;
+
+		if (_sensor_gyro_sub.update(&sensor_gyro)) {
+
+			if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) {
+				PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation());
+			}
+
+			_gyro_calibration.set_device_id(sensor_gyro.device_id);
+			_gyro_calibration.SensorCorrectionsUpdate();
+
+			const float dt_s = (sensor_gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f;
+			_gyro_timestamp_sample_last = sensor_gyro.timestamp_sample;
+
+			gyroSample gyro_sample;
+			gyro_sample.time_us = sensor_gyro.timestamp_sample;
+			gyro_sample.data = _gyro_calibration.Correct(Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z});
+			gyro_sample.dt = dt_s;
+
+			_gyro_buffer.push(gyro_sample);
+		}
+	}
+
+	sensor_optical_flow_s sensor_optical_flow;
+
+	if (_sensor_flow_sub.update(&sensor_optical_flow)) {
+
+		// only process valid samples
+		if ((sensor_optical_flow.integration_timespan_us > 1000) && (sensor_optical_flow.integration_timespan_us < UINT16_MAX)
+		    && (sensor_optical_flow.quality > 0)
+		    && PX4_ISFINITE(sensor_optical_flow.pixel_flow[0])
+		    && PX4_ISFINITE(sensor_optical_flow.pixel_flow[1])) {
+
+			// clear data accumulation if there's a gap in data
+			if ((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last) > sensor_optical_flow.integration_timespan_us *
+			    1.5f) {
+				// clear accumulated data
+				_flow_integral.zero();
+				_integration_timespan_us = 0;
+
+				_delta_angle.zero();
+
+				_quality_sum = 0;
+				_accumulated_count = 0;
+
+				_gyro_integrator.reset();
+			}
+
+			gyroSample gyro_sample;
+			const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf(
+					sensor_optical_flow.integration_timespan_us);
+			const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp;
+
+			while (_gyro_buffer.pop_oldest(timestamp_oldest, timestamp_newest, &gyro_sample)) {
+
+				_gyro_integrator.put(gyro_sample.data, gyro_sample.dt);
+
+				float min_interval_s = (sensor_optical_flow.integration_timespan_us * 1e-6f) * 0.99f;
+
+				if (_gyro_integrator.integral_dt() > min_interval_s) {
+					//PX4_INFO("integral dt: %.6f, min interval: %.6f", (double)_gyro_integrator.integral_dt(),(double) min_interval_s);
+					break;
+				}
+			}
+
+			if (sensor_optical_flow.delta_angle_available
+			    && PX4_ISFINITE(sensor_optical_flow.delta_angle[0])
+			    && PX4_ISFINITE(sensor_optical_flow.delta_angle[1])
+			    && PX4_ISFINITE(sensor_optical_flow.delta_angle[2])
+			   ) {
+				// passthrough integrated gyro if available
+				_delta_angle += Vector3f{sensor_optical_flow.delta_angle};
+
+			} else {
+				Vector3f delta_angle{NAN, NAN, NAN};
+				uint16_t delta_angle_dt;
+
+				if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) {
+					_delta_angle += delta_angle;
+
+				} else {
+					// force integrator reset
+					_gyro_integrator.reset();
+				}
+			}
+
+			_flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample;
+			_flow_integral(0) += sensor_optical_flow.pixel_flow[0];
+			_flow_integral(1) += sensor_optical_flow.pixel_flow[1];
+
+			_integration_timespan_us += sensor_optical_flow.integration_timespan_us;
+
+			_quality_sum += sensor_optical_flow.quality;
+			_accumulated_count++;
+
+			bool publish = true;
+
+			if (_param_sens_flow_rate.get() > 0) {
+				const float interval_us = 1e6f / _param_sens_flow_rate.get();
+
+				// don't allow publishing faster than SENS_FLOW_RATE
+				if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) {
+					publish = false;
+				}
+
+				// integrate for full interval unless we haven't published recently
+				if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) {
+					publish = false;
+				}
+			}
+
+			if (publish) {
+				vehicle_optical_flow_s vehicle_optical_flow{};
+
+				vehicle_optical_flow.timestamp_sample = sensor_optical_flow.timestamp_sample;
+				vehicle_optical_flow.device_id = sensor_optical_flow.device_id;
+
+				_flow_integral.copyTo(vehicle_optical_flow.pixel_flow);
+				_delta_angle.copyTo(vehicle_optical_flow.delta_angle);
+
+				vehicle_optical_flow.integration_timespan_us = _integration_timespan_us;
+
+				vehicle_optical_flow.quality = _quality_sum / _accumulated_count;
+
+				// SENS_FLOW_MAXR
+				if (PX4_ISFINITE(sensor_optical_flow.max_flow_rate)
+				    && (sensor_optical_flow.max_flow_rate <= _param_sens_flow_maxr.get())) {
+
+					vehicle_optical_flow.max_flow_rate = sensor_optical_flow.max_flow_rate;
+
+				} else {
+					vehicle_optical_flow.max_flow_rate = _param_sens_flow_maxr.get();
+				}
+
+				// SENS_FLOW_MINHGT
+				if (PX4_ISFINITE(sensor_optical_flow.min_ground_distance)
+				    && (sensor_optical_flow.min_ground_distance >= _param_sens_flow_minhgt.get())) {
+
+					vehicle_optical_flow.min_ground_distance = sensor_optical_flow.min_ground_distance;
+
+				} else {
+					vehicle_optical_flow.min_ground_distance = _param_sens_flow_minhgt.get();
+				}
+
+				// SENS_FLOW_MAXHGT
+				if (PX4_ISFINITE(sensor_optical_flow.max_ground_distance)
+				    && (sensor_optical_flow.max_ground_distance <= _param_sens_flow_maxhgt.get())) {
+
+					vehicle_optical_flow.max_ground_distance = sensor_optical_flow.max_ground_distance;
+
+				} else {
+					vehicle_optical_flow.max_ground_distance = _param_sens_flow_maxhgt.get();
+				}
+
+
+				// rotate (SENS_FLOW_ROT)
+				float zeroval = 0.f;
+				rotate_3f((enum Rotation)_param_sens_flow_rot.get(), vehicle_optical_flow.pixel_flow[0],
+					  vehicle_optical_flow.pixel_flow[1], zeroval);
+
+				rotate_3f((enum Rotation)_param_sens_flow_rot.get(),
+					  sensor_optical_flow.delta_angle[0],
+					  sensor_optical_flow.delta_angle[1],
+					  sensor_optical_flow.delta_angle[2]);
+
+				vehicle_optical_flow.timestamp = hrt_absolute_time();
+				_vehicle_optical_flow_pub.publish(vehicle_optical_flow);
+				_last_publication_timestamp = vehicle_optical_flow.timestamp_sample;
+
+
+				// vehicle_optical_flow_vel if distance is available (for logging)
+				if (_distance_sensor_sub.updated()) {
+
+					distance_sensor_s distance_sensor{};
+					_distance_sensor_sub.copy(&distance_sensor);
+
+					if ((distance_sensor.current_distance > distance_sensor.min_distance)
+					    && (distance_sensor.current_distance < distance_sensor.max_distance)) {
+
+						const float range = distance_sensor.current_distance;
+
+						vehicle_optical_flow_vel_s flow_vel{};
+
+						flow_vel.timestamp_sample = vehicle_optical_flow.timestamp_sample;
+
+						// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
+						// is produced by a RH rotation of the image about the sensor axis.
+						const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]};
+						const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
+
+						const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us;
+
+						// compensate for body motion to give a LOS rate
+						const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy();
+
+						Vector3f vel_optflow_body;
+						vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt;
+						vel_optflow_body(1) =   range * flow_compensated_XY_rad(0) / flow_dt;
+						vel_optflow_body(2) = 0.f;
+
+						// vel_body
+						flow_vel.vel_body[0] = vel_optflow_body(0);
+						flow_vel.vel_body[1] = vel_optflow_body(1);
+
+						// vel_ne
+						flow_vel.vel_ne[0] = NAN;
+						flow_vel.vel_ne[1] = NAN;
+
+						vehicle_attitude_s vehicle_attitude{};
+
+						if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+							matrix::Dcmf R_to_earth = matrix::Dcmf(vehicle_attitude.q);
+							Vector3f flow_vel_ne = R_to_earth * vel_optflow_body;
+
+							flow_vel.vel_ne[0] = flow_vel_ne(0);
+							flow_vel.vel_ne[1] = flow_vel_ne(1);
+						}
+
+						// flow_uncompensated_integral
+						flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral);
+
+						// flow_compensated_integral
+						flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral);
+
+						const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt));
+
+						// gyro_rate
+						flow_vel.gyro_rate[0] = measured_body_rate(0);
+						flow_vel.gyro_rate[1] = measured_body_rate(1);
+
+						// gyro_rate_integral
+						flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
+						flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
+
+						flow_vel.timestamp = hrt_absolute_time();
+
+						_vehicle_optical_flow_vel_pub.publish(flow_vel);
+					}
+				}
+
+				// clear accumulated data
+				_flow_integral.zero();
+				_integration_timespan_us = 0;
+
+				_delta_angle.zero();
+
+				_quality_sum = 0;
+				_accumulated_count = 0;
+			}
+		}
+	}
+
+	// reschedule backup
+	ScheduleDelayed(10_ms);
+
+	perf_end(_cycle_perf);
+}
+
+void VehicleOpticalFlow::PrintStatus()
+{
+
+}
+
+}; // namespace sensors

src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "data_validator/DataValidatorGroup.hpp"
+#include "RingBuffer.h"
+
+#include <Integrator.hpp>
+
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_optical_flow.h>
+#include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_optical_flow.h>
+#include <uORB/topics/vehicle_optical_flow_vel.h>
+
+namespace sensors
+{
+
+struct gyroSample {
+	uint64_t time_us{}; ///< timestamp of the measurement (uSec)
+	matrix::Vector3f data{};
+	float dt{0.f};
+};
+class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	VehicleOpticalFlow();
+	~VehicleOpticalFlow() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	void Run() override;
+
+	void ParametersUpdate();
+	void SensorCorrectionsUpdate(bool force = false);
+
+	static constexpr int MAX_SENSOR_COUNT = 3;
+
+	uORB::Publication<vehicle_optical_flow_s> _vehicle_optical_flow_pub{ORB_ID(vehicle_optical_flow)};
+	uORB::Publication<vehicle_optical_flow_vel_s> _vehicle_optical_flow_vel_pub{ORB_ID(vehicle_optical_flow_vel)};
+
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
+
+	uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+
+	uORB::SubscriptionCallbackWorkItem _sensor_flow_sub{this, ORB_ID(sensor_optical_flow)};
+	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)};
+	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
+
+	sensors::IntegratorConing _gyro_integrator{};
+
+	hrt_abstime _gyro_timestamp_sample_last{0};
+
+	calibration::Gyroscope _gyro_calibration{};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	hrt_abstime _flow_timestamp_sample_last{0};
+	matrix::Vector2f _flow_integral{};
+	matrix::Vector3f _delta_angle{};
+	uint32_t _integration_timespan_us{};
+	uint16_t _quality_sum{0};
+	uint8_t _accumulated_count{0};
+
+	hrt_abstime _last_publication_timestamp{0};
+
+	bool _advertised[MAX_SENSOR_COUNT] {};
+
+	uint8_t _priority[MAX_SENSOR_COUNT] {};
+
+	int8_t _selected_sensor_sub_index{-1};
+
+	RingBuffer<gyroSample, 32> _gyro_buffer{};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_FLOW_ROT>) _param_sens_flow_rot,
+		(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
+		(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
+		(ParamFloat<px4::params::SENS_FLOW_MAXR>) _param_sens_flow_maxr,
+		(ParamFloat<px4::params::SENS_FLOW_RATE>) _param_sens_flow_rate
+	)
+};
+}; // namespace sensors

src/modules/simulator/simulator.h

@@ -64,10 +64,10 @@
 #include <uORB/topics/esc_report.h>
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/optical_flow.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -191,7 +191,7 @@ private:
 
 	// uORB publisher handlers
 	uORB::Publication<differential_pressure_s>	_differential_pressure_pub{ORB_ID(differential_pressure)};
-	uORB::PublicationMulti<optical_flow_s>		_flow_pub{ORB_ID(optical_flow)};
+	uORB::PublicationMulti<sensor_optical_flow_s>	_sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 	uORB::Publication<irlock_report_s>		_irlock_report_pub{ORB_ID(irlock_report)};
 	uORB::Publication<esc_status_s>			_esc_status_pub{ORB_ID(esc_status)};
 	uORB::Publication<vehicle_odometry_s>		_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};

src/modules/simulator/simulator_mavlink.cpp

@@ -1311,44 +1311,37 @@ void Simulator::check_failure_injections()
 
 int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
 {
-	optical_flow_s flow = {};
-	flow.sensor_id = flow_mavlink->sensor_id;
-	flow.timestamp = hrt_absolute_time();
-	flow.time_since_last_sonar_update = 0;
-	flow.frame_count_since_last_readout = 0; // ?
-	flow.integration_timespan = flow_mavlink->integration_time_us;
+	sensor_optical_flow_s flow{};
+
+	flow.timestamp_sample = hrt_absolute_time();
+
+	flow.device_id = 0; // TODO
+
+	flow.pixel_flow[0] = flow_mavlink->integrated_x;
+	flow.pixel_flow[1] = flow_mavlink->integrated_y;
+
+	flow.integration_timespan_us = flow_mavlink->integration_time_us;
 
-	flow.ground_distance_m = flow_mavlink->distance;
-	flow.gyro_temperature = flow_mavlink->temperature;
-	flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
-	flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
-	flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
-	flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
-	flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
 	flow.quality = flow_mavlink->quality;
 
+	//flow.ground_distance_m = flow_mavlink->distance;
+
+	if (PX4_ISFINITE(flow_mavlink->integrated_xgyro) && PX4_ISFINITE(flow_mavlink->integrated_ygyro)
+	    && PX4_ISFINITE(flow_mavlink->integrated_zgyro)) {
+		flow.delta_angle[0] = flow_mavlink->integrated_xgyro;
+		flow.delta_angle[1] = flow_mavlink->integrated_ygyro;
+		flow.delta_angle[2] = flow_mavlink->integrated_zgyro;
+
+		flow.delta_angle_available = true;
+	}
+
 	/* fill in sensor limits */
-	float flow_rate_max;
-	param_get(param_find("SENS_FLOW_MAXR"), &flow_rate_max);
-	float flow_min_hgt;
-	param_get(param_find("SENS_FLOW_MINHGT"), &flow_min_hgt);
-	float flow_max_hgt;
-	param_get(param_find("SENS_FLOW_MAXHGT"), &flow_max_hgt);
+	flow.max_flow_rate = NAN;
+	flow.min_ground_distance = NAN;
+	flow.max_ground_distance = NAN;
 
-	flow.max_flow_rate = flow_rate_max;
-	flow.min_ground_distance = flow_min_hgt;
-	flow.max_ground_distance = flow_max_hgt;
-
-	/* rotate measurements according to parameter */
-	int32_t flow_rot_int;
-	param_get(param_find("SENS_FLOW_ROT"), &flow_rot_int);
-	const enum Rotation flow_rot = (Rotation)flow_rot_int;
-
-	float zeroval = 0.0f;
-	rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval);
-	rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral);
-
-	_flow_pub.publish(flow);
+	flow.timestamp = hrt_absolute_time();
+	_sensor_optical_flow_pub.publish(flow);
 
 	return PX4_OK;
 }