From d5839e2dd52ca81e7d15051212f17b7eacce39be Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 16:02:09 -0400 Subject: [PATCH] optical flow sensor pipeline overhaul - all sources of optical flow publish sensor_optical_flow - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow Co-authored-by: alexklimaj --- .../init.d-posix/airframes/1010_iris_opt_flow | 4 + .../airframes/1017_iris_opt_flow_mockup | 1 - .../init.d/airframes/4016_holybro_px4vision | 1 - .../init.d/airframes/4061_atl_mantis_edu | 2 +- msg/CMakeLists.txt | 27 +- msg/estimator_optical_flow_vel.msg | 8 - msg/optical_flow.msg | 29 -- msg/sensor_optical_flow.msg | 30 ++ msg/tools/urtps_bridge_topics.yaml | 2 +- msg/vehicle_optical_flow.msg | 21 + msg/vehicle_optical_flow_vel.msg | 13 + src/drivers/drv_sensor.h | 13 +- src/drivers/optical_flow/paa3905/PAA3905.cpp | 27 +- src/drivers/optical_flow/paa3905/PAA3905.hpp | 4 +- src/drivers/optical_flow/paw3902/PAW3902.cpp | 27 +- src/drivers/optical_flow/paw3902/PAW3902.hpp | 4 +- src/drivers/optical_flow/pmw3901/PMW3901.cpp | 37 +- src/drivers/optical_flow/pmw3901/PMW3901.hpp | 7 +- src/drivers/optical_flow/px4flow/px4flow.cpp | 159 ++---- .../optical_flow/thoneflow/thoneflow.cpp | 127 ++--- .../thoneflow/thoneflow_parser.cpp | 6 +- .../optical_flow/thoneflow/thoneflow_parser.h | 4 +- src/drivers/uavcan/sensors/flow.cpp | 44 +- src/drivers/uavcan/sensors/flow.hpp | 2 +- .../uavcannode/Publishers/FlowMeasurement.hpp | 27 +- src/drivers/uavcannode/uavcannode_params.c | 21 - src/modules/ekf2/EKF/ekf.h | 5 +- src/modules/ekf2/EKF2.cpp | 44 +- src/modules/ekf2/EKF2.hpp | 12 +- .../BlockLocalPositionEstimator.hpp | 4 +- .../local_position_estimator/sensors/flow.cpp | 12 +- src/modules/logger/logged_topics.cpp | 5 +- src/modules/mavlink/mavlink_receiver.cpp | 183 +++---- src/modules/mavlink/mavlink_receiver.h | 19 +- .../mavlink/streams/OPTICAL_FLOW_RAD.hpp | 40 +- src/modules/replay/ReplayEkf2.cpp | 6 +- src/modules/sensors/CMakeLists.txt | 11 + .../sensors/{vehicle_imu => }/Integrator.hpp | 11 +- src/modules/sensors/Kconfig | 4 + src/modules/sensors/sensor_params_flow.c | 35 +- src/modules/sensors/sensors.cpp | 59 +++ .../sensors/vehicle_imu/CMakeLists.txt | 1 - .../sensors/vehicle_imu/VehicleIMU.hpp | 6 +- .../vehicle_optical_flow}/CMakeLists.txt | 11 +- .../vehicle_optical_flow/RingBuffer.hpp | 217 ++++++++ .../VehicleOpticalFlow.cpp | 481 ++++++++++++++++++ .../VehicleOpticalFlow.hpp | 147 ++++++ src/modules/simulator/simulator.h | 5 +- src/modules/simulator/simulator_mavlink.cpp | 84 ++- 49 files changed, 1415 insertions(+), 634 deletions(-) delete mode 100644 msg/estimator_optical_flow_vel.msg delete mode 100644 msg/optical_flow.msg create mode 100644 msg/sensor_optical_flow.msg create mode 100644 msg/vehicle_optical_flow.msg create mode 100644 msg/vehicle_optical_flow_vel.msg rename src/modules/sensors/{vehicle_imu => }/Integrator.hpp (96%) rename src/{drivers/optical_flow => modules/sensors/vehicle_optical_flow}/CMakeLists.txt (88%) create mode 100644 src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp create mode 100644 src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp create mode 100644 src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow index f3fe604155..6003d04d16 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow @@ -18,3 +18,7 @@ param set-default LPE_FAKE_ORIGIN 1 param set-default MPC_ALT_MODE 2 +param set-default SENS_FLOW_ROT 6 +param set-default SENS_FLOW_MINHGT 0.7 +param set-default SENS_FLOW_MAXHGT 3.0 +param set-default SENS_FLOW_MAXR 2.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup b/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup index 01246495cb..d2c0dcf4cb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup @@ -9,7 +9,6 @@ # EKF2 param set-default EKF2_AID_MASK 2 -param set-default SENS_FLOW_ROT 0 # LPE: Flow-only mode param set-default LPE_FUSION 242 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index cf15f0cbca..feb06fc88a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -104,7 +104,6 @@ param set-default SDLOG_PROFILE 131 param set-default SENS_CM8JL65_CFG 104 param set-default SENS_FLOW_MAXHGT 25 param set-default SENS_FLOW_MINHGT 0.5 -param set-default SENS_FLOW_ROT 0 param set-default IMU_GYRO_CUTOFF 100 param set-default SENS_EN_PMW3901 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index 56812f15d0..2dacd54acb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -174,7 +174,7 @@ param set-default RC1_TRIM 1000 param set-default SENS_FLOW_MAXR 7.4 param set-default SENS_FLOW_MINHGT 0.15 param set-default SENS_FLOW_MAXHGT 5.0 -param set-default SENS_FLOW_ROT 0 +param set-default SENS_FLOW_ROT 0 # ignore the SD card errors and use normal startup sound set STARTUP_TUNE "1" diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2ae9a92335..fcd004f85f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -65,26 +65,25 @@ set(msg_files differential_pressure.msg distance_sensor.msg ekf2_timestamps.msg - estimator_gps_status.msg esc_report.msg esc_status.msg + estimator_aid_source_1d.msg + estimator_aid_source_2d.msg + estimator_aid_source_3d.msg estimator_baro_bias.msg estimator_event_flags.msg + estimator_gps_status.msg estimator_innovations.msg - estimator_optical_flow_vel.msg estimator_selector_status.msg estimator_sensor_bias.msg estimator_states.msg estimator_status.msg estimator_status_flags.msg - estimator_aid_source_1d.msg - estimator_aid_source_2d.msg - estimator_aid_source_3d.msg event.msg - follow_target_status.msg + failure_detector_status.msg follow_target.msg follow_target_estimator.msg - failure_detector_status.msg + follow_target_status.msg generator_status.msg geofence_result.msg gimbal_device_attitude_status.msg @@ -99,8 +98,8 @@ set(msg_files heater_status.msg home_position.msg hover_thrust_estimate.msg - internal_combustion_engine_status.msg input_rc.msg + internal_combustion_engine_status.msg iridiumsbd_status.msg irlock_report.msg landing_gear.msg @@ -123,17 +122,16 @@ set(msg_files obstacle_distance.msg offboard_control_mode.msg onboard_computer_status.msg - optical_flow.msg orbit_status.msg parameter_update.msg ping.msg - pps_capture.msg position_controller_landing_status.msg position_controller_status.msg position_setpoint.msg position_setpoint_triplet.msg power_button_state.msg power_monitor.msg + pps_capture.msg pwm_input.msg px4io_status.msg radio_status.msg @@ -148,17 +146,18 @@ set(msg_files sensor_baro.msg sensor_combined.msg sensor_correction.msg - sensor_gps.msg sensor_gnss_relative.msg + sensor_gps.msg sensor_gyro.msg sensor_gyro_fft.msg sensor_gyro_fifo.msg sensor_hygrometer.msg sensor_mag.msg + sensor_optical_flow.msg sensor_preflight_mag.msg sensor_selection.msg - sensors_status_imu.msg sensors_status.msg + sensors_status_imu.msg system_power.msg takeoff_status.msg task_stack_info.msg @@ -176,8 +175,8 @@ set(msg_files uavcan_parameter_value.msg ulog_stream.msg ulog_stream_ack.msg - uwb_grid.msg uwb_distance.msg + uwb_grid.msg vehicle_acceleration.msg vehicle_air_data.msg vehicle_angular_acceleration.msg @@ -198,6 +197,8 @@ set(msg_files vehicle_local_position_setpoint.msg vehicle_magnetometer.msg vehicle_odometry.msg + vehicle_optical_flow.msg + vehicle_optical_flow_vel.msg vehicle_rates_setpoint.msg vehicle_roi.msg vehicle_status.msg diff --git a/msg/estimator_optical_flow_vel.msg b/msg/estimator_optical_flow_vel.msg deleted file mode 100644 index 38ee0d7f68..0000000000 --- a/msg/estimator_optical_flow_vel.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) - -float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) -float32[2] vel_ne # same as vel_body but in local frame (m/s) -float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad) -float32[2] flow_compensated_integral # integrated optical flow measurement compensated for angular motion (rad) -float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg deleted file mode 100644 index 4b3796ff9f..0000000000 --- a/msg/optical_flow.msg +++ /dev/null @@ -1,29 +0,0 @@ -# Optical flow in XYZ body frame in SI units. -# http://en.wikipedia.org/wiki/International_System_of_Units - -uint64 timestamp # time since system start (microseconds) - -uint8 sensor_id # id of the sensor emitting the flow value -float32 pixel_flow_x_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis -float32 pixel_flow_y_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis -float32 gyro_x_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 gyro_y_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 gyro_z_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 ground_distance_m # Altitude / distance to ground in meters -uint32 integration_timespan # accumulation timespan in microseconds -uint32 time_since_last_sonar_update # time since last sonar update in microseconds -uint16 frame_count_since_last_readout # number of accumulated frames in timespan -int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius -uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality - -float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably -float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably -float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably - - -uint8 MODE_UNKNOWN = 0 -uint8 MODE_BRIGHT = 1 -uint8 MODE_LOWLIGHT = 2 -uint8 MODE_SUPER_LOWLIGHT = 3 - -uint8 mode diff --git a/msg/sensor_optical_flow.msg b/msg/sensor_optical_flow.msg new file mode 100644 index 0000000000..ce7e8bf08c --- /dev/null +++ b/msg/sensor_optical_flow.msg @@ -0,0 +1,30 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data. +bool delta_angle_available + +float32 distance_m # (meters) Distance to the center of the flow field +bool distance_available + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # quality, 0: bad quality, 255: maximum quality + +uint32 error_count + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably + +uint8 MODE_UNKNOWN = 0 +uint8 MODE_BRIGHT = 1 +uint8 MODE_LOWLIGHT = 2 +uint8 MODE_SUPER_LOWLIGHT = 3 + +uint8 mode diff --git a/msg/tools/urtps_bridge_topics.yaml b/msg/tools/urtps_bridge_topics.yaml index 6f6a538a7c..529811ce5b 100644 --- a/msg/tools/urtps_bridge_topics.yaml +++ b/msg/tools/urtps_bridge_topics.yaml @@ -40,7 +40,7 @@ rtps: receive: true - msg: offboard_control_mode receive: true - - msg: optical_flow + - msg: sensor_optical_flow receive: true - msg: position_setpoint receive: true diff --git a/msg/vehicle_optical_flow.msg b/msg/vehicle_optical_flow.msg new file mode 100644 index 0000000000..13bdb57bbf --- /dev/null +++ b/msg/vehicle_optical_flow.msg @@ -0,0 +1,21 @@ +# Optical flow in XYZ body frame in SI units. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable) + +float32 distance_m # (meters) Distance to the center of the flow field (NAN if unavailable) + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably diff --git a/msg/vehicle_optical_flow_vel.msg b/msg/vehicle_optical_flow_vel.msg new file mode 100644 index 0000000000..c89207f4ab --- /dev/null +++ b/msg/vehicle_optical_flow_vel.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) +float32[2] vel_ne # same as vel_body but in local frame (m/s) + +float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad) +float32[2] flow_compensated_integral # integrated optical flow measurement compensated for angular motion (rad) + +float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s) +float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) + +# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 4c9b1ab90d..4547f28cec 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -68,6 +68,7 @@ #define DRV_IMU_DEVTYPE_SIM 0x14 #define DRV_DIFF_PRESS_DEVTYPE_SIM 0x15 +#define DRV_FLOW_DEVTYPE_SIM 0x16 #define DRV_IMU_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 @@ -136,10 +137,6 @@ #define DRV_PWM_DEVTYPE_PCA9685 0x69 #define DRV_ACC_DEVTYPE_BMI088 0x6a #define DRV_OSD_DEVTYPE_ATXXXX 0x6b -#define DRV_FLOW_DEVTYPE_PMW3901 0x6c -#define DRV_FLOW_DEVTYPE_PAW3902 0x6d -#define DRV_FLOW_DEVTYPE_PX4FLOW 0x6e -#define DRV_FLOW_DEVTYPE_PAA3905 0x6f #define DRV_DIST_DEVTYPE_LL40LS 0x70 @@ -206,9 +203,15 @@ #define DRV_GPS_DEVTYPE_SIM 0xAF +#define DRV_TRNS_DEVTYPE_MXS 0xB0 + #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1 -#define DRV_TRNS_DEVTYPE_MXS 0xB2 +#define DRV_FLOW_DEVTYPE_MAVLINK 0xB2 +#define DRV_FLOW_DEVTYPE_PMW3901 0xB3 +#define DRV_FLOW_DEVTYPE_PAW3902 0xB4 +#define DRV_FLOW_DEVTYPE_PX4FLOW 0xB5 +#define DRV_FLOW_DEVTYPE_PAA3905 0xB6 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index a9a306d803..b6457946a0 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -535,27 +535,26 @@ void PAA3905::RunImpl() return; } - optical_flow_s report{}; - report.timestamp = timestamp_sample; - //report.device_id = get_device_id(); + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f}; - report.pixel_flow_x_integral = pixel_flow_rotated(0); - report.pixel_flow_y_integral = pixel_flow_rotated(1); + report.pixel_flow[0] = pixel_flow_rotated(0); + report.pixel_flow[1] = pixel_flow_rotated(1); - report.frame_count_since_last_readout = _flow_sample_counter; // number of frames - report.integration_timespan = _flow_dt_sum_usec; // microseconds + report.integration_timespan_us = _flow_dt_sum_usec; // microseconds report.quality = _flow_quality_sum / _flow_sample_counter; // No gyro on this board - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; + report.delta_angle[0] = NAN; + report.delta_angle[1] = NAN; + report.delta_angle[2] = NAN; // set (conservative) specs according to datasheet report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s @@ -565,20 +564,20 @@ void PAA3905::RunImpl() switch (_mode) { case Mode::Bright: - report.mode = optical_flow_s::MODE_BRIGHT; + report.mode = sensor_optical_flow_s::MODE_BRIGHT; break; case Mode::LowLight: - report.mode = optical_flow_s::MODE_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; break; case Mode::SuperLowLight: - report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; break; } report.timestamp = hrt_absolute_time(); - _optical_flow_pub.publish(report); + _sensor_optical_flow_pub.publish(report); if (report.quality > 10) { _last_good_publish = report.timestamp; diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index dd15f548aa..8235e923d3 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include using namespace time_literals; using namespace PixArt_PAA3905; @@ -99,7 +99,7 @@ private: void ResetAccumulatedData(); - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index ea3b66d7ae..3df138a14c 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -788,27 +788,26 @@ void PAW3902::RunImpl() return; } - optical_flow_s report{}; - report.timestamp = timestamp_sample; - //report.device_id = get_device_id(); + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f}; - report.pixel_flow_x_integral = pixel_flow_rotated(0); - report.pixel_flow_y_integral = pixel_flow_rotated(1); + report.pixel_flow[0] = pixel_flow_rotated(0); + report.pixel_flow[1] = pixel_flow_rotated(1); - report.frame_count_since_last_readout = _flow_sample_counter; // number of frames - report.integration_timespan = _flow_dt_sum_usec; // microseconds + report.integration_timespan_us = _flow_dt_sum_usec; // microseconds report.quality = _flow_quality_sum / _flow_sample_counter; // No gyro on this board - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; + report.delta_angle[0] = NAN; + report.delta_angle[1] = NAN; + report.delta_angle[2] = NAN; // set (conservative) specs according to datasheet report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s @@ -818,20 +817,20 @@ void PAW3902::RunImpl() switch (_mode) { case Mode::Bright: - report.mode = optical_flow_s::MODE_BRIGHT; + report.mode = sensor_optical_flow_s::MODE_BRIGHT; break; case Mode::LowLight: - report.mode = optical_flow_s::MODE_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; break; case Mode::SuperLowLight: - report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; break; } report.timestamp = hrt_absolute_time(); - _optical_flow_pub.publish(report); + _sensor_optical_flow_pub.publish(report); if (report.quality > 10) { _last_good_publish = report.timestamp; diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index ab8d574f3d..d6ea4ff436 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include using namespace time_literals; using namespace PixArt_PAW3902JF; @@ -97,7 +97,7 @@ private: void ResetAccumulatedData(); - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp index 78809cf554..7c5506c81d 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.cpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -207,16 +207,6 @@ PMW3901::sensorInit() int PMW3901::init() { - // get yaw rotation from sensor frame to body frame - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 0; - param_get(rot, &val); - - _yaw_rotation = (enum Rotation)val; - } - /* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */ SPI::set_lockmode(LOCK_THREADS); @@ -326,28 +316,24 @@ PMW3901::RunImpl() delta_x = (float)_flow_sum_x / 385.0f; // proportional factor + convert from pixels to radians delta_y = (float)_flow_sum_y / 385.0f; // proportional factor + convert from pixels to radians - optical_flow_s report{}; - report.timestamp = timestamp; + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp; - report.pixel_flow_x_integral = static_cast(delta_x); - report.pixel_flow_y_integral = static_cast(delta_y); + report.pixel_flow[0] = static_cast(delta_x); + report.pixel_flow[1] = static_cast(delta_y); - // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT + // rotate measurements in yaw from sensor frame to body frame float zeroval = 0.0f; - rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); - rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); + rotate_3f(_yaw_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval); - report.frame_count_since_last_readout = _flow_sample_counter; // number of frames - report.integration_timespan = _flow_dt_sum_usec; // microseconds + report.integration_timespan_us = _flow_dt_sum_usec; // microseconds - report.sensor_id = 0; report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0; - /* No gyro on this board */ - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; + report.delta_angle[0] = NAN; + report.delta_angle[1] = NAN; + report.delta_angle[2] = NAN; // set (conservative) specs according to datasheet report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s @@ -360,7 +346,8 @@ PMW3901::RunImpl() _flow_sample_counter = 0; _flow_quality_sum = 0; - _optical_flow_pub.publish(report); + report.timestamp = hrt_absolute_time(); + _sensor_optical_flow_pub.publish(report); perf_end(_sample_perf); } diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp index 13d850a66b..2c845fbdcb 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.hpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp @@ -45,10 +45,9 @@ #include #include #include -#include #include #include -#include +#include #include /* Configuration Constants */ @@ -84,14 +83,14 @@ private: const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz) - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; uint64_t _previous_collect_timestamp{0}; - enum Rotation _yaw_rotation; + enum Rotation _yaw_rotation {}; int _flow_sum_x{0}; int _flow_sum_y{0}; diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index c1c59ed7a9..5002a5106d 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,8 +41,6 @@ #include #include -#include -#include #include #include #include @@ -50,8 +48,7 @@ #include #include #include -#include -#include +#include /* Configuration Constants */ #define I2C_FLOW_ADDRESS_DEFAULT 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 @@ -89,28 +86,19 @@ public: * and start a new one. */ void RunImpl(); -protected: - int probe() override; private: + int probe() override; - uint8_t _sonar_rotation; bool _sensor_ok{false}; bool _collect_phase{false}; - uORB::PublicationMulti _px4flow_topic{ORB_ID(optical_flow)}; - uORB::PublicationMulti _distance_sensor_topic{ORB_ID(distance_sensor)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; - enum Rotation _sensor_rotation; - float _sensor_min_range{0.0f}; - float _sensor_max_range{0.0f}; - float _sensor_max_flow_rate{0.0f}; - i2c_frame _frame; - i2c_integral_frame _frame_integral; /** * Test whether the device supported by the driver is present at a @@ -134,15 +122,11 @@ private: }; -extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); - PX4FLOW::PX4FLOW(const I2CSPIDriverConfig &config) : I2C(config), I2CSPIDriver(config), - _sonar_rotation(config.rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), - _sensor_rotation(Rotation::ROTATION_NONE) + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { } @@ -166,44 +150,6 @@ PX4FLOW::init() /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; - /* get yaw rotation from sensor frame to body frame */ - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward - param_get(rot, &val); - - _sensor_rotation = (enum Rotation)val; - } - - /* get operational limits of the sensor */ - param_t hmin = param_find("SENS_FLOW_MINHGT"); - - if (hmin != PARAM_INVALID) { - float val = 0.7; - param_get(hmin, &val); - - _sensor_min_range = val; - } - - param_t hmax = param_find("SENS_FLOW_MAXHGT"); - - if (hmax != PARAM_INVALID) { - float val = 3.0; - param_get(hmax, &val); - - _sensor_max_range = val; - } - - param_t ratemax = param_find("SENS_FLOW_MAXR"); - - if (ratemax != PARAM_INVALID) { - float val = 2.5; - param_get(ratemax, &val); - - _sensor_max_flow_rate = val; - } - start(); return ret; @@ -269,6 +215,8 @@ PX4FLOW::collect() return ret; } + i2c_integral_frame _frame_integral{}; + if (PX4FLOW_REG == 0) { memcpy(&_frame, val, I2C_FRAME_SIZE); memcpy(&_frame_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); @@ -279,52 +227,37 @@ PX4FLOW::collect() } - optical_flow_s report{}; + DeviceId device_id; + device_id.devid = get_device_id(); + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW; + device_id.devid_s.address = get_i2c_address(); + + sensor_optical_flow_s report{}; + + report.timestamp_sample = hrt_absolute_time(); + report.device_id = device_id.devid; + + report.pixel_flow[0] = static_cast(_frame_integral.pixel_flow_x_integral) / 10000.f; //convert to radians + report.pixel_flow[1] = static_cast(_frame_integral.pixel_flow_y_integral) / 10000.f; //convert to radians + + report.delta_angle_available = true; + report.delta_angle[0] = static_cast(_frame_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians + report.delta_angle[1] = static_cast(_frame_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians + report.delta_angle[2] = static_cast(_frame_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians + + report.distance_m = static_cast(_frame_integral.ground_distance) / 1000.f; // convert to meters + report.distance_available = true; + + report.integration_timespan_us = _frame_integral.integration_timespan; // microseconds + + report.quality = _frame_integral.qual; // 0:bad ; 255 max quality + + report.max_flow_rate = 2.5f; + report.min_ground_distance = PX4FLOW_MIN_DISTANCE; + report.max_ground_distance = PX4FLOW_MAX_DISTANCE; report.timestamp = hrt_absolute_time(); - report.pixel_flow_x_integral = static_cast(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians - report.pixel_flow_y_integral = static_cast(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians - report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout; - report.ground_distance_m = static_cast(_frame_integral.ground_distance) / 1000.0f;//convert to meters - report.quality = _frame_integral.qual; //0:bad ; 255 max quality - report.gyro_x_rate_integral = static_cast(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians - report.gyro_y_rate_integral = static_cast(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians - report.gyro_z_rate_integral = static_cast(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians - report.integration_timespan = _frame_integral.integration_timespan; //microseconds - report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds - report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius - report.sensor_id = 0; - report.max_flow_rate = _sensor_max_flow_rate; - report.min_ground_distance = _sensor_min_range; - report.max_ground_distance = _sensor_max_range; - - /* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */ - float zeroval = 0.0f; - - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); - rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); - - _px4flow_topic.publish(report); - - /* publish to the distance_sensor topic as well */ - if (_distance_sensor_topic.get_instance() == 0) { - distance_sensor_s distance_report{}; - DeviceId device_id; - device_id.devid = get_device_id(); - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW; - - distance_report.timestamp = report.timestamp; - distance_report.min_distance = PX4FLOW_MIN_DISTANCE; - distance_report.max_distance = PX4FLOW_MAX_DISTANCE; - distance_report.current_distance = report.ground_distance_m; - distance_report.variance = 0.0f; - distance_report.signal_quality = -1; - distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - distance_report.device_id = device_id.devid; - distance_report.orientation = _sonar_rotation; - - _distance_sensor_topic.publish(distance_report); - } + _sensor_optical_flow_pub.publish(report); perf_end(_sample_perf); @@ -374,28 +307,16 @@ PX4FLOW::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x42); - PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 35, "Rotation (default=downwards)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -px4flow_main(int argc, char *argv[]) +extern "C" __EXPORT int px4flow_main(int argc, char *argv[]) { - int ch; using ThisDriver = PX4FLOW; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = PX4FLOW_I2C_MAX_BUS_SPEED; - cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.i2c_address = I2C_FLOW_ADDRESS_DEFAULT; - while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { - switch (ch) { - case 'R': - cli.rotation = (Rotation)atoi(cli.optArg()); - break; - } - } - const char *verb = cli.optArg(); if (!verb) { @@ -415,13 +336,11 @@ px4flow_main(int argc, char *argv[]) } return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } diff --git a/src/drivers/optical_flow/thoneflow/thoneflow.cpp b/src/drivers/optical_flow/thoneflow/thoneflow.cpp index 743e809baf..91734a26a3 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -50,10 +50,10 @@ #include #include #include -#include -#include -#include -#include +#include + +#include +#include #include "thoneflow_parser.h" @@ -71,21 +71,20 @@ public: void print_info(); private: - char _port[20]; - Rotation _rotation; - int _cycle_interval; - int _fd; - char _linebuf[5]; - unsigned _linebuf_index; - THONEFLOW_PARSE_STATE _parse_state; + char _port[20] {}; + Rotation _rotation{ROTATION_NONE}; + int _cycle_interval{10526}; + int _fd{-1}; + char _linebuf[5] {}; + unsigned _linebuf_index{0}; + THONEFLOW_PARSE_STATE _parse_state{THONEFLOW_PARSE_STATE0_UNSYNC}; - hrt_abstime _last_read; + hrt_abstime _last_read{0}; - optical_flow_s _report; - orb_advert_t _optical_flow_pub; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; /** * Initialise the automatic measurement state machine and start it. @@ -106,23 +105,8 @@ private: }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]); - Thoneflow::Thoneflow(const char *port) : - ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _rotation(Rotation(0)), - _cycle_interval(10526), - _fd(-1), - _linebuf_index(0), - _parse_state(THONEFLOW_PARSE_STATE0_UNSYNC), - _last_read(0), - _report(), - _optical_flow_pub(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "thoneflow_read")), - _comms_errors(perf_alloc(PC_COUNT, "thoneflow_com_err")) + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) { /* store port name */ strncpy(_port, port, sizeof(_port) - 1); @@ -206,41 +190,6 @@ Thoneflow::init() ret = PX4_ERROR; break; } - - /* get yaw rotation from sensor frame to body frame */ - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 0; - param_get(rot, &val); - - _rotation = Rotation(val); - } - - /* Initialise report structure */ - /* No gyro on this board */ - _report.gyro_x_rate_integral = NAN; - _report.gyro_y_rate_integral = NAN; - _report.gyro_z_rate_integral = NAN; - - /* Conservative specs according to datasheet */ - _report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s - _report.min_ground_distance = 0.1f; // Datasheet: 80mm - _report.max_ground_distance = 30.0f; // Datasheet: infinity - - /* Integrated flow is sent at 66fps */ - _report.frame_count_since_last_readout = 1; - _report.integration_timespan = 10526; // microseconds - - /* Get a publish handle on the optical flow topic */ - _optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report); - - if (_optical_flow_pub == nullptr) { - PX4_ERR("Failed to create optical_flow object"); - ret = PX4_ERROR; - break; - } - } while (0); /* Close the fd */ @@ -299,20 +248,33 @@ Thoneflow::collect() _last_read = hrt_absolute_time(); + // publish sensor_optical_flow + sensor_optical_flow_s report{}; + report.timestamp_sample = hrt_absolute_time(); + /* Parse each byte of read buffer */ for (int i = 0; i < ret; i++) { - valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &_report); + valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &report); } /* Publish most recent valid measurement */ if (valid) { - _report.timestamp = hrt_absolute_time(); + + report.device_id = 0; // TODO get_device_id(); + report.integration_timespan_us = 10526; // microseconds /* Rotate measurements from sensor frame to body frame */ float zeroval = 0.0f; - rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval); + rotate_3f(_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval); - orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report); + // Conservative specs according to datasheet + report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s + report.min_ground_distance = 0.08f; // Datasheet: 80mm + report.max_ground_distance = INFINITY; // Datasheet: infinity + + report.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(report); } /* Bytes left to parse */ @@ -478,16 +440,15 @@ $ thoneflow stop PRINT_MODULE_USAGE_NAME("thoneflow", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("optical_flow"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Print driver information"); } } // namespace -int -thoneflow_main(int argc, char *argv[]) +extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]) { int ch; const char *device_path = ""; @@ -522,19 +483,11 @@ thoneflow_main(int argc, char *argv[]) thoneflow::usage(); return -1; } - } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { + } else if (!strcmp(argv[myoptind], "stop")) { return thoneflow::stop(); - } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { + } else if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { thoneflow::info(); return 0; } diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp index 28cc5a5693..3534af002a 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp @@ -61,7 +61,7 @@ const char *parser_state[] = { #endif bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, - optical_flow_s *flow) + sensor_optical_flow_s *flow) { bool parsed_packet = false; @@ -133,8 +133,8 @@ bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TH // Checksum valid, populate sensor report int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0]; int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2]; - flow->pixel_flow_x_integral = static_cast(delta_x) * (3.52e-3f); - flow->pixel_flow_y_integral = static_cast(delta_y) * (3.52e-3f); + flow->pixel_flow[0] = static_cast(delta_x) * (3.52e-3f); + flow->pixel_flow[1] = static_cast(delta_y) * (3.52e-3f); *state = THONEFLOW_PARSE_STATE7_CHECKSUM; } else { diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h index fd27b8c355..d63906fdc6 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h @@ -33,7 +33,7 @@ #pragma once -#include +#include // Data Format for ThoneFlow 3901U // =============================== @@ -62,4 +62,4 @@ enum THONEFLOW_PARSE_STATE { }; bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, - optical_flow_s *report); + sensor_optical_flow_s *report); diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index f9cef4c551..099de615f0 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -38,7 +38,7 @@ const char *const UavcanFlowBridge::NAME = "flow"; UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) : - UavcanSensorBridgeBase("uavcan_flow", ORB_ID(optical_flow)), + UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow)), _sub_flow(node) { } @@ -58,23 +58,41 @@ UavcanFlowBridge::init() void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure &msg) { - optical_flow_s flow{}; + sensor_optical_flow_s flow{}; + flow.timestamp_sample = hrt_absolute_time(); // TODO - // We're only given an 8 bit field for sensor ID; just use the UAVCAN node ID - flow.sensor_id = msg.getSrcNodeID().get(); + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN; + device_id.devid_s.bus = 0; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_UAVCAN; + device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF; - flow.timestamp = hrt_absolute_time(); - flow.integration_timespan = 1.e6f * msg.integration_interval; // s -> micros - flow.pixel_flow_x_integral = msg.flow_integral[0]; - flow.pixel_flow_y_integral = msg.flow_integral[1]; + flow.device_id = device_id.devid; - flow.gyro_x_rate_integral = msg.rate_gyro_integral[0]; - flow.gyro_y_rate_integral = msg.rate_gyro_integral[1]; + flow.pixel_flow[0] = msg.flow_integral[0]; + flow.pixel_flow[1] = msg.flow_integral[1]; + + flow.integration_timespan_us = 1.e6f * msg.integration_interval; // s -> us flow.quality = msg.quality; - flow.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s - flow.min_ground_distance = 0.1f; // Datasheet: 80mm - flow.max_ground_distance = 30.0f; // Datasheet: infinity + + if (PX4_ISFINITE(msg.rate_gyro_integral[0]) && PX4_ISFINITE(msg.rate_gyro_integral[1])) { + flow.delta_angle[0] = msg.rate_gyro_integral[0]; + flow.delta_angle[1] = msg.rate_gyro_integral[1]; + flow.delta_angle[2] = 0.f; + flow.delta_angle_available = true; + + } else { + flow.delta_angle[0] = NAN; + flow.delta_angle[1] = NAN; + flow.delta_angle[2] = NAN; + } + + flow.max_flow_rate = NAN; + flow.min_ground_distance = NAN; + flow.max_ground_distance = NAN; + + flow.timestamp = hrt_absolute_time(); publish(msg.getSrcNodeID().get(), &flow); } diff --git a/src/drivers/uavcan/sensors/flow.hpp b/src/drivers/uavcan/sensors/flow.hpp index 94aec78a0e..3a5484269e 100644 --- a/src/drivers/uavcan/sensors/flow.hpp +++ b/src/drivers/uavcan/sensors/flow.hpp @@ -37,7 +37,7 @@ #include -#include +#include #include diff --git a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp index 7780bd85f0..de1b550afb 100644 --- a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp @@ -39,7 +39,7 @@ #include #include -#include +#include namespace uavcannode { @@ -52,18 +52,9 @@ class FlowMeasurement : public: FlowMeasurement(px4::WorkItem *work_item, uavcan::INode &node) : UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID), - uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_optical_flow)), uavcan::Publisher(node) { - _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, 0.f}}; - - param_t rot = param_find("CANNODE_FLOW_ROT"); - int32_t val = 0; - - if (param_get(rot, &val) == PX4_OK) { - _rotation = get_rot_matrix((enum Rotation)val); - } - this->setPriority(uavcan::TransferPriority::Default); } @@ -80,20 +71,18 @@ public: void BroadcastAnyUpdates() override { // optical_flow -> com::hex::equipment::flow::Measurement - optical_flow_s optical_flow; + vehicle_optical_flow_s optical_flow; if (uORB::SubscriptionCallbackWorkItem::update(&optical_flow)) { com::hex::equipment::flow::Measurement measurement{}; - measurement.integration_interval = optical_flow.integration_timespan * 1e-6f; // us -> s + measurement.integration_interval = optical_flow.integration_timespan_us * 1e-6f; // us -> s // rotate measurements in yaw from sensor frame to body frame - const matrix::Vector3f gyro_flow_rotated = _rotation * matrix::Vector3f{optical_flow.gyro_x_rate_integral, optical_flow.gyro_y_rate_integral, 0.f}; - const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{optical_flow.pixel_flow_x_integral, optical_flow.pixel_flow_y_integral, 0.f}; + measurement.rate_gyro_integral[0] = optical_flow.delta_angle[0]; + measurement.rate_gyro_integral[1] = optical_flow.delta_angle[1]; - measurement.rate_gyro_integral[0] = gyro_flow_rotated(0); - measurement.rate_gyro_integral[1] = gyro_flow_rotated(1); - measurement.flow_integral[0] = pixel_flow_rotated(0); - measurement.flow_integral[1] = pixel_flow_rotated(1); + measurement.flow_integral[0] = optical_flow.pixel_flow[0]; + measurement.flow_integral[1] = optical_flow.pixel_flow[1]; measurement.quality = optical_flow.quality; diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 709e333ee8..8e9a10e833 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/src/drivers/uavcannode/uavcannode_params.c @@ -60,27 +60,6 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000); */ PARAM_DEFINE_INT32(CANNODE_TERM, 0); -/** - * Cannode flow board rotation - * - * This parameter defines the yaw rotation of the Cannode flow board relative to the vehicle body frame. - * Zero rotation is defined as X on flow board pointing towards front of vehicle. - * - * @value 0 No rotation - * @value 1 Yaw 45° - * @value 2 Yaw 90° - * @value 3 Yaw 135° - * @value 4 Yaw 180° - * @value 5 Yaw 225° - * @value 6 Yaw 270° - * @value 7 Yaw 315° - * - * @reboot_required true - * - * @group UAVCAN - */ -PARAM_DEFINE_INT32(CANNODE_FLOW_ROT, 0); - /** * Enable RTCM pub/sub * diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b74517f6bb..66399646a3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -104,9 +104,12 @@ public: const Vector2f &getFlowVelBody() const { return _flow_vel_body; } const Vector2f &getFlowVelNE() const { return _flow_vel_ne; } + const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; } const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; } - const Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; } + + const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } + const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; } void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9b3b40e3d2..349b71c5aa 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1426,14 +1426,18 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) { if (_ekf.getFlowCompensated().longerThan(0.f)) { - estimator_optical_flow_vel_s flow_vel{}; + vehicle_optical_flow_vel_s flow_vel{}; flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; _ekf.getFlowVelBody().copyTo(flow_vel.vel_body); _ekf.getFlowVelNE().copyTo(flow_vel.vel_ne); + _ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral); _ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral); - _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral); + + _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate); + _ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral); + flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel); @@ -1711,29 +1715,29 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF flow sample bool new_optical_flow = false; - const unsigned last_generation = _optical_flow_sub.get_last_generation(); - optical_flow_s optical_flow; + const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation(); + vehicle_optical_flow_s optical_flow; - if (_optical_flow_sub.update(&optical_flow)) { + if (_vehicle_optical_flow_sub.update(&optical_flow)) { if (_msg_missed_optical_flow_perf == nullptr) { _msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed"); - } else if (_optical_flow_sub.get_last_generation() != last_generation + 1) { + } else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) { perf_count(_msg_missed_optical_flow_perf); } flowSample flow { - .time_us = optical_flow.timestamp, + .time_us = optical_flow.timestamp_sample, // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate // is produced by a RH rotation of the image about the sensor axis. - .flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral}, - .gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral}, - .dt = 1e-6f * (float)optical_flow.integration_timespan, + .flow_xy_rad = Vector2f{-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]}, + .gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]}, + .dt = 1e-6f * (float)optical_flow.integration_timespan_us, .quality = optical_flow.quality, }; - if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && - PX4_ISFINITE(optical_flow.pixel_flow_x_integral) && + if (PX4_ISFINITE(optical_flow.pixel_flow[0]) && + PX4_ISFINITE(optical_flow.pixel_flow[1]) && flow.dt < 1) { // Save sensor limits reported by the optical flow sensor @@ -1745,6 +1749,22 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) new_optical_flow = true; } + // use optical_flow distance as range sample if distance_sensor unavailable + if (PX4_ISFINITE(optical_flow.distance_m) && (hrt_elapsed_time(&_last_range_sensor_update) > 1_s)) { + + int8_t quality = static_cast(optical_flow.quality) / static_cast(UINT8_MAX) * 100.f; + + rangeSample range_sample { + .time_us = optical_flow.timestamp_sample, + .rng = optical_flow.distance_m, + .quality = quality, + }; + _ekf.setRangeData(range_sample); + + // set sensor limits + _ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance); + } + ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 9dff6dcf40..e279ff0b56 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -71,13 +71,11 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #include @@ -91,11 +89,12 @@ #include #include #include +#include +#include #include #include #include - extern pthread_mutex_t ekf2_module_mutex; class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem @@ -276,17 +275,18 @@ private: uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; - uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + hrt_abstime _last_range_sensor_update{0}; int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations unsigned _distance_sensor_last_generation{0}; @@ -295,8 +295,6 @@ private: hrt_abstime _last_event_flags_publish{0}; hrt_abstime _last_status_flags_publish{0}; - hrt_abstime _last_range_sensor_update{0}; - uint32_t _filter_control_status{0}; uint32_t _filter_fault_status{0}; uint32_t _innov_check_fail_status{0}; @@ -314,12 +312,12 @@ private: uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; - uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; + uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 196d6bcebb..12f169360a 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -270,7 +270,7 @@ private: uORB::SubscriptionData _sub_land{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionData _sub_att{ORB_ID(vehicle_attitude)}; uORB::SubscriptionData _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)}; - uORB::SubscriptionData _sub_flow{ORB_ID(optical_flow)}; + uORB::SubscriptionData _sub_flow{ORB_ID(vehicle_optical_flow)}; uORB::SubscriptionData _sub_gps{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionData _sub_visual_odom{ORB_ID(vehicle_visual_odometry)}; uORB::SubscriptionData _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)}; diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index ff32dc5310..4213ecfd09 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -61,9 +61,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) // optical flow in x, y axis // TODO consider making flow scale a states of the kalman filter - float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _param_lpe_flw_scale.get(); - float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _param_lpe_flw_scale.get(); - float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f; + float flow_x_rad = _sub_flow.get().pixel_flow[0] * _param_lpe_flw_scale.get(); + float flow_y_rad = _sub_flow.get().pixel_flow[1] * _param_lpe_flw_scale.get(); + float dt_flow = _sub_flow.get().integration_timespan_us / 1.0e6f; if (dt_flow > 0.5f || dt_flow < 1.0e-6f) { return -1; @@ -74,10 +74,8 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) float gyro_y_rad = 0; if (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) { - gyro_x_rad = _flow_gyro_x_high_pass.update( - _sub_flow.get().gyro_x_rate_integral); - gyro_y_rad = _flow_gyro_y_high_pass.update( - _sub_flow.get().gyro_y_rate_integral); + gyro_x_rad = _flow_gyro_x_high_pass.update(_sub_flow.get().delta_angle[0]); + gyro_y_rad = _flow_gyro_y_high_pass.update(_sub_flow.get().delta_angle[1]); } //warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f", diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index e827a43bbd..1d0d80d927 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -174,7 +174,6 @@ void LoggedTopics::add_default_topics() add_topic_multi("battery_status", 200, 2); add_topic_multi("differential_pressure", 1000, 2); add_topic_multi("distance_sensor", 1000, 2); - add_topic_multi("optical_flow", 1000, 1); add_optional_topic_multi("sensor_accel", 1000, 4); add_optional_topic_multi("sensor_baro", 1000, 4); add_topic_multi("sensor_gps", 1000, 2); @@ -182,9 +181,13 @@ void LoggedTopics::add_default_topics() add_optional_topic("pps_capture", 1000); add_optional_topic_multi("sensor_gyro", 1000, 4); add_optional_topic_multi("sensor_mag", 1000, 4); + add_optional_topic_multi("sensor_optical_flow", 1000, 2); + add_topic_multi("vehicle_imu", 500, 4); add_topic_multi("vehicle_imu_status", 1000, 4); add_optional_topic_multi("vehicle_magnetometer", 500, 4); + add_optional_topic("vehicle_optical_flow", 500); + //add_optional_topic("vehicle_optical_flow_vel", 100); // SYS_CTRL_ALLOC: additional dynamic control allocation logging when enabled int32_t sys_ctrl_alloc = 0; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8fa244e799..196d991a6c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -89,12 +89,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _parameters_manager(parent), _mavlink_timesync(parent) { - _handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT"); - _handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR"); - _handle_sens_flow_minhgt = param_find("SENS_FLOW_MINHGT"); - _handle_sens_flow_rot = param_find("SENS_FLOW_ROT"); - _handle_ekf2_min_rng = param_find("EKF2_MIN_RNG"); - _handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX"); } void @@ -795,105 +789,93 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) void MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { - /* optical flow */ mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); - optical_flow_s f{}; + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = _mavlink->get_instance_id(); + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_MAVLINK; - f.timestamp = hrt_absolute_time(); - f.time_since_last_sonar_update = flow.time_delta_distance_us; - f.integration_timespan = flow.integration_time_us; - f.pixel_flow_x_integral = flow.integrated_x; - f.pixel_flow_y_integral = flow.integrated_y; - f.gyro_x_rate_integral = flow.integrated_xgyro; - f.gyro_y_rate_integral = flow.integrated_ygyro; - f.gyro_z_rate_integral = flow.integrated_zgyro; - f.gyro_temperature = flow.temperature; - f.ground_distance_m = flow.distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - f.max_flow_rate = _param_sens_flow_maxr; - f.min_ground_distance = _param_sens_flow_minhgt; - f.max_ground_distance = _param_sens_flow_maxhgt; + sensor_optical_flow_s sensor_optical_flow{}; - /* read flow sensor parameters */ - const Rotation flow_rot = (Rotation)_param_sens_flow_rot; + sensor_optical_flow.timestamp_sample = hrt_absolute_time(); + sensor_optical_flow.device_id = device_id.devid; - /* rotate measurements according to parameter */ - float zero_val = 0.0f; - rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val); - rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral); + sensor_optical_flow.pixel_flow[0] = flow.integrated_x; + sensor_optical_flow.pixel_flow[1] = flow.integrated_y; - _flow_pub.publish(f); + sensor_optical_flow.integration_timespan_us = flow.integration_time_us; + sensor_optical_flow.quality = flow.quality; - /* Use distance value for distance sensor topic */ - if (flow.distance > 0.0f) { // negative values signal invalid data - - distance_sensor_s d{}; - - device::Device::DeviceId device_id; - device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; - device_id.devid_s.address = msg->sysid; - - d.timestamp = f.timestamp; - d.min_distance = _param_ekf2_min_rng; - d.max_distance = _param_ekf2_rng_a_hmax; - d.current_distance = flow.distance; /* both are in m */ - d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - d.device_id = device_id.devid; - d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - d.variance = 0.01; - d.signal_quality = -1; - - _flow_distance_sensor_pub.publish(d); + if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { + sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; + sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; + sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + sensor_optical_flow.delta_angle_available = true; } + + sensor_optical_flow.max_flow_rate = NAN; + sensor_optical_flow.min_ground_distance = NAN; + sensor_optical_flow.max_ground_distance = NAN; + + // Use distance value for distance sensor topic + if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) { + // Positive value (including zero): distance known. Negative value: Unknown distance. + sensor_optical_flow.distance_m = flow.distance; + sensor_optical_flow.distance_available = true; + } + + sensor_optical_flow.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(sensor_optical_flow); } void MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) { - /* optical flow */ mavlink_hil_optical_flow_t flow; mavlink_msg_hil_optical_flow_decode(msg, &flow); - optical_flow_s f{}; - - f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; - f.integration_timespan = flow.integration_time_us; - f.pixel_flow_x_integral = flow.integrated_x; - f.pixel_flow_y_integral = flow.integrated_y; - f.gyro_x_rate_integral = flow.integrated_xgyro; - f.gyro_y_rate_integral = flow.integrated_ygyro; - f.gyro_z_rate_integral = flow.integrated_zgyro; - f.time_since_last_sonar_update = flow.time_delta_distance_us; - f.ground_distance_m = flow.distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - f.gyro_temperature = flow.temperature; - - _flow_pub.publish(f); - - /* Use distance value for distance sensor topic */ - distance_sensor_s d{}; - device::Device::DeviceId device_id; - device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = _mavlink->get_instance_id(); device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; - d.timestamp = hrt_absolute_time(); - d.min_distance = 0.3f; - d.max_distance = 5.0f; - d.current_distance = flow.distance; /* both are in m */ - d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - d.device_id = device_id.devid; - d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - d.variance = 0.01; - d.signal_quality = -1; + sensor_optical_flow_s sensor_optical_flow{}; - _flow_distance_sensor_pub.publish(d); + sensor_optical_flow.timestamp_sample = hrt_absolute_time(); + sensor_optical_flow.device_id = device_id.devid; + + sensor_optical_flow.pixel_flow[0] = flow.integrated_x; + sensor_optical_flow.pixel_flow[1] = flow.integrated_y; + + sensor_optical_flow.integration_timespan_us = flow.integration_time_us; + sensor_optical_flow.quality = flow.quality; + + if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { + sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; + sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; + sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + sensor_optical_flow.delta_angle_available = true; + } + + sensor_optical_flow.max_flow_rate = NAN; + sensor_optical_flow.min_ground_distance = NAN; + sensor_optical_flow.max_ground_distance = NAN; + + // Use distance value for distance sensor topic + if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) { + // Positive value (including zero): distance known. Negative value: Unknown distance. + sensor_optical_flow.distance_m = flow.distance; + sensor_optical_flow.distance_available = true; + } + + sensor_optical_flow.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(sensor_optical_flow); } void @@ -934,9 +916,10 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) distance_sensor_s ds{}; device::Device::DeviceId device_id; - device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = _mavlink->get_instance_id(); + device_id.devid_s.address = msg->sysid; device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; - device_id.devid_s.address = dist_sensor.id; ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */ ds.min_distance = static_cast(dist_sensor.min_distance) * 1e-2f; /* cm to m */ @@ -2338,10 +2321,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) sensor_gps_s gps{}; - device::Device::DeviceId device_id{}; + device::Device::DeviceId device_id; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = _mavlink->get_instance_id(); device_id.devid_s.address = msg->sysid; device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + gps.device_id = device_id.devid; gps.lat = hil_gps.lat; @@ -3475,30 +3460,6 @@ MavlinkReceiver::updateParams() { // update parameters from storage ModuleParams::updateParams(); - - if (_handle_sens_flow_maxhgt != PARAM_INVALID) { - param_get(_handle_sens_flow_maxhgt, &_param_sens_flow_maxhgt); - } - - if (_handle_sens_flow_maxr != PARAM_INVALID) { - param_get(_handle_sens_flow_maxr, &_param_sens_flow_maxr); - } - - if (_handle_sens_flow_minhgt != PARAM_INVALID) { - param_get(_handle_sens_flow_minhgt, &_param_sens_flow_minhgt); - } - - if (_handle_sens_flow_rot != PARAM_INVALID) { - param_get(_handle_sens_flow_rot, &_param_sens_flow_rot); - } - - if (_handle_ekf2_min_rng != PARAM_INVALID) { - param_get(_handle_ekf2_min_rng, &_param_ekf2_min_rng); - } - - if (_handle_ekf2_rng_a_hmax != PARAM_INVALID) { - param_get(_handle_ekf2_rng_a_hmax, &_param_ekf2_rng_a_hmax); - } } void *MavlinkReceiver::start_trampoline(void *context) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 38cb1c5fa4..93723fea62 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -86,13 +86,13 @@ #include #include #include -#include #include #include #include #include #include #include +#include #include #include #include @@ -308,7 +308,6 @@ private: uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; - uORB::Publication _flow_pub{ORB_ID(optical_flow)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)}; @@ -331,13 +330,13 @@ private: // ORB publications (multi) uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; - uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; uORB::PublicationMulti _ping_pub{ORB_ID(ping)}; uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; // ORB publications (queue length > 1) uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; @@ -396,20 +395,6 @@ private: hrt_abstime _heartbeat_component_udp_bridge{0}; hrt_abstime _heartbeat_component_uart_bridge{0}; - param_t _handle_sens_flow_maxhgt{PARAM_INVALID}; - param_t _handle_sens_flow_maxr{PARAM_INVALID}; - param_t _handle_sens_flow_minhgt{PARAM_INVALID}; - param_t _handle_sens_flow_rot{PARAM_INVALID}; - param_t _handle_ekf2_min_rng{PARAM_INVALID}; - param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID}; - - float _param_sens_flow_maxhgt{-1.0f}; - float _param_sens_flow_maxr{-1.0f}; - float _param_sens_flow_minhgt{-1.0f}; - int32_t _param_sens_flow_rot{0}; - float _param_ekf2_min_rng{NAN}; - float _param_ekf2_rng_a_hmax{NAN}; - DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, diff --git a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp index ebbe35ea1f..02f368b4c3 100644 --- a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp +++ b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp @@ -34,7 +34,7 @@ #ifndef OPTICAL_FLOW_RAD_HPP #define OPTICAL_FLOW_RAD_HPP -#include +#include class MavlinkStreamOpticalFlowRad : public MavlinkStream { @@ -49,34 +49,40 @@ public: unsigned get_size() override { - return _optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _vehicle_optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : + 0; } private: explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; + uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)}; bool send() override { - optical_flow_s flow; + vehicle_optical_flow_s flow; - if (_optical_flow_sub.update(&flow)) { + if (_vehicle_optical_flow_sub.update(&flow)) { mavlink_optical_flow_rad_t msg{}; - msg.time_usec = flow.timestamp; - msg.sensor_id = flow.sensor_id; - msg.integrated_x = flow.pixel_flow_x_integral; - msg.integrated_y = flow.pixel_flow_y_integral; - msg.integrated_xgyro = flow.gyro_x_rate_integral; - msg.integrated_ygyro = flow.gyro_y_rate_integral; - msg.integrated_zgyro = flow.gyro_z_rate_integral; - msg.distance = flow.ground_distance_m; + msg.time_usec = flow.timestamp_sample; + msg.sensor_id = _vehicle_optical_flow_sub.get_instance(); + msg.integration_time_us = flow.integration_timespan_us; + msg.integrated_x = flow.pixel_flow[0]; + msg.integrated_y = flow.pixel_flow[1]; + msg.integrated_xgyro = flow.delta_angle[0]; + msg.integrated_ygyro = flow.delta_angle[1]; + msg.integrated_zgyro = flow.delta_angle[2]; + // msg.temperature = 0; msg.quality = flow.quality; - msg.integration_time_us = flow.integration_timespan; - msg.sensor_id = flow.sensor_id; - msg.time_delta_distance_us = flow.time_since_last_sonar_update; - msg.temperature = flow.gyro_temperature; + // msg.time_delta_distance_us = 0; + + if (PX4_ISFINITE(flow.distance_m)) { + msg.distance = flow.distance_m; + + } else { + msg.distance = -1; + } mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 941718323b..c096da6ab5 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #include #include @@ -47,6 +46,7 @@ #include #include #include +#include #include #include @@ -91,7 +91,7 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(distance_sensor)) { _distance_sensor_msg_id = msg_id; - } else if (sub.orb_meta == ORB_ID(optical_flow)) { + } else if (sub.orb_meta == ORB_ID(vehicle_optical_flow)) { _optical_flow_msg_id = msg_id; } else if (sub.orb_meta == ORB_ID(vehicle_air_data)) { @@ -204,7 +204,7 @@ ReplayEkf2::onExitMainLoop() print_sensor_statistics(_airspeed_msg_id, "airspeed"); print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor"); - print_sensor_statistics(_optical_flow_msg_id, "optical_flow"); + print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow"); print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined"); print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data"); print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer"); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 120c478212..9af0ab04cb 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -51,13 +51,20 @@ if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) add_subdirectory(vehicle_magnetometer) endif() +if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + add_subdirectory(vehicle_optical_flow) +endif() + px4_add_module( MODULE modules__sensors MAIN sensors + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} SRCS sensors.cpp voted_sensors_update.cpp voted_sensors_update.h + Integrator.hpp MODULE_CONFIG module.yaml DEPENDS @@ -85,3 +92,7 @@ endif() if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) target_link_libraries(modules__sensors PRIVATE vehicle_magnetometer) endif() + +if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + target_link_libraries(modules__sensors PRIVATE vehicle_optical_flow) +endif() diff --git a/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/modules/sensors/Integrator.hpp similarity index 96% rename from src/modules/sensors/vehicle_imu/Integrator.hpp rename to src/modules/sensors/Integrator.hpp index 80cd6f46e9..b6c26f331e 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.hpp +++ b/src/modules/sensors/Integrator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +45,9 @@ #include #include +namespace sensors +{ + class Integrator { public: @@ -94,6 +97,8 @@ public: */ inline bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); } + float integral_dt() const { return _integral_dt; } + void reset() { _alpha.zero(); @@ -137,7 +142,7 @@ protected: matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */ float _integral_dt{0}; - float _reset_interval_min{0.005f}; /**< the interval after which the content will be published and the integrator reset */ + float _reset_interval_min{0.001f}; /**< the interval after which the content will be published and the integrator reset */ uint8_t _reset_samples_min{1}; uint8_t _integrated_samples{0}; @@ -214,3 +219,5 @@ private: matrix::Vector3f _last_alpha{0.f, 0.f, 0.f}; /**< previous value of _alpha */ }; + +}; // namespace sensors diff --git a/src/modules/sensors/Kconfig b/src/modules/sensors/Kconfig index 67863ec53f..163ac29506 100644 --- a/src/modules/sensors/Kconfig +++ b/src/modules/sensors/Kconfig @@ -28,4 +28,8 @@ if MODULES_SENSORS bool "Include vehicle magnetometer" default y + config SENSORS_VEHICLE_OPTICAL_FLOW + bool "Include vehicle optical flow" + default y + endif #MODULES_SENSORS diff --git a/src/modules/sensors/sensor_params_flow.c b/src/modules/sensors/sensor_params_flow.c index b8a2b7574e..7d3de7ff7a 100644 --- a/src/modules/sensors/sensor_params_flow.c +++ b/src/modules/sensors/sensor_params_flow.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,11 +32,10 @@ ****************************************************************************/ /** - * PX4Flow board rotation + * Optical flow rotation * - * This parameter defines the yaw rotation of the PX4FLOW board relative to the vehicle body frame. + * This parameter defines the yaw rotation of the optical flow relative to the vehicle body frame. * Zero rotation is defined as X on flow board pointing towards front of vehicle. - * The recommneded installation default for the PX4FLOW board is with the Y axis forward (270 deg yaw). * * @value 0 No rotation * @value 1 Yaw 45° @@ -47,11 +46,9 @@ * @value 6 Yaw 270° * @value 7 Yaw 315° * - * @reboot_required true - * * @group Sensors */ -PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6); +PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); /** * Minimum height above ground when reliant on optical flow. @@ -66,7 +63,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6); * @decimal 1 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f); +PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.08f); /** * Maximum height above ground when reliant on optical flow. @@ -78,12 +75,12 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f); * * @unit m * @min 1.0 - * @max 25.0 + * @max 100.0 * @increment 0.1 * @decimal 1 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f); +PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 100.f); /** * Magnitude of maximum angular flow rate reliably measurable by the optical flow sensor. @@ -97,4 +94,20 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f); * @decimal 2 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 2.5f); +PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 8.f); + +/** + * Optical flow max rate. + * + * Optical flow data maximum publication rate. This is an upper bound, + * actual optical flow data rate is still dependant on the sensor. + * + * @min 1 + * @max 200 + * @group Sensors + * @unit Hz + * + * @reboot_required true + * + */ +PARAM_DEFINE_FLOAT(SENS_FLOW_RATE, 70.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a9d1840fb4..840d722597 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -93,6 +93,10 @@ # include #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +# include "vehicle_optical_flow/VehicleOpticalFlow.hpp" +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + using namespace sensors; using namespace time_literals; @@ -210,6 +214,11 @@ private: uint8_t _n_gps{0}; #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + VehicleOpticalFlow *_vehicle_optical_flow {nullptr}; + uint8_t _n_optical_flow{0}; +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; uint8_t _n_accel{0}; @@ -242,6 +251,7 @@ private: void InitializeVehicleGPSPosition(); void InitializeVehicleIMU(); void InitializeVehicleMagnetometer(); + void InitializeVehicleOpticalFlow(); DEFINE_PARAMETERS( #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) @@ -335,6 +345,15 @@ Sensors::~Sensors() #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + + if (_vehicle_optical_flow) { + _vehicle_optical_flow->Stop(); + delete _vehicle_optical_flow; + } + +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + for (auto &vehicle_imu : _vehicle_imu_list) { if (vehicle_imu) { vehicle_imu->Stop(); @@ -445,6 +464,10 @@ int Sensors::parameters_update() InitializeVehicleMagnetometer(); #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + InitializeVehicleOpticalFlow(); +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + return PX4_OK; } @@ -688,6 +711,23 @@ void Sensors::InitializeVehicleMagnetometer() } #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +void Sensors::InitializeVehicleOpticalFlow() +{ + if (_vehicle_optical_flow == nullptr) { + uORB::Subscription sensor_optical_flow_sub{ORB_ID(sensor_optical_flow)}; + + if (sensor_optical_flow_sub.advertised()) { + _vehicle_optical_flow = new VehicleOpticalFlow(); + + if (_vehicle_optical_flow) { + _vehicle_optical_flow->Start(); + } + } + } +} +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + void Sensors::Run() { if (should_exit()) { @@ -747,6 +787,16 @@ void Sensors::Run() #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow)); + + if (n_optical_flow != _n_optical_flow) { + _n_optical_flow = n_optical_flow; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + const int n_accel = orb_group_count(ORB_ID(sensor_accel)); const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); @@ -877,6 +927,15 @@ int Sensors::print_status() _airspeed_validator.print(); #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + + if (_vehicle_optical_flow) { + PX4_INFO_RAW("\n"); + _vehicle_optical_flow->PrintStatus(); + } + +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + PX4_INFO_RAW("\n"); _vehicle_acceleration.PrintStatus(); diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index 8176327f4d..02139c4a7f 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -32,7 +32,6 @@ ############################################################################ px4_add_library(vehicle_imu - Integrator.hpp VehicleIMU.cpp VehicleIMU.hpp ) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 6420fbb13b..f4f5f520c1 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -33,7 +33,7 @@ #pragma once -#include "Integrator.hpp" +#include #include #include @@ -108,8 +108,8 @@ private: calibration::Accelerometer _accel_calibration{}; calibration::Gyroscope _gyro_calibration{}; - Integrator _accel_integrator{}; - IntegratorConing _gyro_integrator{}; + sensors::Integrator _accel_integrator{}; + sensors::IntegratorConing _gyro_integrator{}; uint32_t _imu_integration_interval_us{5000}; diff --git a/src/drivers/optical_flow/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt similarity index 88% rename from src/drivers/optical_flow/CMakeLists.txt rename to src/modules/sensors/vehicle_optical_flow/CMakeLists.txt index ab6c477290..ba8761aa00 100644 --- a/src/drivers/optical_flow/CMakeLists.txt +++ b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2020 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,7 +31,8 @@ # ############################################################################ -add_subdirectory(paw3902) -add_subdirectory(pmw3901) -add_subdirectory(px4flow) -add_subdirectory(thoneflow) +px4_add_library(vehicle_optical_flow + VehicleOpticalFlow.cpp + VehicleOpticalFlow.hpp +) +target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp b/src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp new file mode 100644 index 0000000000..591088bc30 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file RingBuffer.hpp + * Template RingBuffer + */ + +#include +#include +#include + +template +class RingBuffer +{ +public: + void push(const T &sample) + { + uint8_t head_new = _head; + + if (!_first_write) { + head_new = (_head + 1) % SIZE; + } + + _buffer[head_new] = sample; + _head = head_new; + + // move tail if we overwrite it + if (_head == _tail && !_first_write) { + _tail = (_tail + 1) % SIZE; + + } else { + _first_write = false; + } + } + + uint8_t get_length() const { return SIZE; } + + T &operator[](const uint8_t index) { return _buffer[index]; } + + const T &get_newest() const { return _buffer[_head]; } + const T &get_oldest() const { return _buffer[_tail]; } + + uint8_t get_oldest_index() const { return _tail; } + + bool peak_first_older_than(const uint64_t ×tamp, T *sample) + { + // start looking from newest observation data + for (uint8_t i = 0; i < SIZE; i++) { + int index = (_head - i); + index = index < 0 ? SIZE + index : index; + + if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) { + *sample = _buffer[index]; + return true; + } + + if (index == _tail) { + // we have reached the tail and haven't got a + // match + return false; + } + } + + return false; + } + + bool pop_first_older_than(const uint64_t ×tamp, T *sample) + { + // start looking from newest observation data + for (uint8_t i = 0; i < SIZE; i++) { + int index = (_head - i); + index = index < 0 ? SIZE + index : index; + + if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) { + *sample = _buffer[index]; + + // Now we can set the tail to the item which + // comes after the one we removed since we don't + // want to have any older data in the buffer + if (index == _head) { + _tail = _head; + _first_write = true; + + } else { + _tail = (index + 1) % SIZE; + } + + _buffer[index].time_us = 0; + + return true; + } + + if (index == _tail) { + // we have reached the tail and haven't got a + // match + return false; + } + } + + return false; + } + + bool pop_oldest(const uint64_t ×tamp_oldest, const uint64_t ×tamp_newest, T *sample) + { + if (timestamp_oldest >= timestamp_newest) { + return false; + } + + for (uint8_t i = 0; i < SIZE; i++) { + + uint8_t index = (_tail + i) % SIZE; + + if (_buffer[index].time_us >= timestamp_oldest && _buffer[index].time_us <= timestamp_newest) { + *sample = _buffer[index]; + + // Now we can set the tail to the item which + // comes after the one we removed since we don't + // want to have any older data in the buffer + if (index == _head) { + _tail = _head; + _first_write = true; + + } else { + _tail = (index + 1) % SIZE; + } + + _buffer[index] = {}; + + return true; + } + } + + return false; + } + + bool pop_oldest(T *sample) + { + if (_tail == _head) { + return false; + } + + *sample = _buffer[_tail]; + _buffer[_tail].time_us = 0; + + _tail = (_tail + 1) % SIZE; + + return true; + } + + bool pop_newest(T *sample) + { + if (_tail == _head) { + return false; + } + + *sample = _buffer[_head]; + _buffer[_head].time_us = 0; + + _head = (_head - 1) % SIZE; + + return true; + } + + int get_total_size() const { return sizeof(*this) + sizeof(T) * SIZE; } + + uint8_t entries() const + { + int count = 0; + + for (uint8_t i = 0; i < SIZE; i++) { + if (_buffer[i].time_us != 0) { + count++; + } + } + + return count; + } + +private: + T _buffer[SIZE] {}; + + uint8_t _head{0}; + uint8_t _tail{0}; + + bool _first_write{true}; +}; diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp new file mode 100644 index 0000000000..11d43ee0b9 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -0,0 +1,481 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "VehicleOpticalFlow.hpp" + +#include + +namespace sensors +{ + +using namespace matrix; +using namespace time_literals; + +static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; + +VehicleOpticalFlow::VehicleOpticalFlow() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + _vehicle_optical_flow_pub.advertise(); + + _gyro_integrator.set_reset_samples(1); +} + +VehicleOpticalFlow::~VehicleOpticalFlow() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleOpticalFlow::Start() +{ + _sensor_flow_sub.registerCallback(); + + _sensor_gyro_sub.registerCallback(); + _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); + + _sensor_selection_sub.registerCallback(); + + ScheduleNow(); + return true; +} + +void VehicleOpticalFlow::Stop() +{ + Deinit(); + + // clear all registered callbacks + _sensor_flow_sub.unregisterCallback(); + _sensor_gyro_sub.unregisterCallback(); + _sensor_selection_sub.unregisterCallback(); +} + +void VehicleOpticalFlow::ParametersUpdate() +{ + // Check if parameters have changed + if (_params_sub.updated()) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + + _flow_rotation = get_rot_matrix((enum Rotation)_param_sens_flow_rot.get()); + } +} + +void VehicleOpticalFlow::Run() +{ + perf_begin(_cycle_perf); + + ParametersUpdate(); + + UpdateDistanceSensor(); + UpdateSensorGyro(); + + sensor_optical_flow_s sensor_optical_flow; + + if (_sensor_flow_sub.update(&sensor_optical_flow)) { + + // clear data accumulation if there's a gap in data + if (((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last) + > sensor_optical_flow.integration_timespan_us * 1.5f) + || (_accumulated_count > 0 && _quality_sum == 0)) { + ClearAccumulatedData(); + } + + + const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf( + sensor_optical_flow.integration_timespan_us); + const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp; + + // delta angle + // - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available + if (sensor_optical_flow.delta_angle_available + && PX4_ISFINITE(sensor_optical_flow.delta_angle[0]) + && PX4_ISFINITE(sensor_optical_flow.delta_angle[1]) + && PX4_ISFINITE(sensor_optical_flow.delta_angle[2]) + ) { + // passthrough integrated gyro if available + _delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle}; + + } else { + // integrate synchronized gyro + gyroSample gyro_sample; + + while (_gyro_buffer.pop_oldest(timestamp_oldest, timestamp_newest, &gyro_sample)) { + + _gyro_integrator.put(gyro_sample.data, gyro_sample.dt); + + float min_interval_s = (sensor_optical_flow.integration_timespan_us * 1e-6f) * 0.99f; + + if (_gyro_integrator.integral_dt() > min_interval_s) { + //PX4_INFO("integral dt: %.6f, min interval: %.6f", (double)_gyro_integrator.integral_dt(),(double) min_interval_s); + break; + } + } + + Vector3f delta_angle{NAN, NAN, NAN}; + uint16_t delta_angle_dt; + + if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) { + _delta_angle += delta_angle; + + } else { + // force integrator reset + _gyro_integrator.reset(); + } + } + + // distance + // - from sensor_optical_flow if available, otherwise use downward distance_sensor if available + if (sensor_optical_flow.distance_available && PX4_ISFINITE(sensor_optical_flow.distance_m)) { + if (!PX4_ISFINITE(_distance_sum)) { + _distance_sum = sensor_optical_flow.distance_m; + _distance_sum_count = 1; + + } else { + _distance_sum += sensor_optical_flow.distance_m; + _distance_sum_count += 1; + } + + } else { + // otherwise use buffered downward facing distance_sensor if available + rangeSample range_sample; + + if (_range_buffer.peak_first_older_than(sensor_optical_flow.timestamp_sample, &range_sample)) { + if (!PX4_ISFINITE(_distance_sum)) { + _distance_sum = range_sample.data; + _distance_sum_count = 1; + + } else { + _distance_sum += range_sample.data; + _distance_sum_count += 1; + } + } + } + + _flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample; + _flow_integral(0) += sensor_optical_flow.pixel_flow[0]; + _flow_integral(1) += sensor_optical_flow.pixel_flow[1]; + + _integration_timespan_us += sensor_optical_flow.integration_timespan_us; + + _quality_sum += sensor_optical_flow.quality; + _accumulated_count++; + + bool publish = true; + + if (_param_sens_flow_rate.get() > 0) { + const float interval_us = 1e6f / _param_sens_flow_rate.get(); + + // don't allow publishing faster than SENS_FLOW_RATE + if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) { + publish = false; + } + + // integrate for full interval unless we haven't published recently + if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) { + publish = false; + } + } + + if (publish) { + vehicle_optical_flow_s vehicle_optical_flow{}; + + vehicle_optical_flow.timestamp_sample = sensor_optical_flow.timestamp_sample; + vehicle_optical_flow.device_id = sensor_optical_flow.device_id; + + _flow_integral.copyTo(vehicle_optical_flow.pixel_flow); + _delta_angle.copyTo(vehicle_optical_flow.delta_angle); + + vehicle_optical_flow.integration_timespan_us = _integration_timespan_us; + + vehicle_optical_flow.quality = _quality_sum / _accumulated_count; + + if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { + vehicle_optical_flow.distance_m = _distance_sum / _distance_sum_count; + + } else { + vehicle_optical_flow.distance_m = NAN; + } + + // SENS_FLOW_MAXR + if (PX4_ISFINITE(sensor_optical_flow.max_flow_rate) + && (sensor_optical_flow.max_flow_rate <= _param_sens_flow_maxr.get())) { + + vehicle_optical_flow.max_flow_rate = sensor_optical_flow.max_flow_rate; + + } else { + vehicle_optical_flow.max_flow_rate = _param_sens_flow_maxr.get(); + } + + // SENS_FLOW_MINHGT + if (PX4_ISFINITE(sensor_optical_flow.min_ground_distance) + && (sensor_optical_flow.min_ground_distance >= _param_sens_flow_minhgt.get())) { + + vehicle_optical_flow.min_ground_distance = sensor_optical_flow.min_ground_distance; + + } else { + vehicle_optical_flow.min_ground_distance = _param_sens_flow_minhgt.get(); + } + + // SENS_FLOW_MAXHGT + if (PX4_ISFINITE(sensor_optical_flow.max_ground_distance) + && (sensor_optical_flow.max_ground_distance <= _param_sens_flow_maxhgt.get())) { + + vehicle_optical_flow.max_ground_distance = sensor_optical_flow.max_ground_distance; + + } else { + vehicle_optical_flow.max_ground_distance = _param_sens_flow_maxhgt.get(); + } + + + // rotate (SENS_FLOW_ROT) + float zeroval = 0.f; + rotate_3f((enum Rotation)_param_sens_flow_rot.get(), vehicle_optical_flow.pixel_flow[0], + vehicle_optical_flow.pixel_flow[1], zeroval); + + vehicle_optical_flow.timestamp = hrt_absolute_time(); + _vehicle_optical_flow_pub.publish(vehicle_optical_flow); + _last_publication_timestamp = vehicle_optical_flow.timestamp_sample; + + + // vehicle_optical_flow_vel if distance is available (for logging) + if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { + const float range = _distance_sum / _distance_sum_count; + + vehicle_optical_flow_vel_s flow_vel{}; + + flow_vel.timestamp_sample = vehicle_optical_flow.timestamp_sample; + + // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate + // is produced by a RH rotation of the image about the sensor axis. + const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]}; + const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]}; + + const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us; + + // compensate for body motion to give a LOS rate + const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy(); + + Vector3f vel_optflow_body; + vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt; + vel_optflow_body(1) = range * flow_compensated_XY_rad(0) / flow_dt; + vel_optflow_body(2) = 0.f; + + // vel_body + flow_vel.vel_body[0] = vel_optflow_body(0); + flow_vel.vel_body[1] = vel_optflow_body(1); + + // vel_ne + flow_vel.vel_ne[0] = NAN; + flow_vel.vel_ne[1] = NAN; + + vehicle_attitude_s vehicle_attitude{}; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + const matrix::Dcmf R_to_earth = matrix::Quatf(vehicle_attitude.q); + const Vector3f flow_vel_ne = R_to_earth * vel_optflow_body; + + flow_vel.vel_ne[0] = flow_vel_ne(0); + flow_vel.vel_ne[1] = flow_vel_ne(1); + } + + // flow_uncompensated_integral + flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral); + + // flow_compensated_integral + flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral); + + const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt)); + + // gyro_rate + flow_vel.gyro_rate[0] = measured_body_rate(0); + flow_vel.gyro_rate[1] = measured_body_rate(1); + + // gyro_rate_integral + flow_vel.gyro_rate_integral[0] = gyro_xyz(0); + flow_vel.gyro_rate_integral[1] = gyro_xyz(1); + + flow_vel.timestamp = hrt_absolute_time(); + + _vehicle_optical_flow_vel_pub.publish(flow_vel); + } + + ClearAccumulatedData(); + } + } + + // reschedule backup + ScheduleDelayed(10_ms); + + perf_end(_cycle_perf); +} + +void VehicleOpticalFlow::UpdateDistanceSensor() +{ + // update range finder buffer + distance_sensor_s distance_sensor; + + if ((_distance_sensor_selected < 0) && _distance_sensor_subs.advertised()) { + for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { + + if (_distance_sensor_subs[i].update(&distance_sensor)) { + // only use the first instace which has the correct orientation + if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms) + && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { + + int ndist = orb_group_count(ORB_ID(distance_sensor)); + + if (ndist > 1) { + PX4_INFO("selected distance_sensor:%d (%d advertised)", i, ndist); + } + + _distance_sensor_selected = i; + _last_range_sensor_update = distance_sensor.timestamp; + break; + } + } + } + } + + if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) { + // range sample + if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { + + if ((distance_sensor.current_distance >= distance_sensor.min_distance) + && (distance_sensor.current_distance <= distance_sensor.max_distance)) { + + rangeSample sample; + sample.time_us = distance_sensor.timestamp; + sample.data = distance_sensor.current_distance; + + _range_buffer.push(sample); + + _last_range_sensor_update = distance_sensor.timestamp; + + return; + } + + } else { + _distance_sensor_selected = -1; + } + } + + if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) { + _distance_sensor_selected = -1; + } +} + +void VehicleOpticalFlow::UpdateSensorGyro() +{ + if (_sensor_selection_sub.updated()) { + sensor_selection_s sensor_selection{}; + _sensor_selection_sub.copy(&sensor_selection); + + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + + if (sensor_gyro_sub.advertised() + && (sensor_gyro_sub.get().timestamp != 0) + && (sensor_gyro_sub.get().device_id != 0) + && (hrt_elapsed_time(&sensor_gyro_sub.get().timestamp) < 1_s)) { + + if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) { + if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) { + + _gyro_calibration.set_device_id(sensor_gyro_sub.get().device_id); + PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); + break; + + } else { + PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); + } + } + } + } + } + + // buffer + while (_sensor_gyro_sub.updated()) { + const unsigned last_generation = _sensor_gyro_sub.get_last_generation(); + + sensor_gyro_s sensor_gyro; + + if (_sensor_gyro_sub.copy(&sensor_gyro)) { + + if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation()); + } + + _gyro_calibration.set_device_id(sensor_gyro.device_id); + _gyro_calibration.SensorCorrectionsUpdate(); + + const float dt_s = (sensor_gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; + _gyro_timestamp_sample_last = sensor_gyro.timestamp_sample; + + gyroSample gyro_sample; + gyro_sample.time_us = sensor_gyro.timestamp_sample; + gyro_sample.data = _gyro_calibration.Correct(Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z}); + gyro_sample.dt = dt_s; + + _gyro_buffer.push(gyro_sample); + } + } +} + +void VehicleOpticalFlow::ClearAccumulatedData() +{ + // clear accumulated data + _flow_integral.zero(); + _integration_timespan_us = 0; + + _delta_angle.zero(); + + _distance_sum = NAN; + _distance_sum_count = 0; + + _quality_sum = 0; + _accumulated_count = 0; + + _gyro_integrator.reset(); +} + +void VehicleOpticalFlow::PrintStatus() +{ + +} + +}; // namespace sensors diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp new file mode 100644 index 0000000000..611cf7c2e8 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "data_validator/DataValidatorGroup.hpp" +#include "RingBuffer.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sensors +{ + +class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + VehicleOpticalFlow(); + ~VehicleOpticalFlow() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void ClearAccumulatedData(); + void UpdateDistanceSensor(); + void UpdateSensorGyro(); + + void Run() override; + + void ParametersUpdate(); + void SensorCorrectionsUpdate(bool force = false); + + static constexpr int MAX_SENSOR_COUNT = 3; + + uORB::Publication _vehicle_optical_flow_pub{ORB_ID(vehicle_optical_flow)}; + uORB::Publication _vehicle_optical_flow_vel_pub{ORB_ID(vehicle_optical_flow_vel)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + + uORB::SubscriptionCallbackWorkItem _sensor_flow_sub{this, ORB_ID(sensor_optical_flow)}; + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)}; + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; + + sensors::IntegratorConing _gyro_integrator{}; + + hrt_abstime _gyro_timestamp_sample_last{0}; + + calibration::Gyroscope _gyro_calibration{}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + matrix::Dcmf _flow_rotation{matrix::eye()}; + + hrt_abstime _flow_timestamp_sample_last{0}; + matrix::Vector2f _flow_integral{}; + matrix::Vector3f _delta_angle{}; + uint32_t _integration_timespan_us{}; + float _distance_sum{NAN}; + uint8_t _distance_sum_count{0}; + uint16_t _quality_sum{0}; + uint8_t _accumulated_count{0}; + + hrt_abstime _last_publication_timestamp{0}; + + int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations + hrt_abstime _last_range_sensor_update{0}; + + struct gyroSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + matrix::Vector3f data{}; + float dt{0.f}; + }; + + struct rangeSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + float data{}; + }; + + RingBuffer _gyro_buffer{}; + RingBuffer _range_buffer{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_flow_rot, + (ParamFloat) _param_sens_flow_minhgt, + (ParamFloat) _param_sens_flow_maxhgt, + (ParamFloat) _param_sens_flow_maxr, + (ParamFloat) _param_sens_flow_rate + ) +}; +}; // namespace sensors diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 0dd34d2fc4..4c0964f1c3 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -64,10 +64,10 @@ #include #include #include -#include #include #include #include +#include #include #include #include @@ -158,7 +158,6 @@ private: void check_failure_injections(); - int publish_flow_topic(const mavlink_hil_optical_flow_t *flow); int publish_odometry_topic(const mavlink_message_t *odom_mavlink); int publish_distance_topic(const mavlink_distance_sensor_t *dist); @@ -191,7 +190,7 @@ private: // uORB publisher handlers uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; - uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 09b8925092..981c8efb18 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -665,7 +665,45 @@ void Simulator::handle_message_optical_flow(const mavlink_message_t *msg) { mavlink_hil_optical_flow_t flow; mavlink_msg_hil_optical_flow_decode(msg, &flow); - publish_flow_topic(&flow); + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = 0; + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; + + sensor_optical_flow_s sensor_optical_flow{}; + + sensor_optical_flow.timestamp_sample = hrt_absolute_time(); + sensor_optical_flow.device_id = device_id.devid; + + sensor_optical_flow.pixel_flow[0] = flow.integrated_x; + sensor_optical_flow.pixel_flow[1] = flow.integrated_y; + + sensor_optical_flow.integration_timespan_us = flow.integration_time_us; + sensor_optical_flow.quality = flow.quality; + + if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { + sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; + sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; + sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + sensor_optical_flow.delta_angle_available = true; + } + + sensor_optical_flow.max_flow_rate = NAN; + sensor_optical_flow.min_ground_distance = NAN; + sensor_optical_flow.max_ground_distance = NAN; + + // Use distance value for distance sensor topic + if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) { + // Positive value (including zero): distance known. Negative value: Unknown distance. + sensor_optical_flow.distance_m = flow.distance; + sensor_optical_flow.distance_available = true; + } + + sensor_optical_flow.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(sensor_optical_flow); } void Simulator::handle_message_rc_channels(const mavlink_message_t *msg) @@ -1309,50 +1347,6 @@ void Simulator::check_failure_injections() } } -int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink) -{ - optical_flow_s flow = {}; - flow.sensor_id = flow_mavlink->sensor_id; - flow.timestamp = hrt_absolute_time(); - flow.time_since_last_sonar_update = 0; - flow.frame_count_since_last_readout = 0; // ? - flow.integration_timespan = flow_mavlink->integration_time_us; - - flow.ground_distance_m = flow_mavlink->distance; - flow.gyro_temperature = flow_mavlink->temperature; - flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro; - flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro; - flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro; - flow.pixel_flow_x_integral = flow_mavlink->integrated_x; - flow.pixel_flow_y_integral = flow_mavlink->integrated_y; - flow.quality = flow_mavlink->quality; - - /* fill in sensor limits */ - float flow_rate_max; - param_get(param_find("SENS_FLOW_MAXR"), &flow_rate_max); - float flow_min_hgt; - param_get(param_find("SENS_FLOW_MINHGT"), &flow_min_hgt); - float flow_max_hgt; - param_get(param_find("SENS_FLOW_MAXHGT"), &flow_max_hgt); - - flow.max_flow_rate = flow_rate_max; - flow.min_ground_distance = flow_min_hgt; - flow.max_ground_distance = flow_max_hgt; - - /* rotate measurements according to parameter */ - int32_t flow_rot_int; - param_get(param_find("SENS_FLOW_ROT"), &flow_rot_int); - const enum Rotation flow_rot = (Rotation)flow_rot_int; - - float zeroval = 0.0f; - rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval); - rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral); - - _flow_pub.publish(flow); - - return PX4_OK; -} - int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) { uint64_t timestamp = hrt_absolute_time();