mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 11:07:36 +08:00
optical flow sensor pipeline overhaul
- all sources of optical flow publish sensor_optical_flow - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow Co-authored-by: alexklimaj <alex@arkelectron.com>
This commit is contained in:
Daniel Agar
@@ -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; }
|
||||
|
||||
+32
-12
@@ -1426,14 +1426,18 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
|
||||
void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_ekf.getFlowCompensated().longerThan(0.f)) {
|
||||
estimator_optical_flow_vel_s flow_vel{};
|
||||
vehicle_optical_flow_vel_s flow_vel{};
|
||||
flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
|
||||
|
||||
_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
|
||||
_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
|
||||
|
||||
_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
|
||||
_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
|
||||
_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
|
||||
|
||||
_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate);
|
||||
_ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral);
|
||||
|
||||
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
|
||||
_estimator_optical_flow_vel_pub.publish(flow_vel);
|
||||
@@ -1711,29 +1715,29 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
// EKF flow sample
|
||||
bool new_optical_flow = false;
|
||||
const unsigned last_generation = _optical_flow_sub.get_last_generation();
|
||||
optical_flow_s optical_flow;
|
||||
const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation();
|
||||
vehicle_optical_flow_s optical_flow;
|
||||
|
||||
if (_optical_flow_sub.update(&optical_flow)) {
|
||||
if (_vehicle_optical_flow_sub.update(&optical_flow)) {
|
||||
if (_msg_missed_optical_flow_perf == nullptr) {
|
||||
_msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed");
|
||||
|
||||
} else if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
|
||||
} else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) {
|
||||
perf_count(_msg_missed_optical_flow_perf);
|
||||
}
|
||||
|
||||
flowSample flow {
|
||||
.time_us = optical_flow.timestamp,
|
||||
.time_us = optical_flow.timestamp_sample,
|
||||
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
|
||||
// is produced by a RH rotation of the image about the sensor axis.
|
||||
.flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral},
|
||||
.gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral},
|
||||
.dt = 1e-6f * (float)optical_flow.integration_timespan,
|
||||
.flow_xy_rad = Vector2f{-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]},
|
||||
.gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]},
|
||||
.dt = 1e-6f * (float)optical_flow.integration_timespan_us,
|
||||
.quality = optical_flow.quality,
|
||||
};
|
||||
|
||||
if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
|
||||
PX4_ISFINITE(optical_flow.pixel_flow_x_integral) &&
|
||||
if (PX4_ISFINITE(optical_flow.pixel_flow[0]) &&
|
||||
PX4_ISFINITE(optical_flow.pixel_flow[1]) &&
|
||||
flow.dt < 1) {
|
||||
|
||||
// Save sensor limits reported by the optical flow sensor
|
||||
@@ -1745,6 +1749,22 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
new_optical_flow = true;
|
||||
}
|
||||
|
||||
// use optical_flow distance as range sample if distance_sensor unavailable
|
||||
if (PX4_ISFINITE(optical_flow.distance_m) && (hrt_elapsed_time(&_last_range_sensor_update) > 1_s)) {
|
||||
|
||||
int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;
|
||||
|
||||
rangeSample range_sample {
|
||||
.time_us = optical_flow.timestamp_sample,
|
||||
.rng = optical_flow.distance_m,
|
||||
.quality = quality,
|
||||
};
|
||||
_ekf.setRangeData(range_sample);
|
||||
|
||||
// set sensor limits
|
||||
_ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance);
|
||||
}
|
||||
|
||||
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
|
||||
(int64_t)ekf2_timestamps.timestamp / 100);
|
||||
}
|
||||
|
||||
@@ -71,13 +71,11 @@
|
||||
#include <uORB/topics/estimator_event_flags.h>
|
||||
#include <uORB/topics/estimator_gps_status.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
#include <uORB/topics/estimator_optical_flow_vel.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/estimator_states.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/estimator_status_flags.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/sensor_selection.h>
|
||||
@@ -91,11 +89,12 @@
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_magnetometer.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/vehicle_optical_flow.h>
|
||||
#include <uORB/topics/vehicle_optical_flow_vel.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/wind.h>
|
||||
#include <uORB/topics/yaw_estimator_status.h>
|
||||
|
||||
|
||||
extern pthread_mutex_t ekf2_module_mutex;
|
||||
|
||||
class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
|
||||
@@ -276,17 +275,18 @@ private:
|
||||
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
|
||||
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
|
||||
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
|
||||
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
|
||||
|
||||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
|
||||
hrt_abstime _last_range_sensor_update{0};
|
||||
int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
|
||||
unsigned _distance_sensor_last_generation{0};
|
||||
|
||||
@@ -295,8 +295,6 @@ private:
|
||||
hrt_abstime _last_event_flags_publish{0};
|
||||
hrt_abstime _last_status_flags_publish{0};
|
||||
|
||||
hrt_abstime _last_range_sensor_update{0};
|
||||
|
||||
uint32_t _filter_control_status{0};
|
||||
uint32_t _filter_fault_status{0};
|
||||
uint32_t _innov_check_fail_status{0};
|
||||
@@ -314,12 +312,12 @@ private:
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
|
||||
uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
|
||||
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
|
||||
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
|
||||
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
|
||||
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
|
||||
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
|
||||
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
|
||||
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
|
||||
|
||||
@@ -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)};
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -174,7 +174,6 @@ void LoggedTopics::add_default_topics()
|
||||
add_topic_multi("battery_status", 200, 2);
|
||||
add_topic_multi("differential_pressure", 1000, 2);
|
||||
add_topic_multi("distance_sensor", 1000, 2);
|
||||
add_topic_multi("optical_flow", 1000, 1);
|
||||
add_optional_topic_multi("sensor_accel", 1000, 4);
|
||||
add_optional_topic_multi("sensor_baro", 1000, 4);
|
||||
add_topic_multi("sensor_gps", 1000, 2);
|
||||
@@ -182,9 +181,13 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic("pps_capture", 1000);
|
||||
add_optional_topic_multi("sensor_gyro", 1000, 4);
|
||||
add_optional_topic_multi("sensor_mag", 1000, 4);
|
||||
add_optional_topic_multi("sensor_optical_flow", 1000, 2);
|
||||
|
||||
add_topic_multi("vehicle_imu", 500, 4);
|
||||
add_topic_multi("vehicle_imu_status", 1000, 4);
|
||||
add_optional_topic_multi("vehicle_magnetometer", 500, 4);
|
||||
add_optional_topic("vehicle_optical_flow", 500);
|
||||
//add_optional_topic("vehicle_optical_flow_vel", 100);
|
||||
|
||||
// SYS_CTRL_ALLOC: additional dynamic control allocation logging when enabled
|
||||
int32_t sys_ctrl_alloc = 0;
|
||||
|
||||
@@ -89,12 +89,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_parameters_manager(parent),
|
||||
_mavlink_timesync(parent)
|
||||
{
|
||||
_handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT");
|
||||
_handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR");
|
||||
_handle_sens_flow_minhgt = param_find("SENS_FLOW_MINHGT");
|
||||
_handle_sens_flow_rot = param_find("SENS_FLOW_ROT");
|
||||
_handle_ekf2_min_rng = param_find("EKF2_MIN_RNG");
|
||||
_handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX");
|
||||
}
|
||||
|
||||
void
|
||||
@@ -795,105 +789,93 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
|
||||
void
|
||||
MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
||||
{
|
||||
/* optical flow */
|
||||
mavlink_optical_flow_rad_t flow;
|
||||
mavlink_msg_optical_flow_rad_decode(msg, &flow);
|
||||
|
||||
optical_flow_s f{};
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = _mavlink->get_instance_id();
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_MAVLINK;
|
||||
|
||||
f.timestamp = hrt_absolute_time();
|
||||
f.time_since_last_sonar_update = flow.time_delta_distance_us;
|
||||
f.integration_timespan = flow.integration_time_us;
|
||||
f.pixel_flow_x_integral = flow.integrated_x;
|
||||
f.pixel_flow_y_integral = flow.integrated_y;
|
||||
f.gyro_x_rate_integral = flow.integrated_xgyro;
|
||||
f.gyro_y_rate_integral = flow.integrated_ygyro;
|
||||
f.gyro_z_rate_integral = flow.integrated_zgyro;
|
||||
f.gyro_temperature = flow.temperature;
|
||||
f.ground_distance_m = flow.distance;
|
||||
f.quality = flow.quality;
|
||||
f.sensor_id = flow.sensor_id;
|
||||
f.max_flow_rate = _param_sens_flow_maxr;
|
||||
f.min_ground_distance = _param_sens_flow_minhgt;
|
||||
f.max_ground_distance = _param_sens_flow_maxhgt;
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
|
||||
/* read flow sensor parameters */
|
||||
const Rotation flow_rot = (Rotation)_param_sens_flow_rot;
|
||||
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||
sensor_optical_flow.device_id = device_id.devid;
|
||||
|
||||
/* rotate measurements according to parameter */
|
||||
float zero_val = 0.0f;
|
||||
rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val);
|
||||
rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);
|
||||
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
|
||||
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
|
||||
|
||||
_flow_pub.publish(f);
|
||||
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
|
||||
sensor_optical_flow.quality = flow.quality;
|
||||
|
||||
/* Use distance value for distance sensor topic */
|
||||
if (flow.distance > 0.0f) { // negative values signal invalid data
|
||||
|
||||
distance_sensor_s d{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
|
||||
d.timestamp = f.timestamp;
|
||||
d.min_distance = _param_ekf2_min_rng;
|
||||
d.max_distance = _param_ekf2_rng_a_hmax;
|
||||
d.current_distance = flow.distance; /* both are in m */
|
||||
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
|
||||
d.device_id = device_id.devid;
|
||||
d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
||||
d.variance = 0.01;
|
||||
d.signal_quality = -1;
|
||||
|
||||
_flow_distance_sensor_pub.publish(d);
|
||||
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
|
||||
sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro;
|
||||
sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro;
|
||||
sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro;
|
||||
sensor_optical_flow.delta_angle_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.max_flow_rate = NAN;
|
||||
sensor_optical_flow.min_ground_distance = NAN;
|
||||
sensor_optical_flow.max_ground_distance = NAN;
|
||||
|
||||
// Use distance value for distance sensor topic
|
||||
if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
|
||||
// Positive value (including zero): distance known. Negative value: Unknown distance.
|
||||
sensor_optical_flow.distance_m = flow.distance;
|
||||
sensor_optical_flow.distance_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||
{
|
||||
/* optical flow */
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
|
||||
optical_flow_s f{};
|
||||
|
||||
f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
|
||||
f.integration_timespan = flow.integration_time_us;
|
||||
f.pixel_flow_x_integral = flow.integrated_x;
|
||||
f.pixel_flow_y_integral = flow.integrated_y;
|
||||
f.gyro_x_rate_integral = flow.integrated_xgyro;
|
||||
f.gyro_y_rate_integral = flow.integrated_ygyro;
|
||||
f.gyro_z_rate_integral = flow.integrated_zgyro;
|
||||
f.time_since_last_sonar_update = flow.time_delta_distance_us;
|
||||
f.ground_distance_m = flow.distance;
|
||||
f.quality = flow.quality;
|
||||
f.sensor_id = flow.sensor_id;
|
||||
f.gyro_temperature = flow.temperature;
|
||||
|
||||
_flow_pub.publish(f);
|
||||
|
||||
/* Use distance value for distance sensor topic */
|
||||
distance_sensor_s d{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = _mavlink->get_instance_id();
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
|
||||
|
||||
d.timestamp = hrt_absolute_time();
|
||||
d.min_distance = 0.3f;
|
||||
d.max_distance = 5.0f;
|
||||
d.current_distance = flow.distance; /* both are in m */
|
||||
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
|
||||
d.device_id = device_id.devid;
|
||||
d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
||||
d.variance = 0.01;
|
||||
d.signal_quality = -1;
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
|
||||
_flow_distance_sensor_pub.publish(d);
|
||||
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||
sensor_optical_flow.device_id = device_id.devid;
|
||||
|
||||
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
|
||||
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
|
||||
|
||||
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
|
||||
sensor_optical_flow.quality = flow.quality;
|
||||
|
||||
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
|
||||
sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro;
|
||||
sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro;
|
||||
sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro;
|
||||
sensor_optical_flow.delta_angle_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.max_flow_rate = NAN;
|
||||
sensor_optical_flow.min_ground_distance = NAN;
|
||||
sensor_optical_flow.max_ground_distance = NAN;
|
||||
|
||||
// Use distance value for distance sensor topic
|
||||
if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
|
||||
// Positive value (including zero): distance known. Negative value: Unknown distance.
|
||||
sensor_optical_flow.distance_m = flow.distance;
|
||||
sensor_optical_flow.distance_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
}
|
||||
|
||||
void
|
||||
@@ -934,9 +916,10 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
|
||||
distance_sensor_s ds{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = _mavlink->get_instance_id();
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
device_id.devid_s.address = dist_sensor.id;
|
||||
|
||||
ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
|
||||
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
|
||||
@@ -2338,10 +2321,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
|
||||
|
||||
sensor_gps_s gps{};
|
||||
|
||||
device::Device::DeviceId device_id{};
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = _mavlink->get_instance_id();
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
|
||||
|
||||
gps.device_id = device_id.devid;
|
||||
|
||||
gps.lat = hil_gps.lat;
|
||||
@@ -3475,30 +3460,6 @@ MavlinkReceiver::updateParams()
|
||||
{
|
||||
// update parameters from storage
|
||||
ModuleParams::updateParams();
|
||||
|
||||
if (_handle_sens_flow_maxhgt != PARAM_INVALID) {
|
||||
param_get(_handle_sens_flow_maxhgt, &_param_sens_flow_maxhgt);
|
||||
}
|
||||
|
||||
if (_handle_sens_flow_maxr != PARAM_INVALID) {
|
||||
param_get(_handle_sens_flow_maxr, &_param_sens_flow_maxr);
|
||||
}
|
||||
|
||||
if (_handle_sens_flow_minhgt != PARAM_INVALID) {
|
||||
param_get(_handle_sens_flow_minhgt, &_param_sens_flow_minhgt);
|
||||
}
|
||||
|
||||
if (_handle_sens_flow_rot != PARAM_INVALID) {
|
||||
param_get(_handle_sens_flow_rot, &_param_sens_flow_rot);
|
||||
}
|
||||
|
||||
if (_handle_ekf2_min_rng != PARAM_INVALID) {
|
||||
param_get(_handle_ekf2_min_rng, &_param_ekf2_min_rng);
|
||||
}
|
||||
|
||||
if (_handle_ekf2_rng_a_hmax != PARAM_INVALID) {
|
||||
param_get(_handle_ekf2_rng_a_hmax, &_param_ekf2_rng_a_hmax);
|
||||
}
|
||||
}
|
||||
|
||||
void *MavlinkReceiver::start_trampoline(void *context)
|
||||
|
||||
@@ -86,13 +86,13 @@
|
||||
#include <uORB/topics/obstacle_distance.h>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <uORB/topics/onboard_computer_status.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/ping.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/radio_status.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_optical_flow.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/transponder_report.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
@@ -308,7 +308,6 @@ private:
|
||||
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
|
||||
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
|
||||
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
|
||||
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
|
||||
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
|
||||
@@ -331,13 +330,13 @@ private:
|
||||
|
||||
// ORB publications (multi)
|
||||
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
|
||||
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
|
||||
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping)};
|
||||
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
|
||||
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
|
||||
|
||||
// ORB publications (queue length > 1)
|
||||
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
|
||||
@@ -396,20 +395,6 @@ private:
|
||||
hrt_abstime _heartbeat_component_udp_bridge{0};
|
||||
hrt_abstime _heartbeat_component_uart_bridge{0};
|
||||
|
||||
param_t _handle_sens_flow_maxhgt{PARAM_INVALID};
|
||||
param_t _handle_sens_flow_maxr{PARAM_INVALID};
|
||||
param_t _handle_sens_flow_minhgt{PARAM_INVALID};
|
||||
param_t _handle_sens_flow_rot{PARAM_INVALID};
|
||||
param_t _handle_ekf2_min_rng{PARAM_INVALID};
|
||||
param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID};
|
||||
|
||||
float _param_sens_flow_maxhgt{-1.0f};
|
||||
float _param_sens_flow_maxr{-1.0f};
|
||||
float _param_sens_flow_minhgt{-1.0f};
|
||||
int32_t _param_sens_flow_rot{0};
|
||||
float _param_ekf2_min_rng{NAN};
|
||||
float _param_ekf2_rng_a_hmax{NAN};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
|
||||
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
|
||||
|
||||
@@ -34,7 +34,7 @@
|
||||
#ifndef OPTICAL_FLOW_RAD_HPP
|
||||
#define OPTICAL_FLOW_RAD_HPP
|
||||
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/vehicle_optical_flow.h>
|
||||
|
||||
class MavlinkStreamOpticalFlowRad : public MavlinkStream
|
||||
{
|
||||
@@ -49,34 +49,40 @@ public:
|
||||
|
||||
unsigned get_size() override
|
||||
{
|
||||
return _optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
|
||||
return _vehicle_optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) :
|
||||
0;
|
||||
}
|
||||
|
||||
private:
|
||||
explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {}
|
||||
|
||||
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
|
||||
uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
|
||||
|
||||
bool send() override
|
||||
{
|
||||
optical_flow_s flow;
|
||||
vehicle_optical_flow_s flow;
|
||||
|
||||
if (_optical_flow_sub.update(&flow)) {
|
||||
if (_vehicle_optical_flow_sub.update(&flow)) {
|
||||
mavlink_optical_flow_rad_t msg{};
|
||||
|
||||
msg.time_usec = flow.timestamp;
|
||||
msg.sensor_id = flow.sensor_id;
|
||||
msg.integrated_x = flow.pixel_flow_x_integral;
|
||||
msg.integrated_y = flow.pixel_flow_y_integral;
|
||||
msg.integrated_xgyro = flow.gyro_x_rate_integral;
|
||||
msg.integrated_ygyro = flow.gyro_y_rate_integral;
|
||||
msg.integrated_zgyro = flow.gyro_z_rate_integral;
|
||||
msg.distance = flow.ground_distance_m;
|
||||
msg.time_usec = flow.timestamp_sample;
|
||||
msg.sensor_id = _vehicle_optical_flow_sub.get_instance();
|
||||
msg.integration_time_us = flow.integration_timespan_us;
|
||||
msg.integrated_x = flow.pixel_flow[0];
|
||||
msg.integrated_y = flow.pixel_flow[1];
|
||||
msg.integrated_xgyro = flow.delta_angle[0];
|
||||
msg.integrated_ygyro = flow.delta_angle[1];
|
||||
msg.integrated_zgyro = flow.delta_angle[2];
|
||||
// msg.temperature = 0;
|
||||
msg.quality = flow.quality;
|
||||
msg.integration_time_us = flow.integration_timespan;
|
||||
msg.sensor_id = flow.sensor_id;
|
||||
msg.time_delta_distance_us = flow.time_since_last_sonar_update;
|
||||
msg.temperature = flow.gyro_temperature;
|
||||
// msg.time_delta_distance_us = 0;
|
||||
|
||||
if (PX4_ISFINITE(flow.distance_m)) {
|
||||
msg.distance = flow.distance_m;
|
||||
|
||||
} else {
|
||||
msg.distance = -1;
|
||||
}
|
||||
|
||||
mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
|
||||
@@ -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");
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -32,7 +32,6 @@
|
||||
############################################################################
|
||||
|
||||
px4_add_library(vehicle_imu
|
||||
Integrator.hpp
|
||||
VehicleIMU.cpp
|
||||
VehicleIMU.hpp
|
||||
)
|
||||
|
||||
@@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Integrator.hpp"
|
||||
#include <Integrator.hpp>
|
||||
|
||||
#include <lib/mathlib/math/Limits.hpp>
|
||||
#include <lib/mathlib/math/WelfordMean.hpp>
|
||||
@@ -108,8 +108,8 @@ private:
|
||||
calibration::Accelerometer _accel_calibration{};
|
||||
calibration::Gyroscope _gyro_calibration{};
|
||||
|
||||
Integrator _accel_integrator{};
|
||||
IntegratorConing _gyro_integrator{};
|
||||
sensors::Integrator _accel_integrator{};
|
||||
sensors::IntegratorConing _gyro_integrator{};
|
||||
|
||||
uint32_t _imu_integration_interval_us{5000};
|
||||
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(vehicle_optical_flow
|
||||
VehicleOpticalFlow.cpp
|
||||
VehicleOpticalFlow.hpp
|
||||
)
|
||||
target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue)
|
||||
@@ -0,0 +1,217 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file RingBuffer.hpp
|
||||
* Template RingBuffer
|
||||
*/
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
|
||||
template <typename T, size_t SIZE>
|
||||
class RingBuffer
|
||||
{
|
||||
public:
|
||||
void push(const T &sample)
|
||||
{
|
||||
uint8_t head_new = _head;
|
||||
|
||||
if (!_first_write) {
|
||||
head_new = (_head + 1) % SIZE;
|
||||
}
|
||||
|
||||
_buffer[head_new] = sample;
|
||||
_head = head_new;
|
||||
|
||||
// move tail if we overwrite it
|
||||
if (_head == _tail && !_first_write) {
|
||||
_tail = (_tail + 1) % SIZE;
|
||||
|
||||
} else {
|
||||
_first_write = false;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t get_length() const { return SIZE; }
|
||||
|
||||
T &operator[](const uint8_t index) { return _buffer[index]; }
|
||||
|
||||
const T &get_newest() const { return _buffer[_head]; }
|
||||
const T &get_oldest() const { return _buffer[_tail]; }
|
||||
|
||||
uint8_t get_oldest_index() const { return _tail; }
|
||||
|
||||
bool peak_first_older_than(const uint64_t ×tamp, T *sample)
|
||||
{
|
||||
// start looking from newest observation data
|
||||
for (uint8_t i = 0; i < SIZE; i++) {
|
||||
int index = (_head - i);
|
||||
index = index < 0 ? SIZE + index : index;
|
||||
|
||||
if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) {
|
||||
*sample = _buffer[index];
|
||||
return true;
|
||||
}
|
||||
|
||||
if (index == _tail) {
|
||||
// we have reached the tail and haven't got a
|
||||
// match
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool pop_first_older_than(const uint64_t ×tamp, T *sample)
|
||||
{
|
||||
// start looking from newest observation data
|
||||
for (uint8_t i = 0; i < SIZE; i++) {
|
||||
int index = (_head - i);
|
||||
index = index < 0 ? SIZE + index : index;
|
||||
|
||||
if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) {
|
||||
*sample = _buffer[index];
|
||||
|
||||
// Now we can set the tail to the item which
|
||||
// comes after the one we removed since we don't
|
||||
// want to have any older data in the buffer
|
||||
if (index == _head) {
|
||||
_tail = _head;
|
||||
_first_write = true;
|
||||
|
||||
} else {
|
||||
_tail = (index + 1) % SIZE;
|
||||
}
|
||||
|
||||
_buffer[index].time_us = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
if (index == _tail) {
|
||||
// we have reached the tail and haven't got a
|
||||
// match
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool pop_oldest(const uint64_t ×tamp_oldest, const uint64_t ×tamp_newest, T *sample)
|
||||
{
|
||||
if (timestamp_oldest >= timestamp_newest) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < SIZE; i++) {
|
||||
|
||||
uint8_t index = (_tail + i) % SIZE;
|
||||
|
||||
if (_buffer[index].time_us >= timestamp_oldest && _buffer[index].time_us <= timestamp_newest) {
|
||||
*sample = _buffer[index];
|
||||
|
||||
// Now we can set the tail to the item which
|
||||
// comes after the one we removed since we don't
|
||||
// want to have any older data in the buffer
|
||||
if (index == _head) {
|
||||
_tail = _head;
|
||||
_first_write = true;
|
||||
|
||||
} else {
|
||||
_tail = (index + 1) % SIZE;
|
||||
}
|
||||
|
||||
_buffer[index] = {};
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool pop_oldest(T *sample)
|
||||
{
|
||||
if (_tail == _head) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*sample = _buffer[_tail];
|
||||
_buffer[_tail].time_us = 0;
|
||||
|
||||
_tail = (_tail + 1) % SIZE;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool pop_newest(T *sample)
|
||||
{
|
||||
if (_tail == _head) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*sample = _buffer[_head];
|
||||
_buffer[_head].time_us = 0;
|
||||
|
||||
_head = (_head - 1) % SIZE;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int get_total_size() const { return sizeof(*this) + sizeof(T) * SIZE; }
|
||||
|
||||
uint8_t entries() const
|
||||
{
|
||||
int count = 0;
|
||||
|
||||
for (uint8_t i = 0; i < SIZE; i++) {
|
||||
if (_buffer[i].time_us != 0) {
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
private:
|
||||
T _buffer[SIZE] {};
|
||||
|
||||
uint8_t _head{0};
|
||||
uint8_t _tail{0};
|
||||
|
||||
bool _first_write{true};
|
||||
};
|
||||
@@ -0,0 +1,481 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "VehicleOpticalFlow.hpp"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
|
||||
|
||||
VehicleOpticalFlow::VehicleOpticalFlow() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
|
||||
{
|
||||
_vehicle_optical_flow_pub.advertise();
|
||||
|
||||
_gyro_integrator.set_reset_samples(1);
|
||||
}
|
||||
|
||||
VehicleOpticalFlow::~VehicleOpticalFlow()
|
||||
{
|
||||
Stop();
|
||||
perf_free(_cycle_perf);
|
||||
}
|
||||
|
||||
bool VehicleOpticalFlow::Start()
|
||||
{
|
||||
_sensor_flow_sub.registerCallback();
|
||||
|
||||
_sensor_gyro_sub.registerCallback();
|
||||
_sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2);
|
||||
|
||||
_sensor_selection_sub.registerCallback();
|
||||
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void VehicleOpticalFlow::Stop()
|
||||
{
|
||||
Deinit();
|
||||
|
||||
// clear all registered callbacks
|
||||
_sensor_flow_sub.unregisterCallback();
|
||||
_sensor_gyro_sub.unregisterCallback();
|
||||
_sensor_selection_sub.unregisterCallback();
|
||||
}
|
||||
|
||||
void VehicleOpticalFlow::ParametersUpdate()
|
||||
{
|
||||
// Check if parameters have changed
|
||||
if (_params_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s param_update;
|
||||
_params_sub.copy(¶m_update);
|
||||
|
||||
updateParams();
|
||||
|
||||
_flow_rotation = get_rot_matrix((enum Rotation)_param_sens_flow_rot.get());
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleOpticalFlow::Run()
|
||||
{
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
ParametersUpdate();
|
||||
|
||||
UpdateDistanceSensor();
|
||||
UpdateSensorGyro();
|
||||
|
||||
sensor_optical_flow_s sensor_optical_flow;
|
||||
|
||||
if (_sensor_flow_sub.update(&sensor_optical_flow)) {
|
||||
|
||||
// clear data accumulation if there's a gap in data
|
||||
if (((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last)
|
||||
> sensor_optical_flow.integration_timespan_us * 1.5f)
|
||||
|| (_accumulated_count > 0 && _quality_sum == 0)) {
|
||||
ClearAccumulatedData();
|
||||
}
|
||||
|
||||
|
||||
const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf(
|
||||
sensor_optical_flow.integration_timespan_us);
|
||||
const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp;
|
||||
|
||||
// delta angle
|
||||
// - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available
|
||||
if (sensor_optical_flow.delta_angle_available
|
||||
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[0])
|
||||
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[1])
|
||||
&& PX4_ISFINITE(sensor_optical_flow.delta_angle[2])
|
||||
) {
|
||||
// passthrough integrated gyro if available
|
||||
_delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle};
|
||||
|
||||
} else {
|
||||
// integrate synchronized gyro
|
||||
gyroSample gyro_sample;
|
||||
|
||||
while (_gyro_buffer.pop_oldest(timestamp_oldest, timestamp_newest, &gyro_sample)) {
|
||||
|
||||
_gyro_integrator.put(gyro_sample.data, gyro_sample.dt);
|
||||
|
||||
float min_interval_s = (sensor_optical_flow.integration_timespan_us * 1e-6f) * 0.99f;
|
||||
|
||||
if (_gyro_integrator.integral_dt() > min_interval_s) {
|
||||
//PX4_INFO("integral dt: %.6f, min interval: %.6f", (double)_gyro_integrator.integral_dt(),(double) min_interval_s);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f delta_angle{NAN, NAN, NAN};
|
||||
uint16_t delta_angle_dt;
|
||||
|
||||
if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) {
|
||||
_delta_angle += delta_angle;
|
||||
|
||||
} else {
|
||||
// force integrator reset
|
||||
_gyro_integrator.reset();
|
||||
}
|
||||
}
|
||||
|
||||
// distance
|
||||
// - from sensor_optical_flow if available, otherwise use downward distance_sensor if available
|
||||
if (sensor_optical_flow.distance_available && PX4_ISFINITE(sensor_optical_flow.distance_m)) {
|
||||
if (!PX4_ISFINITE(_distance_sum)) {
|
||||
_distance_sum = sensor_optical_flow.distance_m;
|
||||
_distance_sum_count = 1;
|
||||
|
||||
} else {
|
||||
_distance_sum += sensor_optical_flow.distance_m;
|
||||
_distance_sum_count += 1;
|
||||
}
|
||||
|
||||
} else {
|
||||
// otherwise use buffered downward facing distance_sensor if available
|
||||
rangeSample range_sample;
|
||||
|
||||
if (_range_buffer.peak_first_older_than(sensor_optical_flow.timestamp_sample, &range_sample)) {
|
||||
if (!PX4_ISFINITE(_distance_sum)) {
|
||||
_distance_sum = range_sample.data;
|
||||
_distance_sum_count = 1;
|
||||
|
||||
} else {
|
||||
_distance_sum += range_sample.data;
|
||||
_distance_sum_count += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample;
|
||||
_flow_integral(0) += sensor_optical_flow.pixel_flow[0];
|
||||
_flow_integral(1) += sensor_optical_flow.pixel_flow[1];
|
||||
|
||||
_integration_timespan_us += sensor_optical_flow.integration_timespan_us;
|
||||
|
||||
_quality_sum += sensor_optical_flow.quality;
|
||||
_accumulated_count++;
|
||||
|
||||
bool publish = true;
|
||||
|
||||
if (_param_sens_flow_rate.get() > 0) {
|
||||
const float interval_us = 1e6f / _param_sens_flow_rate.get();
|
||||
|
||||
// don't allow publishing faster than SENS_FLOW_RATE
|
||||
if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) {
|
||||
publish = false;
|
||||
}
|
||||
|
||||
// integrate for full interval unless we haven't published recently
|
||||
if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) {
|
||||
publish = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (publish) {
|
||||
vehicle_optical_flow_s vehicle_optical_flow{};
|
||||
|
||||
vehicle_optical_flow.timestamp_sample = sensor_optical_flow.timestamp_sample;
|
||||
vehicle_optical_flow.device_id = sensor_optical_flow.device_id;
|
||||
|
||||
_flow_integral.copyTo(vehicle_optical_flow.pixel_flow);
|
||||
_delta_angle.copyTo(vehicle_optical_flow.delta_angle);
|
||||
|
||||
vehicle_optical_flow.integration_timespan_us = _integration_timespan_us;
|
||||
|
||||
vehicle_optical_flow.quality = _quality_sum / _accumulated_count;
|
||||
|
||||
if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) {
|
||||
vehicle_optical_flow.distance_m = _distance_sum / _distance_sum_count;
|
||||
|
||||
} else {
|
||||
vehicle_optical_flow.distance_m = NAN;
|
||||
}
|
||||
|
||||
// SENS_FLOW_MAXR
|
||||
if (PX4_ISFINITE(sensor_optical_flow.max_flow_rate)
|
||||
&& (sensor_optical_flow.max_flow_rate <= _param_sens_flow_maxr.get())) {
|
||||
|
||||
vehicle_optical_flow.max_flow_rate = sensor_optical_flow.max_flow_rate;
|
||||
|
||||
} else {
|
||||
vehicle_optical_flow.max_flow_rate = _param_sens_flow_maxr.get();
|
||||
}
|
||||
|
||||
// SENS_FLOW_MINHGT
|
||||
if (PX4_ISFINITE(sensor_optical_flow.min_ground_distance)
|
||||
&& (sensor_optical_flow.min_ground_distance >= _param_sens_flow_minhgt.get())) {
|
||||
|
||||
vehicle_optical_flow.min_ground_distance = sensor_optical_flow.min_ground_distance;
|
||||
|
||||
} else {
|
||||
vehicle_optical_flow.min_ground_distance = _param_sens_flow_minhgt.get();
|
||||
}
|
||||
|
||||
// SENS_FLOW_MAXHGT
|
||||
if (PX4_ISFINITE(sensor_optical_flow.max_ground_distance)
|
||||
&& (sensor_optical_flow.max_ground_distance <= _param_sens_flow_maxhgt.get())) {
|
||||
|
||||
vehicle_optical_flow.max_ground_distance = sensor_optical_flow.max_ground_distance;
|
||||
|
||||
} else {
|
||||
vehicle_optical_flow.max_ground_distance = _param_sens_flow_maxhgt.get();
|
||||
}
|
||||
|
||||
|
||||
// rotate (SENS_FLOW_ROT)
|
||||
float zeroval = 0.f;
|
||||
rotate_3f((enum Rotation)_param_sens_flow_rot.get(), vehicle_optical_flow.pixel_flow[0],
|
||||
vehicle_optical_flow.pixel_flow[1], zeroval);
|
||||
|
||||
vehicle_optical_flow.timestamp = hrt_absolute_time();
|
||||
_vehicle_optical_flow_pub.publish(vehicle_optical_flow);
|
||||
_last_publication_timestamp = vehicle_optical_flow.timestamp_sample;
|
||||
|
||||
|
||||
// vehicle_optical_flow_vel if distance is available (for logging)
|
||||
if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) {
|
||||
const float range = _distance_sum / _distance_sum_count;
|
||||
|
||||
vehicle_optical_flow_vel_s flow_vel{};
|
||||
|
||||
flow_vel.timestamp_sample = vehicle_optical_flow.timestamp_sample;
|
||||
|
||||
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
|
||||
// is produced by a RH rotation of the image about the sensor axis.
|
||||
const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]};
|
||||
const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
|
||||
|
||||
const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us;
|
||||
|
||||
// compensate for body motion to give a LOS rate
|
||||
const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy();
|
||||
|
||||
Vector3f vel_optflow_body;
|
||||
vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt;
|
||||
vel_optflow_body(1) = range * flow_compensated_XY_rad(0) / flow_dt;
|
||||
vel_optflow_body(2) = 0.f;
|
||||
|
||||
// vel_body
|
||||
flow_vel.vel_body[0] = vel_optflow_body(0);
|
||||
flow_vel.vel_body[1] = vel_optflow_body(1);
|
||||
|
||||
// vel_ne
|
||||
flow_vel.vel_ne[0] = NAN;
|
||||
flow_vel.vel_ne[1] = NAN;
|
||||
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
|
||||
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
|
||||
const matrix::Dcmf R_to_earth = matrix::Quatf(vehicle_attitude.q);
|
||||
const Vector3f flow_vel_ne = R_to_earth * vel_optflow_body;
|
||||
|
||||
flow_vel.vel_ne[0] = flow_vel_ne(0);
|
||||
flow_vel.vel_ne[1] = flow_vel_ne(1);
|
||||
}
|
||||
|
||||
// flow_uncompensated_integral
|
||||
flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral);
|
||||
|
||||
// flow_compensated_integral
|
||||
flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral);
|
||||
|
||||
const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt));
|
||||
|
||||
// gyro_rate
|
||||
flow_vel.gyro_rate[0] = measured_body_rate(0);
|
||||
flow_vel.gyro_rate[1] = measured_body_rate(1);
|
||||
|
||||
// gyro_rate_integral
|
||||
flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
|
||||
flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
|
||||
|
||||
flow_vel.timestamp = hrt_absolute_time();
|
||||
|
||||
_vehicle_optical_flow_vel_pub.publish(flow_vel);
|
||||
}
|
||||
|
||||
ClearAccumulatedData();
|
||||
}
|
||||
}
|
||||
|
||||
// reschedule backup
|
||||
ScheduleDelayed(10_ms);
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
void VehicleOpticalFlow::UpdateDistanceSensor()
|
||||
{
|
||||
// update range finder buffer
|
||||
distance_sensor_s distance_sensor;
|
||||
|
||||
if ((_distance_sensor_selected < 0) && _distance_sensor_subs.advertised()) {
|
||||
for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
|
||||
|
||||
if (_distance_sensor_subs[i].update(&distance_sensor)) {
|
||||
// only use the first instace which has the correct orientation
|
||||
if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
|
||||
&& (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
|
||||
|
||||
int ndist = orb_group_count(ORB_ID(distance_sensor));
|
||||
|
||||
if (ndist > 1) {
|
||||
PX4_INFO("selected distance_sensor:%d (%d advertised)", i, ndist);
|
||||
}
|
||||
|
||||
_distance_sensor_selected = i;
|
||||
_last_range_sensor_update = distance_sensor.timestamp;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
|
||||
// range sample
|
||||
if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
|
||||
|
||||
if ((distance_sensor.current_distance >= distance_sensor.min_distance)
|
||||
&& (distance_sensor.current_distance <= distance_sensor.max_distance)) {
|
||||
|
||||
rangeSample sample;
|
||||
sample.time_us = distance_sensor.timestamp;
|
||||
sample.data = distance_sensor.current_distance;
|
||||
|
||||
_range_buffer.push(sample);
|
||||
|
||||
_last_range_sensor_update = distance_sensor.timestamp;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
} else {
|
||||
_distance_sensor_selected = -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
|
||||
_distance_sensor_selected = -1;
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleOpticalFlow::UpdateSensorGyro()
|
||||
{
|
||||
if (_sensor_selection_sub.updated()) {
|
||||
sensor_selection_s sensor_selection{};
|
||||
_sensor_selection_sub.copy(&sensor_selection);
|
||||
|
||||
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
|
||||
uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
|
||||
|
||||
if (sensor_gyro_sub.advertised()
|
||||
&& (sensor_gyro_sub.get().timestamp != 0)
|
||||
&& (sensor_gyro_sub.get().device_id != 0)
|
||||
&& (hrt_elapsed_time(&sensor_gyro_sub.get().timestamp) < 1_s)) {
|
||||
|
||||
if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) {
|
||||
if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) {
|
||||
|
||||
_gyro_calibration.set_device_id(sensor_gyro_sub.get().device_id);
|
||||
PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
|
||||
break;
|
||||
|
||||
} else {
|
||||
PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// buffer
|
||||
while (_sensor_gyro_sub.updated()) {
|
||||
const unsigned last_generation = _sensor_gyro_sub.get_last_generation();
|
||||
|
||||
sensor_gyro_s sensor_gyro;
|
||||
|
||||
if (_sensor_gyro_sub.copy(&sensor_gyro)) {
|
||||
|
||||
if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) {
|
||||
PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation());
|
||||
}
|
||||
|
||||
_gyro_calibration.set_device_id(sensor_gyro.device_id);
|
||||
_gyro_calibration.SensorCorrectionsUpdate();
|
||||
|
||||
const float dt_s = (sensor_gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f;
|
||||
_gyro_timestamp_sample_last = sensor_gyro.timestamp_sample;
|
||||
|
||||
gyroSample gyro_sample;
|
||||
gyro_sample.time_us = sensor_gyro.timestamp_sample;
|
||||
gyro_sample.data = _gyro_calibration.Correct(Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z});
|
||||
gyro_sample.dt = dt_s;
|
||||
|
||||
_gyro_buffer.push(gyro_sample);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleOpticalFlow::ClearAccumulatedData()
|
||||
{
|
||||
// clear accumulated data
|
||||
_flow_integral.zero();
|
||||
_integration_timespan_us = 0;
|
||||
|
||||
_delta_angle.zero();
|
||||
|
||||
_distance_sum = NAN;
|
||||
_distance_sum_count = 0;
|
||||
|
||||
_quality_sum = 0;
|
||||
_accumulated_count = 0;
|
||||
|
||||
_gyro_integrator.reset();
|
||||
}
|
||||
|
||||
void VehicleOpticalFlow::PrintStatus()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
}; // namespace sensors
|
||||
@@ -0,0 +1,147 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "data_validator/DataValidatorGroup.hpp"
|
||||
#include "RingBuffer.hpp"
|
||||
|
||||
#include <Integrator.hpp>
|
||||
|
||||
#include <lib/mathlib/math/Limits.hpp>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/sensor_calibration/Gyroscope.hpp>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/sensor_optical_flow.h>
|
||||
#include <uORB/topics/sensor_selection.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_optical_flow.h>
|
||||
#include <uORB/topics/vehicle_optical_flow_vel.h>
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
VehicleOpticalFlow();
|
||||
~VehicleOpticalFlow() override;
|
||||
|
||||
bool Start();
|
||||
void Stop();
|
||||
|
||||
void PrintStatus();
|
||||
|
||||
private:
|
||||
void ClearAccumulatedData();
|
||||
void UpdateDistanceSensor();
|
||||
void UpdateSensorGyro();
|
||||
|
||||
void Run() override;
|
||||
|
||||
void ParametersUpdate();
|
||||
void SensorCorrectionsUpdate(bool force = false);
|
||||
|
||||
static constexpr int MAX_SENSOR_COUNT = 3;
|
||||
|
||||
uORB::Publication<vehicle_optical_flow_s> _vehicle_optical_flow_pub{ORB_ID(vehicle_optical_flow)};
|
||||
uORB::Publication<vehicle_optical_flow_vel_s> _vehicle_optical_flow_vel_pub{ORB_ID(vehicle_optical_flow_vel)};
|
||||
|
||||
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
|
||||
|
||||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
|
||||
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_flow_sub{this, ORB_ID(sensor_optical_flow)};
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)};
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
|
||||
|
||||
sensors::IntegratorConing _gyro_integrator{};
|
||||
|
||||
hrt_abstime _gyro_timestamp_sample_last{0};
|
||||
|
||||
calibration::Gyroscope _gyro_calibration{};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
matrix::Dcmf _flow_rotation{matrix::eye<float, 3>()};
|
||||
|
||||
hrt_abstime _flow_timestamp_sample_last{0};
|
||||
matrix::Vector2f _flow_integral{};
|
||||
matrix::Vector3f _delta_angle{};
|
||||
uint32_t _integration_timespan_us{};
|
||||
float _distance_sum{NAN};
|
||||
uint8_t _distance_sum_count{0};
|
||||
uint16_t _quality_sum{0};
|
||||
uint8_t _accumulated_count{0};
|
||||
|
||||
hrt_abstime _last_publication_timestamp{0};
|
||||
|
||||
int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
|
||||
hrt_abstime _last_range_sensor_update{0};
|
||||
|
||||
struct gyroSample {
|
||||
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
|
||||
matrix::Vector3f data{};
|
||||
float dt{0.f};
|
||||
};
|
||||
|
||||
struct rangeSample {
|
||||
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
|
||||
float data{};
|
||||
};
|
||||
|
||||
RingBuffer<gyroSample, 32> _gyro_buffer{};
|
||||
RingBuffer<rangeSample, 5> _range_buffer{};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::SENS_FLOW_ROT>) _param_sens_flow_rot,
|
||||
(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
|
||||
(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
|
||||
(ParamFloat<px4::params::SENS_FLOW_MAXR>) _param_sens_flow_maxr,
|
||||
(ParamFloat<px4::params::SENS_FLOW_RATE>) _param_sens_flow_rate
|
||||
)
|
||||
};
|
||||
}; // namespace sensors
|
||||
@@ -64,10 +64,10 @@
|
||||
#include <uORB/topics/esc_report.h>
|
||||
#include <uORB/topics/irlock_report.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_optical_flow.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
@@ -158,7 +158,6 @@ private:
|
||||
|
||||
void check_failure_injections();
|
||||
|
||||
int publish_flow_topic(const mavlink_hil_optical_flow_t *flow);
|
||||
int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
|
||||
int publish_distance_topic(const mavlink_distance_sensor_t *dist);
|
||||
|
||||
@@ -191,7 +190,7 @@ private:
|
||||
|
||||
// uORB publisher handlers
|
||||
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
|
||||
uORB::PublicationMulti<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
|
||||
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
|
||||
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
|
||||
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||
|
||||
@@ -665,7 +665,45 @@ void Simulator::handle_message_optical_flow(const mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
publish_flow_topic(&flow);
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = 0;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
|
||||
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
|
||||
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||
sensor_optical_flow.device_id = device_id.devid;
|
||||
|
||||
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
|
||||
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
|
||||
|
||||
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
|
||||
sensor_optical_flow.quality = flow.quality;
|
||||
|
||||
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
|
||||
sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro;
|
||||
sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro;
|
||||
sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro;
|
||||
sensor_optical_flow.delta_angle_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.max_flow_rate = NAN;
|
||||
sensor_optical_flow.min_ground_distance = NAN;
|
||||
sensor_optical_flow.max_ground_distance = NAN;
|
||||
|
||||
// Use distance value for distance sensor topic
|
||||
if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
|
||||
// Positive value (including zero): distance known. Negative value: Unknown distance.
|
||||
sensor_optical_flow.distance_m = flow.distance;
|
||||
sensor_optical_flow.distance_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
}
|
||||
|
||||
void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
|
||||
@@ -1309,50 +1347,6 @@ void Simulator::check_failure_injections()
|
||||
}
|
||||
}
|
||||
|
||||
int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
|
||||
{
|
||||
optical_flow_s flow = {};
|
||||
flow.sensor_id = flow_mavlink->sensor_id;
|
||||
flow.timestamp = hrt_absolute_time();
|
||||
flow.time_since_last_sonar_update = 0;
|
||||
flow.frame_count_since_last_readout = 0; // ?
|
||||
flow.integration_timespan = flow_mavlink->integration_time_us;
|
||||
|
||||
flow.ground_distance_m = flow_mavlink->distance;
|
||||
flow.gyro_temperature = flow_mavlink->temperature;
|
||||
flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
|
||||
flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
|
||||
flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
|
||||
flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
|
||||
flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
|
||||
flow.quality = flow_mavlink->quality;
|
||||
|
||||
/* fill in sensor limits */
|
||||
float flow_rate_max;
|
||||
param_get(param_find("SENS_FLOW_MAXR"), &flow_rate_max);
|
||||
float flow_min_hgt;
|
||||
param_get(param_find("SENS_FLOW_MINHGT"), &flow_min_hgt);
|
||||
float flow_max_hgt;
|
||||
param_get(param_find("SENS_FLOW_MAXHGT"), &flow_max_hgt);
|
||||
|
||||
flow.max_flow_rate = flow_rate_max;
|
||||
flow.min_ground_distance = flow_min_hgt;
|
||||
flow.max_ground_distance = flow_max_hgt;
|
||||
|
||||
/* rotate measurements according to parameter */
|
||||
int32_t flow_rot_int;
|
||||
param_get(param_find("SENS_FLOW_ROT"), &flow_rot_int);
|
||||
const enum Rotation flow_rot = (Rotation)flow_rot_int;
|
||||
|
||||
float zeroval = 0.0f;
|
||||
rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval);
|
||||
rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral);
|
||||
|
||||
_flow_pub.publish(flow);
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
|
||||
{
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
Reference in New Issue
Block a user