fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 15:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

LPE land bug fix and switch to fusion bit mask.

Browse Source
This commit is contained in:
James Goppert
2016-12-21 08:43:18 +01:00
committed by Lorenz Meier
parent f263ea7f7e
commit 6ff85fb927
12 changed files with 430 additions and 517 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+23 -21
View File
@@ -29,17 +29,11 @@ void BlockLocalPositionEstimator::flowInit()
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
_flowInitialized = true;
_flowFault = FAULT_NONE;
_sensorTimeout &= ~SENSOR_FLOW;
_sensorFault &= ~SENSOR_FLOW;
}
}
void BlockLocalPositionEstimator::flowDeinit()
{
_flowInitialized = false;
_flowQStats.reset();
}
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
// check for sane pitch/roll
@@ -60,7 +54,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
}
// calculate range to center of image for flow
if (!_validTZ) {
if (!(_estimatorInitialized & EST_TZ)) {
return -1;
}
@@ -82,7 +76,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
float gyro_x_rad = 0;
float gyro_y_rad = 0;
if (_flow_gyro_comp.get()) {
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(
@@ -161,7 +155,15 @@ void BlockLocalPositionEstimator::flowCorrect()
// compute polynomial value
float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev;
float rotrate_sq = _sub_att.get().rollspeed * _sub_att.get().rollspeed
+ _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
+ _sub_att.get().yawspeed * _sub_att.get().yawspeed;
float rot_sq = _eul(0) * _eul(0) + _eul(1) * _eul(1);
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev +
_flow_r.get() * _flow_r.get() * rot_sq +
_flow_rr.get() * _flow_rr.get() * rotrate_sq;
R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
// residual
@@ -179,21 +181,20 @@ void BlockLocalPositionEstimator::flowCorrect()
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_flow]) {
if (_flowFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
if (!(_sensorFault & SENSOR_FLOW)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_FLOW;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
} else if (_sensorFault & SENSOR_FLOW) {
_sensorFault &= ~SENSOR_FLOW;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
}
if (_flowFault < fault_lvl_disable) {
if (!(_sensorFault & SENSOR_FLOW)) {
Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
@@ -204,8 +205,9 @@ void BlockLocalPositionEstimator::flowCorrect()
void BlockLocalPositionEstimator::flowCheckTimeout()
{
if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
if (_flowInitialized) {
flowDeinit();
if (!(_sensorTimeout & SENSOR_FLOW)) {
_sensorTimeout |= SENSOR_FLOW;
_flowQStats.reset();
mavlink_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
}
}