From 6ff85fb927bf65b614e4d8abb6ec3624df411710 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 3 Dec 2016 13:33:43 -0500 Subject: [PATCH] LPE land bug fix and switch to fusion bit mask. --- posix-configs/SITL/init/lpe/iris_opt_flow | 18 +- .../BlockLocalPositionEstimator.cpp | 471 +++++++----------- .../BlockLocalPositionEstimator.hpp | 119 ++--- src/modules/local_position_estimator/params.c | 93 ++-- .../local_position_estimator/sensors/baro.cpp | 28 +- .../local_position_estimator/sensors/flow.cpp | 44 +- .../local_position_estimator/sensors/gps.cpp | 64 +-- .../local_position_estimator/sensors/land.cpp | 26 +- .../sensors/lidar.cpp | 21 +- .../sensors/mocap.cpp | 19 +- .../sensors/sonar.cpp | 21 +- .../sensors/vision.cpp | 23 +- 12 files changed, 430 insertions(+), 517 deletions(-) diff --git a/posix-configs/SITL/init/lpe/iris_opt_flow b/posix-configs/SITL/init/lpe/iris_opt_flow index 76f7e812ef..4284bee8b3 100644 --- a/posix-configs/SITL/init/lpe/iris_opt_flow +++ b/posix-configs/SITL/init/lpe/iris_opt_flow @@ -23,7 +23,7 @@ param set SENS_BOARD_X_OFF 0.000001 param set COM_RC_IN_MODE 1 param set NAV_DLL_ACT 2 param set COM_DISARM_LAND 3 -param set NAV_ACC_RAD 12.0 +param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 param set RTL_LAND_DELAY 0 @@ -41,14 +41,26 @@ param set MPC_Z_VEL_I 0.15 param set MPC_XY_VEL_P 0.15 param set MPC_XY_VEL_I 0.2 -# changes for LPE indoor flight +# changes for optical flow navigation +param set MC_PITCH_P 5.0 +param set MC_ROLL_P 5.0 +param set MC_ROLLRATE_P 0.2 +param set MC_PITCHRATE_P 0.2 +param set MC_ROLLRATE_I 0.05 +param set MC_PITCHRATE_I 0.05 param set LPE_GPS_ON 0 param set MPC_ALT_MODE 1 param set LPE_T_MAX_GRADE 0 param set MPC_XY_VEL_MAX 2 param set MPC_Z_VEL_MAX 2 +param set MPC_TILTMAX_AIR 10 +param set MPC_TILTMAX_LND 10 param set MPC_XY_P 0.5 param set MIS_TAKEOFF_ALT 2 +param set NAV_ACC_RAD 1.0 +param set CBRK_GPSFAIL 240024 +param set LPE_FUSION 246 +# 11110110 no vis (1 << 3) yaw and no gps (1 << 0) replay tryapplyparams simulator start -s @@ -81,6 +93,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -sdlog2 start -r 100 -e -t -a +logger start -e -t mavlink boot_complete replay trystart diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index a9412f34c3..adf6e80b83 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -15,11 +15,12 @@ static const uint32_t EST_SRC_TIMEOUT = 10000; // 0.01 s static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m -static const bool integrate = true; // use accel for integrating static const float P_MAX = 1.0e6f; // max allowed value in state covariance static const float LAND_RATE = 10.0f; // rate of land detector correction +static const char *msg_label = "[lpe] "; // rate of land detector correction + BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // this block has no parent, and has name LPE SuperBlock(NULL, "LPE"), @@ -59,7 +60,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _map_ref(), // block parameters - _pub_agl_z(this, "PUB_AGL_Z"), + _fusion(this, "FUSION"), _vxy_pub_thresh(this, "VXY_PUB"), _z_pub_thresh(this, "Z_PUB"), _sonar_z_stddev(this, "SNR_Z"), @@ -69,7 +70,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _accel_xy_stddev(this, "ACC_XY"), _accel_z_stddev(this, "ACC_Z"), _baro_stddev(this, "BAR_Z"), - _gps_on(this, "GPS_ON"), _gps_delay(this, "GPS_DELAY"), _gps_xy_stddev(this, "GPS_XY"), _gps_z_stddev(this, "GPS_Z"), @@ -80,15 +80,16 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _vision_xy_stddev(this, "VIS_XY"), _vision_z_stddev(this, "VIS_Z"), _vision_delay(this, "VIS_DELAY"), - _vision_on(this, "VIS_ON"), _mocap_p_stddev(this, "VIC_P"), - _flow_gyro_comp(this, "FLW_GYRO_CMP"), _flow_z_offset(this, "FLW_OFF_Z"), _flow_scale(this, "FLW_SCALE"), //_flow_board_x_offs(NULL, "SENS_FLW_XOFF"), //_flow_board_y_offs(NULL, "SENS_FLW_YOFF"), _flow_min_q(this, "FLW_QMIN"), + _flow_r(this, "FLW_R"), + _flow_rr(this, "FLW_RR"), _land_z_stddev(this, "LAND_Z"), + _land_vxy_stddev(this, "LAND_VXY"), _pn_p_noise_density(this, "PN_P"), _pn_v_noise_density(this, "PN_V"), _pn_b_noise_density(this, "PN_B"), @@ -124,10 +125,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // misc _polls(), _timeStamp(hrt_absolute_time()), + _timeStampLastBaro(hrt_absolute_time()), _time_last_hist(0), - _time_last_xy(0), - _time_last_z(0), - _time_last_tz(0), _time_last_flow(0), _time_last_baro(0), _time_last_gps(0), @@ -138,17 +137,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _time_last_mocap(0), _time_last_land(0), - // initialization flags - _receivedGps(false), - _baroInitialized(false), - _gpsInitialized(false), - _lidarInitialized(false), - _sonarInitialized(false), - _flowInitialized(false), - _visionInitialized(false), - _mocapInitialized(false), - _landInitialized(false), - // reference altitudes _altOrigin(0), _altOriginInitialized(false), @@ -156,28 +144,13 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _gpsAltOrigin(0), // status - _validXY(false), - _validZ(false), - _validTZ(false), - _xyTimeout(true), - _zTimeout(true), - _tzTimeout(true), + _receivedGps(false), _lastArmedState(false), - // faults - _baroFault(FAULT_NONE), - _gpsFault(FAULT_NONE), - _lidarFault(FAULT_NONE), - _flowFault(FAULT_NONE), - _sonarFault(FAULT_NONE), - _visionFault(FAULT_NONE), - _mocapFault(FAULT_NONE), - _landFault(FAULT_NONE), - - // loop performance - _loop_perf(), - _interval_perf(), - _err_perf(), + // masks + _sensorTimeout(255), + _sensorFault(0), + _estimatorInitialized(0), // kf matrices _x(), _u(), _P(), _R_att(), _eul() @@ -203,18 +176,22 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _u.setZero(); initSS(); - // perf counters - _loop_perf = perf_alloc(PC_ELAPSED, - "local_position_estimator_runtime"); - //_interval_perf = perf_alloc(PC_INTERVAL, - //"local_position_estimator_interval"); - _err_perf = perf_alloc(PC_COUNT, "local_position_estimator_err"); - // map _map_ref.init_done = false; // intialize parameter dependent matrices updateParams(); + + // print fusion settings to console + printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, " + "vis_yaw: %d, land: %d, pub_agl_z: %d, flow_gyro: %d\n", + (_fusion.get() & FUSE_GPS) != 0, + (_fusion.get() & FUSE_FLOW) != 0, + (_fusion.get() & FUSE_VIS_POS) != 0, + (_fusion.get() & FUSE_VIS_YAW) != 0, + (_fusion.get() & FUSE_LAND) != 0, + (_fusion.get() & FUSE_PUB_AGL_Z) != 0, + (_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0); } BlockLocalPositionEstimator::~BlockLocalPositionEstimator() @@ -236,8 +213,6 @@ void BlockLocalPositionEstimator::update() int ret = px4_poll(_polls, 3, 100); if (ret < 0) { - /* poll error, count it in perf */ - perf_count(_err_perf); return; } @@ -252,7 +227,32 @@ void BlockLocalPositionEstimator::update() bool armedState = _sub_armed.get().armed; if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) { - detectDistanceSensors(); + + // detect distance sensors + for (int i = 0; i < N_DIST_SUBS; i++) { + uORB::Subscription *s = _dist_subs[i]; + + if (s == _sub_lidar || s == _sub_sonar) { continue; } + + if (s->updated()) { + s->update(); + + if (s->get().timestamp == 0) { continue; } + + if (s->get().type == \ + distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && + _sub_lidar == NULL) { + _sub_lidar = s; + mavlink_and_console_log_info(&mavlink_log_pub, "%sLidar detected with ID %i", msg_label, i); + + } else if (s->get().type == \ + distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && + _sub_sonar == NULL) { + _sub_sonar = s; + mavlink_and_console_log_info(&mavlink_log_pub, "%sSonar detected with ID %i", msg_label, i); + } + } + } } // reset pos, vel, and terrain on arming @@ -286,17 +286,29 @@ void BlockLocalPositionEstimator::update() // see which updates are available bool flowUpdated = _sub_flow.updated(); bool paramsUpdated = _sub_param_update.updated(); - bool baroUpdated = _sub_sensor.updated(); - bool gpsUpdated = _gps_on.get() && _sub_gps.updated(); - bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated(); + bool baroUpdated = false; + + if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) { + int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative; + + if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) { + uint64_t baro_timestamp = _sub_sensor.get().timestamp + \ + _sub_sensor.get().baro_timestamp_relative; + + if (baro_timestamp != _timeStampLastBaro) { + baroUpdated = true; + _timeStampLastBaro = baro_timestamp; + } + } + } + + bool gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated(); + bool visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated(); bool mocapUpdated = _sub_mocap.updated(); bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated(); bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated(); - bool landUpdated = ( - (_sub_land.get().landed || - ((!_sub_armed.get().armed) && (!_sub_land.get().freefall))) - && (!(_lidarInitialized || _mocapInitialized || _visionInitialized || _sonarInitialized)) - && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE)); + bool landUpdated = landed() + && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate // get new data updateSubscriptions(); @@ -314,16 +326,21 @@ void BlockLocalPositionEstimator::update() vxy_stddev_ok = true; } - if (_validXY) { + if (_estimatorInitialized & EST_XY) { // if valid and gps has timed out, set to not valid - if (!vxy_stddev_ok && !_gpsInitialized) { - _validXY = false; + if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) { + _estimatorInitialized &= ~EST_XY; } } else { if (vxy_stddev_ok) { - if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) { - _validXY = true; + if (!(_sensorTimeout & SENSOR_GPS) + || !(_sensorTimeout & SENSOR_FLOW) + || !(_sensorTimeout & SENSOR_VISION) + || !(_sensorTimeout & SENSOR_MOCAP) + || !(_sensorTimeout & SENSOR_LAND) + ) { + _estimatorInitialized |= EST_XY; } } } @@ -331,50 +348,37 @@ void BlockLocalPositionEstimator::update() // is z valid? bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get(); - if (_validZ) { + if (_estimatorInitialized & EST_Z) { // if valid and baro has timed out, set to not valid - if (!z_stddev_ok && !_baroInitialized) { - _validZ = false; + if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) { + _estimatorInitialized &= ~EST_Z; } } else { if (z_stddev_ok) { - _validZ = true; + _estimatorInitialized |= EST_Z; } } // is terrain valid? bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get(); - if (_validTZ) { + if (_estimatorInitialized & EST_TZ) { if (!tz_stddev_ok) { - _validTZ = false; + _estimatorInitialized &= ~EST_TZ; } } else { if (tz_stddev_ok) { - _validTZ = true; + _estimatorInitialized |= EST_TZ; } } - // timeouts - if (_validXY) { - _time_last_xy = _timeStamp; - } - - if (_validZ) { - _time_last_z = _timeStamp; - } - - if (_validTZ) { - _time_last_tz = _timeStamp; - } - // check timeouts checkTimeouts(); // if we have no lat, lon initialize projection at 0,0 - if (_validXY && !_map_ref.init_done) { + if ((_estimatorInitialized & EST_XY) && !_map_ref.init_done) { map_projection_init(&_map_ref, _init_origin_lat.get(), _init_origin_lon.get()); @@ -389,7 +393,7 @@ void BlockLocalPositionEstimator::update() // don't want it to take too long if (!PX4_ISFINITE(_x(i))) { reinit_x = true; - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i); + mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i); break; } } @@ -399,7 +403,7 @@ void BlockLocalPositionEstimator::update() _x(i) = 0; } - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x"); + mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label); } // force P symmetry and reinitialize P if necessary @@ -409,7 +413,7 @@ void BlockLocalPositionEstimator::update() for (int j = 0; j <= i; j++) { if (!PX4_ISFINITE(_P(i, j))) { mavlink_and_console_log_info(&mavlink_log_pub, - "[lpe] reinit P (%d, %d) not finite", i, j); + "%sreinit P (%d, %d) not finite", msg_label, i, j); reinit_P = true; } @@ -417,7 +421,7 @@ void BlockLocalPositionEstimator::update() // make sure diagonal elements are positive if (_P(i, i) <= 0) { mavlink_and_console_log_info(&mavlink_log_pub, - "[lpe] reinit P (%d, %d) negative", i, j); + "%sreinit P (%d, %d) negative", msg_label, i, j); reinit_P = true; } @@ -442,7 +446,7 @@ void BlockLocalPositionEstimator::update() // sensor corrections/ initializations if (gpsUpdated) { - if (!_gpsInitialized) { + if (_sensorTimeout & SENSOR_GPS) { gpsInit(); } else { @@ -451,7 +455,7 @@ void BlockLocalPositionEstimator::update() } if (baroUpdated) { - if (!_baroInitialized) { + if (_sensorTimeout & SENSOR_BARO) { baroInit(); } else { @@ -460,7 +464,7 @@ void BlockLocalPositionEstimator::update() } if (lidarUpdated) { - if (!_lidarInitialized) { + if (_sensorTimeout & SENSOR_LIDAR) { lidarInit(); } else { @@ -469,7 +473,7 @@ void BlockLocalPositionEstimator::update() } if (sonarUpdated) { - if (!_sonarInitialized) { + if (_sensorTimeout & SENSOR_SONAR) { sonarInit(); } else { @@ -478,19 +482,16 @@ void BlockLocalPositionEstimator::update() } if (flowUpdated) { - if (!_flowInitialized) { + if (_sensorTimeout & SENSOR_FLOW) { flowInit(); } else { - perf_begin(_loop_perf);// TODO flowCorrect(); - //perf_count(_interval_perf); - perf_end(_loop_perf); } } if (visionUpdated) { - if (!_visionInitialized) { + if (_sensorTimeout & SENSOR_VISION) { visionInit(); } else { @@ -499,7 +500,7 @@ void BlockLocalPositionEstimator::update() } if (mocapUpdated) { - if (!_mocapInitialized) { + if (_sensorTimeout & SENSOR_MOCAP) { mocapInit(); } else { @@ -508,7 +509,7 @@ void BlockLocalPositionEstimator::update() } if (landUpdated) { - if (!_landInitialized) { + if (_sensorTimeout & SENSOR_LAND) { landInit(); } else { @@ -522,7 +523,7 @@ void BlockLocalPositionEstimator::update() publishEstimatorStatus(); _pub_innov.update(); - if (_validXY) { + if ((_estimatorInitialized & EST_XY)) { publishGlobalPos(); } } @@ -542,155 +543,31 @@ void BlockLocalPositionEstimator::update() void BlockLocalPositionEstimator::checkTimeouts() { - if (_timeStamp - _time_last_xy > EST_SRC_TIMEOUT) { - if (!_xyTimeout) { - _xyTimeout = true; - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy timeout "); - } - - } else if (_xyTimeout) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy resume "); - _xyTimeout = false; - } - - if (_timeStamp - _time_last_z > EST_SRC_TIMEOUT) { - if (!_zTimeout) { - _zTimeout = true; - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z timeout "); - } - - } else if (_zTimeout) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z resume "); - _zTimeout = false; - } - - if (_timeStamp - _time_last_tz > EST_SRC_TIMEOUT) { - if (!_tzTimeout) { - _tzTimeout = true; - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz timeout "); - } - - } else if (_tzTimeout) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz resume "); - _tzTimeout = false; - } - - lidarCheckTimeout(); - sonarCheckTimeout(); baroCheckTimeout(); gpsCheckTimeout(); + lidarCheckTimeout(); flowCheckTimeout(); + sonarCheckTimeout(); visionCheckTimeout(); mocapCheckTimeout(); + landCheckTimeout(); } -float BlockLocalPositionEstimator::agl() +bool BlockLocalPositionEstimator::landed() { - return _x(X_tz) - _x(X_z); + if (!(_fusion.get() & FUSE_LAND)) { + return false; + } + + bool disarmed_not_falling = (!_sub_armed.get().armed) && (!_sub_land.get().freefall); + + if (!(_sub_land.get().landed || disarmed_not_falling)) { + return false; + } + + return true; } -void BlockLocalPositionEstimator::correctionLogic(Vector &dx) -{ - // don't correct bias when rotating rapidly - float ang_speed = sqrtf( - _sub_att.get().rollspeed * _sub_att.get().rollspeed + - _sub_att.get().pitchspeed * _sub_att.get().pitchspeed + - _sub_att.get().yawspeed * _sub_att.get().yawspeed); - - if (ang_speed > 1) { - dx(X_bx) = 0; - dx(X_by) = 0; - dx(X_bz) = 0; - } - - // if xy not valid, stop estimating - if (!_validXY) { - dx(X_x) = 0; - dx(X_y) = 0; - dx(X_vx) = 0; - dx(X_vy) = 0; - dx(X_bx) = 0; - dx(X_by) = 0; - } - - // if z not valid, stop estimating - if (!_validZ) { - dx(X_z) = 0; - dx(X_vz) = 0; - dx(X_bz) = 0; - } - - // if terrain not valid, stop estimating - if (!_validTZ) { - dx(X_tz) = 0; - } - - // saturate bias - float bx = dx(X_bx) + _x(X_bx); - float by = dx(X_by) + _x(X_by); - float bz = dx(X_bz) + _x(X_bz); - - if (std::abs(bx) > BIAS_MAX) { - bx = BIAS_MAX * bx / std::abs(bx); - dx(X_bx) = bx - _x(X_bx); - } - - if (std::abs(by) > BIAS_MAX) { - by = BIAS_MAX * by / std::abs(by); - dx(X_by) = by - _x(X_by); - } - - if (std::abs(bz) > BIAS_MAX) { - bz = BIAS_MAX * bz / std::abs(bz); - dx(X_bz) = bz - _x(X_bz); - } -} - - -void BlockLocalPositionEstimator::covPropagationLogic(Matrix &dP) -{ - for (int i = 0; i < n_x; i++) { - if (_P(i, i) > P_MAX) { - // if diagonal element greater than max, stop propagating - dP(i, i) = 0; - - for (int j = 0; j < n_x; j++) { - dP(i, j) = 0; - dP(j, i) = 0; - } - } - } -} - -void BlockLocalPositionEstimator::detectDistanceSensors() -{ - for (int i = 0; i < N_DIST_SUBS; i++) { - uORB::Subscription *s = _dist_subs[i]; - - if (s == _sub_lidar || s == _sub_sonar) { continue; } - - if (s->updated()) { - s->update(); - - if (s->get().timestamp == 0) { continue; } - - if (s->get().type == \ - distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && - _sub_lidar == NULL) { - _sub_lidar = s; - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] Lidar detected with ID %i", i); - - } else if (s->get().type == \ - distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && - _sub_sonar == NULL) { - _sub_sonar = s; - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] Sonar detected with ID %i", i); - } - } - } -} - - void BlockLocalPositionEstimator::publishLocalPos() { const Vector &xLP = _xLowPass.getState(); @@ -717,14 +594,14 @@ void BlockLocalPositionEstimator::publishLocalPos() PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) && PX4_ISFINITE(_x(X_vz))) { _pub_lpos.get().timestamp = _timeStamp; - _pub_lpos.get().xy_valid = _validXY; - _pub_lpos.get().z_valid = _validZ; - _pub_lpos.get().v_xy_valid = _validXY; - _pub_lpos.get().v_z_valid = _validZ; + _pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY; + _pub_lpos.get().z_valid = _estimatorInitialized & EST_Z; + _pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY; + _pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z; _pub_lpos.get().x = xLP(X_x); // north _pub_lpos.get().y = xLP(X_y); // east - if (_pub_agl_z.get()) { + if (_fusion.get() & FUSE_PUB_AGL_Z) { _pub_lpos.get().z = -_aglLowPass.getState(); // agl } else { @@ -736,8 +613,8 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().vz = xLP(X_vz); // down _pub_lpos.get().yaw = _eul(2); - _pub_lpos.get().xy_global = _validXY; - _pub_lpos.get().z_global = _baroInitialized; + _pub_lpos.get().xy_global = _estimatorInitialized & EST_XY; + _pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO); _pub_lpos.get().ref_timestamp = _timeStamp; _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI; _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; @@ -749,7 +626,7 @@ void BlockLocalPositionEstimator::publishLocalPos() // if you are in terrain following mode this is important // so that if terrain estimation fails there isn't a // sudden altitude jump - _pub_lpos.get().dist_bottom_valid = _validZ; + _pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z; _pub_lpos.get().eph = eph; _pub_lpos.get().epv = epv; _pub_lpos.update(); @@ -767,22 +644,8 @@ void BlockLocalPositionEstimator::publishEstimatorStatus() _pub_est_status.get().n_states = n_x; _pub_est_status.get().nan_flags = 0; - _pub_est_status.get().health_flags = - ((_baroFault > FAULT_NONE) << SENSOR_BARO) - + ((_gpsFault > FAULT_NONE) << SENSOR_GPS) - + ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR) - + ((_flowFault > FAULT_NONE) << SENSOR_FLOW) - + ((_sonarFault > FAULT_NONE) << SENSOR_SONAR) - + ((_visionFault > FAULT_NONE) << SENSOR_VISION) - + ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP); - _pub_est_status.get().timeout_flags = - (_baroInitialized << SENSOR_BARO) - + (_gpsInitialized << SENSOR_GPS) - + (_flowInitialized << SENSOR_FLOW) - + (_lidarInitialized << SENSOR_LIDAR) - + (_sonarInitialized << SENSOR_SONAR) - + (_visionInitialized << SENSOR_VISION) - + (_mocapInitialized << SENSOR_MOCAP); + _pub_est_status.get().health_flags = _sensorFault; + _pub_est_status.get().timeout_flags = _sensorTimeout; _pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph; _pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv; @@ -830,8 +693,8 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().eph = eph; _pub_gpos.get().epv = epv; _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); - _pub_gpos.get().terrain_alt_valid = _validTZ; - _pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout; + _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ; + _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY); _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; _pub_gpos.update(); } @@ -931,22 +794,14 @@ void BlockLocalPositionEstimator::updateSSParams() void BlockLocalPositionEstimator::predict() { - // if can't update anything, don't propagate - // state or covariance - if (!_validXY && !_validZ) { return; } - - if (integrate) { - matrix::Quaternion q(&_sub_att.get().q[0]); - _eul = matrix::Euler(q); - _R_att = matrix::Dcm(q); - Vector3f a(_sub_sensor.get().accelerometer_m_s2); - // note, bias is removed in dynamics function - _u = _R_att * a; - _u(U_az) += 9.81f; // add g - - } else { - _u = Vector3f(0, 0, 0); - } + // get acceleration + matrix::Quaternion q(&_sub_att.get().q[0]); + _eul = matrix::Euler(q); + _R_att = matrix::Dcm(q); + Vector3f a(_sub_sensor.get().accelerometer_m_s2); + // note, bias is removed in dynamics function + _u = _R_att * a; + _u(U_az) += 9.81f; // add g // update state space based on new states updateSSStates(); @@ -963,12 +818,62 @@ void BlockLocalPositionEstimator::predict() k4 = dynamics(h, _x + k3 * h, _u); Vector dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6); + // don't integrate position if no valid xy data + if (!(_estimatorInitialized & EST_XY)) { + dx(X_x) = 0; + dx(X_vx) = 0; + dx(X_y) = 0; + dx(X_vy) = 0; + } + + // don't integrate z if no valid z data + if (!(_estimatorInitialized & EST_Z)) { + dx(X_z) = 0; + } + + // don't integrate tz if no valid tz data + if (!(_estimatorInitialized & EST_TZ)) { + dx(X_tz) = 0; + } + + // saturate bias + float bx = dx(X_bx) + _x(X_bx); + float by = dx(X_by) + _x(X_by); + float bz = dx(X_bz) + _x(X_bz); + + if (std::abs(bx) > BIAS_MAX) { + bx = BIAS_MAX * bx / std::abs(bx); + dx(X_bx) = bx - _x(X_bx); + } + + if (std::abs(by) > BIAS_MAX) { + by = BIAS_MAX * by / std::abs(by); + dx(X_by) = by - _x(X_by); + } + + if (std::abs(bz) > BIAS_MAX) { + bz = BIAS_MAX * bz / std::abs(bz); + dx(X_bz) = bz - _x(X_bz); + } + // propagate - correctionLogic(dx); _x += dx; Matrix dP = (_A * _P + _P * _A.transpose() + _B * _R * _B.transpose() + _Q) * getDt(); - covPropagationLogic(dP); + + // covariance propagation logic + for (int i = 0; i < n_x; i++) { + if (_P(i, i) > P_MAX) { + // if diagonal element greater than max, stop propagating + dP(i, i) = 0; + + for (int j = 0; j < n_x; j++) { + dP(i, j) = 0; + dP(j, i) = 0; + } + } + } + _P += dP; _xLowPass.update(_x); @@ -991,7 +896,7 @@ int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods) *periods = i_hist; if (t_delay > DELAY_MAX) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] delayed data too old: %8.4f", double(t_delay)); + mavlink_and_console_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay)); return -1; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 450cdc158e..c324b86001 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -3,7 +3,6 @@ #include #include #include -#include #include #include @@ -40,28 +39,6 @@ static const float BIAS_MAX = 1e-1f; static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP; static const size_t N_DIST_SUBS = 4; -enum fault_t { - FAULT_NONE = 0, - FAULT_MINOR, - FAULT_SEVERE -}; - -enum sensor_t { - SENSOR_BARO = 0, - SENSOR_GPS, - SENSOR_LIDAR, - SENSOR_FLOW, - SENSOR_SONAR, - SENSOR_VISION, - SENSOR_MOCAP, - SENSOR_LAND, -}; - -// change this to set when -// the system will abort correcting a measurement -// given a fault has been detected -static const fault_t fault_lvl_disable = FAULT_SEVERE; - // for fault detection // chi squared distribution, false alarm probability 0.0001 // see fault_table.py @@ -135,15 +112,39 @@ public: enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps}; enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision}; enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap}; - enum {Y_land_vx, Y_land_vy, Y_land_agl = 0, n_y_land}; - enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll}; + enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land}; + enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll}; + enum { + FUSE_GPS = 1 << 0, + FUSE_FLOW = 1 << 1, + FUSE_VIS_POS = 1 << 2, + FUSE_VIS_YAW = 1 << 3, + FUSE_LAND = 1 << 4, + FUSE_PUB_AGL_Z = 1 << 5, + FUSE_FLOW_GYRO_COMP = 1 << 6, + FUSE_BARO = 1 << 7, + }; + enum sensor_t { + SENSOR_BARO = 1 << 0, + SENSOR_GPS = 1 << 1, + SENSOR_LIDAR = 1 << 2, + SENSOR_FLOW = 1 << 3, + SENSOR_SONAR = 1 << 4, + SENSOR_VISION = 1 << 5, + SENSOR_MOCAP = 1 << 6, + SENSOR_LAND = 1 << 7, + }; + + enum estimate_t { + EST_XY = 1 << 0, + EST_Z = 1 << 1, + EST_TZ = 1 << 2, + }; + + // public methods BlockLocalPositionEstimator(); void update(); - Vector dynamics( - float t, - const Vector &x, - const Vector &u); virtual ~BlockLocalPositionEstimator(); private: @@ -153,6 +154,11 @@ private: // methods // ---------------------------- + // + Vector dynamics( + float t, + const Vector &x, + const Vector &u); void initP(); void initSS(); void updateSSStates(); @@ -189,7 +195,6 @@ private: int flowMeasure(Vector &y); void flowCorrect(); void flowInit(); - void flowDeinit(); void flowCheckTimeout(); // vision @@ -214,10 +219,8 @@ private: void checkTimeouts(); // misc - float agl(); - void correctionLogic(Vector &dx); - void covPropagationLogic(Matrix &dP); - void detectDistanceSensors(); + inline float agl() { return _x(X_tz) - _x(X_z); } + bool landed(); int getDelayPeriods(float delay, uint8_t *periods); // publications @@ -257,7 +260,7 @@ private: struct map_projection_reference_s _map_ref; // general parameters - BlockParamInt _pub_agl_z; + BlockParamInt _fusion; BlockParamFloat _vxy_pub_thresh; BlockParamFloat _z_pub_thresh; @@ -277,7 +280,6 @@ private: BlockParamFloat _baro_stddev; // gps parameters - BlockParamInt _gps_on; BlockParamFloat _gps_delay; BlockParamFloat _gps_xy_stddev; BlockParamFloat _gps_z_stddev; @@ -290,21 +292,22 @@ private: BlockParamFloat _vision_xy_stddev; BlockParamFloat _vision_z_stddev; BlockParamFloat _vision_delay; - BlockParamInt _vision_on; // mocap parameters BlockParamFloat _mocap_p_stddev; // flow parameters - BlockParamInt _flow_gyro_comp; BlockParamFloat _flow_z_offset; BlockParamFloat _flow_scale; //BlockParamFloat _flow_board_x_offs; //BlockParamFloat _flow_board_y_offs; BlockParamInt _flow_min_q; + BlockParamFloat _flow_r; + BlockParamFloat _flow_rr; // land parameters BlockParamFloat _land_z_stddev; + BlockParamFloat _land_vxy_stddev; // process noise BlockParamFloat _pn_p_noise_density; @@ -342,10 +345,8 @@ private: // misc px4_pollfd_struct_t _polls[3]; uint64_t _timeStamp; + uint64_t _timeStampLastBaro; uint64_t _time_last_hist; - uint64_t _time_last_xy; - uint64_t _time_last_z; - uint64_t _time_last_tz; uint64_t _time_last_flow; uint64_t _time_last_baro; uint64_t _time_last_gps; @@ -356,17 +357,6 @@ private: uint64_t _time_last_mocap; uint64_t _time_last_land; - // initialization flags - bool _receivedGps; - bool _baroInitialized; - bool _gpsInitialized; - bool _lidarInitialized; - bool _sonarInitialized; - bool _flowInitialized; - bool _visionInitialized; - bool _mocapInitialized; - bool _landInitialized; - // reference altitudes float _altOrigin; bool _altOriginInitialized; @@ -374,28 +364,13 @@ private: float _gpsAltOrigin; // status - bool _validXY; - bool _validZ; - bool _validTZ; - bool _xyTimeout; - bool _zTimeout; - bool _tzTimeout; + bool _receivedGps; bool _lastArmedState; - // sensor faults - fault_t _baroFault; - fault_t _gpsFault; - fault_t _lidarFault; - fault_t _flowFault; - fault_t _sonarFault; - fault_t _visionFault; - fault_t _mocapFault; - fault_t _landFault; - - // performance counters - perf_counter_t _loop_perf; - perf_counter_t _interval_perf; - perf_counter_t _err_perf; + // masks + uint8_t _sensorTimeout; + uint8_t _sensorFault; + uint8_t _estimatorInitialized; // state space Vector _x; // state vector diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 5949d8bfa9..97390a1243 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -2,14 +2,6 @@ // 16 is max name length -/** - * Publish AGL as Z - * - * @group Local Position Estimator - * @boolean - */ -PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0); - /** * Optical flow z offset from center * @@ -33,15 +25,26 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f); PARAM_DEFINE_FLOAT(LPE_FLW_SCALE, 1.3f); /** - * Optical flow gyro compensation + * Optical flow rotation (roll/pitch) noise gain * * @group Local Position Estimator - * @unit m - * @min -1 - * @max 1 + * @unit m/s / (rad) + * @min 0.1 + * @max 10.0 * @decimal 3 */ -PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1); +PARAM_DEFINE_FLOAT(LPE_FLW_R, 7.0f); + +/** + * Optical flow angular velocity noise gain + * + * @group Local Position Estimator + * @unit m/s / (rad/s) + * @min 0.0 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_RR, 7.0f); /** * Optical flow minimum quality threshold @@ -131,19 +134,11 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.02f); * @group Local Position Estimator * @unit m * @min 0.01 - * @max 3 + * @max 100 * @decimal 2 */ PARAM_DEFINE_FLOAT(LPE_BAR_Z, 3.0f); -/** - * Enables GPS data, also forces alt init with GPS - * - * @group Local Position Estimator - * @boolean - */ -PARAM_DEFINE_INT32(LPE_GPS_ON, 1); - /** * GPS delay compensaton * @@ -251,19 +246,11 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.1f); * @group Local Position Estimator * @unit m * @min 0.01 - * @max 2 + * @max 100 * @decimal 3 */ PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); -/** - * Vision correction - * - * @group Local Position Estimator - * @boolean - */ -PARAM_DEFINE_INT32(LPE_VIS_ON, 1); - /** * Vicon position standard deviation. * @@ -357,7 +344,7 @@ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f); * @max 90 * @decimal 8 */ -PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f); +PARAM_DEFINE_FLOAT(LPE_LAT, 47.397742f); /** * Local origin longitude for nav w/o GPS @@ -368,7 +355,7 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f); * @max 180 * @decimal 8 */ -PARAM_DEFINE_FLOAT(LPE_LON, -86.929); +PARAM_DEFINE_FLOAT(LPE_LON, 8.545594); /** * Cut frequency for state publication @@ -413,3 +400,43 @@ PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f); * @decimal 3 */ PARAM_DEFINE_FLOAT(LPE_LAND_Z, 0.03f); + +/** + * Land detector xy velocity standard deviation + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f); + +/** + * Integer bitmask controlling data fusion + * + * Set bits in the following positions to enable: + * 0 : Set to true to fuse GPS data if available, also requires GPS for altitude init + * 1 : Set to true to fuse optical flow data if available + * 2 : Set to true to fuse vision position + * 3 : Set to true to fuse vision yaw + * 4 : Set to true to fuse land detector + * 5 : Set to true to publish AGL as local position down component + * 6 : Set to true to enable flow gyro compensation + * 7 : Set to true to enable baro fusion + * + * default (247, no vision yaw) + * + * @group Local Position Estimator + * @min 0 + * @max 255 + * @bit 0 fuse GPS, requires GPS for alt. init + * @bit 1 fuse optical flow + * @bit 2 fuse vision position + * @bit 3 fuse vision yaw + * @bit 4 fuse land detector + * @bit 5 pub agl as lpos down + * @bit 6 flow gyro compensation + * @bit 7 fuse baro + */ +PARAM_DEFINE_INT32(LPE_FUSION, 247); diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index b119a5537d..13ab3939fc 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -26,8 +26,8 @@ void BlockLocalPositionEstimator::baroInit() "[lpe] baro init %d m std %d cm", (int)_baroStats.getMean()(0), (int)(100 * _baroStats.getStdDev()(0))); - _baroInitialized = true; - _baroFault = FAULT_NONE; + _sensorTimeout &= ~SENSOR_BARO; + _sensorFault &= ~SENSOR_BARO; if (!_altOriginInitialized) { _altOriginInitialized = true; @@ -74,25 +74,21 @@ void BlockLocalPositionEstimator::baroCorrect() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_baro]) { - if (_baroFault < FAULT_MINOR) { - if (beta > 2.0f * BETA_TABLE[n_y_baro]) { - mavlink_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", - double(r(0)), double(beta)); - } - - _baroFault = FAULT_MINOR; + if (!(_sensorFault & SENSOR_BARO)) { + mavlink_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", + double(r(0)), double(beta)); + _sensorFault |= SENSOR_BARO; } - } else if (_baroFault) { - _baroFault = FAULT_NONE; - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK"); + } else if (_sensorFault & SENSOR_BARO) { + _sensorFault &= ~SENSOR_BARO; + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK"); } // kalman filter correction if no fault - if (_baroFault < fault_lvl_disable) { + if (!(_sensorFault & SENSOR_BARO)) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - correctionLogic(dx); _x += dx; _P -= K * C * _P; } @@ -101,8 +97,8 @@ void BlockLocalPositionEstimator::baroCorrect() void BlockLocalPositionEstimator::baroCheckTimeout() { if (_timeStamp - _time_last_baro > BARO_TIMEOUT) { - if (_baroInitialized) { - _baroInitialized = false; + if (!(_sensorTimeout & SENSOR_BARO)) { + _sensorTimeout |= SENSOR_BARO; _baroStats.reset(); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro timeout "); } diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index b6356cd87d..95269388d9 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -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 &y) { // check for sane pitch/roll @@ -60,7 +54,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &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 &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 K = _P * C.transpose() * S_I; Vector 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 "); } } diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 64dae7c6ab..6dad1e972f 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -42,8 +42,8 @@ void BlockLocalPositionEstimator::gpsInit() double gpsLon = _gpsStats.getMean()(1); float gpsAlt = _gpsStats.getMean()(2); - _gpsInitialized = true; - _gpsFault = FAULT_NONE; + _sensorTimeout &= ~SENSOR_GPS; + _sensorFault &= ~SENSOR_GPS; _gpsStats.reset(); if (!_receivedGps) { @@ -55,25 +55,29 @@ void BlockLocalPositionEstimator::gpsInit() _gpsAltOrigin = gpsAlt + _x(X_z); // find lat, lon of current origin by subtracting x and y - double gpsLatOrigin = 0; - double gpsLonOrigin = 0; - // reproject at current coordinates - map_projection_init(&_map_ref, gpsLat, gpsLon); - // find origin - map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin); - // reinit origin - map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin); + // if not using vision position since vision will + // have it's own origin, not necessarily where vehicle starts + if (!(_fusion.get() & FUSE_VIS_POS)) { + double gpsLatOrigin = 0; + double gpsLonOrigin = 0; + // reproject at current coordinates + map_projection_init(&_map_ref, gpsLat, gpsLon); + // find origin + map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin); + // reinit origin + map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin); - // always override alt origin on first GPS to fix - // possible baro offset in global altitude at init - _altOrigin = _gpsAltOrigin; - _altOriginInitialized = true; + // always override alt origin on first GPS to fix + // possible baro offset in global altitude at init + _altOrigin = _gpsAltOrigin; + _altOriginInitialized = true; + } PX4_INFO("[lpe] gps init " "lat %6.2f lon %6.2f alt %5.1f m", - gpsLatOrigin, - gpsLonOrigin, - double(_gpsAltOrigin)); + gpsLat, + gpsLon, + double(gpsAlt)); } } } @@ -187,26 +191,22 @@ void BlockLocalPositionEstimator::gpsCorrect() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_gps]) { - if (_gpsFault < FAULT_MINOR) { - if (beta > 3.0f * BETA_TABLE[n_y_gps]) { - mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", - double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)), - double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5))); - } - - _gpsFault = FAULT_MINOR; + if (!(_sensorFault & SENSOR_GPS)) { + mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", + double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)), + double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5))); + _sensorFault |= SENSOR_GPS; } - } else if (_gpsFault) { - _gpsFault = FAULT_NONE; - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK"); + } else if (_sensorFault & SENSOR_GPS) { + _sensorFault &= ~SENSOR_GPS; + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK"); } // kalman filter correction if no hard fault - if (_gpsFault < fault_lvl_disable) { + if (!(_sensorFault & SENSOR_GPS)) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - correctionLogic(dx); _x += dx; _P -= K * C * _P; } @@ -215,8 +215,8 @@ void BlockLocalPositionEstimator::gpsCorrect() void BlockLocalPositionEstimator::gpsCheckTimeout() { if (_timeStamp - _time_last_gps > GPS_TIMEOUT) { - if (_gpsInitialized) { - _gpsInitialized = false; + if (!(_sensorTimeout & SENSOR_GPS)) { + _sensorTimeout |= SENSOR_GPS; _gpsStats.reset(); mavlink_log_critical(&mavlink_log_pub, "[lpe] GPS timeout "); } diff --git a/src/modules/local_position_estimator/sensors/land.cpp b/src/modules/local_position_estimator/sensors/land.cpp index f65af434a7..8b951edc5f 100644 --- a/src/modules/local_position_estimator/sensors/land.cpp +++ b/src/modules/local_position_estimator/sensors/land.cpp @@ -22,8 +22,8 @@ void BlockLocalPositionEstimator::landInit() // if finished if (_landCount > REQ_LAND_INIT_COUNT) { mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land init"); - _landInitialized = true; - _landFault = FAULT_NONE; + _sensorTimeout &= ~SENSOR_LAND; + _sensorFault &= ~SENSOR_LAND; } } @@ -54,8 +54,8 @@ void BlockLocalPositionEstimator::landCorrect() // use parameter covariance SquareMatrix R; R.setZero(); - R(Y_land_vx, Y_land_vx) = 0.1; - R(Y_land_vy, Y_land_vy) = 0.1; + R(Y_land_vx, Y_land_vx) = _land_vxy_stddev.get() * _land_vxy_stddev.get(); + R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.get(); R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get(); // residual @@ -68,24 +68,23 @@ void BlockLocalPositionEstimator::landCorrect() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_land]) { - if (_landFault < FAULT_MINOR) { - _landFault = FAULT_MINOR; + if (!(_sensorFault & SENSOR_LAND)) { + _sensorFault |= SENSOR_LAND; mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta)); } // abort correction return; - } else if (_landFault) { // disable fault if ok - _landFault = FAULT_NONE; - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK"); + } else if (_sensorFault & SENSOR_LAND) { + _sensorFault &= ~SENSOR_LAND; + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK"); } // kalman filter correction if no fault - if (_landFault < fault_lvl_disable) { + if (!(_sensorFault & SENSOR_LAND)) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - correctionLogic(dx); _x += dx; _P -= K * C * _P; } @@ -94,10 +93,11 @@ void BlockLocalPositionEstimator::landCorrect() void BlockLocalPositionEstimator::landCheckTimeout() { if (_timeStamp - _time_last_land > LAND_TIMEOUT) { - if (_landInitialized) { - _landInitialized = false; + if (!(_sensorTimeout & SENSOR_LAND)) { + _sensorTimeout |= SENSOR_LAND; _landCount = 0; mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout "); } } } + diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index 05b67b25f5..8100d38dbf 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -25,8 +25,8 @@ void BlockLocalPositionEstimator::lidarInit() "mean %d cm stddev %d cm", int(100 * _lidarStats.getMean()(0)), int(100 * _lidarStats.getStdDev()(0))); - _lidarInitialized = true; - _lidarFault = FAULT_NONE; + _sensorTimeout &= ~SENSOR_LIDAR; + _sensorFault &= ~SENSOR_LIDAR; } } @@ -100,24 +100,23 @@ void BlockLocalPositionEstimator::lidarCorrect() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_lidar]) { - if (_lidarFault < FAULT_MINOR) { - _lidarFault = FAULT_MINOR; + if (!(_sensorFault & SENSOR_LIDAR)) { mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta)); + _sensorFault |= SENSOR_LIDAR; } // abort correction return; - } else if (_lidarFault) { // disable fault if ok - _lidarFault = FAULT_NONE; - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK"); + } else if (_sensorFault & SENSOR_LIDAR) { + _sensorFault &= ~SENSOR_LIDAR; + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK"); } // kalman filter correction if no fault - if (_lidarFault < fault_lvl_disable) { + if (!(_sensorFault & SENSOR_LIDAR)) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - correctionLogic(dx); _x += dx; _P -= K * C * _P; } @@ -126,8 +125,8 @@ void BlockLocalPositionEstimator::lidarCorrect() void BlockLocalPositionEstimator::lidarCheckTimeout() { if (_timeStamp - _time_last_lidar > LIDAR_TIMEOUT) { - if (_lidarInitialized) { - _lidarInitialized = false; + if (!(_sensorTimeout & SENSOR_LIDAR)) { + _sensorTimeout |= SENSOR_LIDAR; _lidarStats.reset(); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar timeout "); } diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index 525866b563..c7b729ac6e 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -29,8 +29,8 @@ void BlockLocalPositionEstimator::mocapInit() double(_mocapStats.getStdDev()(0)), double(_mocapStats.getStdDev()(1)), double(_mocapStats.getStdDev()(2))); - _mocapInitialized = true; - _mocapFault = FAULT_NONE; + _sensorTimeout &= ~SENSOR_MOCAP; + _sensorFault &= ~SENSOR_MOCAP; if (!_altOriginInitialized) { _altOriginInitialized = true; @@ -81,21 +81,20 @@ void BlockLocalPositionEstimator::mocapCorrect() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_mocap]) { - if (_mocapFault < FAULT_MINOR) { + if (!(_sensorFault & SENSOR_MOCAP)) { //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta)); - _mocapFault = FAULT_MINOR; + _sensorFault |= SENSOR_MOCAP; } - } else if (_mocapFault) { - _mocapFault = FAULT_NONE; + } else if (_sensorFault & SENSOR_MOCAP) { + _sensorFault &= ~SENSOR_MOCAP; //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK"); } // kalman filter correction if no fault - if (_mocapFault < fault_lvl_disable) { + if (!(_sensorFault & SENSOR_MOCAP)) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - correctionLogic(dx); _x += dx; _P -= K * C * _P; } @@ -104,8 +103,8 @@ void BlockLocalPositionEstimator::mocapCorrect() void BlockLocalPositionEstimator::mocapCheckTimeout() { if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) { - if (_mocapInitialized) { - _mocapInitialized = false; + if (!(_sensorTimeout & SENSOR_MOCAP)) { + _sensorTimeout |= SENSOR_MOCAP; _mocapStats.reset(); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout "); } diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index 4297e654c9..6ea193e758 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -38,8 +38,8 @@ void BlockLocalPositionEstimator::sonarInit() "mean %d cm std %d cm", int(100 * _sonarStats.getMean()(0)), int(100 * _sonarStats.getStdDev()(0))); - _sonarInitialized = true; - _sonarFault = FAULT_NONE; + _sensorTimeout &= ~SENSOR_SONAR; + _sensorFault &= ~SENSOR_SONAR; } } } @@ -116,25 +116,24 @@ void BlockLocalPositionEstimator::sonarCorrect() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_sonar]) { - if (_sonarFault < FAULT_MINOR) { - _sonarFault = FAULT_MINOR; - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta)); + if (!(_sensorFault & SENSOR_SONAR)) { + _sensorFault |= SENSOR_SONAR; + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta)); } // abort correction return; - } else if (_sonarFault) { - _sonarFault = FAULT_NONE; + } else if (_sensorFault & SENSOR_SONAR) { + _sensorFault &= ~SENSOR_SONAR; //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK"); } // kalman filter correction if no fault - if (_sonarFault < fault_lvl_disable) { + if (!(_sensorFault & SENSOR_SONAR)) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - correctionLogic(dx); _x += dx; _P -= K * C * _P; } @@ -144,8 +143,8 @@ void BlockLocalPositionEstimator::sonarCorrect() void BlockLocalPositionEstimator::sonarCheckTimeout() { if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) { - if (_sonarInitialized) { - _sonarInitialized = false; + if (!(_sensorTimeout & SENSOR_SONAR)) { + _sensorTimeout |= SENSOR_SONAR; _sonarStats.reset(); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout "); } diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 694f2bac79..3fd69ea19d 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -34,8 +34,8 @@ void BlockLocalPositionEstimator::visionInit() double(_visionStats.getStdDev()(0)), double(_visionStats.getStdDev()(1)), double(_visionStats.getStdDev()(2))); - _visionInitialized = true; - _visionFault = FAULT_NONE; + _sensorTimeout &= ~SENSOR_VISION; + _sensorFault &= ~SENSOR_VISION; if (!_altOriginInitialized) { _altOriginInitialized = true; @@ -91,21 +91,20 @@ void BlockLocalPositionEstimator::visionCorrect() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_vision]) { - if (_visionFault < FAULT_MINOR) { - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta)); - _visionFault = FAULT_MINOR; + if (!(_sensorFault & SENSOR_VISION)) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta)); + _sensorFault |= SENSOR_VISION; } - } else if (_visionFault) { - _visionFault = FAULT_NONE; - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK"); + } else if (_sensorFault & SENSOR_VISION) { + _sensorFault &= ~SENSOR_VISION; + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK"); } // kalman filter correction if no fault - if (_visionFault < fault_lvl_disable) { + if (!(_sensorFault & SENSOR_VISION)) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; - correctionLogic(dx); _x += dx; _P -= K * C * _P; } @@ -114,8 +113,8 @@ void BlockLocalPositionEstimator::visionCorrect() void BlockLocalPositionEstimator::visionCheckTimeout() { if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) { - if (_visionInitialized) { - _visionInitialized = false; + if (!(_sensorTimeout & SENSOR_VISION)) { + _sensorTimeout |= SENSOR_VISION; _visionStats.reset(); mavlink_log_critical(&mavlink_log_pub, "[lpe] vision position timeout "); }