diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 469898417f67..b451efdc142a 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -102,8 +102,10 @@ pipeline { "px4_fmu-v2_multicopter", "px4_fmu-v2_rover", "px4_fmu-v3_default", + "px4_fmu-v3_visionTargetEst", "px4_fmu-v4_default", "px4_fmu-v4pro_default", + "px4_fmu-v4_visionTargetEst", "px4_fmu-v5_cyphal", "px4_fmu-v5_debug", "px4_fmu-v5_default", @@ -111,6 +113,7 @@ pipeline { "px4_fmu-v5_rover", "px4_fmu-v5_stackcheck", "px4_fmu-v5_uavcanv0periph", + "px4_fmu-v5_visionTargetEst", "px4_fmu-v5x_default", "px4_fmu-v5x_rover", "px4_fmu-v6c_default", @@ -123,6 +126,13 @@ pipeline { "px4_fmu-v6xrt_default", "px4_fmu-v6xrt_rover", "px4_io-v2_default", + "px4_fmu-v5x_visionTargetEst", + "px4_fmu-v6c_default", + "px4_fmu-v6c_visionTargetEst", + "px4_fmu-v6u_default", + "px4_fmu-v6u_visionTargetEst", + "px4_fmu-v6x_default", + "px4_fmu-v6x_visionTargetEst", "raspberrypi_pico_default", "siyi_n7_default", "sky-drones_smartap-airlink_default", diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock index 2f064e2c5ff0..29776d8f9d33 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock @@ -35,4 +35,7 @@ param set-default PLD_HACC_RAD 0.1 param set-default RTL_PLD_MD 2 # Start up Landing Target Estimator module -landing_target_estimator start +if param compare VTE_EN 0 +then + landing_target_estimator start +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index d5d9989c8666..5138f54cde96 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -25,6 +25,15 @@ then mc_autotune_attitude_control start fi +# +# Start Vision Target Estimator. +# + +if param greater -s VTE_EN 0 +then + vision_target_estimator start +fi + # # Start Multicopter Position Controller. # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index c28adef56ae9..6c896aa5dc8a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -25,6 +25,15 @@ then mc_autotune_attitude_control start fi +# +# Start Vision Target Estimator. +# + +if param greater -s VTE_EN 0 +then + vision_target_estimator start +fi + fw_rate_control start vtol fw_att_control start vtol fw_pos_control start vtol diff --git a/boards/px4/fmu-v3/visionTargetEst.px4board b/boards/px4/fmu-v3/visionTargetEst.px4board new file mode 100644 index 000000000000..a5bfaa282cdf --- /dev/null +++ b/boards/px4/fmu-v3/visionTargetEst.px4board @@ -0,0 +1,109 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IMU_L3GD20=y +CONFIG_DRIVERS_IMU_LSM303D=y +CONFIG_COMMON_INS=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=y + +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n diff --git a/boards/px4/fmu-v4/visionTargetEst.px4board b/boards/px4/fmu-v4/visionTargetEst.px4board new file mode 100644 index 000000000000..e08ce726ec18 --- /dev/null +++ b/boards/px4/fmu-v4/visionTargetEst.px4board @@ -0,0 +1,109 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_WIFI="/dev/ttyS0" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_COMMON_INS=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=y + +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n diff --git a/boards/px4/fmu-v4pro/visionTargetEst.px4board b/boards/px4/fmu-v4pro/visionTargetEst.px4board new file mode 100644 index 000000000000..9736b267955e --- /dev/null +++ b/boards/px4/fmu-v4pro/visionTargetEst.px4board @@ -0,0 +1,105 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=y + +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n diff --git a/boards/px4/fmu-v5/visionTargetEst.px4board b/boards/px4/fmu-v5/visionTargetEst.px4board new file mode 100644 index 000000000000..85af4127c44d --- /dev/null +++ b/boards/px4/fmu-v5/visionTargetEst.px4board @@ -0,0 +1,112 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_COMMON_HYGROMETERS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_COMMON_OSD=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_RC=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=y + +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n diff --git a/boards/px4/fmu-v5x/visionTargetEst.px4board b/boards/px4/fmu-v5x/visionTargetEst.px4board new file mode 100644 index 000000000000..103320ecf22d --- /dev/null +++ b/boards/px4/fmu-v5x/visionTargetEst.px4board @@ -0,0 +1,114 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPIO_MCP23009=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y +CONFIG_COMMON_INS=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_COMMON_OSD=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_RC=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_PAYLOAD_DELIVERER=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y + +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n diff --git a/boards/px4/fmu-v6c/visionTargetEst.px4board b/boards/px4/fmu-v6c/visionTargetEst.px4board new file mode 100644 index 000000000000..30254a7ce017 --- /dev/null +++ b/boards/px4/fmu-v6c/visionTargetEst.px4board @@ -0,0 +1,92 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_INS=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y + +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_CAMERA_FEEDBACK=n +CONFIG_MODULES_GIMBAL=n diff --git a/boards/px4/fmu-v6u/visionTargetEst.px4board b/boards/px4/fmu-v6u/visionTargetEst.px4board new file mode 100644 index 000000000000..83ead02d400d --- /dev/null +++ b/boards/px4/fmu-v6u/visionTargetEst.px4board @@ -0,0 +1,100 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=y + +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_CAMERA_FEEDBACK=n +CONFIG_MODULES_GIMBAL=n diff --git a/boards/px4/fmu-v6x/visionTargetEst.px4board b/boards/px4/fmu-v6x/visionTargetEst.px4board new file mode 100644 index 000000000000..719d9535ce50 --- /dev/null +++ b/boards/px4/fmu-v6x/visionTargetEst.px4board @@ -0,0 +1,100 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_INS=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_COMMON_OSD=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y + +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_CAMERA_FEEDBACK=n +CONFIG_MODULES_GIMBAL=n diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index ac228113d5d8..10db774190ee 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -55,6 +55,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 596ca30fcdf7..bb719606fd09 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -264,6 +264,16 @@ set(msg_files ) list(SORT msg_files) +if(NOT CONSTRAINED_FLASH) + list(APPEND msg_files + TargetGnss.msg + VisionTargetEstPosition.msg + VisionTargetEstOrientation.msg + FiducialMarkerPosReport.msg + FiducialMarkerYawReport.msg + PrecLandStatus.msg) +endif() + px4_list_make_absolute(msg_files ${CMAKE_CURRENT_SOURCE_DIR} ${msg_files}) if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "") diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 7bd8ea765827..1bf2c1f2619d 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -24,4 +24,6 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip # TOPICS estimator_aid_src_fake_hgt -# TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw +# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw +# TOPICS estimator_aid_src_terrain_range_finder +# TOPICS vte_aid_ev_yaw diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index b89add28e57e..952d8d3ae346 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -22,3 +22,5 @@ bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag +# TOPICS vte_aid_gps_pos_target vte_aid_gps_pos_mission vte_aid_gps_vel_target vte_aid_gps_vel_rel +# TOPICS vte_aid_fiducial_marker diff --git a/msg/FiducialMarkerPosReport.msg b/msg/FiducialMarkerPosReport.msg new file mode 100644 index 000000000000..ad6fcdc85a65 --- /dev/null +++ b/msg/FiducialMarkerPosReport.msg @@ -0,0 +1,15 @@ +# Relative position of precision land target in body frame + +uint64 timestamp # time since system start (microseconds) + +bool rel_pos_valid # Flag showing whether relative position is valid + +float32 x_rel_body # X/Front position of target, relative to vehicle (body frame) [meters] +float32 y_rel_body # Y/Right position of target, relative to vehicle (body frame) [meters] +float32 z_rel_body # Z/Down position of target, relative to vehicle (body frame) [meters] + +float32 var_x_rel_body # X/Front position variance [meters^2] +float32 var_y_rel_body # Y/Right position variance [meters^2] +float32 var_z_rel_body # Z/Down position variance [meters^2] + +float32[4] q # Quaternion rotation from the sensor frame to the NED earth frame diff --git a/msg/FiducialMarkerYawReport.msg b/msg/FiducialMarkerYawReport.msg new file mode 100644 index 000000000000..22258c09eaf6 --- /dev/null +++ b/msg/FiducialMarkerYawReport.msg @@ -0,0 +1,8 @@ +# Relative orientation of landing target in NED (North, East, Down) + +uint64 timestamp # time since system start (microseconds) + +bool orientation_valid # Flag showing whether relative position is valid + +float32 yaw_ned # orientation of the target relative to the NED frame [rad] [-Pi ; Pi] +float32 yaw_var_ned # orientation uncertainty [rad^2] diff --git a/msg/LandingTargetPose.msg b/msg/LandingTargetPose.msg index 875920f183b8..304c8c948f5f 100644 --- a/msg/LandingTargetPose.msg +++ b/msg/LandingTargetPose.msg @@ -13,12 +13,15 @@ float32 z_rel # Z/down position of target, relative to vehicle (navigation fra float32 vx_rel # X/north velocity of target, relative to vehicle (navigation frame) [meters/second] float32 vy_rel # Y/east velocity of target, relative to vehicle (navigation frame) [meters/second] +float32 vz_rel # Z/down velocity of target, relative to vehicle (navigation frame) [meters/second] float32 cov_x_rel # X/north position variance [meters^2] float32 cov_y_rel # Y/east position variance [meters^2] +float32 cov_z_rel # Z/down position variance [meters^2] float32 cov_vx_rel # X/north velocity variance [(meters/second)^2] float32 cov_vy_rel # Y/east velocity variance [(meters/second)^2] +float32 cov_vz_rel # Z/down velocity variance [(meters/second)^2] bool abs_pos_valid # Flag showing whether absolute position is valid float32 x_abs # X/north position of target, relative to origin (navigation frame) [meters] diff --git a/msg/PrecLandStatus.msg b/msg/PrecLandStatus.msg new file mode 100644 index 000000000000..f7973bad8af1 --- /dev/null +++ b/msg/PrecLandStatus.msg @@ -0,0 +1,16 @@ +# PREC_LAND_STATUS message data +uint8 PREC_LAND_STATE_STOPPED = 0 +uint8 PREC_LAND_STATE_ONGOING = 1 + +uint8 PREC_LAND_NAV_STATE_START = 0 +uint8 PREC_LAND_NAV_STATE_HORIZONTAL = 1 +uint8 PREC_LAND_NAV_STATE_DESCEND = 2 +uint8 PREC_LAND_NAV_STATE_FINAL = 3 +uint8 PREC_LAND_NAV_STATE_SEARCH = 4 +uint8 PREC_LAND_NAV_STATE_FALLBACK = 5 +uint8 PREC_LAND_NAV_STATE_DONE = 6 + +uint64 timestamp # time since system start (microseconds) + +uint8 state # PX4 state of delivery (PREC_LAND_STATE_*) +uint8 nav_state # Precision delivery state (PREC_LAND_NAV_STATE_*) diff --git a/msg/TargetGnss.msg b/msg/TargetGnss.msg new file mode 100644 index 000000000000..256d259e06ec --- /dev/null +++ b/msg/TargetGnss.msg @@ -0,0 +1,23 @@ +# landing target GPS position in WGS84 coordinates. + +uint64 timestamp # time since system start (microseconds) + +uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. +uint8 satellites_used # Number of satellites used + +float64 latitude_deg # Latitude in degrees, allows centimeter level RTK precision +float64 longitude_deg # Longitude in degrees, allows centimeter level RTK precision +float32 altitude_msl_m # Altitude above MSL, meters + +float32 eph # GPS horizontal position accuracy (metres) +float32 epv # GPS vertical position accuracy (metres) + +bool abs_pos_updated # True if WGS84 position is updated + +float32 vel_n_m_s # GPS North velocity, (metres/sec) +float32 vel_e_m_s # GPS East velocity, (metres/sec) +float32 vel_d_m_s # GPS Down velocity, (metres/sec) + +float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) + +bool vel_ned_updated # True if NED velocity is updated diff --git a/msg/VehicleAcceleration.msg b/msg/VehicleAcceleration.msg index 7d555d7f35c9..901461320cf7 100644 --- a/msg/VehicleAcceleration.msg +++ b/msg/VehicleAcceleration.msg @@ -4,3 +4,6 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) float32[3] xyz # Bias corrected acceleration (including gravity) in the FRD body frame XYZ-axis in m/s^2 + +# TOPICS vehicle_acceleration +# TOPICS vte_acc_input diff --git a/msg/VisionTargetEstOrientation.msg b/msg/VisionTargetEstOrientation.msg new file mode 100644 index 000000000000..d42db7b1c6c7 --- /dev/null +++ b/msg/VisionTargetEstOrientation.msg @@ -0,0 +1,11 @@ +# Relative orientation of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames + +uint64 timestamp # time since system start (microseconds) + +bool orientation_valid # Flag showing whether relative position is valid + +float32 theta # Orientation of the target relative to navigation frame [rad] [0 ; 2Pi] +float32 cov_theta # orientation uncertainty + +float32 v_theta # orientation speed of the target relative to navigation frame [rad/s] +float32 cov_v_theta # orientation speed uncertainty diff --git a/msg/VisionTargetEstPosition.msg b/msg/VisionTargetEstPosition.msg new file mode 100644 index 000000000000..91b8a0d7796b --- /dev/null +++ b/msg/VisionTargetEstPosition.msg @@ -0,0 +1,46 @@ +# Target estimator Kalman Filter state + +uint64 timestamp # time since system start (microseconds) + +bool rel_pos_valid # Flag showing whether relative position is valid +bool rel_vel_valid # Flag showing whether relative velocity is valid + +float32 x_rel # X/north position of target, relative to vehicle (navigation frame) [meters] +float32 y_rel # Y/east position of target, relative to vehicle (navigation frame) [meters] +float32 z_rel # Z/down position of target, relative to vehicle (navigation frame) [meters] + +float32 vx_rel # X/north velocity of target, relative to vehicle (navigation frame) [meters/second] +float32 vy_rel # Y/east velocity of target, relative to vehicle (navigation frame) [meters/second] +float32 vz_rel # Z/down velocity of target, relative to vehicle (navigation frame) [meters/second] + +float32 vx_target # X/north absolute velocity of target (navigation frame) [meters/second] +float32 vy_target # Y/east absolute velocity of target (navigation frame) [meters/second] +float32 vz_target # Z/down absolute velocity of target (navigation frame) [meters/second] + +float32 x_bias # X/north bias of drone and target GPS units (navigation frame) [meters] +float32 y_bias # Y/east bias of drone and target GPS units (navigation frame) [meters] +float32 z_bias # Z/down bias of drone and target GPS units (navigation frame) [meters] + +float32 ax_target # X/north absolute acceleration of target (navigation frame) [meters/second^2] +float32 ay_target # Y/east absolute acceleration of target (navigation frame) [meters/second^2] +float32 az_target # Z/down absolute acceleration of target (navigation frame) [meters/second^2] + +float32 cov_x_rel # X/north position variance [meters^2] +float32 cov_y_rel # Y/east position variance [meters^2] +float32 cov_z_rel # Z/down position variance [meters^2] + +float32 cov_vx_rel # X/north velocity variance [(meters/second)^2] +float32 cov_vy_rel # Y/east velocity variance [(meters/second)^2] +float32 cov_vz_rel # Z/down velocity variance [(meters/second)^2] + +float32 cov_x_bias # X/north bias variance [(meters)^2] +float32 cov_y_bias # Y/east bias variance [(meters)^2] +float32 cov_z_bias # Z/down bias variance [(meters)^2] + +float32 cov_vx_target # X/north velocity variance [(meters/second)^2] +float32 cov_vy_target # Y/east velocity variance [(meters/second)^2] +float32 cov_vz_target # Z/down velocity variance [(meters/second)^2] + +float32 cov_ax_target # X/north velocity variance [(meters/second^2)^2] +float32 cov_ay_target # Y/east velocity variance [(meters/second^2)^2] +float32 cov_az_target # Z/down velocity variance [(meters/second^2)^2] diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 68ac3f1ffea9..d62c0863977b 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -67,14 +67,16 @@ static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12}; // PX4 att/pos controllers, highest priority after sensors. static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 2240, -13}; -static constexpr wq_config_t INS0{"wq:INS0", 6000, -14}; -static constexpr wq_config_t INS1{"wq:INS1", 6000, -15}; -static constexpr wq_config_t INS2{"wq:INS2", 6000, -16}; -static constexpr wq_config_t INS3{"wq:INS3", 6000, -17}; +static constexpr wq_config_t vte{"wq:vte", 6000, -14}; -static constexpr wq_config_t hp_default{"wq:hp_default", 2800, -18}; +static constexpr wq_config_t INS0{"wq:INS0", 6000, -15}; +static constexpr wq_config_t INS1{"wq:INS1", 6000, -16}; +static constexpr wq_config_t INS2{"wq:INS2", 6000, -17}; +static constexpr wq_config_t INS3{"wq:INS3", 6000, -18}; -static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; +static constexpr wq_config_t hp_default{"wq:hp_default", 2392, -19}; + +static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -20}; static constexpr wq_config_t ttyS0{"wq:ttyS0", 1728, -21}; static constexpr wq_config_t ttyS1{"wq:ttyS1", 1728, -22}; diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index b2bb5632b7bf..ab088507245e 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -254,6 +254,11 @@ void FlightTaskAuto::_prepareLandSetpoints() // Update xy-position in case of landing position changes (etc. precision landing) _land_position = Vector3f(_target(0), _target(1), NAN); + // Control yaw while landing + if (_param_pld_yaw_en.get()) { + _land_heading = _yaw_setpoint; + } + // User input assisted landing if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { // Stick full up -1 -> stop, stick full down 1 -> double the speed diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 90a98cac23ad..d9df967ac6b6 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -188,7 +188,8 @@ class FlightTaskAuto : public FlightTask (ParamFloat) _param_mpc_z_v_auto_dn, (ParamFloat) _param_mpc_tko_speed, (ParamFloat) - _param_mpc_tko_ramp_t // time constant for smooth takeoff ramp + _param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp + (ParamInt) _param_pld_yaw_en ); private: diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 0b312b91835b..9dfd2fc29f33 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -298,12 +298,6 @@ class BlockLocalPositionEstimator : public ModuleBase) _param_lpe_t_max_grade, (ParamFloat) _param_lpe_lt_cov, - (ParamInt) _param_ltest_mode, // init origin (ParamInt) _param_lpe_fake_origin, diff --git a/src/modules/local_position_estimator/sensors/landing_target.cpp b/src/modules/local_position_estimator/sensors/landing_target.cpp index 6472fc198fab..315286288935 100644 --- a/src/modules/local_position_estimator/sensors/landing_target.cpp +++ b/src/modules/local_position_estimator/sensors/landing_target.cpp @@ -8,7 +8,7 @@ static const uint64_t TARGET_TIMEOUT = 2000000; // [us] void BlockLocalPositionEstimator::landingTargetInit() { - if (_param_ltest_mode.get() == Target_Moving) { + if (!_sub_landing_target_pose.get().is_static) { // target is in moving mode, do not initialize return; } @@ -24,7 +24,7 @@ void BlockLocalPositionEstimator::landingTargetInit() int BlockLocalPositionEstimator::landingTargetMeasure(Vector &y) { - if (_param_ltest_mode.get() == Target_Stationary) { + if (_sub_landing_target_pose.get().is_static) { if (_sub_landing_target_pose.get().rel_vel_valid) { y(0) = _sub_landing_target_pose.get().vx_rel; y(1) = _sub_landing_target_pose.get().vy_rel; @@ -43,7 +43,7 @@ int BlockLocalPositionEstimator::landingTargetMeasure(Vector void BlockLocalPositionEstimator::landingTargetCorrect() { - if (_param_ltest_mode.get() == Target_Moving) { + if (!_sub_landing_target_pose.get().is_static) { // nothing to do in this mode return; } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d468563597b7..fc7efbc03e7b 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -176,6 +176,19 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_status_flags", 10); add_optional_topic_multi("yaw_estimator_status", 1000); + // Vision target estimator topics +#if !defined(CONSTRAINED_FLASH) + add_topic("vision_target_est_position", 100); + add_topic("vision_target_est_orientation", 100); + add_optional_topic("vte_aid_gps_pos_target", 100); + add_optional_topic("vte_aid_gps_pos_mission", 100); + add_optional_topic("vte_aid_gps_vel_rel", 100); + add_optional_topic("vte_aid_gps_vel_target", 100); + add_optional_topic("vte_aid_fiducial_marker", 100); + add_optional_topic("vte_aid_ev_yaw", 100); + add_optional_topic("vte_acc_input", 50); +#endif // !CONSTRAINED_FLASH + // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 2); add_topic_multi("differential_pressure", 1000, 2); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9c53b16f3693..96bc73ee3351 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -313,6 +313,21 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY: handle_message_debug_float_array(msg); break; + +#if defined(MAVLINK_MSG_ID_TARGET_RELATIVE) + + case MAVLINK_MSG_ID_TARGET_RELATIVE: + handle_message_target_relative(msg); + break; +#endif + +#if defined(MAVLINK_MSG_ID_TARGET_ABSOLUTE) + + case MAVLINK_MSG_ID_TARGET_ABSOLUTE: + handle_message_target_absolute(msg); + break; +#endif + #endif // !CONSTRAINED_FLASH case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: @@ -2865,6 +2880,189 @@ MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg) _debug_array_pub.publish(debug_topic); } + +#if defined(MAVLINK_MSG_ID_TARGET_ABSOLUTE) +void +MavlinkReceiver::handle_message_target_absolute(mavlink_message_t *msg) +{ + mavlink_target_absolute_t target_absolute; + mavlink_msg_target_absolute_decode(msg, &target_absolute); + + target_gnss_s target_GNSS_report{}; + target_GNSS_report.timestamp = target_absolute.timestamp; + + bool updated = false; + + // Position: bit 0 + if (!(target_absolute.sensor_capabilities & (1 << 0))) { + target_GNSS_report.abs_pos_updated = false; + + } else { + target_GNSS_report.abs_pos_updated = true; + target_GNSS_report.latitude_deg = target_absolute.lat * 1e-7; + target_GNSS_report.longitude_deg = target_absolute.lon * 1e-7; + target_GNSS_report.altitude_msl_m = target_absolute.alt; + target_GNSS_report.eph = target_absolute.position_std[0]; + target_GNSS_report.epv = target_absolute.position_std[1]; + updated = true; + } + + // Velocity: bit 1 + if (!(target_absolute.sensor_capabilities & (1 << 1))) { + target_GNSS_report.vel_ned_updated = false; + + } else { + target_GNSS_report.vel_ned_updated = true; + target_GNSS_report.vel_n_m_s = target_absolute.vel[0]; + target_GNSS_report.vel_e_m_s = target_absolute.vel[1]; + target_GNSS_report.vel_d_m_s = target_absolute.vel[2]; + target_GNSS_report.s_variance_m_s = target_absolute.vel_std[0]; + updated = true; + } + + if (updated) { _target_gnss_pub.publish(target_GNSS_report); } +} +#endif + +#if defined(MAVLINK_MSG_ID_TARGET_RELATIVE) +void +MavlinkReceiver::handle_message_target_relative(mavlink_message_t *msg) +{ + mavlink_target_relative_t target_relative; + mavlink_msg_target_relative_decode(msg, &target_relative); + + vehicle_attitude_s vehicle_attitude; + vehicle_local_position_s vehicle_local_position; + + bool publish_target = false; + matrix::Quatf q_sensor(target_relative.q_sensor); + + // Unsupported sensor frame + if (target_relative.frame != TARGET_OBS_FRAME_LOCAL_OFFSET_NED && target_relative.frame != TARGET_OBS_FRAME_LOCAL_NED && + target_relative.frame != TARGET_OBS_FRAME_BODY_FRD && target_relative.frame != TARGET_OBS_FRAME_OTHER) { + mavlink_log_critical(&_mavlink_log_pub, "landing target: coordinate frame %" PRIu8 " unsupported.\t", + target_relative.frame); + events::send(events::ID("mavlink_rcv_target_rel_unsup_coord_frame"), events::Log::Error, + "Target relative: unsupported coordinate frame {1}", target_relative.frame); + + } else { + /* Check if the sensor's attitude field is filled. */ + if ((fabsf(q_sensor(0)) + fabsf(q_sensor(1)) + fabsf(q_sensor(2)) + fabsf(q_sensor(3))) > (float)1e-6) { + publish_target = true; + + } else { + /* Fill q_sensor depending on the frame*/ + if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_OFFSET_NED || target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) { + // Observation already in NED, set quaternion to identity (1,0,0,0) + q_sensor.setIdentity(); + publish_target = true; + + } else if (target_relative.frame == TARGET_OBS_FRAME_BODY_FRD && _vehicle_attitude_sub.update(&vehicle_attitude)) { + // Set the quaternion to the current attitude + const matrix::Quaternionf quat_att(&vehicle_attitude.q[0]); + q_sensor = quat_att; + publish_target = true; + + } else if (target_relative.frame == TARGET_OBS_FRAME_OTHER) { + // Target sensor frame: OTHER and no quaternion is provided + mavlink_log_critical(&_mavlink_log_pub, + "landing target: coordinate frame %" PRIu8 " unsupported when the q_sensor field is not filled.\t", + target_relative.frame); + events::send(events::ID("mavlink_rcv_target_rel_q_sensor_not_filled"), events::Log::Error, + "Target relative: unsupported coordinate frame {1} when the q_sensor field is not filled", target_relative.frame); + } + } + } + + if (publish_target) { + int32_t vte_enabled; + param_get(param_find("VTE_EN"), &vte_enabled); + + if (!vte_enabled) { + + // If the landing target estimator is disabled, send the target directly to the precision landing algorithm in local NED frame + landing_target_pose_s landing_target_pose{}; + landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(target_relative.timestamp); + + // Measurement already in local NED + if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) { + + landing_target_pose.x_abs = target_relative.x; + landing_target_pose.y_abs = target_relative.y; + landing_target_pose.z_abs = target_relative.z; + landing_target_pose.abs_pos_valid = true; + + _landing_target_pose_pub.publish(landing_target_pose); + + } else if (_vehicle_local_position_sub.update(&vehicle_local_position) && vehicle_local_position.xy_valid) { + + // If the local position is available, convert from sensor frame to local NED + matrix::Vector3f target_relative_frame(target_relative.x, target_relative.y, target_relative.z); + + if (target_relative.frame == TARGET_OBS_FRAME_BODY_FRD || target_relative.frame == TARGET_OBS_FRAME_OTHER) { + // Rotate measurement into vehicle-carried NED + target_relative_frame = q_sensor.rotateVector(target_relative_frame); + } + + // Convert from vehicle-carried NED to local NED + landing_target_pose.x_abs = target_relative_frame(0) + vehicle_local_position.x; + landing_target_pose.y_abs = target_relative_frame(1) + vehicle_local_position.y; + landing_target_pose.z_abs = target_relative_frame(2) + vehicle_local_position.z; + landing_target_pose.abs_pos_valid = true; + + _landing_target_pose_pub.publish(landing_target_pose); + } + + } else { + + // Send target to the landing target estimator + if (target_relative.type == LANDING_TARGET_TYPE_LIGHT_BEACON) { + + irlock_report_s irlock_report{}; + + irlock_report.timestamp = hrt_absolute_time(); + irlock_report.pos_x = target_relative.x; + irlock_report.pos_y = target_relative.y; + + // q_sensor.copyTo(irlock_report.q); + _irlock_report_pub.publish(irlock_report); + + } else if (target_relative.type == LANDING_TARGET_TYPE_VISION_FIDUCIAL) { + + // Position report + fiducial_marker_pos_report_s fiducial_marker_pos_report{}; + + fiducial_marker_pos_report.timestamp = _mavlink_timesync.sync_stamp(target_relative.timestamp); + fiducial_marker_pos_report.x_rel_body = target_relative.x; + fiducial_marker_pos_report.y_rel_body = target_relative.y; + fiducial_marker_pos_report.z_rel_body = target_relative.z; + + fiducial_marker_pos_report.var_x_rel_body = target_relative.pos_std[0] * target_relative.pos_std[0]; + fiducial_marker_pos_report.var_y_rel_body = target_relative.pos_std[1] * target_relative.pos_std[1]; + fiducial_marker_pos_report.var_z_rel_body = target_relative.pos_std[2] * target_relative.pos_std[2]; + + q_sensor.copyTo(fiducial_marker_pos_report.q); + _fiducial_marker_pos_report_pub.publish(fiducial_marker_pos_report); + + // Yaw report + fiducial_marker_yaw_report_s fiducial_marker_yaw_report{}; + fiducial_marker_yaw_report.timestamp = _mavlink_timesync.sync_stamp(target_relative.timestamp); + + // Transform quaternion from the target's frame to the TARGET_OBS_FRAME to the yaw relative to NED + const matrix::Quatf q_target(target_relative.q_target); + const matrix::Quatf q_in_ned = q_sensor * q_target; + const float target_yaw_ned = matrix::wrap_pi(matrix::Eulerf(q_in_ned).psi()); + + fiducial_marker_yaw_report.yaw_ned = target_yaw_ned; + fiducial_marker_yaw_report.yaw_var_ned = target_relative.yaw_std * target_relative.yaw_std; + _fiducial_marker_yaw_report_pub.publish(fiducial_marker_yaw_report); + + } + } + } +} +#endif + #endif // !CONSTRAINED_FLASH void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 39fe7c33f9c7..571a4ab3944e 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -120,6 +120,9 @@ # include # include # include +# include +# include +# include #endif // !CONSTRAINED_FLASH using namespace time_literals; @@ -219,6 +222,8 @@ class MavlinkReceiver : public ModuleParams void handle_message_debug_float_array(mavlink_message_t *msg); void handle_message_debug_vect(mavlink_message_t *msg); void handle_message_named_value_float(mavlink_message_t *msg); + void handle_message_target_relative(mavlink_message_t *msg); + void handle_message_target_absolute(mavlink_message_t *msg); #endif // !CONSTRAINED_FLASH void handle_message_request_event(mavlink_message_t *msg); @@ -340,6 +345,9 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _debug_key_value_pub{ORB_ID(debug_key_value)}; uORB::Publication _debug_value_pub{ORB_ID(debug_value)}; uORB::Publication _debug_vect_pub{ORB_ID(debug_vect)}; + uORB::Publication _fiducial_marker_pos_report_pub{ORB_ID(fiducial_marker_pos_report)}; + uORB::Publication _fiducial_marker_yaw_report_pub{ORB_ID(fiducial_marker_yaw_report)}; + uORB::Publication _target_gnss_pub{ORB_ID(target_gnss)}; #endif // !CONSTRAINED_FLASH // ORB publications (multi) diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 74c81569cf3d..119f7f71cdcb 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -122,6 +122,25 @@ PrecLand::on_active() _target_pose_valid = false; } +#if !defined(CONSTRAINED_FLASH) + + // get orientation measurement + if (_param_pld_yaw_en.get()) { + vision_target_est_orientation_s target_orientation; + + if (_target_orientation_sub.update(&target_orientation) && target_orientation.orientation_valid) { + _target_yaw_valid = true; + _last_target_yaw_update = target_orientation.timestamp; + _target_yaw = target_orientation.theta; + } + + if ((hrt_elapsed_time(&_last_target_yaw_update) / 1e6f) > _param_pld_btout.get()) { + _target_yaw_valid = false; + } + } + +#endif + // stop if we are landed if (_navigator->get_land_detected()->landed) { switch_to_state_done(); @@ -161,11 +180,17 @@ PrecLand::on_active() break; } +#if !defined(CONSTRAINED_FLASH) + _publish_prec_land_status(_state != PrecLandState::Done); +#endif } void PrecLand::on_inactivation() { +#if !defined(CONSTRAINED_FLASH) + _publish_prec_land_status(false); +#endif _is_activated = false; } @@ -268,6 +293,14 @@ PrecLand::run_state_horizontal_approach() pos_sp_triplet->current.alt = _approach_alt; pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; +#if !defined(CONSTRAINED_FLASH) + + if (_param_pld_yaw_en.get() && _target_yaw_valid) { + pos_sp_triplet->current.yaw = _target_yaw; + } + +#endif + _navigator->set_position_setpoint_triplet_updated(); } @@ -298,6 +331,13 @@ PrecLand::run_state_descend_above_target() _map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon); pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; +#if !defined(CONSTRAINED_FLASH) + + if (_param_pld_yaw_en.get() && _target_yaw_valid) { + pos_sp_triplet->current.yaw = _target_yaw; + } + +#endif _navigator->set_position_setpoint_triplet_updated(); } @@ -577,3 +617,21 @@ void PrecLand::slewrate(float &sp_x, float &sp_y) sp_x = sp_curr(0); sp_y = sp_curr(1); } + + +#if !defined(CONSTRAINED_FLASH) +void PrecLand::_publish_prec_land_status(const bool prec_land_ongoing) +{ + prec_land_status_s prec_land_status{}; + + if (prec_land_ongoing) { + prec_land_status.state = prec_land_status_s::PREC_LAND_STATE_ONGOING; + + } else { + prec_land_status.state = prec_land_status_s::PREC_LAND_STATE_STOPPED; + } + + prec_land_status.nav_state = (int)_state; + _prec_land_status_pub.publish(prec_land_status); +} +#endif diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h index df07ff7bd5f2..89fe0a9a5c6b 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -46,6 +46,11 @@ #include #include +#if !defined(CONSTRAINED_FLASH) +#include +#include +#endif + #include "navigator_mode.h" #include "mission_block.h" @@ -110,6 +115,16 @@ class PrecLand : public MissionBlock, public ModuleParams landing_target_pose_s _target_pose{}; /**< precision landing target position */ uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)}; + +#if !defined(CONSTRAINED_FLASH) + uORB::Subscription _target_orientation_sub {ORB_ID(vision_target_est_orientation)}; + float _target_yaw{0.f}; + bool _target_yaw_valid{false}; + hrt_abstime _last_target_yaw_update{0}; + uORB::Publication _prec_land_status_pub {ORB_ID(prec_land_status)}; + void _publish_prec_land_status(const bool prec_land_ongoing); +#endif + bool _target_pose_valid{false}; /**< whether we have received a landing target position message */ bool _target_pose_updated{false}; /**< wether the landing target position message is updated */ @@ -138,7 +153,8 @@ class PrecLand : public MissionBlock, public ModuleParams (ParamFloat) _param_pld_fappr_alt, (ParamFloat) _param_pld_srch_alt, (ParamFloat) _param_pld_srch_tout, - (ParamInt) _param_pld_max_srch + (ParamInt) _param_pld_max_srch, + (ParamInt) _param_pld_yaw_en ) // non-navigator parameters diff --git a/src/modules/navigator/precland_params.c b/src/modules/navigator/precland_params.c index 9c2e63ac3258..5c730519ac5e 100644 --- a/src/modules/navigator/precland_params.c +++ b/src/modules/navigator/precland_params.c @@ -119,3 +119,14 @@ PARAM_DEFINE_FLOAT(PLD_SRCH_TOUT, 10.0f); * @group Precision Land */ PARAM_DEFINE_INT32(PLD_MAX_SRCH, 3); + +/** + * Set to true to control yaw while landing. + * + * Control the orientation when landing. The orientation comes from the topic vision_target_est_orientation. + * + * @boolean + * + * @group Precision Land + */ +PARAM_DEFINE_INT32(PLD_YAW_EN, 0); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 20eed499b5f0..44bb8632fb06 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -383,6 +383,20 @@ void SimulatorMavlink::handle_message(const mavlink_message_t *msg) handle_message_landing_target(msg); break; +#if defined(MAVLINK_MSG_ID_TARGET_RELATIVE) + + case MAVLINK_MSG_ID_TARGET_RELATIVE: + handle_message_target_relative(msg); + break; +#endif + +#if defined(MAVLINK_MSG_ID_TARGET_ABSOLUTE) + + case MAVLINK_MSG_ID_TARGET_ABSOLUTE: + handle_message_target_absolute(msg); + break; +#endif + case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: handle_message_hil_state_quaternion(msg); break; @@ -637,6 +651,183 @@ void SimulatorMavlink::handle_message_landing_target(const mavlink_message_t *ms } } + +#if defined(MAVLINK_MSG_ID_TARGET_ABSOLUTE) +void +SimulatorMavlink::handle_message_target_absolute(const mavlink_message_t *msg) +{ + mavlink_target_absolute_t target_absolute; + mavlink_msg_target_absolute_decode(msg, &target_absolute); + + target_gnss_s target_GNSS_report{}; + target_GNSS_report.timestamp = hrt_absolute_time();; + + bool updated = false; + + // Position: bit 0 + if (!(target_absolute.sensor_capabilities & (1 << 0))) { + target_GNSS_report.abs_pos_updated = false; + + } else { + target_GNSS_report.abs_pos_updated = true; + target_GNSS_report.latitude_deg = target_absolute.lat * 1e-7; + target_GNSS_report.longitude_deg = target_absolute.lon * 1e-7; + target_GNSS_report.altitude_msl_m = target_absolute.alt; + target_GNSS_report.eph = target_absolute.position_std[0]; + target_GNSS_report.epv = target_absolute.position_std[1]; + updated = true; + } + + // Velocity: bit 1 + if (!(target_absolute.sensor_capabilities & (1 << 1))) { + target_GNSS_report.vel_ned_updated = false; + + } else { + target_GNSS_report.vel_ned_updated = true; + target_GNSS_report.vel_n_m_s = target_absolute.vel[0]; + target_GNSS_report.vel_e_m_s = target_absolute.vel[1]; + target_GNSS_report.vel_d_m_s = target_absolute.vel[2]; + target_GNSS_report.s_variance_m_s = target_absolute.vel_std[0]; + updated = true; + } + + if (updated) { _target_gnss_pub.publish(target_GNSS_report); } +} +#endif + + +#if defined(MAVLINK_MSG_ID_TARGET_RELATIVE) +void SimulatorMavlink::handle_message_target_relative(const mavlink_message_t *msg) +{ + mavlink_target_relative_t target_relative; + mavlink_msg_target_relative_decode(msg, &target_relative); + + vehicle_attitude_s vehicle_attitude; + vehicle_local_position_s vehicle_local_position; + + bool publish_target = false; + matrix::Quatf q_sensor(target_relative.q_sensor); + + // Unsupported sensor frame + if (target_relative.frame != TARGET_OBS_FRAME_LOCAL_OFFSET_NED && target_relative.frame != TARGET_OBS_FRAME_LOCAL_NED && + target_relative.frame != TARGET_OBS_FRAME_BODY_FRD && target_relative.frame != TARGET_OBS_FRAME_OTHER) { + PX4_WARN("Target relative: coordinate frame unsupported."); + + } else { + /* Check if the sensor's attitude field is filled. */ + if ((fabsf(q_sensor(0)) + fabsf(q_sensor(1)) + fabsf(q_sensor(2)) + fabsf(q_sensor(3))) > (float)1e-6) { + publish_target = true; + + } else { + /* Fill q_sensor depending on the frame*/ + if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_OFFSET_NED || target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) { + // Observation already in NED, set quaternion to identity (1,0,0,0) + q_sensor.setIdentity(); + publish_target = true; + + } else if (target_relative.frame == TARGET_OBS_FRAME_BODY_FRD && _vehicle_attitude_sub.update(&vehicle_attitude)) { + // Set the quaternion to the current attitude + const matrix::Quaternionf quat_att(&vehicle_attitude.q[0]); + q_sensor = quat_att; + publish_target = true; + + } else if (target_relative.frame == TARGET_OBS_FRAME_OTHER) { + // Target sensor frame: OTHER and no quaternion is provided + PX4_WARN("Target relative: coordinate frame unsupported when the q_sensor field is not filled."); + } + } + } + + if (publish_target) { + + int32_t vte_enabled; + param_get(param_find("VTE_EN"), &vte_enabled); + + if (!vte_enabled) { + + // If the landing target estimator is disabled, send the target directly to the precision landing algorithm in local NED frame + landing_target_pose_s landing_target_pose{}; + landing_target_pose.timestamp = target_relative.timestamp; + + // Measurement already in local NED + if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) { + + landing_target_pose.x_abs = target_relative.x; + landing_target_pose.y_abs = target_relative.y; + landing_target_pose.z_abs = target_relative.z; + landing_target_pose.abs_pos_valid = true; + + _landing_target_pose_pub.publish(landing_target_pose); + + } else if (_vehicle_local_position_sub.update(&vehicle_local_position) && vehicle_local_position.xy_valid) { + + // If the local position is available, convert from sensor frame to local NED + matrix::Vector3f target_relative_frame(target_relative.x, target_relative.y, target_relative.z); + + if (target_relative.frame == TARGET_OBS_FRAME_BODY_FRD || target_relative.frame == TARGET_OBS_FRAME_OTHER) { + // Rotate measurement into vehicle-carried NED + target_relative_frame = q_sensor.rotateVector(target_relative_frame); + } + + // Convert from vehicle-carried NED to local NED + landing_target_pose.x_abs = target_relative_frame(0) + vehicle_local_position.x; + landing_target_pose.y_abs = target_relative_frame(1) + vehicle_local_position.y; + landing_target_pose.z_abs = target_relative_frame(2) + vehicle_local_position.z; + landing_target_pose.abs_pos_valid = true; + + _landing_target_pose_pub.publish(landing_target_pose); + } + + } else { + + // Send target to the landing target estimator + if (target_relative.type == LANDING_TARGET_TYPE_LIGHT_BEACON) { + + irlock_report_s irlock_report{}; + + irlock_report.timestamp = hrt_absolute_time(); + irlock_report.pos_x = target_relative.x; + irlock_report.pos_y = target_relative.y; + + // q_sensor.copyTo(irlock_report.q); + _irlock_report_pub.publish(irlock_report); + + } else if (target_relative.type == LANDING_TARGET_TYPE_VISION_FIDUCIAL) { + + // Position report + fiducial_marker_pos_report_s fiducial_marker_pos_report{}; + + fiducial_marker_pos_report.timestamp = hrt_absolute_time(); + fiducial_marker_pos_report.x_rel_body = target_relative.x; + fiducial_marker_pos_report.y_rel_body = target_relative.y; + fiducial_marker_pos_report.z_rel_body = target_relative.z; + + fiducial_marker_pos_report.var_x_rel_body = target_relative.pos_std[0] * target_relative.pos_std[0]; + fiducial_marker_pos_report.var_y_rel_body = target_relative.pos_std[1] * target_relative.pos_std[1]; + fiducial_marker_pos_report.var_z_rel_body = target_relative.pos_std[2] * target_relative.pos_std[2]; + + q_sensor.copyTo(fiducial_marker_pos_report.q); + _fiducial_marker_pos_report_pub.publish(fiducial_marker_pos_report); + + // Yaw report + fiducial_marker_yaw_report_s fiducial_marker_yaw_report{}; + fiducial_marker_yaw_report.timestamp = hrt_absolute_time(); + + // Transform quaternion from the target's frame to the TARGET_OBS_FRAME to the yaw relative to NED + const matrix::Quatf q_target(target_relative.q_target); + const matrix::Quatf q_in_ned = q_sensor * q_target; + const float target_yaw_ned = matrix::wrap_pi(matrix::Eulerf(q_in_ned).psi()); + + fiducial_marker_yaw_report.yaw_ned = target_yaw_ned; + fiducial_marker_yaw_report.yaw_var_ned = target_relative.yaw_std * target_relative.yaw_std; + _fiducial_marker_yaw_report_pub.publish(fiducial_marker_yaw_report); + + } + } + } +} +#endif + void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) { mavlink_odometry_t odom_in; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 291148b9cb48..94b512b8a1d1 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -63,6 +63,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -227,6 +231,8 @@ class SimulatorMavlink : public ModuleParams void handle_message_hil_sensor(const mavlink_message_t *msg); void handle_message_hil_state_quaternion(const mavlink_message_t *msg); void handle_message_landing_target(const mavlink_message_t *msg); + void handle_message_target_relative(const mavlink_message_t *msg); + void handle_message_target_absolute(const mavlink_message_t *msg); void handle_message_odometry(const mavlink_message_t *msg); void handle_message_optical_flow(const mavlink_message_t *msg); void handle_message_rc_channels(const mavlink_message_t *msg); @@ -252,6 +258,10 @@ class SimulatorMavlink : public ModuleParams uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; + uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; + uORB::Publication _fiducial_marker_pos_report_pub{ORB_ID(fiducial_marker_pos_report)}; + uORB::Publication _fiducial_marker_yaw_report_pub{ORB_ID(fiducial_marker_yaw_report)}; + uORB::Publication _target_gnss_pub{ORB_ID(target_gnss)}; //rpm uORB::Publication _rpm_pub{ORB_ID(rpm)}; @@ -269,6 +279,8 @@ class SimulatorMavlink : public ModuleParams uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // hil map_ref data MapProjection _global_local_proj_ref{}; diff --git a/src/modules/vision_target_estimator/CMakeLists.txt b/src/modules/vision_target_estimator/CMakeLists.txt new file mode 100644 index 000000000000..9ad189218a3a --- /dev/null +++ b/src/modules/vision_target_estimator/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__vision_target_estimator + MAIN vision_target_estimator + COMPILE_FLAGS + STACK_MAX + 3600 + SRCS + VisionTargetEst.cpp + Position/VTEPosition.cpp + Position/KF_position_moving.cpp + Position/KF_position_static.cpp + Orientation/VTEOrientation.cpp + Orientation/KF_orientation_moving.cpp + Orientation/KF_orientation_static.cpp + DEPENDS + mathlib + px4_work_queue + ) diff --git a/src/modules/vision_target_estimator/Kconfig b/src/modules/vision_target_estimator/Kconfig new file mode 100644 index 000000000000..3fe9dea1ba33 --- /dev/null +++ b/src/modules/vision_target_estimator/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_VISION_TARGET_ESTIMATOR + bool "vision_target_estimator" + default n + ---help--- + Enable support for vision_target_estimator + +menuconfig USER_VISION_TARGET_ESTIMATOR + bool "vision_target_estimator running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_VISION_TARGET_ESTIMATOR + ---help--- + Put vision_target_estimator in userspace memory diff --git a/src/modules/vision_target_estimator/Orientation/KF_orientation_moving.cpp b/src/modules/vision_target_estimator/Orientation/KF_orientation_moving.cpp new file mode 100644 index 000000000000..8c4c6a1ec82e --- /dev/null +++ b/src/modules/vision_target_estimator/Orientation/KF_orientation_moving.cpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KF_orientation_moving.cpp + * @brief Filter to estimate the orientation of moving targets. State: [yaw, yaw_rate] + * + * @author Jonas Perolini + * + */ + +#include "KF_orientation_moving.h" + +namespace vision_target_estimator +{ + +void KF_orientation_moving::predictState(float dt) +{ + /* + ⎡dt⋅yaw_rate + yaw ⎤ + ⎣ yaw_rate ⎦ + */ + + _state(State::yaw) = _state(State::yaw) + _state(State::yaw_rate) * dt; + _state(State::yaw_rate) = _state(State::yaw_rate); +} + +void KF_orientation_moving::predictCov(float dt) +{ + /* + ⎡dt⋅p(0;1) + dt⋅(dt⋅p(1;1) + p(0;1)) + p(0;0) dt⋅p(1;1) + p(0;1)⎤ + ⎢ ⎥ + ⎣ dt⋅p(1;1) + p(0;1) p(1;1) ⎦ + */ + + const float off_diag = dt * _state_covariance(1, 1) + _state_covariance(0, 1); + _state_covariance(0, 0) += dt * _state_covariance(0, 1) + dt * (dt * _state_covariance(1, 1) + _state_covariance(0, 1)); + _state_covariance(1, 0) = off_diag; + _state_covariance(0, 1) = off_diag; +} + + +bool KF_orientation_moving::update() +{ + // Avoid zero-division + if (fabsf(_innov_cov) < 1e-6f) { + return false; + } + + const float beta = _innov / _innov_cov * _innov; + + // Normalized innovation Squared threshold. Checks whether innovation is consistent with innovation covariance. + if (beta > _nis_threshold) { + return false; + } + + const matrix::Matrix kalmanGain = _state_covariance * _meas_matrix_row_vect / _innov_cov; + + _state = _state + kalmanGain * _innov; + + _state(State::yaw) = matrix::wrap_pi(_state(State::yaw)); + _state(State::yaw_rate) = matrix::wrap_pi(_state(State::yaw_rate)); + + _state_covariance = _state_covariance - kalmanGain * _meas_matrix_row_vect.transpose() * _state_covariance; + + return true; +} + +void KF_orientation_moving::setH(const matrix::Vector &h_meas) +{ + _meas_matrix_row_vect(State::yaw) = h_meas(AugmentedState::yaw); + _meas_matrix_row_vect(State::yaw_rate) = h_meas(AugmentedState::yaw_rate); +} + +void KF_orientation_moving::syncState(float dt) +{ + _sync_state(State::yaw) = matrix::wrap_pi(_state(State::yaw) - _state(State::yaw_rate) * dt); + _sync_state(State::yaw_rate) = _state(State::yaw_rate); +} + +float KF_orientation_moving::computeInnovCov(float meas_unc) +{ + /* + [h(0)⋅(cov(0;0)⋅h(0) + cov(0;1)⋅h(1)) + h(1)⋅(cov(0;1)⋅h(0) + cov(1;1)⋅h(1)) + r] + */ + + _innov_cov = _meas_matrix_row_vect(State::yaw) * (_state_covariance(0, + 0) * _meas_matrix_row_vect(State::yaw) + _state_covariance(0, + 1) * _meas_matrix_row_vect(State::yaw_rate)) + _meas_matrix_row_vect(State::yaw_rate) * (_state_covariance(0, + 1) * _meas_matrix_row_vect(State::yaw) + _state_covariance(1, + 1) * _meas_matrix_row_vect(State::yaw_rate)) + meas_unc; + return _innov_cov; +} + +float KF_orientation_moving::computeInnov(float meas) +{ + /* z - H*x */ + _innov = matrix::wrap_pi(meas - (_meas_matrix_row_vect.transpose() * _sync_state)(0, 0)); + return _innov; +} + +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Orientation/KF_orientation_moving.h b/src/modules/vision_target_estimator/Orientation/KF_orientation_moving.h new file mode 100644 index 000000000000..6865d5168e5e --- /dev/null +++ b/src/modules/vision_target_estimator/Orientation/KF_orientation_moving.h @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KF_orientation_moving.h + * @brief Filter to estimate the orientation of moving targets. State: [theta, theta_dot] + * + * @author Jonas Perolini + * + */ + +#include +#include +#include +#include + +#include "base_KF_orientation.h" + +#pragma once + +namespace vision_target_estimator +{ +class KF_orientation_moving : public Base_KF_orientation +{ +public: + /** + * Default constructor, state not initialized + */ + KF_orientation_moving() {}; + + /** + * Default desctructor + */ + ~KF_orientation_moving() {}; + + //Prediction step: + void predictState(float dt) override; + void predictCov(float dt) override; + + // Backwards state prediciton + void syncState(float dt) override; + + void setH(const matrix::Vector &h_meas) override; + + float computeInnovCov(float measUnc) override; + float computeInnov(float meas) override; + + bool update() override; + + void setNISthreshold(float nis_threshold) override { _nis_threshold = nis_threshold; }; + + // Init: x_0 + void setPosition(float pos) override { _state(0) = pos; }; + void setVelocity(float vel) override { _state(1) = vel; }; + + // Init: P_0 + void setStatePosVar(float pos_unc) override { _state_covariance(0, 0) = pos_unc; }; + void setStateVelVar(float vel_unc) override { _state_covariance(1, 1) = vel_unc; }; + + // Retreive output of filter + float getPosition() override { return _state(0); }; + float getVelocity() override { return _state(1); }; + + float getPosVar() override { return _state_covariance(0, 0); }; + float getVelVar() override { return _state_covariance(1, 1); }; + + float getTestRatio() override {if (fabsf(_innov_cov) < 1e-6f) {return -1.f;} else {return _innov / _innov_cov * _innov;} }; + +private: + + enum State { + yaw = 0, + yaw_rate = 1, + }; + + matrix::Vector _state; + + matrix::Vector _sync_state; + + matrix::Vector _meas_matrix_row_vect; // row of measurement matrix + + matrix::Matrix _state_covariance; + + float _innov{0.0f}; // residual of last measurement update + + float _innov_cov{0.0f}; // innovation covariance of last measurement update + + float _nis_threshold{0.0f}; // Normalized innovation squared test threshold +}; +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Orientation/KF_orientation_static.cpp b/src/modules/vision_target_estimator/Orientation/KF_orientation_static.cpp new file mode 100644 index 000000000000..f348f4416084 --- /dev/null +++ b/src/modules/vision_target_estimator/Orientation/KF_orientation_static.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KF_orientation_static.cpp + * @brief Filter to estimate the orientation of static targets. State: [yaw] + * + * @author Jonas Perolini + * + */ + +#include "KF_orientation_static.h" + +namespace vision_target_estimator +{ + +bool KF_orientation_static::update() +{ + // Avoid zero-division + if (fabsf(_innov_cov) < 1e-6f) { + return false; + } + + const float beta = _innov / _innov_cov * _innov; + + // Normalized innovation Squared threshold. Checks whether innovation is consistent with innovation covariance. + if (beta > _nis_threshold) { + return false; + } + + const float kalmanGain = _state_covariance * _meas_matrix_row_vect / _innov_cov; + + _state = matrix::wrap_pi(_state + kalmanGain * _innov); + + _state_covariance = _state_covariance - kalmanGain * _meas_matrix_row_vect * _state_covariance; + + return true; +} + +void KF_orientation_static::setH(const matrix::Vector &h_meas) +{ + _meas_matrix_row_vect = h_meas(AugmentedState::yaw); +} + +void KF_orientation_static::syncState(float dt) +{ + _sync_state = _state; +} + +float KF_orientation_static::computeInnovCov(float meas_unc) +{ + _innov_cov = _meas_matrix_row_vect * _state_covariance * _meas_matrix_row_vect + meas_unc; + return _innov_cov; +} + +float KF_orientation_static::computeInnov(float meas) +{ + /* z - H*x */ + _innov = matrix::wrap_pi(meas - (_meas_matrix_row_vect * _sync_state)); + return _innov; +} + +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Orientation/KF_orientation_static.h b/src/modules/vision_target_estimator/Orientation/KF_orientation_static.h new file mode 100644 index 000000000000..5ccb255597cd --- /dev/null +++ b/src/modules/vision_target_estimator/Orientation/KF_orientation_static.h @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KF_orientation_static.h + * @brief Filter to estimate the orientation of static targets. State: [theta] + * + * @author Jonas Perolini + * + */ + +#include +#include +#include +#include + +#include "base_KF_orientation.h" + +#pragma once + +namespace vision_target_estimator +{ +class KF_orientation_static : public Base_KF_orientation +{ +public: + /** + * Default constructor, state not initialized + */ + KF_orientation_static() {}; + + /** + * Default desctructor + */ + ~KF_orientation_static() {}; + + //Prediction step: + void predictState(float dt) override {}; + void predictCov(float dt) override {}; + + // Backwards state prediciton + void syncState(float dt) override; + + void setH(const matrix::Vector &h_meas) override; + + float computeInnovCov(float measUnc) override; + float computeInnov(float meas) override; + + bool update() override; + + void setNISthreshold(float nis_threshold) override { _nis_threshold = nis_threshold; }; + + // Init: x_0 + void setPosition(float pos) override { _state = pos; }; + + // Init: P_0 + void setStatePosVar(float pos_unc) override { _state_covariance = pos_unc; }; + + // Retreive output of filter + float getPosition() override { return _state; }; + + float getPosVar() override { return _state_covariance; }; + + float getTestRatio() override {if (fabsf(_innov_cov) < 1e-6f) {return -1.f;} else {return _innov / _innov_cov * _innov;} }; + + float getVelVar() override { return 0.f; }; + float getVelocity() override { return 0.f; }; + void setStateVelVar(float vel_unc) override { }; + void setVelocity(float vel) override { }; + + +private: + + float _state; + + float _sync_state; + + float _meas_matrix_row_vect; + + float _state_covariance; + + float _innov{0.0f}; // residual of last measurement update + + float _innov_cov{0.0f}; // innovation covariance of last measurement update + + float _nis_threshold{0.0f}; // Normalized innovation squared test threshold + +}; +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp b/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp new file mode 100644 index 000000000000..7ae3517ec0c8 --- /dev/null +++ b/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp @@ -0,0 +1,343 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VTEOrientation.cpp + * @brief Estimate the orientation of a target by processessing and fusing sensor data in a Kalman Filter. + * + * @author Jonas Perolini + * + */ + +#include +#include +#include + +#include "VTEOrientation.h" + +namespace vision_target_estimator +{ + +using namespace matrix; + +VTEOrientation::VTEOrientation() : + ModuleParams(nullptr) +{ + _vte_aid_ev_yaw_pub.advertise(); + _targetOrientationPub.advertise(); + + _check_params(true); +} + +VTEOrientation::~VTEOrientation() +{ + delete _target_estimator_orientation; +} + +bool VTEOrientation::init() +{ + + _target_mode = (TargetMode)_param_vte_mode.get(); + _vte_TIMEOUT_US = (uint32_t)(_param_vte_btout.get() * 1_s); + + return selectTargetEstimator(); +} + +void VTEOrientation::resetFilter() +{ + _estimator_initialized = false; + _has_timed_out = false; +} + +void VTEOrientation::update() +{ + _check_params(false); + + // predict the target state using a constant relative acceleration model + if (_estimator_initialized) { + + if (hrt_absolute_time() - _last_update > _vte_TIMEOUT_US) { + PX4_WARN("VTE orientation estimator timeout"); + _has_timed_out = true; + + } else { + predictionStep(); + _last_predict = hrt_absolute_time(); + } + } + + // Update and fuse the observations and pulishes innovations + if (update_step()) { + _last_update = _last_predict; + } + + if (_estimator_initialized) {publishTarget();} +} + +bool VTEOrientation::initEstimator(const float theta_init) +{ + + PX4_INFO("Theta init %.2f", (double)theta_init); + + _target_estimator_orientation->setPosition(theta_init); + _target_estimator_orientation->setVelocity(0.f); + _target_estimator_orientation->setStatePosVar(_yaw_unc); + _target_estimator_orientation->setStateVelVar(_yaw_unc); + + return true; +} + + +void VTEOrientation::predictionStep() +{ + // Time from last prediciton + float dt = (hrt_absolute_time() - _last_predict) / 1_s; + + _target_estimator_orientation->predictState(dt); + _target_estimator_orientation->predictCov(dt); +} + + + +bool VTEOrientation::update_step() +{ + fiducial_marker_yaw_report_s fiducial_marker_orientation; + targetObsOrientation obs_fiducial_marker_orientation; + + bool orientation_valid = false; + + // Process data from all topics + if (_fiducial_marker_orientation_sub.update(&fiducial_marker_orientation)) { + + if (processObsVisionOrientation(fiducial_marker_orientation, obs_fiducial_marker_orientation)) { + + orientation_valid = ((hrt_absolute_time() - obs_fiducial_marker_orientation.timestamp) < measurement_valid_TIMEOUT_US); + } + } + + // If we have a new sensor and the estimator is initialized: fuse available measurements and publish innov. + if (orientation_valid && _estimator_initialized) { + + return fuse_orientation(obs_fiducial_marker_orientation); + + } else if (orientation_valid && !_estimator_initialized) { + + const float theta_init = obs_fiducial_marker_orientation.meas_theta; + + if (initEstimator(theta_init)) { + PX4_INFO("VTE Orientation Estimator properly initialized."); + _estimator_initialized = true; + _last_update = hrt_absolute_time(); + _last_predict = _last_update; + + } else { + resetFilter(); + } + } + + return false; +} + + +bool VTEOrientation::processObsVisionOrientation(const fiducial_marker_yaw_report_s &fiducial_marker_orientation, + targetObsOrientation &obs) +{ + + const float vision_r_theta = wrap_pi(fiducial_marker_orientation.yaw_ned); + + float vision_r_theta_unc; + + if (_ev_noise_md) { + vision_r_theta_unc = _range_sensor.valid ? (_ev_angle_noise * _ev_angle_noise * fmaxf(_range_sensor.dist_bottom, + 1.f)) : (_ev_angle_noise * _ev_angle_noise * 10); + + } else { + vision_r_theta_unc = fmaxf(fiducial_marker_orientation.yaw_var_ned, _ev_angle_noise * _ev_angle_noise); + } + + hrt_abstime vision_timestamp = fiducial_marker_orientation.timestamp; + + /* ORIENTATION */ + if (!PX4_ISFINITE(vision_r_theta)) { + PX4_WARN("VISION orientation is corrupt!"); + + } else { + + obs.timestamp = vision_timestamp; + obs.updated_theta = true; + obs.meas_unc_theta = vision_r_theta_unc; + obs.meas_theta = vision_r_theta; + obs.meas_h_theta(Base_KF_orientation::AugmentedState::yaw) = 1; + + return true; + } + + return false; +} + +bool VTEOrientation::fuse_orientation(const targetObsOrientation &target_orientation_obs) +{ + // Update step for orientation + bool meas_fused = false; + + estimator_aid_source1d_s target_innov; + + // Compute the measurement's time delay (difference between state and measurement time on validity) + const float dt_sync_us = (_last_predict - target_orientation_obs.timestamp); + + if (dt_sync_us > measurement_valid_TIMEOUT_US) { + + PX4_INFO("Orientation obs. rejected because too old. Time sync: %.2f [seconds] > timeout: %.2f [seconds]", + (double)(dt_sync_us / 1_s), (double)(measurement_valid_TIMEOUT_US / 1_s)); + + // No measurement update, set to false + target_innov.fused = false; + + } else if (target_orientation_obs.updated_theta) { + + // Convert time sync to seconds + const float dt_sync_s = dt_sync_us / 1_s; + + _target_estimator_orientation->syncState(dt_sync_s); + _target_estimator_orientation->setH(target_orientation_obs.meas_h_theta); + target_innov.innovation_variance = _target_estimator_orientation->computeInnovCov( + target_orientation_obs.meas_unc_theta); + target_innov.innovation = _target_estimator_orientation->computeInnov(target_orientation_obs.meas_theta); + // Set the Normalized Innovation Squared (NIS) check threshold. Used to reject outlier measurements + _target_estimator_orientation->setNISthreshold(_nis_threshold); + meas_fused = _target_estimator_orientation->update(); + + // Fill the target innovation field + target_innov.innovation_rejected = !meas_fused; + target_innov.fused = meas_fused; + + target_innov.observation = target_orientation_obs.meas_theta; + target_innov.observation_variance = target_orientation_obs.meas_unc_theta; + target_innov.timestamp_sample = target_orientation_obs.timestamp; + target_innov.timestamp = hrt_absolute_time(); + + // log test ratio defined as _innov / _innov_cov * _innov. If test_ratio > _nis_threshold, no fusion + target_innov.test_ratio = _target_estimator_orientation->getTestRatio(); + + } else { + // No yaw measurement + target_innov.fused = false; + } + + _vte_aid_ev_yaw_pub.publish(target_innov); + + return meas_fused; +} + + +void VTEOrientation::publishTarget() +{ + vision_target_est_orientation_s vision_target_orientation{}; + + vision_target_orientation.timestamp = _last_predict; + vision_target_orientation.orientation_valid = (hrt_absolute_time() - _last_update < target_valid_TIMEOUT_US); + + vision_target_orientation.theta = _target_estimator_orientation->getPosition(); + vision_target_orientation.cov_theta = _target_estimator_orientation->getPosVar(); + + if (_target_mode == TargetMode::Moving) { + vision_target_orientation.v_theta = _target_estimator_orientation->getVelocity(); + vision_target_orientation.cov_v_theta = _target_estimator_orientation->getVelVar(); + } + + _targetOrientationPub.publish(vision_target_orientation); +} + +void VTEOrientation::_check_params(const bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + updateParams(); + } + + if (_range_sensor.valid) { + _range_sensor.valid = (hrt_absolute_time() - _range_sensor.last_update) < measurement_updated_TIMEOUT_US; + } +} + +void VTEOrientation::updateParams() +{ + + ModuleParams::updateParams(); + + _yaw_unc = _param_vte_yaw_unc_in.get(); + _ev_angle_noise = _param_vte_ev_angle_noise.get(); + _ev_noise_md = _param_vte_ev_noise_md.get(); + _nis_threshold = _param_vte_yaw_nis_thre.get(); +} + +void VTEOrientation::set_range_sensor(const float dist, const bool valid) +{ + _range_sensor.valid = valid; + _range_sensor.dist_bottom = dist; + _range_sensor.last_update = hrt_absolute_time(); +} + +bool VTEOrientation::selectTargetEstimator() +{ + Base_KF_orientation *tmp_theta = nullptr; + + bool init_failed = true; + + if (_target_mode == TargetMode::Moving) { + tmp_theta = new KF_orientation_moving; + PX4_INFO("VTE orientation init for moving targets."); + + } else { + tmp_theta = new KF_orientation_static; + PX4_INFO("VTE orientation init for static targets."); + } + + init_failed = (tmp_theta == nullptr); + + if (init_failed) { + PX4_ERR("VTE orientation init failed"); + return false; + + } else { + + delete _target_estimator_orientation; + _target_estimator_orientation = tmp_theta; + + return true; + } +} + +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Orientation/VTEOrientation.h b/src/modules/vision_target_estimator/Orientation/VTEOrientation.h new file mode 100644 index 000000000000..dee3e8c7641c --- /dev/null +++ b/src/modules/vision_target_estimator/Orientation/VTEOrientation.h @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VTEOrientation.h + * @brief Estimate the orientation of a target by processessing and fusing sensor data in a Kalman Filter. + * + * @author Jonas Perolini + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "KF_orientation_moving.h" +#include "KF_orientation_static.h" +#include "base_KF_orientation.h" + +using namespace time_literals; + +namespace vision_target_estimator +{ + +class VTEOrientation: public ModuleParams +{ +public: + + VTEOrientation(); + virtual ~VTEOrientation(); + + /* + * Get new measurements and update the state estimate + */ + void update(); + + bool init(); + + void resetFilter(); + + void set_range_sensor(const float dist, const bool valid); + + bool has_timed_out() {return _has_timed_out;}; + +protected: + + /* + * Update parameters. + */ + void updateParams() override; + + + /* timeout after which the target is not valid if no measurements are seen*/ + static constexpr uint32_t target_valid_TIMEOUT_US = 2_s; + + /* timeout after which the measurement is not valid*/ + static constexpr uint32_t measurement_valid_TIMEOUT_US = 1_s; + + /* timeout after which the measurement is not considered updated*/ + static constexpr uint32_t measurement_updated_TIMEOUT_US = 100_ms; + + uORB::Publication _targetOrientationPub{ORB_ID(vision_target_est_orientation)}; + + // publish innovations target_estimator_gps_pos + uORB::Publication _vte_aid_ev_yaw_pub{ORB_ID(vte_aid_ev_yaw)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + +private: + + bool _has_timed_out{false}; + + enum class TargetMode { + Stationary = 0, + Moving = 1, + NotInit + }; + + TargetMode _target_mode{TargetMode::NotInit}; + + struct targetObsOrientation { + hrt_abstime timestamp; + // Theta + bool updated_theta = false; + float meas_theta = 0.f; + float meas_unc_theta = 0.f; + matrix::Vector2f meas_h_theta; + }; + + bool selectTargetEstimator(); + bool initEstimator(const float theta_init); + bool update_step(); + void predictionStep(); + + bool processObsVisionOrientation(const fiducial_marker_yaw_report_s &fiducial_marker_pose, targetObsOrientation &obs); + + bool fuse_orientation(const targetObsOrientation &target_pos_obs); + void publishTarget(); + + uORB::Subscription _fiducial_marker_orientation_sub{ORB_ID(fiducial_marker_yaw_report)}; + + struct localOrientation { + bool valid = false; + float yaw = 0.f; + hrt_abstime last_update = 0; + }; + + localOrientation _local_orientation{}; + + struct rangeSensor { + bool valid = false; + float dist_bottom; + hrt_abstime last_update = 0; + }; + + rangeSensor _range_sensor{}; + + bool _estimator_initialized{false}; + + Base_KF_orientation *_target_estimator_orientation {nullptr}; + + hrt_abstime _last_predict{0}; // timestamp of last filter prediction + hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout) + + void _check_params(const bool force); + + /* parameters from vision_target_estimator_params.c*/ + uint32_t _vte_TIMEOUT_US = 3_s; // timeout after which filter is reset if target not seen + float _yaw_unc; + float _ev_angle_noise; + bool _ev_noise_md{false}; + float _nis_threshold; + + DEFINE_PARAMETERS( + (ParamFloat) _param_vte_yaw_unc_in, + (ParamFloat) _param_vte_btout, + (ParamInt) _param_vte_mode, + (ParamInt) _param_vte_yaw_en, + (ParamFloat) _param_vte_ev_angle_noise, + (ParamInt) _param_vte_ev_noise_md, + (ParamFloat) _param_vte_yaw_nis_thre + ) +}; +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Orientation/base_KF_orientation.h b/src/modules/vision_target_estimator/Orientation/base_KF_orientation.h new file mode 100644 index 000000000000..ff94c4d57614 --- /dev/null +++ b/src/modules/vision_target_estimator/Orientation/base_KF_orientation.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file base_KF_orientation.h + * @brief Interface for orientation target estimator + * @author Jonas Perolini + */ + +#pragma once + +class Base_KF_orientation +{ +public: + Base_KF_orientation() = default; + virtual ~Base_KF_orientation() = default; + + enum AugmentedState { + yaw = 0, + yaw_rate = 1, + COUNT = 2 + }; + + //Prediction step: + virtual void predictState(float dt) = 0; + virtual void predictCov(float dt) = 0; + + // Backwards state prediciton + virtual void syncState(float dt) = 0; + + virtual void setH(const matrix::Vector &h_meas) = 0; + + virtual float computeInnovCov(float measUnc) = 0; + virtual float computeInnov(float meas) = 0; + + virtual bool update() = 0; + + // Normalized innovation squared (NIS) threshold. Used to reject measurements. + virtual void setNISthreshold(float nis_threshold) = 0; + + // Init: x_0 + virtual void setPosition(float pos) = 0; + virtual void setVelocity(float vel) = 0; + + // Init: P_0 + virtual void setStatePosVar(float var) = 0; + virtual void setStateVelVar(float var) = 0; + + // Retreive output of filter + virtual float getPosition() = 0; + virtual float getVelocity() = 0; + + virtual float getPosVar() = 0; + virtual float getVelVar() = 0; + + virtual float getTestRatio() = 0; + +}; diff --git a/src/modules/vision_target_estimator/Position/KF_position_moving.cpp b/src/modules/vision_target_estimator/Position/KF_position_moving.cpp new file mode 100644 index 000000000000..d5cedec8d28f --- /dev/null +++ b/src/modules/vision_target_estimator/Position/KF_position_moving.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KF_position_moving.cpp + * @brief Filter to estimate the pose of moving targets. State: [pos_rel, vel_uav, bias, acc_target, vel_target] + * + * @author Jonas Perolini + * + */ + +#include "KF_position_moving.h" +#include "python_derivation/generated/decoupled_moving_xyzb_v/predictCov.h" +#include "python_derivation/generated/decoupled_moving_xyzb_v/computeInnovCov.h" + +namespace vision_target_estimator +{ + +void KF_xyzb_v_decoupled_moving::predictState(float dt, float acc_uav) +{ + // Constant acceleration model. Prediction: x(t1) = Phi*x(t0) + G*u + _state(State::pos_rel) = _state(State::pos_rel) + dt * (_state(State::vel_target) - _state( + State::vel_uav)) + 0.5f * dt * dt * (_state(State::acc_target) - acc_uav); + _state(State::vel_uav) = _state(State::vel_uav) + acc_uav * dt; + _state(State::vel_target) = _state(State::vel_target) + dt * _state(State::acc_target); +} + +void KF_xyzb_v_decoupled_moving::predictCov(float dt) +{ + matrix::Matrix cov_updated; + sym::Predictcov(dt, _input_var, _bias_var, _acc_var, _state_covariance, &cov_updated); + _state_covariance = cov_updated; +} + + +bool KF_xyzb_v_decoupled_moving::update() +{ + // Avoid zero-division + if (fabsf(_innov_cov) < 1e-6f) { + return false; + } + + const float beta = _innov / _innov_cov * _innov; + + // Normalized innovation Squared threshold. Checks whether innovation is consistent with innovation covariance. + if (beta > _nis_threshold) { + return false; + } + + const matrix::Matrix kalmanGain = _state_covariance * _meas_matrix_row_vect / _innov_cov; + + _state = _state + kalmanGain * _innov; + + _state_covariance = _state_covariance - kalmanGain * _meas_matrix_row_vect.transpose() * _state_covariance; + + return true; +} + +void KF_xyzb_v_decoupled_moving::syncState(float dt, float acc_uav) +{ + // Prediction: x(t1) = Phi*x(t0) + G*u <--> Backwards prediction: x(t0) = Phi.inv()*[x(t1) - G*u] + _sync_state(State::pos_rel) = _state(State::pos_rel) - dt * (_state(State::vel_target) - _state( + State::vel_uav)) + 0.5f * dt * dt * (_state(State::acc_target) - acc_uav); + _sync_state(State::vel_uav) = _state(State::vel_uav) - acc_uav * dt; + _sync_state(State::bias) = _state(State::bias); + _sync_state(State::acc_target) = _state(State::acc_target); + _sync_state(State::vel_target) = _state(State::vel_target) - _state(State::acc_target) * dt; +} + +float KF_xyzb_v_decoupled_moving::computeInnovCov(float meas_unc) +{ + float innov_cov_updated; + sym::Computeinnovcov(meas_unc, _state_covariance, _meas_matrix_row_vect.transpose(), &innov_cov_updated); + _innov_cov = innov_cov_updated; + + return _innov_cov; +} + +float KF_xyzb_v_decoupled_moving::computeInnov(float meas) +{ + /* z - H*x */ + _innov = meas - (_meas_matrix_row_vect.transpose() * _sync_state)(0, 0); + return _innov; +} + +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Position/KF_position_moving.h b/src/modules/vision_target_estimator/Position/KF_position_moving.h new file mode 100644 index 000000000000..76f7bebd9f25 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/KF_position_moving.h @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KF_position_moving.h + * @brief Filter to estimate the pose of moving targets. State: [r, vd, b, at, vt] + * + * @author Jonas Perolini + * + */ + +#include +#include +#include +#include + +#include "base_KF_position.h" + +#pragma once + +namespace vision_target_estimator +{ +class KF_xyzb_v_decoupled_moving : public Base_KF_decoupled +{ +public: + /** + * Default constructor, state not initialized + */ + KF_xyzb_v_decoupled_moving() {}; + + /** + * Default desctructor + */ + ~KF_xyzb_v_decoupled_moving() {}; + + //Prediction step: + void predictState(float dt, float acc) override; + void predictCov(float dt) override; + + // Backwards state prediciton + void syncState(float dt, float acc) override; + + void setH(const matrix::Vector &h_meas) override {_meas_matrix_row_vect = h_meas;} + + void setState(const matrix::Vector &state) override {_state = state;} + + void setStateVar(const matrix::Vector &var) override + { + const matrix::SquareMatrix var_mat = diag(var); + _state_covariance = var_mat; + }; + + matrix::Vector getAugmentedState() override { return _state;} + + matrix::Vector getAugmentedStateVar() override + { + const matrix::SquareMatrix var_mat = _state_covariance; + return var_mat.diag(); + }; + + float computeInnovCov(float measUnc) override; + float computeInnov(float meas) override; + + bool update() override; + + void setNISthreshold(float nis_threshold) override { _nis_threshold = nis_threshold; }; + + float getTestRatio() override {if (fabsf(_innov_cov) < 1e-6f) {return -1.f;} else {return _innov / _innov_cov * _innov;} }; + + void setInputAccVar(float var) override { _input_var = var;}; + void setBiasVar(float var) override { _bias_var = var; }; + void setTargetAccVar(float var) override { _acc_var = var; }; + +private: + + // Nb states must be smaller or equal to AugmentedState::COUNT + enum State { + pos_rel = 0, + vel_uav = 1, + bias = 2, + acc_target = 3, + vel_target = 4, + nb_states = 5, + }; + + matrix::Vector _state; + + matrix::Vector _sync_state; + + matrix::Vector _meas_matrix_row_vect; + + matrix::Matrix _state_covariance; + + float _bias_var{0.f}; // target/UAV GPS bias variance + + float _acc_var{0.f}; // Target acceleration variance + + float _input_var{0.f}; // UAV acceleration variance + + float _innov{0.0f}; // residual of last measurement update + + float _innov_cov{0.0f}; // innovation covariance of last measurement update + + float _nis_threshold{0.0f}; // Normalized innovation squared test threshold + +}; +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Position/KF_position_static.cpp b/src/modules/vision_target_estimator/Position/KF_position_static.cpp new file mode 100644 index 000000000000..9510d58edca4 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/KF_position_static.cpp @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KF_position_static.cpp + * @brief Filter to estimate the pose of static targets. State: [pos_rel, vel_rel, bias] + * + * @author Jonas Perolini + * + */ + +#include "KF_position_static.h" +#include "python_derivation/generated/decoupled_static_xyzb/predictCov.h" +#include "python_derivation/generated/decoupled_static_xyzb/computeInnovCov.h" + +namespace vision_target_estimator +{ + +void KF_xyzb_decoupled_static::predictState(float dt, float acc_uav) +{ + _state(State::pos_rel) = _state(State::pos_rel) + _state(State::vel_rel) * dt - 0.5f * acc_uav * dt * dt; + _state(State::vel_rel) = _state(State::vel_rel) - acc_uav * dt; +} + +void KF_xyzb_decoupled_static::predictCov(float dt) +{ + matrix::Matrix cov_updated; + sym::Predictcov(dt, _input_var, _bias_var, _state_covariance, &cov_updated); + _state_covariance = cov_updated; +} + + +bool KF_xyzb_decoupled_static::update() +{ + // Avoid zero-division + if (fabsf(_innov_cov) < 1e-6f) { + return false; + } + + const float beta = _innov / _innov_cov * _innov; + + // Normalized innovation Squared threshold. Checks whether innovation is consistent with innovation covariance. + if (beta > _nis_threshold) { + return false; + } + + const matrix::Matrix kalmanGain = _state_covariance * _meas_matrix_row_vect / _innov_cov; + + _state = _state + kalmanGain * _innov; + _state_covariance = _state_covariance - kalmanGain * _meas_matrix_row_vect.transpose() * _state_covariance; + + return true; +} + +void KF_xyzb_decoupled_static::syncState(float dt, float acc_uav) +{ + // Prediction: x(t1) = Phi*x(t0) + G*u <--> Backwards prediction: x(t0) = Phi.inv()*[x(t1) - G*u] + _sync_state(State::pos_rel) = _state(State::pos_rel) - _state(State::vel_rel) * dt - 0.5f * acc_uav * dt * dt; + _sync_state(State::vel_rel) = _state(State::vel_rel) + acc_uav * dt; + _sync_state(State::bias) = _state(State::bias); +} + +float KF_xyzb_decoupled_static::computeInnovCov(float meas_unc) +{ + float innov_cov_updated; + sym::Computeinnovcov(meas_unc, _state_covariance, _meas_matrix_row_vect.transpose(), &innov_cov_updated); + _innov_cov = innov_cov_updated; + + return _innov_cov; +} + +float KF_xyzb_decoupled_static::computeInnov(float meas) +{ + /* z - H*x */ + _innov = meas - (_meas_matrix_row_vect.transpose() * _sync_state)(0, 0); + return _innov; +} + +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Position/KF_position_static.h b/src/modules/vision_target_estimator/Position/KF_position_static.h new file mode 100644 index 000000000000..924083323e52 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/KF_position_static.h @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KF_position_static.h + * @brief Filter to estimate the pose of static targets. State: [r, r_dot, bias] + * + * @author Jonas Perolini + * + */ + +#include +#include +#include +#include + +#include "base_KF_position.h" + +#pragma once + +namespace vision_target_estimator +{ +class KF_xyzb_decoupled_static : public Base_KF_decoupled +{ +public: + /** + * Default constructor, state not initialized + */ + KF_xyzb_decoupled_static() {}; + + /** + * Default desctructor + */ + ~KF_xyzb_decoupled_static() {}; + + //Prediction step: + void predictState(float dt, float acc) override; + void predictCov(float dt) override; + + // Backwards state prediciton + void syncState(float dt, float acc) override; + + void setH(const matrix::Vector &h_meas) override + { + _meas_matrix_row_vect(State::pos_rel) = h_meas(AugmentedState::pos_rel); + _meas_matrix_row_vect(State::vel_rel) = -h_meas(AugmentedState::vel_uav); + _meas_matrix_row_vect(State::bias) = h_meas(AugmentedState::bias); + }; + + void setState(const matrix::Vector &augmented_state) override + { + _state(State::pos_rel) = augmented_state(AugmentedState::pos_rel); + _state(State::vel_rel) = -augmented_state(AugmentedState::vel_uav); + _state(State::bias) = augmented_state(AugmentedState::bias); + }; + + void setStateVar(const matrix::Vector &augmented_state_var) override + { + _state_covariance(State::pos_rel, State::pos_rel) = augmented_state_var(AugmentedState::pos_rel); + _state_covariance(State::vel_rel, State::vel_rel) = augmented_state_var( + AugmentedState::vel_uav); // Variance of vel_uav is equivalent to the variance of vel_rel because Var(aX) = a^2 Var(X) + _state_covariance(State::bias, State::bias) = augmented_state_var(AugmentedState::bias); + }; + + matrix::Vector getAugmentedState() override + { + matrix::Vector augmented_state; + augmented_state(AugmentedState::pos_rel) = _state(State::pos_rel); + augmented_state(AugmentedState::vel_uav) = -_state(State::vel_rel); + augmented_state(AugmentedState::bias) = _state(State::bias); + + return augmented_state; + }; + + matrix::Vector getAugmentedStateVar() override + { + matrix::Vector augmented_state_var; + augmented_state_var(AugmentedState::pos_rel) = _state_covariance(State::pos_rel, State::pos_rel); + augmented_state_var(AugmentedState::vel_uav) = _state_covariance(State::vel_rel, State::vel_rel); + augmented_state_var(AugmentedState::bias) = _state_covariance(State::bias, State::bias); + + return augmented_state_var; + }; + + float computeInnovCov(float measUnc) override; + float computeInnov(float meas) override; + + bool update() override; + + void setNISthreshold(float nis_threshold) override { _nis_threshold = nis_threshold; }; + + float getTestRatio() override {if (fabsf(_innov_cov) < 1e-6f) {return -1.f;} else {return _innov / _innov_cov * _innov;} }; + void setInputAccVar(float var) override { _input_var = var; }; + void setBiasVar(float var) override { _bias_var = var; }; + + // Unused function + void setTargetAccVar(float var)override {}; + +private: + + // Nb states must be smaller or equal to AugmentedState::COUNT + enum State { + pos_rel = 0, + vel_rel = 1, + bias = 2, + nb_states = 3 + }; + + matrix::Vector _state; + + matrix::Vector _sync_state; + + matrix::Vector _meas_matrix_row_vect; + + matrix::Matrix _state_covariance; + + float _bias_var{0.f}; // target/UAV GPS bias variance + + float _input_var{0.f}; // UAV acceleration variance + + float _innov{0.0f}; // residual of last measurement update + + float _innov_cov{0.0f}; // innovation covariance of last measurement update + + float _nis_threshold{0.0f}; // Normalized innovation squared test threshold +}; +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.cpp b/src/modules/vision_target_estimator/Position/VTEPosition.cpp new file mode 100644 index 000000000000..cddac390aec7 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/VTEPosition.cpp @@ -0,0 +1,1194 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VTEPosition.cpp + * @brief Estimate the state of a target by processing and fusing sensor data in a Kalman Filter. + * + * @author Jonas Perolini + * + */ + +#include +#include +#include + +#include "VTEPosition.h" + +namespace vision_target_estimator +{ + +using namespace matrix; + +VTEPosition::VTEPosition() : + ModuleParams(nullptr) +{ + _targetPosePub.advertise(); + _targetEstimatorStatePub.advertise(); + _vte_aid_gps_pos_target_pub.advertise(); + _vte_aid_gps_pos_mission_pub.advertise(); + _vte_aid_gps_vel_rel_pub.advertise(); + _vte_aid_gps_vel_target_pub.advertise(); + _vte_aid_fiducial_marker_pub.advertise(); + + _check_params(true); +} + +VTEPosition::~VTEPosition() +{ + for (int i = 0; i < 3; i++) { + delete _target_estimator[i]; + } + + perf_free(_vte_predict_perf); + perf_free(_vte_update_perf); +} + +bool VTEPosition::init() +{ + bool return_bool = false; + + _target_mode = (TargetMode)_param_vte_mode.get(); + _vte_aid_mask = _param_vte_aid_mask.get(); + _vte_TIMEOUT_US = (uint32_t)(_param_vte_btout.get() * 1_s); + + if (_vte_aid_mask == SensorFusionMask::NO_SENSOR_FUSION) { + PX4_ERR("VTE: no data fusion enabled. Modify VTE_AID_MASK and reboot"); + return false; + } + + if (selectTargetEstimator()) { + + if (_vte_aid_mask & SensorFusionMask::USE_MISSION_POS && _target_mode == TargetMode::Moving) { + PX4_WARN("VTE mission land position data fusion cannot be enabled for moving targets."); + PX4_WARN("Disabling mission land position fusion."); + _vte_aid_mask -= SensorFusionMask::USE_MISSION_POS; + } + + if ((_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_POS) && (_vte_aid_mask & SensorFusionMask::USE_MISSION_POS)) { + PX4_WARN("VTE both target GPS position and mission land position data fusion enabled."); + PX4_WARN("Disabling mission land position fusion."); + _vte_aid_mask -= SensorFusionMask::USE_MISSION_POS; + } + + if (_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_POS) { PX4_INFO("VTE target GPS position data fusion enabled");} + + if (_vte_aid_mask & SensorFusionMask::USE_MISSION_POS) { PX4_INFO("VTE PX4 mission position fusion enabled");} + + if (_vte_aid_mask & SensorFusionMask::USE_GPS_REL_VEL) { PX4_INFO("VTE relative GPS velocity data fusion enabled");} + + if (_vte_aid_mask & SensorFusionMask::USE_EXT_VIS_POS) { PX4_INFO("VTE target external vision-based relative position data fusion enabled");} + + return_bool = true; + } + + return return_bool; +} + +void VTEPosition::resetFilter() +{ + _estimator_initialized = false; + _last_vision_obs_fused_time = 0; + _bias_set = false; + _mission_position.valid = false; + _has_timed_out = false; +} + +void VTEPosition::update(const Vector3f &acc_ned) +{ + _check_params(false); + + // predict the target state using a constant relative acceleration model + if (_estimator_initialized) { + + if (hrt_absolute_time() - _last_update > _vte_TIMEOUT_US) { + PX4_WARN("VTE Position estimator timeout"); + _has_timed_out = true; + + } else { + perf_begin(_vte_predict_perf); + predictionStep(acc_ned); + perf_end(_vte_predict_perf); + + _last_predict = hrt_absolute_time(); + } + } + + // Update and fuse the observations and publishes innovations + if (update_step(acc_ned)) { + _last_update = _last_predict; + } + + if (_estimator_initialized) {publishTarget();} +} + +bool VTEPosition::initEstimator(const Matrix + &state_init) +{ + // Get initial variance from params + const float state_pos_var = _param_vte_pos_unc_in.get(); + const float state_vel_var = _param_vte_vel_unc_in.get(); + const float state_bias_var = _param_vte_bias_unc_in.get(); + const float state_acc_var = _param_vte_acc_unc_in.get(); + const float state_target_vel_var = _param_vte_vel_unc_in.get(); + + const Vector3f state_pos_var_vect(state_pos_var, state_pos_var, state_pos_var); + const Vector3f state_vel_var_vect(state_vel_var, state_vel_var, state_vel_var); + const Vector3f state_bias_var_vect(state_bias_var, state_bias_var, state_bias_var); + const Vector3f state_acc_var_vect(state_acc_var, state_acc_var, state_acc_var); + const Vector3f state_target_vel_var_vect(state_target_vel_var, state_target_vel_var, state_target_vel_var); + + matrix::Matrix state_var_init; + state_var_init.col(Base_KF_decoupled::AugmentedState::pos_rel) = state_pos_var_vect; + state_var_init.col(Base_KF_decoupled::AugmentedState::vel_uav) = state_vel_var_vect; + state_var_init.col(Base_KF_decoupled::AugmentedState::bias) = state_bias_var_vect; + state_var_init.col(Base_KF_decoupled::AugmentedState::acc_target) = state_acc_var_vect; + state_var_init.col(Base_KF_decoupled::AugmentedState::vel_target) = state_target_vel_var_vect; + + for (int i = 0; i < Direction::nb_directions; i++) { + + _target_estimator[i]->setState(state_init.row(i)); + _target_estimator[i]->setStateVar(state_var_init.row(i)); + } + + // Debug INFO + PX4_INFO("Pos init %.2f %.2f %.2f", (double)state_init(Direction::x, Base_KF_decoupled::AugmentedState::pos_rel), + (double)state_init(Direction::y, Base_KF_decoupled::AugmentedState::pos_rel), (double)state_init(Direction::z, + Base_KF_decoupled::AugmentedState::pos_rel)); + PX4_INFO("Vel uav init %.2f %.2f %.2f", (double)state_init(Direction::x, Base_KF_decoupled::AugmentedState::vel_uav), + (double)state_init(Direction::y, Base_KF_decoupled::AugmentedState::vel_uav), (double)state_init(Direction::z, + Base_KF_decoupled::AugmentedState::vel_uav)); + PX4_INFO("Bias init %.2f %.2f %.2f", (double)state_init(Direction::x, Base_KF_decoupled::AugmentedState::bias), + (double)state_init(Direction::y, Base_KF_decoupled::AugmentedState::bias), (double)state_init(Direction::z, + Base_KF_decoupled::AugmentedState::bias)); + PX4_INFO("Target acc init %.2f %.2f %.2f", (double)state_init(Direction::x, + Base_KF_decoupled::AugmentedState::acc_target), + (double)state_init(Direction::y, Base_KF_decoupled::AugmentedState::acc_target), (double)state_init(Direction::z, + Base_KF_decoupled::AugmentedState::acc_target)); + PX4_INFO("Target vel init %.2f %.2f %.2f", (double)state_init(Direction::x, + Base_KF_decoupled::AugmentedState::vel_target), + (double)state_init(Direction::y, Base_KF_decoupled::AugmentedState::vel_target), (double)state_init(Direction::z, + Base_KF_decoupled::AugmentedState::vel_target)); + + return true; +} + + +void VTEPosition::predictionStep(const Vector3f &vehicle_acc_ned) +{ + // predict target position with the help of accel data + + // Time from last prediciton + const float dt = (hrt_absolute_time() - _last_predict) / 1_s; + + // The rotated input cov (from body to NED R*cov*R^T) is the same as the original input cov since input_cov = acc_unc * Identiy and R*R^T = Identity + const SquareMatrix input_cov = diag(Vector3f(_drone_acc_unc, _drone_acc_unc, + _drone_acc_unc)); + const SquareMatrix target_acc_cov = diag(Vector3f(_target_acc_unc, _target_acc_unc, + _target_acc_unc)); + const SquareMatrix bias_cov = diag(Vector3f(_bias_unc, _bias_unc, _bias_unc)); + + for (int i = 0; i < Direction::nb_directions; i++) { + //Decoupled dynamics, we neglect the off diag elements. + if (_target_mode == TargetMode::Moving) {_target_estimator[i]->setTargetAccVar(target_acc_cov(i, i));} + + _target_estimator[i]->setBiasVar(bias_cov(i, i)); + _target_estimator[i]->setInputAccVar(input_cov(i, i)); + + _target_estimator[i]->predictState(dt, vehicle_acc_ned(i)); + _target_estimator[i]->predictCov(dt); + } +} + + + +bool VTEPosition::update_step(const Vector3f &vehicle_acc_ned) +{ + + sensor_gps_s vehicle_gps_position; + target_gnss_s target_GNSS_report; + fiducial_marker_pos_report_s fiducial_marker_pose; + + targetObsPos obs_gps_pos_target; + targetObsPos obs_gps_pos_mission; + targetObsPos obs_gps_vel_rel; + targetObsPos obs_gps_vel_target; + targetObsPos obs_fiducial_marker; + + int vte_fusion_aid_mask = ObservationValidMask::NO_VALID_DATA; + + // Process data from all topics + + /* VISION */ + if ((_vte_aid_mask & SensorFusionMask::USE_EXT_VIS_POS) + && _fiducial_marker_report_sub.update(&fiducial_marker_pose)) { + + obs_fiducial_marker.type = fiducial_marker; + + if ((_is_meas_valid(fiducial_marker_pose.timestamp)) && processObsVision(fiducial_marker_pose, obs_fiducial_marker)) { + vte_fusion_aid_mask += ObservationValidMask::FUSE_EXT_VIS_POS; + } + } + + /* GPS BASED OBSERVATIONS */ + bool vehicle_gps_position_updated = _vehicle_gps_position_sub.update(&vehicle_gps_position); + + if (_is_meas_updated(vehicle_gps_position.timestamp)) { + + bool target_GPS_updated = _target_gnss_sub.update(&target_GNSS_report); + + /* TARGET GPS */ + if ((_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_POS) && target_GPS_updated + && target_GNSS_report.abs_pos_updated) { + + obs_gps_pos_target.type = target_gps_pos; + + if ((_is_meas_valid(target_GNSS_report.timestamp)) + && processObsGNSSPosTarget(target_GNSS_report, vehicle_gps_position, obs_gps_pos_target)) { + vte_fusion_aid_mask += ObservationValidMask::FUSE_TARGET_GPS_POS; + } + } + + /* MISSION GPS POSE */ + if ((_vte_aid_mask & SensorFusionMask::USE_MISSION_POS) && vehicle_gps_position_updated && _mission_position.valid) { + + obs_gps_pos_mission.type = mission_gps_pos; + + if ((_is_meas_valid(vehicle_gps_position.timestamp)) + && processObsGNSSPosMission(vehicle_gps_position, obs_gps_pos_mission)) { + vte_fusion_aid_mask += ObservationValidMask::FUSE_MISSION_POS; + } + } + + // Keep track of the drone GPS velocity + _uav_gps_vel.timestamp = vehicle_gps_position.timestamp; + _uav_gps_vel.valid = (vehicle_gps_position.vel_ned_valid && (PX4_ISFINITE(vehicle_gps_position.vel_n_m_s) + && PX4_ISFINITE(vehicle_gps_position.vel_e_m_s) && PX4_ISFINITE(vehicle_gps_position.vel_d_m_s))); + _uav_gps_vel.xyz(Direction::x) = vehicle_gps_position.vel_n_m_s; + _uav_gps_vel.xyz(Direction::y) = vehicle_gps_position.vel_e_m_s; + _uav_gps_vel.xyz(Direction::z) = vehicle_gps_position.vel_d_m_s; + + // Keep track of the target GPS velocity + _target_gps_vel.timestamp = target_GNSS_report.timestamp; + _target_gps_vel.valid = (target_GNSS_report.vel_ned_updated && PX4_ISFINITE(target_GNSS_report.vel_n_m_s) + && PX4_ISFINITE(target_GNSS_report.vel_e_m_s) + && PX4_ISFINITE(target_GNSS_report.vel_d_m_s)); + _target_gps_vel.xyz(Direction::x) = target_GNSS_report.vel_n_m_s; + _target_gps_vel.xyz(Direction::y) = target_GNSS_report.vel_e_m_s; + _target_gps_vel.xyz(Direction::z) = target_GNSS_report.vel_d_m_s; + + if ((_vte_aid_mask & SensorFusionMask::USE_GPS_REL_VEL)) { + + /* TARGET GPS VELOCITY */ + if (_target_mode == TargetMode::Moving && target_GPS_updated && _target_gps_vel.valid) { + + obs_gps_vel_target.type = vel_target_gps; + + if ((_is_meas_valid(target_GNSS_report.timestamp)) && processObsGNSSVelTarget(target_GNSS_report, obs_gps_vel_target)) { + vte_fusion_aid_mask += ObservationValidMask::FUSE_TARGET_GPS_VEL; + } + } + + /* RELATIVE GPS velocity */ + if (_uav_gps_vel.valid && vehicle_gps_position_updated) { + + obs_gps_vel_rel.type = vel_rel_gps; + + if ((_is_meas_valid(vehicle_gps_position.timestamp)) && processObsGNSSVelRel(vehicle_gps_position, obs_gps_vel_rel)) { + vte_fusion_aid_mask += ObservationValidMask::FUSE_GPS_REL_VEL; + } + } + } + } + + const bool new_gnss_pos_sensor = (vte_fusion_aid_mask & ObservationValidMask::FUSE_MISSION_POS) || + (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_POS); + const bool new_non_gnss_pos_sensor = (vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS); + const bool new_pos_sensor = new_non_gnss_pos_sensor || new_gnss_pos_sensor; + const bool new_vel_sensor = (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_VEL) || + (vte_fusion_aid_mask & ObservationValidMask::FUSE_GPS_REL_VEL); + + // Only estimate the GNSS bias if we have a GNSS estimation and a secondary source of position + const bool should_set_bias = new_non_gnss_pos_sensor && (_is_meas_valid(_pos_rel_gnss.timestamp)); + + if (!_estimator_initialized) { + + if (!new_pos_sensor) { + return false; + } + + const bool has_initial_velocity_estimate = (_local_velocity.valid && (_is_meas_valid(_local_velocity.timestamp))) || + (_uav_gps_vel.valid && (_is_meas_valid(_uav_gps_vel.timestamp))); + + if (!has_initial_velocity_estimate) { + PX4_WARN("No UAV velocity estimate. Estimator cannot be started."); + return false; + } + + Vector3f pos_init; + Vector3f uav_vel_init; + Vector3f target_acc_init; // Assume null target absolute acceleration + Vector3f bias_init; + Vector3f target_vel_init; + + // Define the initial relative position of target w.r.t. the drone in NED frame using the available measurement + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS) { + pos_init = obs_fiducial_marker.meas_xyz; + + } else if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_POS) { + pos_init = obs_gps_pos_target.meas_xyz; + + } else if (vte_fusion_aid_mask & ObservationValidMask::FUSE_MISSION_POS) { + pos_init = obs_gps_pos_mission.meas_xyz; + } + + // Compute the initial bias as the difference between the GPS and external position estimate. + if (should_set_bias) { + // We assume that gnss observations have a bias but other position obs don't. It follows: gnss_obs = state + bias <--> bias = gnss_obs - state + PX4_INFO("VTE Position setting GNSS bias."); + bias_init = _pos_rel_gnss.xyz - pos_init; + _bias_set = true; + } + + if (_target_gps_vel.valid && (_is_meas_valid(_target_gps_vel.timestamp))) { + target_vel_init = _target_gps_vel.xyz; + } + + // Define initial relative velocity of the target w.r.t. to the drone in NED frame + if (_uav_gps_vel.valid && (_is_meas_valid(_uav_gps_vel.timestamp))) { + + uav_vel_init = _uav_gps_vel.xyz; + + } else if (_local_velocity.valid && (_is_meas_valid(_local_velocity.timestamp))) { + + uav_vel_init = _local_velocity.xyz; + } + + // Initial state + matrix::Matrix state_init; + state_init.col(Base_KF_decoupled::AugmentedState::pos_rel) = pos_init; + state_init.col(Base_KF_decoupled::AugmentedState::vel_uav) = uav_vel_init; + state_init.col(Base_KF_decoupled::AugmentedState::bias) = bias_init; + state_init.col(Base_KF_decoupled::AugmentedState::acc_target) = target_acc_init; + state_init.col(Base_KF_decoupled::AugmentedState::vel_target) = target_vel_init; + + if (initEstimator(state_init)) { + PX4_INFO("VTE Position Estimator properly initialized."); + _estimator_initialized = true; + _uav_gps_vel.valid = false; + _target_gps_vel.valid = false; + _last_update = hrt_absolute_time(); + _last_predict = _last_update; + + } else { + resetFilter(); + } + } + + if (!_estimator_initialized) { + return false; + } + + // Once a position measurement other than the target GPS is available, reset the position and bias but keep the velocity estimate. + if (!_bias_set && should_set_bias) { + + PX4_INFO("Second relative position measurement available, re-setting position and bias."); + + const float state_pos_var = _param_vte_pos_unc_in.get(); + const float state_bias_var = _param_vte_bias_unc_in.get(); + + const Vector3f state_pos_var_vect(state_pos_var, state_pos_var, state_pos_var); + const Vector3f state_bias_var_vect(state_bias_var, state_bias_var, state_bias_var); + const Vector3f pos_init = obs_fiducial_marker.meas_xyz; + + // Compute the initial bias as the difference between the GPS and external position estimate. + // We assume that gnss observations have a bias but other position obs don't. It follows: gnss_obs = state + bias <--> bias = gnss_obs - state + const Vector3f bias_init = _pos_rel_gnss.xyz - pos_init; + + /* Reset filter's state and variance*/ + for (int i = 0; i < Direction::nb_directions; i++) { + + matrix::Vector temp_state = _target_estimator[i]->getAugmentedState(); + temp_state(Base_KF_decoupled::AugmentedState::bias) = bias_init(i); + temp_state(Base_KF_decoupled::AugmentedState::pos_rel) = pos_init(i); + _target_estimator[i]->setState(temp_state); + + matrix::Vector temp_state_var = + _target_estimator[i]->getAugmentedStateVar(); + temp_state_var(Base_KF_decoupled::AugmentedState::bias) = state_bias_var_vect(i); + temp_state_var(Base_KF_decoupled::AugmentedState::pos_rel) = state_pos_var_vect(i); + _target_estimator[i]->setStateVar(temp_state_var); + } + + _bias_set = true; + } + + // If we have a new sensor: fuse available measurements and publish innov. + if (new_pos_sensor || new_vel_sensor) { + + if (vte_fusion_aid_mask == ObservationValidMask::NO_VALID_DATA) { + return false; + } + + bool pos_fused = false; + + /*TARGET GPS*/ + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_POS) { + + if (fuse_meas(vehicle_acc_ned, obs_gps_pos_target)) { + pos_fused = true; + } + } + + /*MISSION POS GPS*/ + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_MISSION_POS) { + + if (fuse_meas(vehicle_acc_ned, obs_gps_pos_mission)) { + pos_fused = true; + } + } + + /*VISION*/ + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS) { + + if (fuse_meas(vehicle_acc_ned, obs_fiducial_marker)) { + _last_vision_obs_fused_time = hrt_absolute_time(); + pos_fused = true; + } + } + + /*GPS RELATIVE VELOCITY*/ + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_GPS_REL_VEL) { + fuse_meas(vehicle_acc_ned, obs_gps_vel_rel); + } + + /*TARGET GPS VELOCITY*/ + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_VEL) { + fuse_meas(vehicle_acc_ned, obs_gps_vel_target); + } + + // If at least one pos measurement was fused, consider the filter updated + return pos_fused; + + } + + return false; +} + +/*Vision observation: [rx, ry, rz]*/ +bool VTEPosition::processObsVision(const fiducial_marker_pos_report_s &fiducial_marker_pose, targetObsPos &obs) +{ + /* Rotate vision observation from body FRD - to vc-NED */ + const matrix::Quaternionf quat_att(fiducial_marker_pose.q); + const Vector3f vision_body(fiducial_marker_pose.x_rel_body, fiducial_marker_pose.y_rel_body, + fiducial_marker_pose.z_rel_body); + const Vector3f vision_ned = quat_att.rotateVector(vision_body); + + // Rotate covariance matrix to vc-NED + SquareMatrix Cov_rotated; + + // Use uncertainty from parameters or from vision messages + if (_ev_noise_md) { + // Uncertainty proportional to the vertical distance + const float meas_uncertainty = _range_sensor.valid ? ((_ev_pos_noise * _ev_pos_noise) * fmaxf(_range_sensor.dist_bottom, + 1.f)) : ((_ev_pos_noise * _ev_pos_noise) * 10); + Cov_rotated = diag(Vector3f(meas_uncertainty, meas_uncertainty, meas_uncertainty)); + + } else { + const SquareMatrix covMat = diag(Vector3f(fmaxf(fiducial_marker_pose.var_x_rel_body, + _ev_pos_noise * _ev_pos_noise), + fmaxf(fiducial_marker_pose.var_y_rel_body, _ev_pos_noise * _ev_pos_noise), + fmaxf(fiducial_marker_pose.var_z_rel_body, _ev_pos_noise * _ev_pos_noise))); + const matrix::Dcmf R_att = matrix::Dcm(quat_att); + Cov_rotated = R_att * covMat * R_att.transpose(); + } + + /* RELATIVE POSITION*/ + if (!PX4_ISFINITE(vision_ned(0)) || !PX4_ISFINITE(vision_ned(1)) || !PX4_ISFINITE(vision_ned(2))) { + PX4_WARN("VISION position is corrupt!"); + + } else { + + obs.timestamp = fiducial_marker_pose.timestamp; + + obs.updated_xyz.setAll(true); + + // Assume noise correlation negligible: + obs.meas_h_xyz(Direction::x, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + obs.meas_h_xyz(Direction::y, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + obs.meas_h_xyz(Direction::z, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + + obs.meas_xyz = vision_ned; + + // Assume off diag elements ~ 0 + obs.meas_unc_xyz(Direction::x) = Cov_rotated(0, 0); + obs.meas_unc_xyz(Direction::y) = Cov_rotated(1, 1); + obs.meas_unc_xyz(Direction::z) = Cov_rotated(2, 2); + + return true; + } + + return false; +} + +/*Drone GNSS velocity observation: [r_dotx, r_doty, r_dotz]*/ +bool VTEPosition::processObsGNSSVelRel(const sensor_gps_s &vehicle_gps_position, targetObsPos &obs) +{ + + // Make sure measurement are valid + if (!PX4_ISFINITE(vehicle_gps_position.vel_n_m_s) || !PX4_ISFINITE(vehicle_gps_position.vel_e_m_s) + || !PX4_ISFINITE(vehicle_gps_position.vel_d_m_s)) { + PX4_WARN("UAV GPS velocity is corrupt!"); + return false; + + } else { + + Vector3f vel_uav_ned(vehicle_gps_position.vel_n_m_s, vehicle_gps_position.vel_e_m_s, vehicle_gps_position.vel_d_m_s); + + if (_gps_pos_is_offset) { + if ((_velocity_offset_ned.valid) + && ((_velocity_offset_ned.timestamp - vehicle_gps_position.timestamp) < measurement_updated_TIMEOUT_US)) { + + vel_uav_ned -= _velocity_offset_ned.xyz; + + } else { + return false; + } + } + + obs.meas_xyz = vel_uav_ned; + + obs.meas_h_xyz(Direction::x, Base_KF_decoupled::AugmentedState::vel_uav) = 1; + obs.meas_h_xyz(Direction::y, Base_KF_decoupled::AugmentedState::vel_uav) = 1; + obs.meas_h_xyz(Direction::z, Base_KF_decoupled::AugmentedState::vel_uav) = 1; + + const float unc = fmaxf(vehicle_gps_position.s_variance_m_s * vehicle_gps_position.s_variance_m_s, + _gps_vel_noise * _gps_vel_noise); + obs.meas_unc_xyz(Direction::x) = unc; + obs.meas_unc_xyz(Direction::y) = unc; + obs.meas_unc_xyz(Direction::z) = unc; + + obs.timestamp = vehicle_gps_position.timestamp; + + obs.updated_xyz.setAll(true); + + return true; + } + + return false; + +} + +/*Target GNSS velocity observation: [r_dotx, r_doty, r_dotz]*/ +bool VTEPosition::processObsGNSSVelTarget(const target_gnss_s &target_GNSS_report, targetObsPos &obs) +{ + + // Make sure measurement are valid + if (!PX4_ISFINITE(target_GNSS_report.vel_n_m_s) || !PX4_ISFINITE(target_GNSS_report.vel_e_m_s) + || !PX4_ISFINITE(target_GNSS_report.vel_d_m_s)) { + PX4_WARN("Target GPS velocity is corrupt!"); + return false; + + } else { + + // If the target is moving, the relative velocity is expressed as the drone verlocity - the target velocity + obs.meas_xyz(Direction::x) = target_GNSS_report.vel_n_m_s; + obs.meas_xyz(Direction::y) = target_GNSS_report.vel_e_m_s; + obs.meas_xyz(Direction::z) = target_GNSS_report.vel_d_m_s; + + const float unc = fmaxf(target_GNSS_report.s_variance_m_s * target_GNSS_report.s_variance_m_s, + _gps_vel_noise * _gps_vel_noise); + + obs.meas_unc_xyz(Direction::x) = unc; + obs.meas_unc_xyz(Direction::y) = unc; + obs.meas_unc_xyz(Direction::z) = unc; + + obs.meas_h_xyz(Direction::x, Base_KF_decoupled::AugmentedState::vel_target) = 1; + obs.meas_h_xyz(Direction::y, Base_KF_decoupled::AugmentedState::vel_target) = 1; + obs.meas_h_xyz(Direction::z, Base_KF_decoupled::AugmentedState::vel_target) = 1; + + obs.timestamp = target_GNSS_report.timestamp; + + obs.updated_xyz.setAll(true); + + return true; + } + + return false; +} + +/*Target GNSS mission observation: [rx + bx, ry + by, rz + bz]*/ +bool VTEPosition::processObsGNSSPosMission(const sensor_gps_s &vehicle_gps_position, targetObsPos &obs) +{ + + + if (((fabs(vehicle_gps_position.latitude_deg) < DBL_EPSILON) + && (fabs(vehicle_gps_position.longitude_deg) < DBL_EPSILON)) + || !PX4_ISFINITE(vehicle_gps_position.altitude_msl_m)) { + PX4_WARN("vehicle GPS position is corrupt!"); + return false; + } + + if (_gps_pos_is_offset && !_gps_pos_offset_ned.valid) { + return false; + } + + + // Obtain GPS relative measurements in NED as target_global - uav_gps_global followed by global2local transformation + Vector3f gps_relative_pos; + get_vector_to_next_waypoint(vehicle_gps_position.latitude_deg, vehicle_gps_position.longitude_deg, + _mission_position.lat_deg, _mission_position.lon_deg, + &gps_relative_pos(0), &gps_relative_pos(1)); + + // Down direction (if the drone is above the target, the relative position is positive) + gps_relative_pos(2) = (float)vehicle_gps_position.altitude_msl_m - _mission_position.alt_m; + + // Offset gps relative position to the center of mass: + if (_gps_pos_is_offset && _gps_pos_offset_ned.valid + && ((_gps_pos_offset_ned.timestamp - vehicle_gps_position.timestamp) < measurement_updated_TIMEOUT_US)) { + gps_relative_pos += _gps_pos_offset_ned.xyz; + } + + const float gps_unc_horizontal = fmaxf(vehicle_gps_position.eph * vehicle_gps_position.eph, + _gps_pos_noise * _gps_pos_noise); + const float gps_unc_vertical = fmaxf(vehicle_gps_position.epv * vehicle_gps_position.epv, + _gps_pos_noise * _gps_pos_noise); + + // GPS already in NED, no rotation required. + // Obs: [pos_rel + bias] + obs.meas_h_xyz(Direction::x, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + obs.meas_h_xyz(Direction::y, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + obs.meas_h_xyz(Direction::z, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + + if (_bias_set) { + obs.meas_h_xyz(Direction::x, Base_KF_decoupled::AugmentedState::bias) = 1; + obs.meas_h_xyz(Direction::y, Base_KF_decoupled::AugmentedState::bias) = 1; + obs.meas_h_xyz(Direction::z, Base_KF_decoupled::AugmentedState::bias) = 1; + } + + obs.timestamp = vehicle_gps_position.timestamp; + + obs.meas_xyz = gps_relative_pos; + + obs.meas_unc_xyz(Direction::x) = gps_unc_horizontal; + obs.meas_unc_xyz(Direction::y) = gps_unc_horizontal; + obs.meas_unc_xyz(Direction::z) = gps_unc_vertical; + + obs.updated_xyz.setAll(true); + + // If the target GPS fusion is enabled give it priority over the GPS relative position measurement. + if (!(_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_POS) || !(_is_meas_valid(_pos_rel_gnss.timestamp))) { + _pos_rel_gnss.timestamp = obs.timestamp; + _pos_rel_gnss.valid = (PX4_ISFINITE(gps_relative_pos(0)) && PX4_ISFINITE(gps_relative_pos(1)) + && PX4_ISFINITE(gps_relative_pos(2))); + _pos_rel_gnss.xyz = gps_relative_pos; + } + + return true; +} + +/*Target GNSS observation: [rx + bx, ry + by, rz + bz]*/ +bool VTEPosition::processObsGNSSPosTarget(const target_gnss_s &target_GNSS_report, + const sensor_gps_s &vehicle_gps_position, targetObsPos &obs) +{ + + const float dt_sync_us = fabsf((float)(vehicle_gps_position.timestamp - target_GNSS_report.timestamp)); + + if ((fabs(vehicle_gps_position.latitude_deg) < DBL_EPSILON + && fabs(vehicle_gps_position.longitude_deg) < DBL_EPSILON) + || !PX4_ISFINITE(vehicle_gps_position.altitude_msl_m)) { + PX4_WARN("vehicle GPS position is corrupt!"); + return false; + + } else if ((fabs(target_GNSS_report.latitude_deg) < DBL_EPSILON + && fabs(target_GNSS_report.longitude_deg) < DBL_EPSILON) || !PX4_ISFINITE(target_GNSS_report.altitude_msl_m)) { + PX4_WARN("Target GPS position is corrupt!"); + return false; + + } else if (dt_sync_us > measurement_valid_TIMEOUT_US) { + PX4_INFO("Time diff. between UAV GNSS and target GNNS too high: %.2f [ms] > timeout: %.2f [ms]", + (double)(dt_sync_us / 1000), (double)(measurement_valid_TIMEOUT_US / 1000)); + return false; + } + + // Obtain GPS relative measurements in NED as target_global - uav_gps_global followed by global2local transformation + Vector3f gps_relative_pos; + get_vector_to_next_waypoint(vehicle_gps_position.latitude_deg, vehicle_gps_position.longitude_deg, + target_GNSS_report.latitude_deg, target_GNSS_report.longitude_deg, + &gps_relative_pos(0), &gps_relative_pos(1)); + + // Down direction (if the drone is above the target, the relative position is positive) + gps_relative_pos(2) = (float)(vehicle_gps_position.altitude_msl_m) - target_GNSS_report.altitude_msl_m; + + // Offset gps relative position to the center of mass: + if (_gps_pos_is_offset) { + if ((_gps_pos_offset_ned.valid) + && ((_gps_pos_offset_ned.timestamp - vehicle_gps_position.timestamp) < measurement_updated_TIMEOUT_US)) { + gps_relative_pos += _gps_pos_offset_ned.xyz; + + } else { + return false; + } + } + + // Var(aX - bY) = a^2 Var(X) + b^2Var(Y) - 2ab Cov(X,Y) + const float gps_unc_horizontal = fmaxf(vehicle_gps_position.eph * vehicle_gps_position.eph, + _gps_pos_noise * _gps_pos_noise) + fmaxf(target_GNSS_report.eph * target_GNSS_report.eph, + _gps_pos_noise * _gps_pos_noise); + const float gps_unc_vertical = fmaxf(vehicle_gps_position.epv * vehicle_gps_position.epv, + _gps_pos_noise * _gps_pos_noise) + fmaxf(target_GNSS_report.epv * target_GNSS_report.epv, + _gps_pos_noise * _gps_pos_noise); + + // GPS already in NED, no rotation required. + // Obs: [pos_rel + bias] + obs.meas_h_xyz(Direction::x, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + obs.meas_h_xyz(Direction::y, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + obs.meas_h_xyz(Direction::z, Base_KF_decoupled::AugmentedState::pos_rel) = 1; + + if (_bias_set) { + obs.meas_h_xyz(Direction::x, Base_KF_decoupled::AugmentedState::bias) = 1; + obs.meas_h_xyz(Direction::y, Base_KF_decoupled::AugmentedState::bias) = 1; + obs.meas_h_xyz(Direction::z, Base_KF_decoupled::AugmentedState::bias) = 1; + } + + obs.timestamp = target_GNSS_report.timestamp; + + obs.meas_xyz = gps_relative_pos; + + obs.meas_unc_xyz(Direction::x) = gps_unc_horizontal; + obs.meas_unc_xyz(Direction::y) = gps_unc_horizontal; + obs.meas_unc_xyz(Direction::z) = gps_unc_vertical; + + obs.updated_xyz.setAll(true); + + // Keep track of the gps relative position + _pos_rel_gnss.timestamp = obs.timestamp; + _pos_rel_gnss.valid = (PX4_ISFINITE(gps_relative_pos(0)) && PX4_ISFINITE(gps_relative_pos(1)) + && PX4_ISFINITE(gps_relative_pos(2))); + _pos_rel_gnss.xyz = gps_relative_pos; + + return true; +} + +bool VTEPosition::fuse_meas(const Vector3f &vehicle_acc_ned, const targetObsPos &target_pos_obs) +{ + perf_begin(_vte_update_perf); + + estimator_aid_source3d_s target_innov; + bool all_directions_fused = true; + Vector meas_h_row; + + // Compute the measurement's time delay (difference between state and measurement time on validity) + const float dt_sync_us = (_last_predict - target_pos_obs.timestamp); + target_innov.time_last_fuse = (int)(dt_sync_us / 1000); // For debug: log the time sync. TODO: remove + target_innov.timestamp_sample = target_pos_obs.timestamp; + target_innov.timestamp = hrt_absolute_time(); + + if (dt_sync_us > measurement_valid_TIMEOUT_US) { + + PX4_DEBUG("Obs i = %d too old. Time sync: %.2f [ms] > timeout: %.2f [ms]", + target_pos_obs.type, + (double)(dt_sync_us / 1000), (double)(measurement_valid_TIMEOUT_US / 1000)); + + target_innov.fused = false; + perf_end(_vte_update_perf); + publishInnov(target_innov, target_pos_obs.type); + return false; + } + + const float dt_sync_s = dt_sync_us / 1_s; + + for (int j = 0; j < Direction::nb_directions; j++) { + + if (!target_pos_obs.updated_xyz(j)) { + all_directions_fused = false; + PX4_DEBUG("Obs i = %d : non-valid obs in direction: %d", target_pos_obs.type, j); + continue; + } + + //Get the corresponding row of the H matrix. + meas_h_row = target_pos_obs.meas_h_xyz.row(j); + + // Move state back to the measurement time of validity. The state synchronized with the measurement will be used to compute the innovation. + _target_estimator[j]->syncState(dt_sync_s, vehicle_acc_ned(j)); + + _target_estimator[j]->setH(meas_h_row); + target_innov.innovation_variance[j] = _target_estimator[j]->computeInnovCov( + target_pos_obs.meas_unc_xyz(j)); + target_innov.innovation[j] = _target_estimator[j]->computeInnov(target_pos_obs.meas_xyz(j)); + + // Set the Normalized Innovation Squared (NIS) check threshold. Used to reject outlier measurements + _target_estimator[j]->setNISthreshold(_nis_threshold); + + if (!_target_estimator[j]->update()) { + all_directions_fused = false; + PX4_DEBUG("Obs i = %d : not fused in direction: %d", target_pos_obs.type, j); + } + + target_innov.observation[j] = target_pos_obs.meas_xyz(j); + target_innov.observation_variance[j] = target_pos_obs.meas_unc_xyz(j); + + // log test ratio defined as _innov / _innov_cov * _innov. If test_ratio > 3.84, no fusion + target_innov.test_ratio[j] = _target_estimator[j]->getTestRatio(); + } + + target_innov.fused = all_directions_fused; + target_innov.innovation_rejected = !all_directions_fused; + + perf_end(_vte_update_perf); + publishInnov(target_innov, target_pos_obs.type); + + return all_directions_fused; +} + +void VTEPosition::publishInnov(const estimator_aid_source3d_s &target_innov, const ObservationType type) +{ + + // Publish innovations + switch (type) { + case ObservationType::target_gps_pos: + _vte_aid_gps_pos_target_pub.publish(target_innov); + break; + + case ObservationType::mission_gps_pos: + _vte_aid_gps_pos_mission_pub.publish(target_innov); + break; + + case ObservationType::vel_rel_gps: + _vte_aid_gps_vel_rel_pub.publish(target_innov); + break; + + case ObservationType::vel_target_gps: + _vte_aid_gps_vel_target_pub.publish(target_innov); + break; + + case ObservationType::fiducial_marker: + _vte_aid_fiducial_marker_pub.publish(target_innov); + break; + } +} + +void VTEPosition::publishTarget() +{ + + vision_target_est_position_s vte_state{}; + + landing_target_pose_s target_pose{}; + + target_pose.timestamp = _last_predict; + vte_state.timestamp = _last_predict; + target_pose.is_static = (_target_mode == TargetMode::Stationary); + + target_pose.rel_pos_valid = _is_meas_valid(_last_update); + + matrix::Vector augmented_state_x = + _target_estimator[Direction::x]->getAugmentedState(); + matrix::Vector augmented_state_y = + _target_estimator[Direction::y]->getAugmentedState(); + matrix::Vector augmented_state_z = + _target_estimator[Direction::z]->getAugmentedState(); + + matrix::Vector augmented_state_var_x = + _target_estimator[Direction::x]->getAugmentedStateVar(); + matrix::Vector augmented_state_var_y = + _target_estimator[Direction::y]->getAugmentedStateVar(); + matrix::Vector augmented_state_var_z = + _target_estimator[Direction::z]->getAugmentedStateVar(); + + // Fill target pose + target_pose.x_rel = augmented_state_x(Base_KF_decoupled::AugmentedState::pos_rel); + target_pose.y_rel = augmented_state_y(Base_KF_decoupled::AugmentedState::pos_rel); + target_pose.z_rel = augmented_state_z(Base_KF_decoupled::AugmentedState::pos_rel); + + target_pose.cov_x_rel = augmented_state_var_x(Base_KF_decoupled::AugmentedState::pos_rel); + target_pose.cov_y_rel = augmented_state_var_y(Base_KF_decoupled::AugmentedState::pos_rel); + target_pose.cov_z_rel = augmented_state_var_z(Base_KF_decoupled::AugmentedState::pos_rel); + + target_pose.vx_rel = augmented_state_x(Base_KF_decoupled::AugmentedState::vel_target) - augmented_state_x( + Base_KF_decoupled::AugmentedState::vel_uav); + target_pose.vy_rel = augmented_state_y(Base_KF_decoupled::AugmentedState::vel_target) - augmented_state_y( + Base_KF_decoupled::AugmentedState::vel_uav); + target_pose.vz_rel = augmented_state_z(Base_KF_decoupled::AugmentedState::vel_target) - augmented_state_z( + Base_KF_decoupled::AugmentedState::vel_uav); + + /* Var(aX + bY) = a^2 Var(x) + b^2 Var(y) + 2abCov(X,Y) */ + target_pose.cov_vx_rel = augmented_state_var_x(Base_KF_decoupled::AugmentedState::vel_target) + augmented_state_var_x( + Base_KF_decoupled::AugmentedState::vel_uav); + target_pose.cov_vy_rel = augmented_state_var_y(Base_KF_decoupled::AugmentedState::vel_target) + augmented_state_var_y( + Base_KF_decoupled::AugmentedState::vel_uav); + target_pose.cov_vz_rel = augmented_state_var_z(Base_KF_decoupled::AugmentedState::vel_target) + augmented_state_var_z( + Base_KF_decoupled::AugmentedState::vel_uav); + + // Fill vision target estimator state + vte_state.x_rel = target_pose.x_rel; + vte_state.y_rel = target_pose.y_rel; + vte_state.z_rel = target_pose.z_rel; + + vte_state.cov_x_rel = target_pose.cov_x_rel; + vte_state.cov_y_rel = target_pose.cov_y_rel; + vte_state.cov_z_rel = target_pose.cov_z_rel; + + vte_state.vx_rel = target_pose.vx_rel; + vte_state.vy_rel = target_pose.vy_rel; + vte_state.vz_rel = target_pose.vz_rel; + + vte_state.cov_vx_rel = target_pose.cov_vx_rel; + vte_state.cov_vy_rel = target_pose.cov_vy_rel; + vte_state.cov_vz_rel = target_pose.cov_vz_rel; + + vte_state.vx_target = augmented_state_x(Base_KF_decoupled::AugmentedState::vel_target); + vte_state.vy_target = augmented_state_y(Base_KF_decoupled::AugmentedState::vel_target); + vte_state.vz_target = augmented_state_z(Base_KF_decoupled::AugmentedState::vel_target); + + vte_state.cov_vx_target = augmented_state_var_x(Base_KF_decoupled::AugmentedState::vel_target); + vte_state.cov_vy_target = augmented_state_var_y(Base_KF_decoupled::AugmentedState::vel_target); + vte_state.cov_vz_target = augmented_state_var_z(Base_KF_decoupled::AugmentedState::vel_target); + + vte_state.x_bias = augmented_state_x(Base_KF_decoupled::AugmentedState::bias); + vte_state.y_bias = augmented_state_y(Base_KF_decoupled::AugmentedState::bias); + vte_state.z_bias = augmented_state_z(Base_KF_decoupled::AugmentedState::bias); + + vte_state.cov_x_bias = augmented_state_var_x(Base_KF_decoupled::AugmentedState::bias); + vte_state.cov_y_bias = augmented_state_var_y(Base_KF_decoupled::AugmentedState::bias); + vte_state.cov_z_bias = augmented_state_var_z(Base_KF_decoupled::AugmentedState::bias); + + vte_state.ax_target = augmented_state_x(Base_KF_decoupled::AugmentedState::acc_target); + vte_state.ay_target = augmented_state_y(Base_KF_decoupled::AugmentedState::acc_target); + vte_state.az_target = augmented_state_z(Base_KF_decoupled::AugmentedState::acc_target); + + vte_state.cov_ax_target = augmented_state_var_x(Base_KF_decoupled::AugmentedState::acc_target); + vte_state.cov_ay_target = augmented_state_var_y(Base_KF_decoupled::AugmentedState::acc_target); + vte_state.cov_az_target = augmented_state_var_z(Base_KF_decoupled::AugmentedState::acc_target); + + // Prec land does not check target_pose.abs_pos_valid. Only send the target if abs pose valid. + if (_local_position.valid && target_pose.rel_pos_valid) { + target_pose.x_abs = target_pose.x_rel + _local_position.xyz(0); + target_pose.y_abs = target_pose.y_rel + _local_position.xyz(1); + target_pose.z_abs = target_pose.z_rel + _local_position.xyz(2); + target_pose.abs_pos_valid = true; + + if (_target_mode == TargetMode::Moving) { + // If the target is moving, move towards its expected location + float mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); + param_get(param_find("MPC_Z_V_AUTO_DN"), &mpc_z_v_auto_dn); + float intersection_time_s = fabsf(target_pose.z_rel / mpc_z_v_auto_dn); + intersection_time_s = math::constrain(intersection_time_s, _param_vte_moving_t_min.get(), + _param_vte_moving_t_max.get()); + + // Anticipate where the target will be + target_pose.x_abs += vte_state.vx_target * intersection_time_s; + target_pose.y_abs += vte_state.vy_target * intersection_time_s; + target_pose.z_abs += vte_state.vz_target * intersection_time_s; + } + + _targetPosePub.publish(target_pose); + + } + + _targetEstimatorStatePub.publish(vte_state); + + // If the target is static, valid and vision obs was fused recently, use the relative to aid the EKF2 state estimation. + // Check performed in EKF2 to use target vel: if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) + target_pose.rel_vel_valid = target_pose.is_static && _param_vte_ekf_aid.get() && target_pose.rel_pos_valid && + (hrt_absolute_time() - _last_vision_obs_fused_time) < measurement_valid_TIMEOUT_US; + + // TODO: decide what to do with Bias lim + float bias_lim = _param_vte_bias_lim.get(); + + if (((float)fabs(vte_state.x_bias) > bias_lim + || (float)fabs(vte_state.y_bias) > bias_lim || (float)fabs(vte_state.z_bias) > bias_lim)) { + + PX4_DEBUG("Bias exceeds limit: %.2f bias x: %.2f bias y: %.2f bias z: %.2f", (double)bias_lim, + (double)vte_state.x_bias, (double)vte_state.y_bias, (double)vte_state.z_bias); + + // resetFilter(); + } + +} + +void VTEPosition::_check_params(const bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + updateParams(); + } + + // Make sure range sensor, local position and local velocity are up to date. + if (_range_sensor.valid) { + _range_sensor.valid = _is_meas_updated(_range_sensor.timestamp); + } + + if (_local_position.valid) { + _local_position.valid = _is_meas_updated(_local_position.timestamp); + } + + if (_local_velocity.valid) { + _local_velocity.valid = _is_meas_updated(_local_velocity.timestamp); + } +} + +void VTEPosition::set_gps_pos_offset(const matrix::Vector3f &xyz, const bool gps_is_offset) +{ + _gps_pos_is_offset = gps_is_offset; + _gps_pos_offset_ned.xyz = xyz; + _gps_pos_offset_ned.valid = (PX4_ISFINITE(_gps_pos_offset_ned.xyz(0)) && PX4_ISFINITE(_gps_pos_offset_ned.xyz(0)) + && PX4_ISFINITE(_gps_pos_offset_ned.xyz(2))); + _gps_pos_offset_ned.timestamp = hrt_absolute_time(); +} + +void VTEPosition::set_velocity_offset(const matrix::Vector3f &xyz) +{ + _velocity_offset_ned.xyz = xyz; + _velocity_offset_ned.valid = (PX4_ISFINITE(_velocity_offset_ned.xyz(0)) && PX4_ISFINITE(_velocity_offset_ned.xyz(0)) + && PX4_ISFINITE(_velocity_offset_ned.xyz(2))); + _velocity_offset_ned.timestamp = hrt_absolute_time(); +} + +void VTEPosition::set_range_sensor(const float dist, const bool valid, hrt_abstime timestamp) +{ + _range_sensor.valid = valid && _is_meas_updated(timestamp); + _range_sensor.dist_bottom = dist; + _range_sensor.timestamp = timestamp; + +} + +void VTEPosition::set_local_velocity(const matrix::Vector3f &vel_xyz, const bool vel_valid, hrt_abstime timestamp) +{ + _local_velocity.xyz = vel_xyz; + _local_velocity.valid = vel_valid && _is_meas_updated(timestamp); + _local_velocity.timestamp = timestamp; +} + +void VTEPosition::set_local_position(const matrix::Vector3f &xyz, const bool pos_valid, hrt_abstime timestamp) +{ + _local_position.xyz = xyz; + _local_position.valid = pos_valid && _is_meas_updated(timestamp); + _local_position.timestamp = timestamp; +} + +void VTEPosition::set_mission_position(const double lat_deg, const double lon_deg, const float alt_m) +{ + if (_vte_aid_mask & SensorFusionMask::USE_MISSION_POS) { + _mission_position.lat_deg = lat_deg; + _mission_position.lon_deg = lon_deg; + _mission_position.alt_m = alt_m; + _mission_position.valid = (((fabs(_mission_position.lat_deg) > DBL_EPSILON) + || (fabs(_mission_position.lon_deg) > DBL_EPSILON)) + && PX4_ISFINITE(_mission_position.alt_m)); + + if (_mission_position.valid) { + PX4_INFO("Mission position used lat: %.8f [deg] -- lon: %.8f [deg] -- alt %.2f [m]", lat_deg, + lon_deg, (double)(alt_m)); + + } else { + PX4_INFO("Mission position not used because not valid. lat: %.8f [deg] -- lon: %.8f [deg] -- alt %.2f [m]", + lat_deg, + lon_deg, (double)(alt_m)); + } + + } else { + _mission_position.valid = false; + } +} + +void VTEPosition::updateParams() +{ + ModuleParams::updateParams(); + + _target_acc_unc = _param_vte_acc_t_unc.get(); + _bias_unc = _param_vte_bias_unc.get(); + _drone_acc_unc = _param_vte_acc_d_unc.get(); + _gps_vel_noise = _param_vte_gps_vel_noise.get(); + _gps_pos_noise = _param_vte_gps_pos_noise.get(); + _ev_noise_md = _param_vte_ev_noise_md.get(); + _ev_pos_noise = _param_vte_ev_pos_noise.get(); + _vte_aid_mask = _param_vte_aid_mask.get(); + _nis_threshold = _param_vte_pos_nis_thre.get(); +} + +bool VTEPosition::selectTargetEstimator() +{ + Base_KF_decoupled *tmp_x = nullptr; + Base_KF_decoupled *tmp_y = nullptr; + Base_KF_decoupled *tmp_z = nullptr; + + switch (_target_mode) { + case TargetMode::Stationary: + + tmp_x = new KF_xyzb_decoupled_static; + tmp_y = new KF_xyzb_decoupled_static; + tmp_z = new KF_xyzb_decoupled_static; + PX4_INFO("VTE postion init for static target, [x,y,z,b] decoupled in x,y,z filters."); + + break; + + case TargetMode::Moving: + + tmp_x = new KF_xyzb_v_decoupled_moving; + tmp_y = new KF_xyzb_v_decoupled_moving; + tmp_z = new KF_xyzb_v_decoupled_moving; + PX4_INFO("VTE position init for moving target, [x,y,z,b,v] decoupled in x,y,z filters."); + + break; + + case TargetMode::NotInit: + break; + } + + const bool init_failed = ((tmp_x == nullptr) || (tmp_y == nullptr) || (tmp_z == nullptr)); + + if (init_failed) { + delete tmp_x; + delete tmp_y; + delete tmp_z; + + PX4_ERR("VTE position init failed"); + return false; + + } else { + + delete _target_estimator[Direction::x]; + delete _target_estimator[Direction::y]; + delete _target_estimator[Direction::z]; + + _target_estimator[Direction::x] = tmp_x; + _target_estimator[Direction::y] = tmp_y; + _target_estimator[Direction::z] = tmp_z; + + return true; + } +} + +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.h b/src/modules/vision_target_estimator/Position/VTEPosition.h new file mode 100644 index 000000000000..ec99f8565a26 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/VTEPosition.h @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VTEPosition.h + * @brief Estimate the state of a target by processessing and fusing sensor data in a Kalman Filter. + * + * @author Jonas Perolini + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "KF_position_static.h" +#include "KF_position_moving.h" + +using namespace time_literals; + +namespace vision_target_estimator +{ + +class VTEPosition: public ModuleParams +{ +public: + + VTEPosition(); + virtual ~VTEPosition(); + + /* + * Get new measurements and update the state estimate + */ + void update(const matrix::Vector3f &acc_ned); + + bool init(); + + void resetFilter(); + + void set_mission_position(const double lat_deg, const double lon_deg, const float alt_m); + + void set_range_sensor(const float dist, const bool valid, const hrt_abstime timestamp); + + void set_local_velocity(const matrix::Vector3f &vel_xyz, const bool valid, const hrt_abstime timestamp); + + void set_local_position(const matrix::Vector3f &xyz, const bool valid, const hrt_abstime timestamp); + + void set_gps_pos_offset(const matrix::Vector3f &xyz, const bool gps_is_offset); + + void set_velocity_offset(const matrix::Vector3f &xyz); + + bool has_timed_out() {return _has_timed_out;}; + +private: + struct accInput { + + bool acc_ned_valid = false; + matrix::Vector3f vehicle_acc_ned; + }; + +protected: + + /* + * Update parameters. + */ + void updateParams() override; + + + /* timeout after which the target is not valid if no measurements are seen*/ + static constexpr uint32_t target_valid_TIMEOUT_US = 2_s; + + /* timeout after which the measurement is not valid*/ + static constexpr uint32_t measurement_valid_TIMEOUT_US = 1_s; + + /* timeout after which the measurement is not considered updated*/ + static constexpr uint32_t measurement_updated_TIMEOUT_US = 100_ms; + + uORB::Publication _targetPosePub{ORB_ID(landing_target_pose)}; + uORB::Publication _targetEstimatorStatePub{ORB_ID(vision_target_est_position)}; + + uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + + // publish innovations target_estimator_gps_pos + uORB::Publication _vte_aid_gps_pos_target_pub{ORB_ID(vte_aid_gps_pos_target)}; + uORB::Publication _vte_aid_gps_pos_mission_pub{ORB_ID(vte_aid_gps_pos_mission)}; + uORB::Publication _vte_aid_gps_vel_target_pub{ORB_ID(vte_aid_gps_vel_target)}; + uORB::Publication _vte_aid_gps_vel_rel_pub{ORB_ID(vte_aid_gps_vel_rel)}; + uORB::Publication _vte_aid_fiducial_marker_pub{ORB_ID(vte_aid_fiducial_marker)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + +private: + + enum Direction { + x = 0, + y = 1, + z = 2, + nb_directions = 3, + }; + + static inline bool _is_meas_valid(hrt_abstime time_stamp) {return (hrt_absolute_time() - time_stamp) < measurement_valid_TIMEOUT_US;}; + static inline bool _is_meas_updated(hrt_abstime time_stamp) {return (hrt_absolute_time() - time_stamp) < measurement_updated_TIMEOUT_US;}; + + bool _has_timed_out{false}; + + enum class TargetMode { + Stationary = 0, + Moving = 1, + NotInit + }; + + TargetMode _target_mode{TargetMode::NotInit}; + + enum ObservationType { + target_gps_pos = 0, + mission_gps_pos = 1, + vel_rel_gps = 2, + vel_target_gps = 3, + fiducial_marker = 4, + }; + + struct targetObsPos { + + ObservationType type; + hrt_abstime timestamp; + + matrix::Vector + updated_xyz; // Indicates if we have an observation in the x, y or z direction + matrix::Vector3f meas_xyz; // Measurements (meas_x, meas_y, meas_z) + matrix::Vector3f meas_unc_xyz; // Measurements' uncertainties + matrix::Matrix + meas_h_xyz; // Observation matrix where the rows correspond to the x,y,z observations and the columns to the AugmentedState + }; + + enum SensorFusionMask : uint8_t { + // Bit locations for fusion_mode + NO_SENSOR_FUSION = 0, + USE_TARGET_GPS_POS = (1 << 0), ///< set to true to use target GPS position data + USE_GPS_REL_VEL = (1 << 1), ///< set to true to use drone GPS velocity data (and target GPS velocity data if the target is moving) + USE_EXT_VIS_POS = (1 << 2), ///< set to true to use target external vision-based relative position data + USE_MISSION_POS = (1 << 3), ///< set to true to use the PX4 mission position + }; + + enum ObservationValidMask : uint8_t { + // Bit locations for valid observations + NO_VALID_DATA = 0, + FUSE_TARGET_GPS_POS = (1 << 0), ///< set to true if target GPS position data is ready to be fused + FUSE_GPS_REL_VEL = (1 << 1), ///< set to true if drone GPS velocity data (and target GPS velocity data if the target is moving) + FUSE_EXT_VIS_POS = (1 << 2), ///< set to true if target external vision-based relative position data is ready to be fused + FUSE_MISSION_POS = (1 << 3), ///< set to true if the PX4 mission position is ready to be fused + FUSE_TARGET_GPS_VEL = (1 << 4), ///< set to true if target GPS velocity data is ready to be fused + }; + + bool selectTargetEstimator(); + bool initEstimator(const matrix::Matrix + &state_init); + bool update_step(const matrix::Vector3f &vehicle_acc_ned); + void predictionStep(const matrix::Vector3f &acc); + + bool processObsVision(const fiducial_marker_pos_report_s &fiducial_marker_pose, targetObsPos &obs); + bool processObsGNSSPosTarget(const target_gnss_s &target_GNSS_report, + const sensor_gps_s &vehicle_gps_position, targetObsPos &obs); + bool processObsGNSSPosMission(const sensor_gps_s &vehicle_gps_position, targetObsPos &obs); + bool processObsGNSSVelRel(const sensor_gps_s &vehicle_gps_position, targetObsPos &obs); + bool processObsGNSSVelTarget(const target_gnss_s &target_GNSS_report, targetObsPos &obs); + + bool fuse_meas(const matrix::Vector3f &vehicle_acc_ned, const targetObsPos &target_pos_obs); + void publishTarget(); + void publishInnov(const estimator_aid_source3d_s &target_innov, const ObservationType type); + + + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _fiducial_marker_report_sub{ORB_ID(fiducial_marker_pos_report)}; + uORB::Subscription _target_gnss_sub{ORB_ID(target_gnss)}; + + perf_counter_t _vte_predict_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE prediction")}; + perf_counter_t _vte_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE update")}; + + struct rangeSensor { + bool valid = false; + float dist_bottom; + hrt_abstime timestamp = 0; + }; + + rangeSensor _range_sensor{}; + + struct globalPos { + bool valid = false; + double lat_deg = 0.0; // Latitude in degrees + double lon_deg = 0.0; // Longitude in degrees + float alt_m = 0.f; // Altitude in meters AMSL + }; + + globalPos _mission_position{}; + + struct vecStamped { + hrt_abstime timestamp; + bool valid = false; + matrix::Vector3f xyz; + }; + + vecStamped _local_position{}; + vecStamped _local_velocity{}; + vecStamped _uav_gps_vel{}; + vecStamped _target_gps_vel{}; + vecStamped _pos_rel_gnss{}; + vecStamped _velocity_offset_ned{}; + vecStamped _gps_pos_offset_ned{}; + bool _gps_pos_is_offset{false}; + bool _bias_set{false}; + + uint64_t _last_vision_obs_fused_time{0}; + bool _estimator_initialized{false}; + + Base_KF_decoupled *_target_estimator[Direction::nb_directions] {nullptr, nullptr, nullptr}; + hrt_abstime _last_predict{0}; // timestamp of last filter prediction + hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout) + + void _check_params(const bool force); + + /* parameters from vision_target_estimator_params.c*/ + uint32_t _vte_TIMEOUT_US = 3_s; + int _vte_aid_mask{0}; + float _target_acc_unc; + float _bias_unc; + float _drone_acc_unc; + float _gps_vel_noise; + float _gps_pos_noise; + bool _ev_noise_md{false}; + float _ev_pos_noise; + float _nis_threshold; + + DEFINE_PARAMETERS( + (ParamInt) _param_vte_aid_mask, + (ParamFloat) _param_vte_btout, + (ParamFloat) _param_vte_acc_d_unc, + (ParamFloat) _param_vte_acc_t_unc, + (ParamFloat) _param_vte_bias_lim, + (ParamFloat) _param_vte_bias_unc, + (ParamFloat) _param_vte_pos_unc_in, + (ParamFloat) _param_vte_vel_unc_in, + (ParamFloat) _param_vte_bias_unc_in, + (ParamFloat) _param_vte_acc_unc_in, + (ParamFloat) _param_vte_gps_vel_noise, + (ParamFloat) _param_vte_gps_pos_noise, + (ParamInt) _param_vte_ev_noise_md, + (ParamFloat) _param_vte_ev_pos_noise, + (ParamInt) _param_vte_mode, + (ParamInt) _param_vte_ekf_aid, + (ParamFloat) _param_vte_moving_t_max, + (ParamFloat) _param_vte_moving_t_min, + (ParamFloat) _param_vte_pos_nis_thre + ) +}; +} // namespace vision_target_estimator diff --git a/src/modules/vision_target_estimator/Position/base_KF_position.h b/src/modules/vision_target_estimator/Position/base_KF_position.h new file mode 100644 index 000000000000..3687992f8106 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/base_KF_position.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Base_KF_position.h + * @brief Interface for decoupled target estimators + * @author Jonas Perolini + */ + +#pragma once + +class Base_KF_decoupled +{ +public: + Base_KF_decoupled() = default; + virtual ~Base_KF_decoupled() = default; + + enum AugmentedState { + pos_rel = 0, + vel_uav = 1, + bias = 2, + acc_target = 3, + vel_target = 4, + COUNT = 5, + }; + + //Prediction step: + virtual void predictState(float dt, float acc) = 0; + virtual void predictCov(float dt) = 0; + + // Backwards state prediciton + virtual void syncState(float dt, float acc) = 0; + + virtual void setH(const matrix::Vector &h_meas) = 0; + + virtual void setState(const matrix::Vector &augmented_state) = 0; + virtual void setStateVar(const matrix::Vector &augmented_state_var) = 0; + + virtual matrix::Vector getAugmentedState() = 0; + virtual matrix::Vector getAugmentedStateVar() = 0; + + virtual float computeInnovCov(float measUnc) = 0; + virtual float computeInnov(float meas) = 0; + + virtual bool update() = 0; + + // Normalized innovation squared (NIS) threshold. Used to reject measurements. + virtual void setNISthreshold(float nis_threshold) = 0; + + virtual float getTestRatio() = 0; + + virtual void setInputAccVar(float var) = 0; + virtual void setTargetAccVar(float var) = 0; + virtual void setBiasVar(float var) = 0; +}; diff --git a/src/modules/vision_target_estimator/Position/python_derivation/KF_xyzb_decoupled_static.py b/src/modules/vision_target_estimator/Position/python_derivation/KF_xyzb_decoupled_static.py new file mode 100644 index 000000000000..b54a752b7d3c --- /dev/null +++ b/src/modules/vision_target_estimator/Position/python_derivation/KF_xyzb_decoupled_static.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 2023 PX4 Development Team + 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: KF_xyzb_decoupled_static.py +Description: Derive the Kalman filter equations for static targets with decoupled dynamics using symforce +Author: Jonas Perolini +""" + +import symforce.symbolic as sf + +def generate_px4_function(function_name, output_names): + from symforce.codegen import Codegen, CppConfig + import os + import fileinput + + codegen = Codegen.function( + function_name, + output_names=output_names, + config=CppConfig()) + metadata = codegen.generate_function( + output_dir="src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_static_xyzb", + skip_directory_nesting=True) + + print("Files generated in {}:\n".format(metadata.output_dir)) + for f in metadata.generated_files: + print(" |- {}".format(os.path.relpath(f, metadata.output_dir))) + + # Replace cstdlib and Eigen functions by PX4 equivalents + with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True) as file: + for line in file: + line = line.replace("std::max", "math::max") + line = line.replace("std::min", "math::min") + line = line.replace("Eigen", "matrix") + line = line.replace("matrix/Dense", "matrix/math.hpp") + print(line, end='') + + + +#-------------------------------- DECOUPLED DYNAMICS, STATIC TARGET ------------------------------------------ # + +class State: + r = 0 + r_dot = 1 + b = 2 + n_states = 3 + +class Input: + a = 0 + n_inputs = 1 + +class Direction: + x = 0, + nb_directions = 1 + +class MState(sf.Matrix): + SHAPE = (State.n_states, State.n_states) + +class VMeas(sf.Matrix): + SHAPE = (1, State.n_states) + +class VState(sf.Matrix): + SHAPE = (State.n_states, 1) + +class VInput(sf.Matrix): + SHAPE = (Input.n_inputs, 1) + +class MDirections(sf.Matrix): + SHAPE = (Direction.nb_directions, Direction.nb_directions) + + +def predictState(dt: sf.Scalar, state: VState, acc: VInput) -> (VState): + + Phi = sf.Matrix([ [1, dt, 0], + [0, 1, 0], + [0, 0, 1]]) + + G = sf.Matrix([-0.5*dt*dt, -dt, 0]) + + return (Phi * state + G*acc).simplify() + + +def syncState(dt: sf.Scalar, state: VState, acc: VInput) -> (VState): + + Phi = sf.Matrix([ [1, dt, 0], + [0, 1, 0], + [0, 0, 1]]) + + G = sf.Matrix([-0.5*dt*dt, -dt, 0]) + + return (Phi.inv() * (state - G*acc)).simplify() + +def predictCov(dt: sf.Scalar, input_var: sf.Scalar, bias_var: sf.Scalar, covariance: MState) -> (MState): + Phi = sf.Matrix([ [1, dt, 0], + [0, 1, 0], + [0, 0, 1]]) + + G = sf.Matrix([-0.5*dt*dt, -dt, 0]) + + Q_bias = sf.Matrix([ [0, 0, 0], + [0, 0, 0], + [0, 0, bias_var]]) + + return G*input_var*G.T + Q_bias + Phi*covariance*Phi.T + +def computeInnovCov(meas_unc: sf.Scalar, covariance: MState, meas_matrix: VMeas) -> (sf.Scalar): + return (meas_matrix*covariance*meas_matrix.T)[0,0] + meas_unc + +# generate_px4_function(predictState, output_names=["predict_state"]) +# generate_px4_function(syncState, output_names=["sync_state"]) +generate_px4_function(predictCov, output_names=["cov_updated"]) +generate_px4_function(computeInnovCov, output_names=["innov_cov_updated"]) diff --git a/src/modules/vision_target_estimator/Position/python_derivation/KF_xyzb_v_decoupled_moving.py b/src/modules/vision_target_estimator/Position/python_derivation/KF_xyzb_v_decoupled_moving.py new file mode 100644 index 000000000000..4911c709a588 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/python_derivation/KF_xyzb_v_decoupled_moving.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 2023 PX4 Development Team + 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: KF_xyzb_v_decoupled_moving.py +Description: Derive the Kalman filter equations for moving targets with decoupled dynamics using symforce (augmented state) +Author: Jonas Perolini +""" + +import symforce.symbolic as sf + +def generate_px4_function(function_name, output_names): + from symforce.codegen import Codegen, CppConfig + import os + import fileinput + + codegen = Codegen.function( + function_name, + output_names=output_names, + config=CppConfig()) + metadata = codegen.generate_function( + output_dir="src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_moving_xyzb_v", + skip_directory_nesting=True) + + print("Files generated in {}:\n".format(metadata.output_dir)) + for f in metadata.generated_files: + print(" |- {}".format(os.path.relpath(f, metadata.output_dir))) + + # Replace cstdlib and Eigen functions by PX4 equivalents + with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True) as file: + for line in file: + line = line.replace("std::max", "math::max") + line = line.replace("std::min", "math::min") + line = line.replace("Eigen", "matrix") + line = line.replace("matrix/Dense", "matrix/math.hpp") + print(line, end='') + +#-------------------------------- DECOUPLED DYNAMICS, MOVING TARGET ------------------------------------------ # + +class State: + r = 0 + rx_dot = 1 + bx = 2 + atx = 3, + vtx = 4, + n_states = 5 + +class Input: + ax = 0 + n_inputs = 1 + +class Direction: + x = 0, + nb_directions = 1 + +class VState(sf.Matrix): + SHAPE = (State.n_states, 1) + +class MState(sf.Matrix): + SHAPE = (State.n_states, State.n_states) + +class VInput(sf.Matrix): + SHAPE = (Input.n_inputs, 1) + +class VMeas(sf.Matrix): + SHAPE = (1, State.n_states) + +class MDirections(sf.Matrix): + SHAPE = (Direction.nb_directions, Direction.nb_directions) + + +def predictState(dt: sf.Scalar, state: VState, acc: VInput) -> (VState): + + Phi = sf.Matrix([ [1, -dt, 0 , 0.5*dt*dt, dt], + [0, 1, 0, 0, 0], + [0, 0, 1, 0, 0], + [0, 0, 0, 1, 0], + [0, 0, 0, dt, 1],]) + + + G = sf.Matrix([ [-0.5*dt*dt], [dt], [0], [0], [0]]) + + return (Phi * state + G*acc).simplify() + + +def syncState(dt: sf.Scalar, state: VState, acc: VInput) -> (VState): + + Phi = sf.Matrix([ [1, -dt, 0 , 0.5*dt*dt, dt], + [0, 1, 0, 0, 0], + [0, 0, 1, 0, 0], + [0, 0, 0, 1, 0], + [0, 0, 0, dt, 1],]) + + + G = sf.Matrix([ [-0.5*dt*dt], [dt], [0], [0], [0]]) + + return (Phi.inv() * (state - G*acc)).simplify() + +def predictCov(dt: sf.Scalar, input_var: sf.Scalar, bias_var: sf.Scalar, acc_var: sf.Scalar, covariance: MState) -> (MState): + + Phi = sf.Matrix([ [1, -dt, 0 , 0.5*dt*dt, dt], + [0, 1, 0, 0, 0], + [0, 0, 1, 0, 0], + [0, 0, 0, 1, 0], + [0, 0, 0, dt, 1],]) + + + G = sf.Matrix([ [-0.5*dt*dt], [dt], [0], [0], [0]]) + + Q_acc = sf.Matrix([ [0, 0, 0, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 0, acc_var, 0], + [0, 0, 0, 0, 0]]) + + Q_bias = sf.Matrix([ [0, 0, 0, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, bias_var, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 0, 0, 0]]) + + return G*input_var*G.T + Q_bias + Q_acc + Phi*covariance*Phi.T + + +def computeInnovCov(meas_unc: sf.Scalar, covariance: MState, meas_matrix: VMeas) -> (sf.Scalar): + return (meas_matrix*covariance*meas_matrix.T)[0,0] + meas_unc + +# generate_px4_function(predictState, output_names=["predict_state"]) +# generate_px4_function(syncState, output_names=["sync_state"]) +generate_px4_function(predictCov, output_names=["cov_updated"]) +generate_px4_function(computeInnovCov, output_names=["innov_cov_updated"]) diff --git a/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_moving_xyzb_v/computeInnovCov.h b/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_moving_xyzb_v/computeInnovCov.h new file mode 100644 index 000000000000..3524c011b828 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_moving_xyzb_v/computeInnovCov.h @@ -0,0 +1,68 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym +{ + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: computeInnovCov + * + * Args: + * meas_unc: Scalar + * covariance: Matrix55 + * meas_matrix: Matrix15 + * + * Outputs: + * innov_cov_updated: Scalar + */ +template +void Computeinnovcov(const Scalar meas_unc, const matrix::Matrix &covariance, + const matrix::Matrix &meas_matrix, + Scalar *const innov_cov_updated = nullptr) +{ + // Total ops: 55 + + // Input arrays + + // Intermediate terms (0) + + // Output terms (1) + if (innov_cov_updated != nullptr) { + Scalar &_innov_cov_updated = (*innov_cov_updated); + + _innov_cov_updated = + meas_matrix(0, 0) * + (covariance(0, 0) * meas_matrix(0, 0) + covariance(1, 0) * meas_matrix(0, 1) + + covariance(2, 0) * meas_matrix(0, 2) + covariance(3, 0) * meas_matrix(0, 3) + + covariance(4, 0) * meas_matrix(0, 4)) + + meas_matrix(0, 1) * + (covariance(0, 1) * meas_matrix(0, 0) + covariance(1, 1) * meas_matrix(0, 1) + + covariance(2, 1) * meas_matrix(0, 2) + covariance(3, 1) * meas_matrix(0, 3) + + covariance(4, 1) * meas_matrix(0, 4)) + + meas_matrix(0, 2) * + (covariance(0, 2) * meas_matrix(0, 0) + covariance(1, 2) * meas_matrix(0, 1) + + covariance(2, 2) * meas_matrix(0, 2) + covariance(3, 2) * meas_matrix(0, 3) + + covariance(4, 2) * meas_matrix(0, 4)) + + meas_matrix(0, 3) * + (covariance(0, 3) * meas_matrix(0, 0) + covariance(1, 3) * meas_matrix(0, 1) + + covariance(2, 3) * meas_matrix(0, 2) + covariance(3, 3) * meas_matrix(0, 3) + + covariance(4, 3) * meas_matrix(0, 4)) + + meas_matrix(0, 4) * + (covariance(0, 4) * meas_matrix(0, 0) + covariance(1, 4) * meas_matrix(0, 1) + + covariance(2, 4) * meas_matrix(0, 2) + covariance(3, 4) * meas_matrix(0, 3) + + covariance(4, 4) * meas_matrix(0, 4)) + + meas_unc; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_moving_xyzb_v/predictCov.h b/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_moving_xyzb_v/predictCov.h new file mode 100644 index 000000000000..f8c95c2e311d --- /dev/null +++ b/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_moving_xyzb_v/predictCov.h @@ -0,0 +1,97 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym +{ + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: predictCov + * + * Args: + * dt: Scalar + * input_var: Scalar + * bias_var: Scalar + * acc_var: Scalar + * covariance: Matrix55 + * + * Outputs: + * cov_updated: Matrix55 + */ +template +void Predictcov(const Scalar dt, const Scalar input_var, const Scalar bias_var, + const Scalar acc_var, const matrix::Matrix &covariance, + matrix::Matrix *const cov_updated = nullptr) +{ + // Total ops: 90 + + // Input arrays + + // Intermediate terms (16) + const Scalar _tmp0 = std::pow(dt, Scalar(2)); + const Scalar _tmp1 = Scalar(0.5) * _tmp0; + const Scalar _tmp2 = -covariance(1, 1) * dt; + const Scalar _tmp3 = _tmp1 * covariance(3, 1) + _tmp2 + covariance(0, 1) + covariance(4, 1) * dt; + const Scalar _tmp4 = covariance(1, 4) * dt; + const Scalar _tmp5 = _tmp1 * covariance(3, 4) - _tmp4 + covariance(0, 4) + covariance(4, 4) * dt; + const Scalar _tmp6 = _tmp1 * covariance(3, 3); + const Scalar _tmp7 = covariance(1, 3) * dt; + const Scalar _tmp8 = _tmp6 - _tmp7 + covariance(0, 3) + covariance(4, 3) * dt; + const Scalar _tmp9 = -Scalar(0.5) * [&]() { + const Scalar base = dt; + return base * base * base; + }() * input_var; + const Scalar _tmp10 = covariance(3, 4) * dt; + const Scalar _tmp11 = covariance(3, 1) * dt; + const Scalar _tmp12 = _tmp10 + covariance(4, 4); + const Scalar _tmp13 = _tmp11 + covariance(4, 1); + const Scalar _tmp14 = covariance(3, 3) * dt; + const Scalar _tmp15 = _tmp14 + covariance(4, 3); + + // Output terms (1) + if (cov_updated != nullptr) { + matrix::Matrix &_cov_updated = (*cov_updated); + + _cov_updated(0, 0) = _tmp1 * _tmp8 + _tmp1 * covariance(3, 0) - _tmp3 * dt + _tmp5 * dt + + covariance(0, 0) - covariance(1, 0) * dt + covariance(4, 0) * dt + + Scalar(0.25) * std::pow(dt, Scalar(4)) * input_var; + _cov_updated(1, 0) = _tmp1 * covariance(1, 3) + _tmp2 + _tmp4 + _tmp9 + covariance(1, 0); + _cov_updated(2, 0) = + _tmp1 * covariance(2, 3) + covariance(2, 0) - covariance(2, 1) * dt + covariance(2, 4) * dt; + _cov_updated(3, 0) = _tmp10 - _tmp11 + _tmp6 + covariance(3, 0); + _cov_updated(4, 0) = + _tmp1 * _tmp15 + _tmp12 * dt - _tmp13 * dt + covariance(3, 0) * dt + covariance(4, 0); + _cov_updated(0, 1) = _tmp3 + _tmp9; + _cov_updated(1, 1) = _tmp0 * input_var + covariance(1, 1); + _cov_updated(2, 1) = covariance(2, 1); + _cov_updated(3, 1) = covariance(3, 1); + _cov_updated(4, 1) = _tmp13; + _cov_updated(0, 2) = + _tmp1 * covariance(3, 2) + covariance(0, 2) - covariance(1, 2) * dt + covariance(4, 2) * dt; + _cov_updated(1, 2) = covariance(1, 2); + _cov_updated(2, 2) = bias_var + covariance(2, 2); + _cov_updated(3, 2) = covariance(3, 2); + _cov_updated(4, 2) = covariance(3, 2) * dt + covariance(4, 2); + _cov_updated(0, 3) = _tmp8; + _cov_updated(1, 3) = covariance(1, 3); + _cov_updated(2, 3) = covariance(2, 3); + _cov_updated(3, 3) = acc_var + covariance(3, 3); + _cov_updated(4, 3) = _tmp15; + _cov_updated(0, 4) = _tmp5 + _tmp8 * dt; + _cov_updated(1, 4) = _tmp7 + covariance(1, 4); + _cov_updated(2, 4) = covariance(2, 3) * dt + covariance(2, 4); + _cov_updated(3, 4) = _tmp14 + covariance(3, 4); + _cov_updated(4, 4) = _tmp12 + _tmp15 * dt; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_static_xyzb/computeInnovCov.h b/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_static_xyzb/computeInnovCov.h new file mode 100644 index 000000000000..0180b0bc21c8 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_static_xyzb/computeInnovCov.h @@ -0,0 +1,56 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym +{ + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: computeInnovCov + * + * Args: + * meas_unc: Scalar + * covariance: Matrix33 + * meas_matrix: Matrix13 + * + * Outputs: + * innov_cov_updated: Scalar + */ +template +void Computeinnovcov(const Scalar meas_unc, const matrix::Matrix &covariance, + const matrix::Matrix &meas_matrix, + Scalar *const innov_cov_updated = nullptr) +{ + // Total ops: 21 + + // Input arrays + + // Intermediate terms (0) + + // Output terms (1) + if (innov_cov_updated != nullptr) { + Scalar &_innov_cov_updated = (*innov_cov_updated); + + _innov_cov_updated = meas_matrix(0, 0) * (covariance(0, 0) * meas_matrix(0, 0) + + covariance(1, 0) * meas_matrix(0, 1) + + covariance(2, 0) * meas_matrix(0, 2)) + + meas_matrix(0, 1) * (covariance(0, 1) * meas_matrix(0, 0) + + covariance(1, 1) * meas_matrix(0, 1) + + covariance(2, 1) * meas_matrix(0, 2)) + + meas_matrix(0, 2) * (covariance(0, 2) * meas_matrix(0, 0) + + covariance(1, 2) * meas_matrix(0, 1) + + covariance(2, 2) * meas_matrix(0, 2)) + + meas_unc; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_static_xyzb/predictCov.h b/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_static_xyzb/predictCov.h new file mode 100644 index 000000000000..1d23dcd6db42 --- /dev/null +++ b/src/modules/vision_target_estimator/Position/python_derivation/generated/decoupled_static_xyzb/predictCov.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym +{ + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: predictCov + * + * Args: + * dt: Scalar + * input_var: Scalar + * bias_var: Scalar + * covariance: Matrix33 + * + * Outputs: + * cov_updated: Matrix33 + */ +template +void Predictcov(const Scalar dt, const Scalar input_var, const Scalar bias_var, + const matrix::Matrix &covariance, + matrix::Matrix *const cov_updated = nullptr) +{ + // Total ops: 24 + + // Input arrays + + // Intermediate terms (3) + const Scalar _tmp0 = covariance(1, 1) * dt; + const Scalar _tmp1 = _tmp0 + covariance(0, 1); + const Scalar _tmp2 = Scalar(0.5) * [&]() { + const Scalar base = dt; + return base * base * base; + }() * input_var; + + // Output terms (1) + if (cov_updated != nullptr) { + matrix::Matrix &_cov_updated = (*cov_updated); + + _cov_updated(0, 0) = _tmp1 * dt + covariance(0, 0) + covariance(1, 0) * dt + + Scalar(0.25) * std::pow(dt, Scalar(4)) * input_var; + _cov_updated(1, 0) = _tmp0 + _tmp2 + covariance(1, 0); + _cov_updated(2, 0) = covariance(2, 0) + covariance(2, 1) * dt; + _cov_updated(0, 1) = _tmp1 + _tmp2; + _cov_updated(1, 1) = covariance(1, 1) + std::pow(dt, Scalar(2)) * input_var; + _cov_updated(2, 1) = covariance(2, 1); + _cov_updated(0, 2) = covariance(0, 2) + covariance(1, 2) * dt; + _cov_updated(1, 2) = covariance(1, 2); + _cov_updated(2, 2) = bias_var + covariance(2, 2); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/vision_target_estimator/VisionTargetEst.cpp b/src/modules/vision_target_estimator/VisionTargetEst.cpp new file mode 100644 index 000000000000..99547be2815e --- /dev/null +++ b/src/modules/vision_target_estimator/VisionTargetEst.cpp @@ -0,0 +1,629 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VisionTargetEst.cpp + * @brief Handles the position and orientation estimators. + * + * @author Jonas Perolini + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "VisionTargetEst.h" + + +namespace vision_target_estimator +{ + +static constexpr uint32_t vte_pos_UPDATE_RATE_HZ = 50; +static constexpr uint32_t vte_yaw_UPDATE_RATE_HZ = 50; +static constexpr uint32_t acc_downsample_TIMEOUT_US = 40_ms; // 40 ms -> 25Hz +static constexpr uint32_t estimator_restart_time_US = 3_s; // Wait at least one second before re-starting the filter +static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2 + +VisionTargetEst::VisionTargetEst() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::vte) +{ + _vte_acc_input_pub.advertise(); +} + +VisionTargetEst::~VisionTargetEst() +{ + delete _vte_position; + delete _vte_orientation; + + perf_free(_cycle_perf_pos); + perf_free(_cycle_perf_yaw); + perf_free(_cycle_perf); +} + +int VisionTargetEst::task_spawn(int argc, char *argv[]) +{ + + VisionTargetEst *instance = new VisionTargetEst(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +bool VisionTargetEst::init() +{ + if (!_vehicle_attitude_sub.registerCallback()) { + PX4_ERR("vehicle_attitude callback registration failed!"); + return false; + } + + updateParams(); + + delete _vte_position; + delete _vte_orientation; + _vte_position = nullptr; + _vte_orientation = nullptr; + + _orientation_estimator_running = false; + _position_estimator_running = false; + + _vte_position_enabled = false; + _vte_orientation_enabled = false; + + // Structure allows for different tasks using the VTE in the future. + _vte_task_mask = _param_vte_task_mask.get(); + + if (_vte_task_mask < 1) { + PX4_ERR("VTE invalid task mask, target estimator not enabled."); + return false; + + } else { + + if (_vte_task_mask & VisionTargetEstTask::VTE_FOR_PREC_LAND) { PX4_INFO("VTE for precision landing.");} + } + + if (_param_vte_pos_en.get()) { + PX4_INFO("VTE position estimator enabled."); + _vte_position = new VTEPosition; + _vte_position_enabled = (_vte_position != nullptr && _vte_position->init()); + } + + if (_param_vte_yaw_en.get()) { + PX4_INFO("VTE yaw estimator enabled."); + _vte_orientation = new VTEOrientation; + _vte_orientation_enabled = (_vte_orientation != nullptr && _vte_orientation->init()); + } + + return _vte_position_enabled || _vte_orientation_enabled; +} + +void VisionTargetEst::updateParams() +{ + ModuleParams::updateParams(); + + _vte_task_mask = _param_vte_task_mask.get(); + + float gps_pos_x; + param_get(param_find("EKF2_GPS_POS_X"), &gps_pos_x); + + float gps_pos_y; + param_get(param_find("EKF2_GPS_POS_Y"), &gps_pos_y); + + float gps_pos_z; + param_get(param_find("EKF2_GPS_POS_Z"), &gps_pos_z); + + _gps_pos_is_offset = ((gps_pos_x > 0.01f) || (gps_pos_y > 0.01f) || (gps_pos_z > 0.01f)); + _gps_pos_offset = matrix::Vector3f(gps_pos_x, gps_pos_y, gps_pos_z); +} + +void VisionTargetEst::reset_acc_downsample() +{ + _vehicle_acc_ned_sum.setAll(0); + _loops_count = 0; + _last_acc_reset = hrt_absolute_time(); +} + +bool VisionTargetEst::start_orientation_estimator() +{ + if ((hrt_absolute_time() - _vte_orientation_stop_time) < estimator_restart_time_US) { + return false; + } + + PX4_INFO("Starting Orientation Vision Target Estimator."); + return true; +} + +bool VisionTargetEst::start_position_estimator() +{ + // Don't start estimator if it was stopped recently + if ((hrt_absolute_time() - _vte_position_stop_time) < estimator_restart_time_US) { + return false; + } + + reset_acc_downsample(); + + PX4_INFO("Starting Position Vision Target Estimator."); + + if (_vte_current_task & VisionTargetEstTask::VTE_FOR_PREC_LAND) { + + bool next_sp_is_land = false; + bool current_sp_is_land = false; + + position_setpoint_triplet_s pos_sp_triplet; + + if (_pos_sp_triplet_sub.update(&pos_sp_triplet)) { + next_sp_is_land = (pos_sp_triplet.next.type == position_setpoint_s::SETPOINT_TYPE_LAND); + current_sp_is_land = (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND); + } + + if (next_sp_is_land) { + PX4_INFO("VTE for precision landing, next sp is land."); + _vte_position->set_mission_position(pos_sp_triplet.next.lat, pos_sp_triplet.next.lon, + pos_sp_triplet.next.alt); + + } else if (current_sp_is_land) { + PX4_INFO("VTE for precision landing, current sp is land."); + _vte_position->set_mission_position(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, + pos_sp_triplet.current.alt); + + } else { + PX4_WARN("VTE for precision landing, land position cannot be used."); + _vte_position->set_mission_position(0.0, 0.0, NAN); + } + } + + return true; +} + +bool VisionTargetEst::new_task_available() +{ + + if (_vte_task_mask & VisionTargetEstTask::VTE_FOR_PREC_LAND && _is_in_prec_land) { + + // Precision land task already running + if (_vte_current_task == VisionTargetEstTask::VTE_FOR_PREC_LAND) { + return false; + } + + PX4_INFO("VTE, precision landing task requested."); + _vte_current_task = VisionTargetEstTask::VTE_FOR_PREC_LAND; + return true; + + } + + // To add a new task: + // else if (_vte_task_mask & VisionTargetEstTask::VTE_FOR_NEW_TASK && _is_in_new_task) {...} + + return false; +} + +bool VisionTargetEst::is_current_task_done() +{ + + // Prec-land + if (_vte_current_task & VisionTargetEstTask::VTE_FOR_PREC_LAND) { + vehicle_land_detected_s vehicle_land_detected; + + // Stop computations once the drone has landed + if (_vehicle_land_detected_sub.update(&vehicle_land_detected) && vehicle_land_detected.landed) { + PX4_INFO("Land detected, precision landing task completed."); + _is_in_prec_land = false; + return true; + } + + // Stop computations once precision landing is over + if (!_is_in_prec_land) { + PX4_INFO("Precision landing task completed."); + return true; + } + } + + // The structure allows to add additional tasks here E.g. precision delivery, follow me, precision takeoff. + + return false; +} + +void VisionTargetEst::update_task_topics() +{ + + // The structure allows to add additional tasks status here E.g. precision delivery, follow me, precision takeoff. + +#if !defined(CONSTRAINED_FLASH) + + if (_vte_task_mask & VisionTargetEstTask::VTE_FOR_PREC_LAND) { + prec_land_status_s prec_land_status; + + if (_prec_land_status_sub.update(&prec_land_status)) { + _is_in_prec_land = prec_land_status.state == prec_land_status_s::PREC_LAND_STATE_ONGOING; + } + } + +#endif + +} + +void VisionTargetEst::Run() +{ + if (should_exit()) { + _vehicle_attitude_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + update_task_topics(); + + // If a new task is available, stop the estimators if they were already running + if (new_task_available()) { + if (_vte_position_enabled && _position_estimator_running) { + stop_position_estimator(); + } + + if (_vte_orientation_enabled && _orientation_estimator_running) { + stop_orientation_estimator(); + } + + } + + // No task running, early return + if (_vte_current_task == VisionTargetEstTask::VTE_NO_TASK) { + return; + } + + // Task is running, check if an estimator must be started or re-started + if ((!_position_estimator_running && _vte_position_enabled)) { + + _position_estimator_running = start_position_estimator(); + } + + if ((!_orientation_estimator_running && _vte_orientation_enabled)) { + + _orientation_estimator_running = start_orientation_estimator(); + } + + // Early return if no estimator is running or activated + if ((!_vte_orientation_enabled || !_orientation_estimator_running) && (!_vte_position_enabled + || !_position_estimator_running)) { + + if (is_current_task_done()) { + _vte_current_task = VisionTargetEstTask::VTE_NO_TASK; + } + + return; + } + + // Stop estimators once task is completed + if (is_current_task_done()) { + + if (_vte_position_enabled && _position_estimator_running) { + stop_position_estimator(); + } + + if (_vte_orientation_enabled && _orientation_estimator_running) { + stop_orientation_estimator(); + } + + _vte_current_task = VisionTargetEstTask::VTE_NO_TASK; + + return; + } + + // Stop computations if the position estimator timedout + if (_position_estimator_running && _vte_position->has_timed_out()) { + stop_position_estimator(); + PX4_INFO("Estimator TIMEOUT, position VTE stopped."); + + // Early return if no other estimator is running + if (!_orientation_estimator_running) { + return; + } + } + + // Stop computations if the position estimator timedout + if (_orientation_estimator_running && _vte_orientation->has_timed_out()) { + stop_orientation_estimator(); + PX4_INFO("Estimator TIMEOUT, orientation VTE stopped."); + + // Early return if no other estimator is running + if (!_position_estimator_running) { + return; + } + } + + // Early return checks passed, start filter computations. + perf_begin(_cycle_perf); + + localPose local_pose; + + const bool local_pose_updated = get_local_pose(local_pose); + + /* Update position filter at vte_pos_UPDATE_RATE_HZ */ + if (_vte_position_enabled) { + + matrix::Vector3f gps_pos_offset_ned; + matrix::Vector3f vel_offset; + const bool vel_offset_updated = get_gps_velocity_offset(vel_offset); + + matrix::Vector3f vehicle_acc_ned; + + /* Downsample acceleration ned */ + if (get_input(vehicle_acc_ned, gps_pos_offset_ned, vel_offset, vel_offset_updated)) { + + /* If the acceleration has been averaged for too long, early return */ + if ((hrt_absolute_time() - _last_acc_reset) > acc_downsample_TIMEOUT_US) { + PX4_INFO("Forced acc downsample reset"); + reset_acc_downsample(); + return; + } + + _vehicle_acc_ned_sum += vehicle_acc_ned; + _loops_count ++; + + if (has_elapsed(_last_update_pos, (1_s / vte_pos_UPDATE_RATE_HZ))) { + + perf_begin(_cycle_perf_pos); + + if (local_pose_updated) { + _vte_position->set_local_velocity(local_pose.vel_xyz, local_pose.vel_valid, local_pose.timestamp); + _vte_position->set_local_position(local_pose.xyz, local_pose.pos_valid, local_pose.timestamp); + _vte_position->set_range_sensor(local_pose.dist_bottom, local_pose.dist_valid, local_pose.timestamp); + } + + _vte_position->set_gps_pos_offset(gps_pos_offset_ned, _gps_pos_is_offset); + + if (vel_offset_updated) { + _vte_position->set_velocity_offset(vel_offset); + } + + const matrix::Vector3f vehicle_acc_ned_sampled = _vehicle_acc_ned_sum / _loops_count; + + _vte_position->update(vehicle_acc_ned_sampled); + + /* Publish downsampled acceleration*/ + vehicle_acceleration_s vte_acc_input_report; + vte_acc_input_report.timestamp = hrt_absolute_time(); + + for (int i = 0; i < 3; i++) { + vte_acc_input_report.xyz[i] = vehicle_acc_ned_sampled(i); + } + + _vte_acc_input_pub.publish(vte_acc_input_report); + + reset_acc_downsample(); + perf_end(_cycle_perf_pos); + } + } + } + + /* Update orientation filter at vte_yaw_UPDATE_RATE_HZ */ + if (_vte_orientation_enabled && has_elapsed(_last_update_yaw, (1_s / vte_yaw_UPDATE_RATE_HZ))) { + perf_begin(_cycle_perf_yaw); + + if (local_pose_updated) { + _vte_orientation->set_range_sensor(local_pose.dist_bottom, local_pose.dist_valid); + } + + _vte_orientation->update(); + perf_end(_cycle_perf_yaw); + } + + perf_end(_cycle_perf); + +} + +void VisionTargetEst::stop_position_estimator() +{ + + PX4_INFO("Stopping Position Vision Target Estimator."); + + if (_vte_position_enabled) { + _vte_position->resetFilter(); + } + + _position_estimator_running = false; + _vte_position_stop_time = hrt_absolute_time(); +} + +void VisionTargetEst::stop_orientation_estimator() +{ + + PX4_INFO("Stopping Orientation Vision Target Estimator."); + + if (_vte_orientation_enabled) { + _vte_orientation->resetFilter(); + } + + _orientation_estimator_running = false; + _vte_orientation_stop_time = hrt_absolute_time(); +} + +bool VisionTargetEst::get_gps_velocity_offset(matrix::Vector3f &vel_offset_body) +{ + if (!_gps_pos_is_offset) { + return false; + } + + vehicle_angular_velocity_s vehicle_angular_velocity; + + if (!_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) { + return false; + + } + + // If the GPS antenna is not at the center of mass, when the drone rotates around the center of mass, the GPS will record a velocity. + const matrix::Vector3f ang_vel = matrix::Vector3f(vehicle_angular_velocity.xyz); + vel_offset_body = ang_vel % _gps_pos_offset; // Get extra velocity from drone's rotation + + return true; +} + +bool VisionTargetEst::get_local_pose(localPose &local_pose) +{ + + vehicle_local_position_s vehicle_local_position; + + if (!_vehicle_local_position_sub.update(&vehicle_local_position)) { + return false; + } + + if ((hrt_absolute_time() - vehicle_local_position.timestamp) > 100_ms) { + PX4_WARN("Local position too old."); + return false; + } + + local_pose.xyz(0) = vehicle_local_position.x; + local_pose.xyz(1) = vehicle_local_position.y; + local_pose.xyz(2) = vehicle_local_position.z; + local_pose.pos_valid = vehicle_local_position.xy_valid && vehicle_local_position.z_valid; + + local_pose.vel_xyz(0) = vehicle_local_position.vx; + local_pose.vel_xyz(1) = vehicle_local_position.vy; + local_pose.vel_xyz(2) = vehicle_local_position.vz; + local_pose.vel_valid = vehicle_local_position.v_xy_valid && vehicle_local_position.v_z_valid; + + local_pose.dist_bottom = vehicle_local_position.dist_bottom; + local_pose.dist_valid = vehicle_local_position.dist_bottom_valid; + + local_pose.yaw_valid = vehicle_local_position.heading_good_for_control; + local_pose.yaw = vehicle_local_position.heading; + + local_pose.timestamp = vehicle_local_position.timestamp; + + return true; +} + +bool VisionTargetEst::get_input(matrix::Vector3f &vehicle_acc_ned, matrix::Vector3f &gps_pos_offset_ned, + matrix::Vector3f &vel_offset_rot_ned, + const bool vel_offset_updated) +{ + + vehicle_attitude_s vehicle_attitude; + vehicle_acceleration_s vehicle_acceleration; + + const bool vehicle_attitude_updated = _vehicle_attitude_sub.update(&vehicle_attitude); + const bool vehicle_acceleration_updated = _vehicle_acceleration_sub.update(&vehicle_acceleration); + + // Minimal requirement: acceleraion (for input) and attitude (to rotate acc in vehicle-carried NED frame) + if (!vehicle_attitude_updated || !vehicle_acceleration_updated) { + return false; + } + + /* Transform FRD body acc to NED */ + const matrix::Quaternionf quat_att(&vehicle_attitude.q[0]); + const matrix::Vector3f vehicle_acc{vehicle_acceleration.xyz}; + + /* Compensate for gravity. */ + const matrix::Vector3f gravity_ned(0, 0, CONSTANTS_ONE_G); + vehicle_acc_ned = quat_att.rotateVector(vehicle_acc) + gravity_ned; + + /* Rotate position and velocity offset into ned frame */ + if (_gps_pos_is_offset) { + gps_pos_offset_ned = quat_att.rotateVector(_gps_pos_offset); + + if (vel_offset_updated) { + vel_offset_rot_ned = quat_att.rotateVector(vel_offset_rot_ned); + } + + } else { + gps_pos_offset_ned.setAll(0.f); + vel_offset_rot_ned.setAll(0.f); + } + + return true; +} + +bool VisionTargetEst::has_elapsed(hrt_abstime &last_time, const hrt_abstime interval) +{ + + if (hrt_elapsed_time(&last_time) > interval) { + last_time = hrt_absolute_time(); + return true; + } + + return false; +} + +int VisionTargetEst::print_status() +{ + PX4_INFO("VTE running"); + return 0; +} +int VisionTargetEst::print_usage(const char *reason) +{ + if (reason != nullptr) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Module to estimate the position and orientation of a target using a vision sensor and GNSS. + +The module runs periodically on the HP work queue. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("vision_target_estimator", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int vision_target_estimator_main(int argc, char *argv[]) +{ + return VisionTargetEst::main(argc, argv); +} + +} // namespace vision_target_est diff --git a/src/modules/vision_target_estimator/VisionTargetEst.h b/src/modules/vision_target_estimator/VisionTargetEst.h new file mode 100644 index 000000000000..e5e0a1ac2208 --- /dev/null +++ b/src/modules/vision_target_estimator/VisionTargetEst.h @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VisionTargetEst.h + * @brief Handles the position and orientation estimators. + * + * @author Jonas Perolini + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if !defined(CONSTRAINED_FLASH) +#include +#endif + +#include +#include +#include + +#include "Position/VTEPosition.h" +#include "Orientation/VTEOrientation.h" + + +namespace vision_target_estimator +{ + +class VisionTargetEst : public ModuleBase, ModuleParams, px4::ScheduledWorkItem +{ +public: + VisionTargetEst(); + virtual ~VisionTargetEst(); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + bool init(); + + static int task_spawn(int argc, char *argv[]); + +private: + void Run() override; + void updateParams() override; + + void update_task_topics(); + bool new_task_available(); + bool is_current_task_done(); + bool start_position_estimator(); + void stop_position_estimator(); + bool start_orientation_estimator(); + void stop_orientation_estimator(); + bool get_input(matrix::Vector3f &acc_ned, matrix::Vector3f &gps_pos_offset, matrix::Vector3f &gps_vel_offset, + bool gps_vel_offset_updated = false); + + perf_counter_t _cycle_perf_pos{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE cycle pos")}; + perf_counter_t _cycle_perf_yaw{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE cycle yaw")}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE cycle ")}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; +#if !defined(CONSTRAINED_FLASH) + uORB::Subscription _prec_land_status_sub {ORB_ID(prec_land_status)}; +#endif + + uORB::Publication _vte_acc_input_pub{ORB_ID(vte_acc_input)}; + + enum VisionTargetEstTask : uint16_t { + // Bit locations for VTE tasks + VTE_NO_TASK = 0, + VTE_FOR_PREC_LAND = (1 << 0), ///< set to true if target GPS position data is ready to be fused + }; + + int _vte_current_task{VisionTargetEstTask::VTE_NO_TASK}; + int _vte_task_mask{VisionTargetEstTask::VTE_NO_TASK}; + + bool _position_estimator_running{false}; + bool _orientation_estimator_running{false}; + bool _is_in_prec_land{false}; // Start target estimator during precision landing + uint64_t _vte_position_stop_time{0}; + uint64_t _vte_orientation_stop_time{0}; + + bool _vte_orientation_enabled{false}; + VTEOrientation *_vte_orientation {nullptr}; + hrt_abstime _last_update_yaw{0}; + + VTEPosition *_vte_position {nullptr}; + bool _vte_position_enabled{false}; + hrt_abstime _last_update_pos{0}; + + struct localPose { + bool pos_valid = false; + matrix::Vector3f xyz; + + bool vel_valid = false; + matrix::Vector3f vel_xyz; + + bool dist_valid = false; + float dist_bottom = 0, f; + + bool yaw_valid = false; + float yaw = 0.f; + + hrt_abstime timestamp{0}; + }; + + matrix::Vector3f _gps_pos_offset; + bool _gps_pos_is_offset; + + bool get_gps_velocity_offset(matrix::Vector3f &vel_offset); + bool get_local_pose(localPose &local_pose); + + bool has_elapsed(hrt_abstime &last_time, const hrt_abstime interval); + + /* Down sample acceleration data */ + matrix::Vector3f _vehicle_acc_ned_sum; + int _loops_count{0}; + hrt_abstime _last_acc_reset{0}; + void reset_acc_downsample(); + + DEFINE_PARAMETERS( + (ParamInt) _param_vte_yaw_en, + (ParamInt) _param_vte_pos_en, + (ParamInt) _param_vte_task_mask + ) +}; + +} // namespace land_detector diff --git a/src/modules/vision_target_estimator/documentation/MT_sensorFusion.pdf b/src/modules/vision_target_estimator/documentation/MT_sensorFusion.pdf new file mode 100644 index 000000000000..c1766a73bbe0 Binary files /dev/null and b/src/modules/vision_target_estimator/documentation/MT_sensorFusion.pdf differ diff --git a/src/modules/vision_target_estimator/vision_target_estimator_params.c b/src/modules/vision_target_estimator/vision_target_estimator_params.c new file mode 100644 index 000000000000..fd9ba85c2166 --- /dev/null +++ b/src/modules/vision_target_estimator/vision_target_estimator_params.c @@ -0,0 +1,398 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vision_target_estimator_params.c + * Vision target Estimator algorithm parameters. + * + * @author Jonas Perolini + * + */ + +/** + * Vision target Estimator module enable + * + * @boolean + * + * @group Vision target Estimator + */ +PARAM_DEFINE_INT32(VTE_EN, 1); + +/** + * Vision target Estimator module enable orientation estimation + * + * @boolean + * + * @group Vision target Estimator + */ +PARAM_DEFINE_INT32(VTE_YAW_EN, 0); + +/** + * Vision target Estimator module enable position estimation + * + * @boolean + * + * @group Vision target Estimator + */ +PARAM_DEFINE_INT32(VTE_POS_EN, 1); + +/** + * Use the target relative velocity to aid the EKF state estimation. + * + * @boolean + * + * @group Vision target Estimator + */ +PARAM_DEFINE_INT32(VTE_EKF_AID, 1); + +/** + * Integer bitmask controlling data fusion and aiding methods. + * + * Set bits in the following positions to enable: + * 0 : Set to true to use the target's GNSS position data if available. (+1) + * 1 : Set to true to use the relative GNSS velocity data if available. (If the target is moving, a target velocity estimate is required) (+2) + * 2 : Set to true to use the target relative position from vision-based data if available (+4) + * 3 : Set to true to use the mission point. Ignored if target GNSS position enabled. (+8) + * + * @group Vision target Estimator + * @min 0 + * @max 63 + * @bit 0 target GNSS position + * @bit 1 relative GNSS velocity + * @bit 2 vision relative position + * @bit 3 mission position + * + * + * @group Vision target Estimator + */ +PARAM_DEFINE_INT32(VTE_AID_MASK, 14); + +/** + * Integer bitmask controlling the tasks of the target estimator. + * + * Set bits in the following positions to enable: + * 0 : Set to true to use the vision target estimator for precision landing. (+1) + * + * @group Vision target Estimator + * @min 0 + * @max 1 + * @bit 0 VTE for precision landing + * + * + * @group Vision target Estimator + */ +PARAM_DEFINE_INT32(VTE_TASK_MASK, 1); + +/** + * Target mode + * + * Configure the mode of the target. Depending on the mode, the state of the estimator (Kalman filter) varies. For static targets, the target observations can be used to aid position estimation. + * + * Mode Static: The target is static, the state of the Kalman filter is: [relative position, relative velocity, bias]. If the observations have a low variance,they can be used to aid position estimation. + * Mode Moving: The target may be moving around, the state of the Kalman filter is: [relative position, drone velocity, bias, target's acceleration, target's velocity]. + * + * @min 0 + * @max 2 + * @group Vision target Estimator + * @value 0 Static + * @value 1 Moving + * + * @group Vision target Estimator + */ +PARAM_DEFINE_INT32(VTE_MODE, 0); + + +/** + * Vision Target Timeout + * + * Time after which the target is considered lost without any new position measurements. Velocity measurements are not considered. + * + * @unit s + * @min 0.0 + * @max 50 + * @decimal 1 + * @increment 0.5 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_BTOUT, 3.0f); + +/** + * Drone acceleration uncertainty + * + * Variance of drone's acceleration used for target position prediction. + * Higher values results in tighter following of the measurements and more lenient outlier rejection + * + * @unit (m/s^2)^2 + * @min 0.01 + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_ACC_D_UNC, 1.0f); + +/** + * Target acceleration uncertainty + * + * Variance of target acceleration (in NED frame) used for target position prediction. + * Higher values results in tighter following of the measurements and more lenient outlier rejection + * + * @unit (m/s^2)^2 + * @min 0.01 + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_ACC_T_UNC, 1.0f); + +/** + * Bias uncertainty + * + * Variance of GPS bias used for target position prediction. + * Higher values results in tighter following of the measurements and more lenient outlier rejection + * + * @unit m^2 + * @min 0.01 + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_BIAS_UNC, 0.05f); + +/** + * Bias limit + * + * Not handled yet. Maximal bias between drone's GNSS and the target's GNSS. + * + * @unit m^2 + * @min 0.01 + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_BIAS_LIM, 1.f); + + +/** + * Initial target and drone relative position uncertainty + * + * Initial variance of the relative target position in x,y,z direction + * + * @unit m^2 + * @min 0.001 + * @decimal 3 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_POS_UNC_IN, 0.5f); + +/** + * Initial target and drone relative velocity uncertainty + * + * Initial variance of the relative target velocity in x,y,z directions + * + * @unit (m/s)^2 + * @min 0.001 + * @decimal 3 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_VEL_UNC_IN, 0.5f); + +/** + * Initial GPS bias uncertainty + * + * Initial variance of the bias between the GPS on the target and the GPS on the drone + * + * @unit m^2 + * @min 0.001 + * @decimal 3 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_BIA_UNC_IN, 1.0f); + +/** + * Initial Orientation uncertainty + * + * Initial variance of the orientation (yaw) of the target + * + * @unit m^2 + * @min 0.001 + * @decimal 3 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_YAW_UNC_IN, 1.0f); + +/** + * Initial target absolute acceleration uncertainty + * + * Initial variance of the relative target acceleration in x,y,z directions + * + * @unit (m/s^2)^2 + * @min 0.001 + * @decimal 3 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_ACC_UNC_IN, 0.1f); + +/** + * Measurement noise for gps horizontal velocity. + * + * minimum allowed observation noise for gps velocity fusion (m/sec) + * + * @min 0.01 + * @max 5.0 + * @unit m/s + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_GPS_V_NOISE, 0.3f); + +/** + * Measurement noise for gps position. + * + * minimum allowed observation noise for gps position fusion (m) + * + * @min 0.01 + * @max 10.0 + * @unit m + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_GPS_P_NOISE, 0.5f); + +/** + * Whether to set the external vision observation noise from the parameter or from vision message + * + * If set to true the observation noise is set from the parameters directly, if set to false the measurement noise is taken from the vision message and the parameter are used as a lower bound. + * + * @boolean + * + * @group Vision target Estimator + */ +PARAM_DEFINE_INT32(VTE_EV_NOISE_MD, 0); + +/** + * Measurement noise for vision angle observations used to lower bound or replace the uncertainty included in the message + * + * @min 0.05 + * @unit rad + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_EVA_NOISE, 0.05f); + +/** + * Measurement noise for vision position observations used to lower bound or replace the uncertainty included in the message. + * + * If used to replace the uncertainty in the message, the measurement noise is lineraly scaled with the altitude i.e. unc = VTE_EVP_NOISE^2 * max(dist_bottom, 1) + * + * @min 0.01 + * @unit m + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_EVP_NOISE, 0.1f); + + +/** + * Maximal time to estimate the future position of the target using the velocity estimation. + * + * When the target is moving, the position setpoint is set where the target will be after a given time in the future. (new position = velocity * time). + * This param should be set depending on the expected target speed. The greater the speed, the greater VTE_MOVING_T_MAX. + * + * @min 0.1 + * @max 60 + * @unit s + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_MOVING_T_MAX, 3.f); + + +/** + * Minimal time to estimate the future position of the target using the velocity estimation. + * + * When the target is moving, the position setpoint is set where the target will be after a given time in the future. (new position = velocity * time). + * As the drone gets close to the target, the time of intersection between the target and the drone gets smaller. To avoid missing the target, a minimal time is required. + * This param should be set depending on the expected target speed. The greater the speed, the greater VTE_MOVING_T_MIN. + * + * @min 0.1 + * @max 30 + * @unit s + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_MOVING_T_MIN, 2.f); + + +/** + * Normalized Innovation Squared (NIS) threshold for position estimator. + * + * Lower values mean that more measurements will be rejected. Null Hypothesis H0: innovation is consistent with the innovation covariance matrix. + * Values of 0.46, 1.64, 2.71, 3.84, 6.63, 10.83 correspond to a 50%, 20%, 10%, 5%, 1%, 0.1% of probability that H0 is incorrectly rejected respectively. + * The confidence interval is computed from the chi-squared distribution. + * + * @min 0.46 + * @max 10.83 + * @unit + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_POS_NIS_THRE, 3.84f); + +/** + * Normalized Innovation Squared (NIS) threshold for orientation estimator. + * + * Lower values mean that more measurements will be rejected. Null Hypothesis H0: innovation is consistent with the innovation covariance matrix. + * Values of 0.46, 1.64, 2.71, 3.84, 6.63, 10.83 correspond to a 50%, 20%, 10%, 5%, 1%, 0.1% of probability that H0 is incorrectly rejected respectively. + * The confidence interval is computed from the chi-squared distribution. + * + * @min 0.46 + * @max 10.83 + * @unit + * @decimal 2 + * + * @group Vision target Estimator + */ +PARAM_DEFINE_FLOAT(VTE_YAW_NIS_THRE, 3.84f);