fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

optical flow sensor pipeline overhaul

Browse Source 
- 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 <alex@arkelectron.com>
This commit is contained in:
Daniel Agar 2022-06-17 16:02:09 -04:00
parent 32544452f0
commit d5839e2dd5
49 changed files with 1415 additions and 634 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup
View File

@ -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

1
ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
View File

@ -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

2
ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
View File

@ -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"

27
msg/CMakeLists.txt
View File

@ -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

8
msg/estimator_optical_flow_vel.msg
View File

@ -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)

29
msg/optical_flow.msg
View File

@ -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

30
msg/sensor_optical_flow.msg Normal file
View File

@ -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

2
msg/tools/urtps_bridge_topics.yaml
View File

@ -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

21
msg/vehicle_optical_flow.msg Normal file
View File

@ -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

13
msg/vehicle_optical_flow_vel.msg Normal file
View File

@ -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

13
src/drivers/drv_sensor.h
View File

@ -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

27
src/drivers/optical_flow/paa3905/PAA3905.cpp
View File

@ -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;

4
src/drivers/optical_flow/paa3905/PAA3905.hpp
View File

@ -51,7 +51,7 @@
#include <lib/parameters/param.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_optical_flow.h>
using namespace time_literals;
using namespace PixArt_PAA3905;
@ -99,7 +99,7 @@ private:
void ResetAccumulatedData();
uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
uORB::PublicationMulti<sensor_optical_flow_s> _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")};

27
src/drivers/optical_flow/paw3902/PAW3902.cpp
View File

@ -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;

4
src/drivers/optical_flow/paw3902/PAW3902.hpp
View File

@ -51,7 +51,7 @@
#include <lib/parameters/param.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_optical_flow.h>
using namespace time_literals;
using namespace PixArt_PAW3902JF;
@ -97,7 +97,7 @@ private:
void ResetAccumulatedData();
uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
uORB::PublicationMulti<sensor_optical_flow_s> _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")};

37
src/drivers/optical_flow/pmw3901/PMW3901.cpp
View File

@ -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<float>(delta_x);
report.pixel_flow_y_integral = static_cast<float>(delta_y);
report.pixel_flow[0] = static_cast<float>(delta_x);
report.pixel_flow[1] = static_cast<float>(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);
}

7
src/drivers/optical_flow/pmw3901/PMW3901.hpp
View File

