Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Optical flow: compute velocity from corrected flow data for logging #16050

Merged
merged 4 commits into from
Oct 26, 2020
Merged

Optical flow: compute velocity from corrected flow data for logging #16050

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
73a4637
OpticalFlow: add optical flow velocity logging
bresch Oct 22, 2020
5fefbe7
pmw3901: Adjust scale factor to match gyro data
bresch Oct 23, 2020
58931aa
Flow: adjust default optical flow delay based on pmw3901
bresch Oct 23, 2020
6ba5f9c
logger: limit default estimator_optical_flow_vel logging
dagar Oct 26, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions msg/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ set(msg_files
esc_report.msg
esc_status.msg
estimator_innovations.msg
estimator_optical_flow_vel.msg
estimator_sensor_bias.msg
estimator_states.msg
estimator_status.msg
Expand Down
8 changes: 8 additions & 0 deletions msg/estimator_optical_flow_vel.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
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)
2 changes: 2 additions & 0 deletions msg/tools/uorb_rtps_message_ids.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -285,6 +285,8 @@ rtps:
id: 136
- msg: navigator_mission_item
id: 137
- msg: estimator_optical_flow_vel
id: 138
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150
Expand Down
4 changes: 2 additions & 2 deletions src/drivers/optical_flow/pmw3901/PMW3901.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -324,8 +324,8 @@ PMW3901::RunImpl()
return;
}

delta_x = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians
delta_y = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians
delta_x = (float)_flow_sum_x / 385.0f; // proportional factor + convert from pixels to radians
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Likely applies to the paw3902 as well.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@dagar I thought the same but don't have one to verify that; Are you sure that the scaling is the same?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are you sure that the scaling is the same?

No, but it's currently the same hard coded magic number and "works". We could leave it alone for now and review optical_flow_vel when someone is able to do a test.

delta_y = (float)_flow_sum_y / 385.0f; // proportional factor + convert from pixels to radians

optical_flow_s report{};
report.timestamp = timestamp;
Expand Down
2 changes: 1 addition & 1 deletion src/lib/ecl
Open in desktop
Submodule ecl updated from f666eb to dd3ffc
22 changes: 22 additions & 0 deletions src/modules/ekf2/EKF2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,6 +159,7 @@ EKF2::EKF2(bool replay_mode):
_estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise();
_estimator_innovations_pub.advertise();
_estimator_optical_flow_vel_pub.advertise();
_estimator_sensor_bias_pub.advertise();
_estimator_states_pub.advertise();
_estimator_status_pub.advertise();
Expand Down Expand Up @@ -489,6 +490,7 @@ void EKF2::Run()
}

if (_optical_flow_sub.updated()) {
_new_optical_flow_data_received = true;
optical_flow_s optical_flow;

if (_optical_flow_sub.copy(&optical_flow)) {
Expand Down Expand Up @@ -1042,6 +1044,11 @@ void EKF2::Run()

publish_yaw_estimator_status(now);

if (_new_optical_flow_data_received) {
publish_estimator_optical_flow_vel(now);
_new_optical_flow_data_received = false;
}

if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) {
_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
}
Expand Down Expand Up @@ -1333,6 +1340,21 @@ void EKF2::publish_wind_estimate(const hrt_abstime &timestamp)
}
}

void EKF2::publish_estimator_optical_flow_vel(const hrt_abstime &timestamp)
{
estimator_optical_flow_vel_s flow_vel{};
flow_vel.timestamp_sample = timestamp;

_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could this be similar to vehicle_visual_odometry_aligned where it only publishes in the same cycle that new data was received, but after the EKF update?

Copy link
Member Author

@bresch bresch Oct 26, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure if it's worth doing this because the optical flow update rate is probably faster than the EKF update rate, but at least we don't publish it when there is no optical flow sensor

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's more of a log visibility (and minor optimization) thing, only having estimator_optical_flow_vel when you actually had optical_flow and were using it.

Getting the optical_flow data rate within the EKF update rate is one of the other reasons I was going to pipe it through the sensors module first.

_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);
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();

_estimator_optical_flow_vel_pub.publish(flow_vel);
}

float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
{
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
Expand Down
5 changes: 5 additions & 0 deletions src/modules/ekf2/EKF2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/yaw_estimator_status.h>
#include <uORB/topics/estimator_optical_flow_vel.h>

#include "Utility/PreFlightChecker.hpp"

Expand Down Expand Up @@ -125,6 +126,7 @@ class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::Sch
void publish_odometry(const hrt_abstime &timestamp, const imuSample &imu, const vehicle_local_position_s &lpos);
void publish_wind_estimate(const hrt_abstime &timestamp);
void publish_yaw_estimator_status(const hrt_abstime &timestamp);
void publish_estimator_optical_flow_vel(const hrt_abstime &timestamp);

/*
* Calculate filtered WGS84 height from estimated AMSL height
Expand Down Expand Up @@ -169,6 +171,8 @@ class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::Sch
bool new_ev_data_received = false;
vehicle_odometry_s _ev_odom{};

bool _new_optical_flow_data_received{false};

uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
Expand Down Expand Up @@ -201,6 +205,7 @@ class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::Sch
uORB::Publication<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::Publication<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::Publication<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::Publication<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::Publication<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationData<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
Expand Down
2 changes: 1 addition & 1 deletion src/modules/ekf2/ekf2_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 110);
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_OF_DELAY, 5);
PARAM_DEFINE_FLOAT(EKF2_OF_DELAY, 20);

/**
* Range finder measurement delay relative to IMU measurements
Expand Down
1 change: 1 addition & 0 deletions src/modules/logger/logged_topics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ void LoggedTopics::add_default_topics()
add_topic("estimator_innovation_test_ratios", 200);
add_topic("estimator_innovation_variances", 200);
add_topic("estimator_innovations", 200);
add_topic("estimator_optical_flow_vel", 200);
add_topic("estimator_sensor_bias", 1000);
add_topic("estimator_states", 1000);
add_topic("estimator_status", 200);
Expand Down