fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 04:31:35 +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:15:55 -04:00
co-authored by alexklimaj
parent 2777f0a19b
commit 1f923f2cda
47 changed files with 1230 additions and 511 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup
-1
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
ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
-1
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
ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
-1
View File
@@ -174,7 +174,6 @@ param set-default RC1_TRIM 1000
param set-default SENS_FLOW_MAXR 7.4
param set-default SENS_FLOW_MINHGT 0.15
param set-default SENS_FLOW_MAXHGT 5.0
param set-default SENS_FLOW_ROT 0
# ignore the SD card errors and use normal startup sound
set STARTUP_TUNE "1"
msg/CMakeLists.txt
+14 -13
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
msg/estimator_optical_flow_vel.msg
-8
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)
msg/optical_flow.msg
-29
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
msg/sensor_optical_flow.msg
+27
View File
@@ -0,0 +1,27 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis
float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
bool delta_angle_available
uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
uint8 quality # quality, 0: bad quality, 255: maximum quality
uint32 error_count
float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably
uint8 MODE_UNKNOWN = 0
uint8 MODE_BRIGHT = 1
uint8 MODE_LOWLIGHT = 2
uint8 MODE_SUPER_LOWLIGHT = 3
uint8 mode
msg/tools/urtps_bridge_topics.yaml
+1 -1
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
msg/vehicle_optical_flow.msg
+19
View File
@@ -0,0 +1,19 @@
# Optical flow in XYZ body frame in SI units.
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32[2] pixel_flow # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis
float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably
msg/vehicle_optical_flow_vel.msg
+13
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
src/drivers/optical_flow/paa3905/PAA3905.cpp
+13 -14
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;
src/drivers/optical_flow/paa3905/PAA3905.hpp
+2 -2
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")};
src/drivers/optical_flow/paw3902/PAW3902.cpp
+13 -14
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;
src/drivers/optical_flow/paw3902/PAW3902.hpp
+2 -2
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")};
src/drivers/optical_flow/pmw3901/PMW3901.cpp
+11 -14
View File
@@ -326,28 +326,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
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 +356,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);
}
src/drivers/optical_flow/pmw3901/PMW3901.hpp
+2 -2
View File
@@ -48,7 +48,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>
#include <px4_platform_common/i2c_spi_buses.h>
/* Configuration Constants */
@@ -84,7 +84,7 @@ 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;
src/drivers/optical_flow/px4flow/px4flow.cpp
+39 -75
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>
@@ -51,7 +49,7 @@
#include <px4_platform_common/module.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_optical_flow.h>
/* Configuration Constants */
#define I2C_FLOW_ADDRESS_DEFAULT 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
@@ -98,8 +96,8 @@ private:
bool _sensor_ok{false};
bool _collect_phase{false};
uORB::PublicationMulti<optical_flow_s> _px4flow_topic{ORB_ID(optical_flow)};
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_topic{ORB_ID(distance_sensor)};
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_topic{ORB_ID(distance_sensor)};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@@ -166,44 +164,6 @@ PX4FLOW::init()
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
/* get yaw rotation from sensor frame to body frame */
param_t rot = param_find("SENS_FLOW_ROT");
if (rot != PARAM_INVALID) {
int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward
param_get(rot, &val);
_sensor_rotation = (enum Rotation)val;
}
/* get operational limits of the sensor */
param_t hmin = param_find("SENS_FLOW_MINHGT");
if (hmin != PARAM_INVALID) {
float val = 0.7;
param_get(hmin, &val);
_sensor_min_range = val;
}
param_t hmax = param_find("SENS_FLOW_MAXHGT");
if (hmax != PARAM_INVALID) {
float val = 3.0;
param_get(hmax, &val);
_sensor_max_range = val;
}
param_t ratemax = param_find("SENS_FLOW_MAXR");
if (ratemax != PARAM_INVALID) {
float val = 2.5;
param_get(ratemax, &val);
_sensor_max_flow_rate = val;
}
start();
return ret;
@@ -279,49 +239,55 @@ PX4FLOW::collect()
}
optical_flow_s report{};
DeviceId device_id;
device_id.devid = get_device_id();
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
device_id.devid_s.address = get_i2c_address();
report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = _frame_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = _frame_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds
report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
report.max_flow_rate = _sensor_max_flow_rate;
report.min_ground_distance = _sensor_min_range;
report.max_ground_distance = _sensor_max_range;
sensor_optical_flow_s report{};
report.timestamp_sample = hrt_absolute_time();
report.device_id = device_id.devid;
report.pixel_flow[0] = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.f; //convert to radians
report.pixel_flow[1] = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.f; //convert to radians
// report.ground_distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.f; //convert to meters
report.integration_timespan_us = _frame_integral.integration_timespan; // microseconds
report.quality = _frame_integral.qual; // 0:bad ; 255 max quality
report.delta_angle_available = true;
report.delta_angle[0] = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians
report.delta_angle[1] = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians
report.delta_angle[2] = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians
/* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */
float zeroval = 0.0f;
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
rotate_3f(_sensor_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
rotate_3f(_sensor_rotation, report.delta_angle[0], report.delta_angle[1], report.delta_angle[2]);
_px4flow_topic.publish(report);
report.max_flow_rate = 2.5f;
report.min_ground_distance = 0.7f;
report.max_ground_distance = 3.f;
report.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(report);
/* publish to the distance_sensor topic as well */
if (_distance_sensor_topic.get_instance() == 0) {
distance_sensor_s distance_report{};
DeviceId device_id;
device_id.devid = get_device_id();
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
distance_report.timestamp = report.timestamp;
distance_report.device_id = device_id.devid;
distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
distance_report.current_distance = report.ground_distance_m;
distance_report.current_distance = static_cast<float>(_frame_integral.ground_distance) / 1000.f; // convert to meters
distance_report.variance = 0.0f;
distance_report.signal_quality = -1;
distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
distance_report.device_id = device_id.devid;
distance_report.orientation = _sonar_rotation;
distance_report.timestamp = hrt_absolute_time();
_distance_sensor_topic.publish(distance_report);
}
@@ -415,13 +381,11 @@ px4flow_main(int argc, char *argv[])
}
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
} else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
} else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
src/drivers/optical_flow/thoneflow/thoneflow.cpp
+40 -87
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;
}
src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp
+3 -3
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 {
src/drivers/optical_flow/thoneflow/thoneflow_parser.h
+2 -2
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);
src/drivers/uavcan/sensors/flow.cpp
+31 -13
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);
}
src/drivers/uavcan/sensors/flow.hpp
+1 -1
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>
src/drivers/uavcannode/Publishers/FlowMeasurement.hpp
+8 -19
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;
src/drivers/uavcannode/uavcannode_params.c
-21
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
*
src/modules/ekf2/EKF/ekf.h
+4 -1
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; }
src/modules/ekf2/EKF2.cpp
+16 -12
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
src/modules/ekf2/EKF2.hpp
+4 -5
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,12 +275,12 @@ private:
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
@@ -314,12 +313,12 @@ private:
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+2 -2
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)};
src/modules/local_position_estimator/sensors/flow.cpp
+5 -7
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",
src/modules/logger/logged_topics.cpp
+5 -2
View File
@@ -162,7 +162,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_optical_flow_vel", 0, MAX_ESTIMATOR_INSTANCES); // TODO: restore
add_optional_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES);
@@ -174,7 +174,6 @@ void LoggedTopics::add_default_topics()
add_topic_multi("battery_status", 200, 2);
add_topic_multi("differential_pressure", 1000, 2);
add_topic_multi("distance_sensor", 1000, 2);
add_topic_multi("optical_flow", 1000, 1);
add_optional_topic_multi("sensor_accel", 1000, 4);
add_optional_topic_multi("sensor_baro", 1000, 4);
add_topic_multi("sensor_gps", 1000, 2);
@@ -182,9 +181,13 @@ void LoggedTopics::add_default_topics()
add_optional_topic("pps_capture", 1000);
add_optional_topic_multi("sensor_gyro", 1000, 4);
add_optional_topic_multi("sensor_mag", 1000, 4);
add_topic_multi("sensor_optical_flow", 0, 1); // TODO: 0 -> 1000 and optional
add_topic_multi("vehicle_imu", 500, 4);
add_topic_multi("vehicle_imu_status", 1000, 4);
add_optional_topic_multi("vehicle_magnetometer", 500, 4);
add_topic_multi("vehicle_optical_flow", 0, 1); // TODO: 0 -> 1000 and optional
add_topic_multi("vehicle_optical_flow_vel", 0, 1); // // TODO: 0 -> 1000 and optional
// SYS_CTRL_ALLOC: additional dynamic control allocation logging when enabled
int32_t sys_ctrl_alloc = 0;
src/modules/mavlink/mavlink_receiver.cpp
+63 -73
View File
@@ -89,10 +89,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_parameters_manager(parent),
_mavlink_timesync(parent)
{
_handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT");
_handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR");
_handle_sens_flow_minhgt = param_find("SENS_FLOW_MINHGT");
_handle_sens_flow_rot = param_find("SENS_FLOW_ROT");
_handle_ekf2_min_rng = param_find("EKF2_MIN_RNG");
_handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX");
}
@@ -799,45 +795,43 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
mavlink_optical_flow_rad_t flow;
mavlink_msg_optical_flow_rad_decode(msg, &flow);
optical_flow_s f{};
device::Device::DeviceId device_id;
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
device_id.devid_s.address = msg->sysid;
sensor_optical_flow_s f{};
f.timestamp_sample = hrt_absolute_time();
f.device_id = device_id.devid;
f.pixel_flow[0] = flow.integrated_x;
f.pixel_flow[1] = flow.integrated_y;
f.integration_timespan_us = flow.integration_time_us;
f.quality = flow.quality;
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
f.delta_angle[0] = flow.integrated_xgyro;
f.delta_angle[1] = flow.integrated_ygyro;
f.delta_angle[2] = flow.integrated_zgyro;
f.delta_angle_available = true;
}
//f.ground_distance_m = flow.distance;
f.max_flow_rate = NAN;
f.min_ground_distance = NAN;
f.max_ground_distance = NAN;
f.timestamp = hrt_absolute_time();
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;
f.pixel_flow_y_integral = flow.integrated_y;
f.gyro_x_rate_integral = flow.integrated_xgyro;
f.gyro_y_rate_integral = flow.integrated_ygyro;
f.gyro_z_rate_integral = flow.integrated_zgyro;
f.gyro_temperature = flow.temperature;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
f.max_flow_rate = _param_sens_flow_maxr;
f.min_ground_distance = _param_sens_flow_minhgt;
f.max_ground_distance = _param_sens_flow_maxhgt;
/* read flow sensor parameters */
const Rotation flow_rot = (Rotation)_param_sens_flow_rot;
/* rotate measurements according to parameter */
float zero_val = 0.0f;
rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val);
rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);
_flow_pub.publish(f);
_sensor_optical_flow_pub.publish(f);
/* Use distance value for distance sensor topic */
if (flow.distance > 0.0f) { // negative values signal invalid data
if (PX4_ISFINITE(flow.distance) && (flow.distance > 0.0f)) { // negative values signal invalid data
distance_sensor_s d{};
device::Device::DeviceId device_id;
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
device_id.devid_s.address = msg->sysid;
d.timestamp = f.timestamp;
d.min_distance = _param_ekf2_min_rng;
d.max_distance = _param_ekf2_rng_a_hmax;
d.current_distance = flow.distance; /* both are in m */
@@ -846,6 +840,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
d.variance = 0.01;
d.signal_quality = -1;
d.timestamp = f.timestamp;
_flow_distance_sensor_pub.publish(d);
}
@@ -858,32 +853,42 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
mavlink_hil_optical_flow_t flow;
mavlink_msg_hil_optical_flow_decode(msg, &flow);
optical_flow_s f{};
f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;
f.pixel_flow_y_integral = flow.integrated_y;
f.gyro_x_rate_integral = flow.integrated_xgyro;
f.gyro_y_rate_integral = flow.integrated_ygyro;
f.gyro_z_rate_integral = flow.integrated_zgyro;
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
f.gyro_temperature = flow.temperature;
_flow_pub.publish(f);
/* Use distance value for distance sensor topic */
distance_sensor_s d{};
device::Device::DeviceId device_id;
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
device_id.devid_s.address = msg->sysid;
d.timestamp = hrt_absolute_time();
sensor_optical_flow_s f{};
f.timestamp_sample = hrt_absolute_time();
f.device_id = device_id.devid;
f.pixel_flow[0] = flow.integrated_x;
f.pixel_flow[1] = flow.integrated_y;
f.integration_timespan_us = flow.integration_time_us;
f.quality = flow.quality;
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
f.delta_angle[0] = flow.integrated_xgyro;
f.delta_angle[1] = flow.integrated_ygyro;
f.delta_angle[2] = flow.integrated_zgyro;
f.delta_angle_available = true;
}
//f.ground_distance_m = flow.distance;
f.max_flow_rate = NAN;
f.min_ground_distance = NAN;
f.max_ground_distance = NAN;
f.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(f);
/* Use distance value for distance sensor topic */
distance_sensor_s d{};
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
@@ -892,6 +897,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
d.variance = 0.01;
d.signal_quality = -1;
d.timestamp = hrt_absolute_time();
_flow_distance_sensor_pub.publish(d);
}
@@ -3476,22 +3482,6 @@ MavlinkReceiver::updateParams()
// update parameters from storage
ModuleParams::updateParams();
if (_handle_sens_flow_maxhgt != PARAM_INVALID) {
param_get(_handle_sens_flow_maxhgt, &_param_sens_flow_maxhgt);
}
if (_handle_sens_flow_maxr != PARAM_INVALID) {
param_get(_handle_sens_flow_maxr, &_param_sens_flow_maxr);
}
if (_handle_sens_flow_minhgt != PARAM_INVALID) {
param_get(_handle_sens_flow_minhgt, &_param_sens_flow_minhgt);
}
if (_handle_sens_flow_rot != PARAM_INVALID) {
param_get(_handle_sens_flow_rot, &_param_sens_flow_rot);
}
if (_handle_ekf2_min_rng != PARAM_INVALID) {
param_get(_handle_ekf2_min_rng, &_param_ekf2_min_rng);
}
src/modules/mavlink/mavlink_receiver.h
+2 -10
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)};
@@ -338,6 +337,7 @@ private:
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
// ORB publications (queue length > 1)
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
@@ -396,17 +396,9 @@ private:
hrt_abstime _heartbeat_component_udp_bridge{0};
hrt_abstime _heartbeat_component_uart_bridge{0};
param_t _handle_sens_flow_maxhgt{PARAM_INVALID};
param_t _handle_sens_flow_maxr{PARAM_INVALID};
param_t _handle_sens_flow_minhgt{PARAM_INVALID};
param_t _handle_sens_flow_rot{PARAM_INVALID};
param_t _handle_ekf2_min_rng{PARAM_INVALID};
param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID};
float _param_sens_flow_maxhgt{-1.0f};
float _param_sens_flow_maxr{-1.0f};
float _param_sens_flow_minhgt{-1.0f};
int32_t _param_sens_flow_rot{0};
float _param_ekf2_min_rng{NAN};
float _param_ekf2_rng_a_hmax{NAN};
src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp
+16 -16
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,34 @@ public:
unsigned get_size() override
{
return _optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _vehicle_optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) :
0;
}
private:
explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
bool send() override
{
optical_flow_s flow;
vehicle_optical_flow_s flow;
if (_optical_flow_sub.update(&flow)) {
if (_vehicle_optical_flow_sub.update(&flow)) {
mavlink_optical_flow_rad_t msg{};
msg.time_usec = flow.timestamp;
msg.sensor_id = flow.sensor_id;
msg.integrated_x = flow.pixel_flow_x_integral;
msg.integrated_y = flow.pixel_flow_y_integral;
msg.integrated_xgyro = flow.gyro_x_rate_integral;
msg.integrated_ygyro = flow.gyro_y_rate_integral;
msg.integrated_zgyro = flow.gyro_z_rate_integral;
msg.distance = flow.ground_distance_m;
msg.sensor_id = _vehicle_optical_flow_sub.get_instance();
msg.integrated_x = flow.pixel_flow[0];
msg.integrated_y = flow.pixel_flow[1];
msg.integrated_xgyro = flow.delta_angle[0];
msg.integrated_ygyro = flow.delta_angle[1];
msg.integrated_zgyro = flow.delta_angle[2];
//msg.distance = flow.ground_distance_m;
msg.quality = flow.quality;
msg.integration_time_us = flow.integration_timespan;
msg.sensor_id = flow.sensor_id;
msg.time_delta_distance_us = flow.time_since_last_sonar_update;
msg.temperature = flow.gyro_temperature;
msg.integration_time_us = flow.integration_timespan_us;
//msg.time_delta_distance_us = flow.time_since_last_sonar_update;
//msg.temperature = flow.gyro_temperature;
mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
src/modules/replay/ReplayEkf2.cpp
+3 -3
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");
src/modules/sensors/CMakeLists.txt
+11
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()
src/modules/sensors/vehicle_imu/Integrator.hpp → src/modules/sensors/Integrator.hpp
+9 -2
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
src/modules/sensors/Kconfig
+4
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
src/modules/sensors/sensor_params_flow.c
+24 -11
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);
src/modules/sensors/sensors.cpp
+59
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();
src/modules/sensors/vehicle_imu/CMakeLists.txt
-1
View File
@@ -32,7 +32,6 @@
############################################################################
px4_add_library(vehicle_imu
Integrator.hpp
VehicleIMU.cpp
VehicleIMU.hpp
)
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+3 -3
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};
src/drivers/optical_flow/CMakeLists.txt → src/modules/sensors/vehicle_optical_flow/CMakeLists.txt
+6 -5
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)
src/modules/sensors/vehicle_optical_flow/RingBuffer.h
+196
View File
@@ -0,0 +1,196 @@
/****************************************************************************
*
* Copyright (C) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file RingBuffer.h
* @author Roman Bapst <bapstroman@gmail.com>
* Template RingBuffer.
*/
#include <inttypes.h>
#include <cstdio>
#include <cstring>
template <typename data_type, size_t SIZE>
class RingBuffer
{
public:
void push(const data_type &sample)
{
uint8_t head_new = _head;
if (!_first_write) {
head_new = (_head + 1) % SIZE;
}
_buffer[head_new] = sample;
_head = head_new;
// move tail if we overwrite it
if (_head == _tail && !_first_write) {
_tail = (_tail + 1) % SIZE;
} else {
_first_write = false;
}
}
uint8_t get_length() const { return SIZE; }
data_type &operator[](const uint8_t index) { return _buffer[index]; }
const data_type &get_newest() const { return _buffer[_head]; }
const data_type &get_oldest() const { return _buffer[_tail]; }
uint8_t get_oldest_index() const { return _tail; }
bool pop_first_older_than(const uint64_t &timestamp, data_type *sample)
{
// start looking from newest observation data
for (uint8_t i = 0; i < SIZE; i++) {
int index = (_head - i);
index = index < 0 ? SIZE + index : index;
if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) {
*sample = _buffer[index];
// Now we can set the tail to the item which
// comes after the one we removed since we don't
// want to have any older data in the buffer
if (index == _head) {
_tail = _head;
_first_write = true;
} else {
_tail = (index + 1) % SIZE;
}
_buffer[index].time_us = 0;
return true;
}
if (index == _tail) {
// we have reached the tail and haven't got a
// match
return false;
}
}
return false;
}
bool pop_oldest(const uint64_t &timestamp_oldest, const uint64_t &timestamp_newest, data_type *sample)
{
if (timestamp_oldest >= timestamp_newest) {
return false;
}
for (uint8_t i = 0; i < SIZE; i++) {
uint8_t index = (_tail + i) % SIZE;
if (_buffer[index].time_us >= timestamp_oldest && _buffer[index].time_us <= timestamp_newest) {
*sample = _buffer[index];
// Now we can set the tail to the item which
// comes after the one we removed since we don't
// want to have any older data in the buffer
if (index == _head) {
_tail = _head;
_first_write = true;
} else {
_tail = (index + 1) % SIZE;
}
_buffer[index] = {};
return true;
}
}
return false;
}
bool pop_oldest(data_type *sample)
{
if (_tail == _head) {
return false;
}
*sample = _buffer[_tail];
_buffer[_tail].time_us = 0;
_tail = (_tail + 1) % SIZE;
return true;
}
bool pop_newest(data_type *sample)
{
if (_tail == _head) {
return false;
}
*sample = _buffer[_head];
_buffer[_head].time_us = 0;
_head = (_head - 1) % SIZE;
return true;
}
int get_totalSIZE() const { return sizeof(*this) + sizeof(data_type) * SIZE; }
int entries() const
{
int count = 0;
for (uint8_t i = 0; i < SIZE; i++) {
if (_buffer[i].time_us != 0) {
count++;
}
}
return count;
}
private:
data_type _buffer[SIZE] {};
uint8_t _head{0};
uint8_t _tail{0};
bool _first_write{true};
};
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
+396
View File
@@ -0,0 +1,396 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "VehicleOpticalFlow.hpp"
#include <px4_platform_common/log.h>
namespace sensors
{
using namespace matrix;
using namespace time_literals;
static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
VehicleOpticalFlow::VehicleOpticalFlow() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_vehicle_optical_flow_pub.advertise();
_gyro_integrator.set_reset_samples(1);
}
VehicleOpticalFlow::~VehicleOpticalFlow()
{
Stop();
perf_free(_cycle_perf);
}
bool VehicleOpticalFlow::Start()
{
_sensor_flow_sub.registerCallback();
_sensor_gyro_sub.registerCallback();
_sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2);
_sensor_selection_sub.registerCallback();
ScheduleNow();
return true;
}
void VehicleOpticalFlow::Stop()
{
Deinit();
// clear all registered callbacks
_sensor_flow_sub.unregisterCallback();
_sensor_gyro_sub.unregisterCallback();
_sensor_selection_sub.unregisterCallback();
}
void VehicleOpticalFlow::ParametersUpdate()
{
// Check if parameters have changed
if (_params_sub.updated()) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
updateParams();
}
}
void VehicleOpticalFlow::Run()
{
perf_begin(_cycle_perf);
ParametersUpdate();
if (_sensor_selection_sub.updated()) {
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
if (sensor_gyro_sub.advertised()
&& (sensor_gyro_sub.get().timestamp != 0)
&& (sensor_gyro_sub.get().device_id != 0)
&& (hrt_elapsed_time(&sensor_gyro_sub.get().timestamp) < 1_s)) {
if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) {
if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) {
_gyro_calibration.set_device_id(sensor_gyro_sub.get().device_id);
PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
break;
} else {
PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
}
}
}
}
}
while (_sensor_gyro_sub.updated()) {
const unsigned last_generation = _sensor_gyro_sub.get_last_generation();
sensor_gyro_s sensor_gyro;
if (_sensor_gyro_sub.update(&sensor_gyro)) {
if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation());
}
_gyro_calibration.set_device_id(sensor_gyro.device_id);
_gyro_calibration.SensorCorrectionsUpdate();
const float dt_s = (sensor_gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f;
_gyro_timestamp_sample_last = sensor_gyro.timestamp_sample;
gyroSample gyro_sample;
gyro_sample.time_us = sensor_gyro.timestamp_sample;
gyro_sample.data = _gyro_calibration.Correct(Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z});
gyro_sample.dt = dt_s;
_gyro_buffer.push(gyro_sample);
}
}
sensor_optical_flow_s sensor_optical_flow;
if (_sensor_flow_sub.update(&sensor_optical_flow)) {
// only process valid samples
if ((sensor_optical_flow.integration_timespan_us > 1000) && (sensor_optical_flow.integration_timespan_us < UINT16_MAX)
&& (sensor_optical_flow.quality > 0)
&& PX4_ISFINITE(sensor_optical_flow.pixel_flow[0])
&& PX4_ISFINITE(sensor_optical_flow.pixel_flow[1])) {
// clear data accumulation if there's a gap in data
if ((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last) > sensor_optical_flow.integration_timespan_us *
1.5f) {
// clear accumulated data
_flow_integral.zero();
_integration_timespan_us = 0;
_delta_angle.zero();
_quality_sum = 0;
_accumulated_count = 0;
_gyro_integrator.reset();
}
gyroSample gyro_sample;
const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf(
sensor_optical_flow.integration_timespan_us);
const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp;
while (_gyro_buffer.pop_oldest(timestamp_oldest, timestamp_newest, &gyro_sample)) {
_gyro_integrator.put(gyro_sample.data, gyro_sample.dt);
float min_interval_s = (sensor_optical_flow.integration_timespan_us * 1e-6f) * 0.99f;
if (_gyro_integrator.integral_dt() > min_interval_s) {
//PX4_INFO("integral dt: %.6f, min interval: %.6f", (double)_gyro_integrator.integral_dt(),(double) min_interval_s);
break;
}
}
if (sensor_optical_flow.delta_angle_available
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[0])
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[1])
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[2])
) {
// passthrough integrated gyro if available
_delta_angle += Vector3f{sensor_optical_flow.delta_angle};
} else {
Vector3f delta_angle{NAN, NAN, NAN};
uint16_t delta_angle_dt;
if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) {
_delta_angle += delta_angle;
} else {
// force integrator reset
_gyro_integrator.reset();
}
}
_flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample;
_flow_integral(0) += sensor_optical_flow.pixel_flow[0];
_flow_integral(1) += sensor_optical_flow.pixel_flow[1];
_integration_timespan_us += sensor_optical_flow.integration_timespan_us;
_quality_sum += sensor_optical_flow.quality;
_accumulated_count++;
bool publish = true;
if (_param_sens_flow_rate.get() > 0) {
const float interval_us = 1e6f / _param_sens_flow_rate.get();
// don't allow publishing faster than SENS_FLOW_RATE
if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) {
publish = false;
}
// integrate for full interval unless we haven't published recently
if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) {
publish = false;
}
}
if (publish) {
vehicle_optical_flow_s vehicle_optical_flow{};
vehicle_optical_flow.timestamp_sample = sensor_optical_flow.timestamp_sample;
vehicle_optical_flow.device_id = sensor_optical_flow.device_id;
_flow_integral.copyTo(vehicle_optical_flow.pixel_flow);
_delta_angle.copyTo(vehicle_optical_flow.delta_angle);
vehicle_optical_flow.integration_timespan_us = _integration_timespan_us;
vehicle_optical_flow.quality = _quality_sum / _accumulated_count;
// SENS_FLOW_MAXR
if (PX4_ISFINITE(sensor_optical_flow.max_flow_rate)
&& (sensor_optical_flow.max_flow_rate <= _param_sens_flow_maxr.get())) {
vehicle_optical_flow.max_flow_rate = sensor_optical_flow.max_flow_rate;
} else {
vehicle_optical_flow.max_flow_rate = _param_sens_flow_maxr.get();
}
// SENS_FLOW_MINHGT
if (PX4_ISFINITE(sensor_optical_flow.min_ground_distance)
&& (sensor_optical_flow.min_ground_distance >= _param_sens_flow_minhgt.get())) {
vehicle_optical_flow.min_ground_distance = sensor_optical_flow.min_ground_distance;
} else {
vehicle_optical_flow.min_ground_distance = _param_sens_flow_minhgt.get();
}
// SENS_FLOW_MAXHGT
if (PX4_ISFINITE(sensor_optical_flow.max_ground_distance)
&& (sensor_optical_flow.max_ground_distance <= _param_sens_flow_maxhgt.get())) {
vehicle_optical_flow.max_ground_distance = sensor_optical_flow.max_ground_distance;
} else {
vehicle_optical_flow.max_ground_distance = _param_sens_flow_maxhgt.get();
}
// rotate (SENS_FLOW_ROT)
float zeroval = 0.f;
rotate_3f((enum Rotation)_param_sens_flow_rot.get(), vehicle_optical_flow.pixel_flow[0],
vehicle_optical_flow.pixel_flow[1], zeroval);
rotate_3f((enum Rotation)_param_sens_flow_rot.get(),
sensor_optical_flow.delta_angle[0],
sensor_optical_flow.delta_angle[1],
sensor_optical_flow.delta_angle[2]);
vehicle_optical_flow.timestamp = hrt_absolute_time();
_vehicle_optical_flow_pub.publish(vehicle_optical_flow);
_last_publication_timestamp = vehicle_optical_flow.timestamp_sample;
// vehicle_optical_flow_vel if distance is available (for logging)
if (_distance_sensor_sub.updated()) {
distance_sensor_s distance_sensor{};
_distance_sensor_sub.copy(&distance_sensor);
if ((distance_sensor.current_distance > distance_sensor.min_distance)
&& (distance_sensor.current_distance < distance_sensor.max_distance)) {
const float range = distance_sensor.current_distance;
vehicle_optical_flow_vel_s flow_vel{};
flow_vel.timestamp_sample = vehicle_optical_flow.timestamp_sample;
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
// is produced by a RH rotation of the image about the sensor axis.
const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]};
const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us;
// compensate for body motion to give a LOS rate
const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy();
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt;
vel_optflow_body(1) = range * flow_compensated_XY_rad(0) / flow_dt;
vel_optflow_body(2) = 0.f;
// vel_body
flow_vel.vel_body[0] = vel_optflow_body(0);
flow_vel.vel_body[1] = vel_optflow_body(1);
// vel_ne
flow_vel.vel_ne[0] = NAN;
flow_vel.vel_ne[1] = NAN;
vehicle_attitude_s vehicle_attitude{};
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
matrix::Dcmf R_to_earth = matrix::Dcmf(vehicle_attitude.q);
Vector3f flow_vel_ne = R_to_earth * vel_optflow_body;
flow_vel.vel_ne[0] = flow_vel_ne(0);
flow_vel.vel_ne[1] = flow_vel_ne(1);
}
// flow_uncompensated_integral
flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral);
// flow_compensated_integral
flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral);
const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt));
// gyro_rate
flow_vel.gyro_rate[0] = measured_body_rate(0);
flow_vel.gyro_rate[1] = measured_body_rate(1);
// gyro_rate_integral
flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
flow_vel.timestamp = hrt_absolute_time();
_vehicle_optical_flow_vel_pub.publish(flow_vel);
}
}
// clear accumulated data
_flow_integral.zero();
_integration_timespan_us = 0;
_delta_angle.zero();
_quality_sum = 0;
_accumulated_count = 0;
}
}
}
// reschedule backup
ScheduleDelayed(10_ms);
perf_end(_cycle_perf);
}
void VehicleOpticalFlow::PrintStatus()
{
}
}; // namespace sensors
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp
+133
View File
@@ -0,0 +1,133 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "data_validator/DataValidatorGroup.hpp"
#include "RingBuffer.h"
#include <Integrator.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_optical_flow.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_optical_flow.h>
#include <uORB/topics/vehicle_optical_flow_vel.h>
namespace sensors
{
struct gyroSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
matrix::Vector3f data{};
float dt{0.f};
};
class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem
{
public:
VehicleOpticalFlow();
~VehicleOpticalFlow() override;
bool Start();
void Stop();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate();
void SensorCorrectionsUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
uORB::Publication<vehicle_optical_flow_s> _vehicle_optical_flow_pub{ORB_ID(vehicle_optical_flow)};
uORB::Publication<vehicle_optical_flow_vel_s> _vehicle_optical_flow_vel_pub{ORB_ID(vehicle_optical_flow_vel)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::SubscriptionCallbackWorkItem _sensor_flow_sub{this, ORB_ID(sensor_optical_flow)};
uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)};
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
sensors::IntegratorConing _gyro_integrator{};
hrt_abstime _gyro_timestamp_sample_last{0};
calibration::Gyroscope _gyro_calibration{};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
hrt_abstime _flow_timestamp_sample_last{0};
matrix::Vector2f _flow_integral{};
matrix::Vector3f _delta_angle{};
uint32_t _integration_timespan_us{};
uint16_t _quality_sum{0};
uint8_t _accumulated_count{0};
hrt_abstime _last_publication_timestamp{0};
bool _advertised[MAX_SENSOR_COUNT] {};
uint8_t _priority[MAX_SENSOR_COUNT] {};
int8_t _selected_sensor_sub_index{-1};
RingBuffer<gyroSample, 32> _gyro_buffer{};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_FLOW_ROT>) _param_sens_flow_rot,
(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
(ParamFloat<px4::params::SENS_FLOW_MAXR>) _param_sens_flow_maxr,
(ParamFloat<px4::params::SENS_FLOW_RATE>) _param_sens_flow_rate
)
};
}; // namespace sensors
src/modules/simulator/simulator.h
+2 -2
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>
@@ -191,7 +191,7 @@ private:
// uORB publisher handlers
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::PublicationMulti<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
src/modules/simulator/simulator_mavlink.cpp
+26 -33
View File
@@ -1311,44 +1311,37 @@ void Simulator::check_failure_injections()
int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
{
optical_flow_s flow = {};
flow.sensor_id = flow_mavlink->sensor_id;
flow.timestamp = hrt_absolute_time();
flow.time_since_last_sonar_update = 0;
flow.frame_count_since_last_readout = 0; // ?
flow.integration_timespan = flow_mavlink->integration_time_us;
sensor_optical_flow_s flow{};
flow.timestamp_sample = hrt_absolute_time();
flow.device_id = 0; // TODO
flow.pixel_flow[0] = flow_mavlink->integrated_x;
flow.pixel_flow[1] = flow_mavlink->integrated_y;
flow.integration_timespan_us = flow_mavlink->integration_time_us;
flow.ground_distance_m = flow_mavlink->distance;
flow.gyro_temperature = flow_mavlink->temperature;
flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
flow.quality = flow_mavlink->quality;
//flow.ground_distance_m = flow_mavlink->distance;
if (PX4_ISFINITE(flow_mavlink->integrated_xgyro) && PX4_ISFINITE(flow_mavlink->integrated_ygyro)
&& PX4_ISFINITE(flow_mavlink->integrated_zgyro)) {
flow.delta_angle[0] = flow_mavlink->integrated_xgyro;
flow.delta_angle[1] = flow_mavlink->integrated_ygyro;
flow.delta_angle[2] = flow_mavlink->integrated_zgyro;
flow.delta_angle_available = true;
}
/* fill in sensor limits */
float flow_rate_max;
param_get(param_find("SENS_FLOW_MAXR"), &flow_rate_max);
float flow_min_hgt;
param_get(param_find("SENS_FLOW_MINHGT"), &flow_min_hgt);
float flow_max_hgt;
param_get(param_find("SENS_FLOW_MAXHGT"), &flow_max_hgt);
flow.max_flow_rate = NAN;
flow.min_ground_distance = NAN;
flow.max_ground_distance = NAN;
flow.max_flow_rate = flow_rate_max;
flow.min_ground_distance = flow_min_hgt;
flow.max_ground_distance = flow_max_hgt;
/* rotate measurements according to parameter */
int32_t flow_rot_int;
param_get(param_find("SENS_FLOW_ROT"), &flow_rot_int);
const enum Rotation flow_rot = (Rotation)flow_rot_int;
float zeroval = 0.0f;
rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval);
rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral);
_flow_pub.publish(flow);
flow.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(flow);
return PX4_OK;
}