@ -45,10 +45,9 @@
#include <drivers/device/spi.h>
#include <conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <lib/parameters/param.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_optical_flow.h>
#include <px4_platform_common/i2c_spi_buses.h>
/* 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_s> _optical_flow_pub{ORB_ID(optical_flow)};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
uint64_t _previous_collect_timestamp{0};
enum Rotation _yaw_rotation;
enum Rotation _yaw_rotation {};
int _flow_sum_x{0};
int _flow_sum_y{0};

159
src/drivers/optical_flow/px4flow/px4flow.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -41,8 +41,6 @@
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <lib/conversion/rotation.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
@ -50,8 +48,7 @@
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_optical_flow.h>
/* Configuration Constants */
#define I2C_FLOW_ADDRESS_DEFAULT 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
@ -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<optical_flow_s> _px4flow_topic{ORB_ID(optical_flow)};
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_topic{ORB_ID(distance_sensor)};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
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<float>(_frame_integral.pixel_flow_x_integral) / 10000.f; //convert to radians
report.pixel_flow[1] = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.f; //convert to radians
report.delta_angle_available = true;
report.delta_angle[0] = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians
report.delta_angle[1] = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians
report.delta_angle[2] = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians
report.distance_m = static_cast<float>(_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<float>(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = _frame_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = _frame_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds
report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
report.max_flow_rate = _sensor_max_flow_rate;
report.min_ground_distance = _sensor_min_range;
report.max_ground_distance = _sensor_max_range;
/* 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);
}

127
src/drivers/optical_flow/thoneflow/thoneflow.cpp
View File

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

6
src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp
View File

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

4
src/drivers/optical_flow/thoneflow/thoneflow_parser.h
View File

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

44
src/drivers/uavcan/sensors/flow.cpp
View File

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

2
src/drivers/uavcan/sensors/flow.hpp
View File

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

27
src/drivers/uavcannode/Publishers/FlowMeasurement.hpp
View File

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

21
src/drivers/uavcannode/uavcannode_params.c
View File

@ -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
*

5
src/modules/ekf2/EKF/ekf.h
View File

@ -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; }

44
src/modules/ekf2/EKF2.cpp
View File

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

12
src/modules/ekf2/EKF2.hpp
View File

@ -71,13 +71,11 @@
#include <uORB/topics/estimator_event_flags.h>
#include <uORB/topics/estimator_gps_status.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_optical_flow_vel.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_states.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/estimator_status_flags.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_selection.h>
@ -91,11 +89,12 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_optical_flow.h>
#include <uORB/topics/vehicle_optical_flow_vel.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind.h>
#include <uORB/topics/yaw_estimator_status.h>
extern pthread_mutex_t ekf2_module_mutex;
class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
@ -276,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_s> _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_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};

4
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

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

12
src/modules/local_position_estimator/sensors/flow.cpp
View File

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

5
src/modules/logger/logged_topics.cpp
View File

@ -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;

183
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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<float>(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)

19
src/modules/mavlink/mavlink_receiver.h
View File

@ -86,13 +86,13 @@
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/onboard_computer_status.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/ping.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_optical_flow.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/trajectory_setpoint.h>
@ -308,7 +308,6 @@ private:
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
@ -331,13 +330,13 @@ private:
// ORB publications (multi)
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping)};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
// ORB publications (queue length > 1)
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
@ -396,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<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,

40
src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp
View File

@ -34,7 +34,7 @@
#ifndef OPTICAL_FLOW_RAD_HPP
#define OPTICAL_FLOW_RAD_HPP
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/vehicle_optical_flow.h>
class MavlinkStreamOpticalFlowRad : public MavlinkStream
{
@ -49,34 +49,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);

6
src/modules/replay/ReplayEkf2.cpp
View File

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

11
src/modules/sensors/CMakeLists.txt
View File

@ -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()

11
src/modules/sensors/vehicle_imu/Integrator.hpp → src/modules/sensors/Integrator.hpp
View File

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

4
src/modules/sensors/Kconfig
View File

@ -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

35
src/modules/sensors/sensor_params_flow.c
View File

@ -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);

59
src/modules/sensors/sensors.cpp
View File

@ -93,6 +93,10 @@
# include <uORB/topics/sensor_mag.h>
#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
# include "vehicle_optical_flow/VehicleOpticalFlow.hpp"
#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
using namespace sensors;
using namespace time_literals;
@ -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();

1
src/modules/sensors/vehicle_imu/CMakeLists.txt
View File

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

6
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

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

11
src/drivers/optical_flow/CMakeLists.txt → src/modules/sensors/vehicle_optical_flow/CMakeLists.txt
View File

@ -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)

217
src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp Normal file
View File

@ -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 <inttypes.h>
#include <cstdio>
#include <cstring>
template <typename T, size_t SIZE>
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 &timestamp, 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 &timestamp, 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 &timestamp_oldest, const uint64_t &timestamp_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};
};

481
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp Normal file
View File

@ -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 <px4_platform_common/log.h>
namespace sensors
{
using namespace matrix;
using namespace time_literals;
static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
VehicleOpticalFlow::VehicleOpticalFlow() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_vehicle_optical_flow_pub.advertise();
_gyro_integrator.set_reset_samples(1);
}
VehicleOpticalFlow::~VehicleOpticalFlow()
{
Stop();
perf_free(_cycle_perf);
}
bool VehicleOpticalFlow::Start()
{
_sensor_flow_sub.registerCallback();
_sensor_gyro_sub.registerCallback();
_sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2);
_sensor_selection_sub.registerCallback();
ScheduleNow();
return true;
}
void VehicleOpticalFlow::Stop()
{
Deinit();
// clear all registered callbacks
_sensor_flow_sub.unregisterCallback();
_sensor_gyro_sub.unregisterCallback();
_sensor_selection_sub.unregisterCallback();
}
void VehicleOpticalFlow::ParametersUpdate()
{
// Check if parameters have changed
if (_params_sub.updated()) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
updateParams();
_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_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
if (sensor_gyro_sub.advertised()
&& (sensor_gyro_sub.get().timestamp != 0)
&& (sensor_gyro_sub.get().device_id != 0)
&& (hrt_elapsed_time(&sensor_gyro_sub.get().timestamp) < 1_s)) {
if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) {
if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) {
_gyro_calibration.set_device_id(sensor_gyro_sub.get().device_id);
PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
break;
} else {
PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
}
}
}
}
}
// 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

147
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp Normal file
View File

@ -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 <Integrator.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_optical_flow.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_optical_flow.h>
#include <uORB/topics/vehicle_optical_flow_vel.h>
namespace sensors
{
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_s> _vehicle_optical_flow_pub{ORB_ID(vehicle_optical_flow)};
uORB::Publication<vehicle_optical_flow_vel_s> _vehicle_optical_flow_vel_pub{ORB_ID(vehicle_optical_flow_vel)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::SubscriptionMultiArray<distance_sensor_s> _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<float, 3>()};
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<gyroSample, 32> _gyro_buffer{};
RingBuffer<rangeSample, 5> _range_buffer{};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_FLOW_ROT>) _param_sens_flow_rot,
(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
(ParamFloat<px4::params::SENS_FLOW_MAXR>) _param_sens_flow_maxr,
(ParamFloat<px4::params::SENS_FLOW_RATE>) _param_sens_flow_rate
)
};
}; // namespace sensors

5
src/modules/simulator/simulator.h
View File

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

84
src/modules/simulator/simulator_mavlink.cpp
View File

@ -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();