-
Notifications
You must be signed in to change notification settings - Fork 13.4k
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.
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
Optical flow: compute velocity from corrected flow data for logging #16050
Changes from all commits
73a4637
5fefbe7
58931aa
6ba5f9c
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
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) |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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(); | ||
|
@@ -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)) { | ||
|
@@ -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); | ||
} | ||
|
@@ -1333,6 +1340,21 @@ void EKF2::publish_wind_estimate(const hrt_abstime ×tamp) | |
} | ||
} | ||
|
||
void EKF2::publish_estimator_optical_flow_vel(const hrt_abstime ×tamp) | ||
{ | ||
estimator_optical_flow_vel_s flow_vel{}; | ||
flow_vel.timestamp_sample = timestamp; | ||
|
||
_ekf.getFlowVelBody().copyTo(flow_vel.vel_body); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Could this be similar to There was a problem hiding this comment. Choose a reason for hiding this commentThe 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 There was a problem hiding this comment. Choose a reason for hiding this commentThe 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 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; | ||
|
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.