int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
// check for agl
if (agl() < flow_min_agl) {
return -1;
}
// check quality
float qual = _sub_flow.get().quality;
if (qual < _flow_min_q.get()) {
return -1;
}
// calculate range to center of image for flow
if (!_validTZ) {
return -1;
}
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
// check for global accuracy
if (_gpsInitialized) {
double lat = _sub_gps.get().lat * 1.0e-7;
double lon = _sub_gps.get().lon * 1.0e-7;
float px = 0;
float py = 0;
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector2f delta(px - _flowX, py - _flowY);
if (delta.norm() > 3) {
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] flow too far from GPS, resetting position");
_flowX = px;
_flowY = py;
return -1;
}
}
// optical flow in x, y axis
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral;
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral;
// angular rotation in x, y axis
float gyro_x_rad = _flow_gyro_x_high_pass.update(
_sub_flow.get().gyro_x_rate_integral);
float gyro_y_rad = _flow_gyro_y_high_pass.update(
_sub_flow.get().gyro_y_rate_integral);
//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
// compute velocities in camera frame using ground distance
// assume camera frame is body frame
Vector3f delta_b(
-(flow_x_rad - gyro_x_rad)*d,
-(flow_y_rad - gyro_y_rad)*d,
0);
// rotation of flow from body to nav frame
Matrix3f R_nb(_sub_att.get().R);
Vector3f delta_n = R_nb * delta_b;
// flow integration
_flowX += delta_n(0);
_flowY += delta_n(1);
// measurement
y(Y_flow_x) = _flowX;
y(Y_flow_y) = _flowY;
_flowQStats.update(Scalarf(_sub_flow.get().quality));
// imporant to timestamp flow even if distance is bad
_time_last_flow = _timeStamp;
return OK;
}
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
// check for sane pitch/roll
if (euler.phi() > 0.5f || euler.theta() > 0.5f) {
return -1;
}
// check for agl
if (agl() < flow_min_agl) {
return -1;
}
// check quality
float qual = _sub_flow.get().quality;
if (qual < _flow_min_q.get()) {
return -1;
}
// calculate range to center of image for flow
if (!(_estimatorInitialized & EST_TZ)) {
return -1;
}
float d = agl() * cosf(euler.phi()) * cosf(euler.theta());
// 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 * _flow_scale.get();
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get();
float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
return -1;
}
// angular rotation in x, y axis
float gyro_x_rad = 0;
float gyro_y_rad = 0;
if (_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);
}
//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
// compute velocities in body frame using ground distance
// note that the integral rates in the optical_flow uORB topic are RH rotations about body axes
Vector3f delta_b(
+(flow_y_rad - gyro_y_rad) * d,
-(flow_x_rad - gyro_x_rad) * d,
0);
// rotation of flow from body to nav frame
Vector3f delta_n = _R_att * delta_b;
// imporant to timestamp flow even if distance is bad
_time_last_flow = _timeStamp;
// measurement
y(Y_flow_vx) = delta_n(0) / dt_flow;
y(Y_flow_vy) = delta_n(1) / dt_flow;
_flowQStats.update(Scalarf(_sub_flow.get().quality));
return OK;
}
