From 0bfe3a44037b06beb507189609a31d3b5429c180 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Apr 2016 14:32:22 -0400 Subject: [PATCH] Updated lpe for separate sensor source files, also made more quiet. --- .../BlockLocalPositionEstimator.cpp | 1181 ++--------------- .../BlockLocalPositionEstimator.hpp | 136 +- .../local_position_estimator/CMakeLists.txt | 7 + .../local_position_estimator_main.cpp | 2 +- src/modules/local_position_estimator/params.c | 48 +- .../local_position_estimator/sensors/baro.cpp | 114 ++ .../local_position_estimator/sensors/flow.cpp | 186 +++ .../local_position_estimator/sensors/gps.cpp | 196 +++ .../sensors/lidar.cpp | 134 ++ .../sensors/mocap.cpp | 115 ++ .../sensors/sonar.cpp | 141 ++ .../sensors/vision.cpp | 113 ++ 12 files changed, 1255 insertions(+), 1118 deletions(-) create mode 100644 src/modules/local_position_estimator/sensors/baro.cpp create mode 100644 src/modules/local_position_estimator/sensors/flow.cpp create mode 100644 src/modules/local_position_estimator/sensors/gps.cpp create mode 100644 src/modules/local_position_estimator/sensors/lidar.cpp create mode 100644 src/modules/local_position_estimator/sensors/mocap.cpp create mode 100644 src/modules/local_position_estimator/sensors/sonar.cpp create mode 100644 src/modules/local_position_estimator/sensors/vision.cpp diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 926018b2b4..b7653a4047 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -4,42 +4,13 @@ #include #include -// required number of samples for sensor -// to initialize -static const int REQ_BARO_INIT_COUNT = 100; -static const int REQ_FLOW_INIT_COUNT = 20; -static const int REQ_GPS_INIT_COUNT = 10; -static const int REQ_LIDAR_INIT_COUNT = 20; -static const int REQ_SONAR_INIT_COUNT = 20; -static const int REQ_VISION_INIT_COUNT = 20; -static const int REQ_MOCAP_INIT_COUNT = 20; +orb_advert_t mavlink_log_pub; // timeouts for sensors in microseconds -static const uint32_t BARO_TIMEOUT = 1000000; // 1.0 s -static const uint32_t FLOW_TIMEOUT = 500000; // 0.5 s -static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s -static const uint32_t RANGER_TIMEOUT = 500000; // 0.5 s -static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s -static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s -static const uint32_t EST_SRC_TIMEOUT = 500000; // 0.5 s +static const uint32_t EST_SRC_TIMEOUT = 10000; // 0.01 s -// change this to set when -// the system will abort correcting a measurement -// given a fault has been detected -static fault_t fault_lvl_disable = FAULT_SEVERE; - -// for fault detection -// chi squared distribution, false alarm probability 0.0001 -// see fault_table.py -// note skip 0 index so we can use degree of freedom as index -static const float BETA_TABLE[7] = {0, - 8.82050518214, - 12.094592431, - 13.9876612368, - 16.0875642296, - 17.8797700658, - 19.6465647819, - }; +// minimum flow altitude +static const float flow_min_agl = 0.3; BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // this block has no parent, and has name LPE @@ -62,7 +33,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()), _sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()), _sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()), - _distance_subs(), + _sub_dist0(ORB_ID(distance_sensor), 0, 0, &getSubscriptions()), + _sub_dist1(ORB_ID(distance_sensor), 0, 1, &getSubscriptions()), + _sub_dist2(ORB_ID(distance_sensor), 0, 2, &getSubscriptions()), + _sub_dist3(ORB_ID(distance_sensor), 0, 3, &getSubscriptions()), + _dist_subs(), _sub_lidar(NULL), _sub_sonar(NULL), @@ -83,6 +58,7 @@ 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"), @@ -91,7 +67,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _gps_eph_max(this, "EPH_MAX"), _vision_xy_stddev(this, "VIS_XY"), _vision_z_stddev(this, "VIS_Z"), - _no_vision(this, "NO_VISION"), + _vision_on(this, "VIS_ON"), _mocap_p_stddev(this, "VIC_P"), _flow_z_offset(this, "FLW_OFF_Z"), _flow_xy_stddev(this, "FLW_XY"), @@ -103,6 +79,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _pn_b_noise_density(this, "PN_B"), _pn_t_noise_density(this, "PN_T"), + // init home + _init_home_lat(this, "LAT"), + _init_home_lon(this, "LON"), + // flow gyro _flow_gyro_x_high_pass(this, "FGYRO_HP"), _flow_gyro_y_high_pass(this, "FGYRO_HP"), @@ -136,6 +116,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _time_last_mocap(0), // initialization flags + _receivedGps(false), _baroInitialized(false), _gpsInitialized(false), _lidarInitialized(false), @@ -157,18 +138,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _flowY(0), _flowMeanQual(0), - // reference lat/lon - _gpsLatHome(0), - _gpsLonHome(0), - // status _canEstimateXY(false), _canEstimateZ(false), _canEstimateT(false), - _canEstimateGlobal(true), _xyTimeout(true), _zTimeout(true), _tzTimeout(true), + _lastArmedState(false), // faults _baroFault(FAULT_NONE), @@ -187,6 +164,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // kf matrices _x(), _u(), _P() { + // assign distance subs to array + _dist_subs[0] = &_sub_dist0; + _dist_subs[1] = &_sub_dist1; + _dist_subs[2] = &_sub_dist2; + _dist_subs[3] = &_sub_dist3; + // setup event triggering based on new flow messages to integrate _polls[POLL_FLOW].fd = _sub_flow.getHandle(); _polls[POLL_FLOW].events = POLLIN; @@ -197,16 +180,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _polls[POLL_SENSORS].fd = _sub_sensor.getHandle(); _polls[POLL_SENSORS].events = POLLIN; - //subscribe to all distance sensors - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - _distance_subs[i] = new uORB::Subscription( - ORB_ID(distance_sensor), 0, i, &getSubscriptions()); - } - // initialize P, x, u initP(); _x.setZero(); _u.setZero(); + _flowX = 0; + _flowY = 0; // perf counters _loop_perf = perf_alloc(PC_ELAPSED, @@ -246,32 +225,44 @@ void BlockLocalPositionEstimator::update() setDt(dt); // auto-detect connected rangefinders while not armed - if (!_sub_armed.get().armed && (_sub_lidar == NULL || _sub_sonar == NULL)) { - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_distance_subs[i]->get().timestamp == 0) { - continue; // ignore sensors with no data coming in - } + bool armedState = _sub_armed.get().armed; - if (_distance_subs[i]->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && - _sub_lidar == NULL) { - _sub_lidar = _distance_subs[i]; - warnx("[lpe] Lidar detected with ID %i", i); - - } else if (_distance_subs[i]->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && - _sub_sonar == NULL) { - _sub_sonar = _distance_subs[i]; - warnx("[lpe] Sonar detected with ID %i", i); - } - } + if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) { + detectDistanceSensors(); } + // reset pos, vel, and terrain on arming + if (!_lastArmedState && armedState) { + + // we just armed, we are at home position on the ground + _x(X_x) = 0; + _x(X_y) = 0; + + // the pressure altitude of home may have drifted, so we don't + // reset z to zero + + // reset flow integral + _flowX = 0; + _flowY = 0; + + // we aren't moving, all velocities are zero + _x(X_vx) = 0; + _x(X_vy) = 0; + _x(X_vz) = 0; + + // assume we are on the ground, so terrain alt is local alt + _x(X_tz) = _x(X_z); + } + + _lastArmedState = armedState; + // see which updates are available bool flowUpdated = _sub_flow.updated(); bool paramsUpdated = _sub_param_update.updated(); bool baroUpdated = _sub_sensor.updated(); - bool gpsUpdated = _sub_gps.updated(); + bool gpsUpdated = _gps_on.get() && _sub_gps.updated(); bool homeUpdated = _sub_home.updated(); - bool visionUpdated = _sub_vision_pos.updated(); + bool visionUpdated = _vision_on.get() && _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(); @@ -318,7 +309,9 @@ void BlockLocalPositionEstimator::update() // if we have no lat, lon initialize projection at 0,0 if (_canEstimateXY && !_map_ref.init_done) { - map_projection_init(&_map_ref, 0, 0); + map_projection_init(&_map_ref, + _init_home_lat.get(), + _init_home_lon.get()); } // reinitialize x if necessary @@ -339,8 +332,7 @@ void BlockLocalPositionEstimator::update() _x(i) = 0; } - mavlink_log_info(&_mavlink_log_pub, "[lpe] reinit x"); - warnx("[lpe] reinit x"); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x"); } // reinitialize P if necessary @@ -358,8 +350,7 @@ void BlockLocalPositionEstimator::update() } if (reinit_P) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] reinit P"); - warnx("[lpe] reinit P"); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P"); initP(); } @@ -369,85 +360,78 @@ void BlockLocalPositionEstimator::update() // sensor corrections/ initializations if (gpsUpdated) { if (!_gpsInitialized) { - initGps(); + gpsInit(); } else { - correctGps(); + gpsCorrect(); } } if (baroUpdated) { if (!_baroInitialized) { - initBaro(); + baroInit(); } else { - correctBaro(); + baroCorrect(); } } if (lidarUpdated) { if (!_lidarInitialized) { - initLidar(); + lidarInit(); } else { - correctLidar(); + lidarCorrect(); } } if (sonarUpdated) { if (!_sonarInitialized) { - initSonar(); + sonarInit(); } else { - correctSonar(); + sonarCorrect(); } } if (flowUpdated) { if (!_flowInitialized) { - initFlow(); + flowInit(); } else { perf_begin(_loop_perf);// TODO - correctFlow(); + flowCorrect(); //perf_count(_interval_perf); perf_end(_loop_perf); } } - if (_no_vision.get() != CBRK_NO_VISION_KEY) { // check if no vision circuit breaker is set - if (visionUpdated) { - if (!_visionInitialized) { - initVision(); + if (visionUpdated) { + if (!_visionInitialized) { + visionInit(); - } else { - correctVision(); - } + } else { + visionCorrect(); } } if (mocapUpdated) { if (!_mocapInitialized) { - initMocap(); + mocapInit(); } else { - correctMocap(); + mocapCorrect(); } } - if (!_xyTimeout && _altHomeInitialized) { + if (_altHomeInitialized) { // update all publications if possible publishLocalPos(); publishEstimatorStatus(); - if (_canEstimateGlobal) { + if (_canEstimateXY) { publishGlobalPos(); } - - } else if (!_zTimeout && _altHomeInitialized) { - // publish only Z estimate - publishLocalPos(); - publishEstimatorStatus(); } // propagate delayed state, no matter what @@ -468,95 +452,74 @@ void BlockLocalPositionEstimator::checkTimeouts() if (_timeStamp - _time_last_xy > EST_SRC_TIMEOUT) { if (!_xyTimeout) { _xyTimeout = true; - mavlink_log_info(&_mavlink_log_pub, "[lpe] xy timeout "); - warnx("[lpe] xy timeout "); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy timeout "); } } else if (_xyTimeout) { - mavlink_log_info(_mavlink_fd, "[lpe] xy resume "); - warnx("[lpe] xy resume "); + 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_log_info(&_mavlink_log_pub, "[lpe] z timeout "); - warnx("[lpe] z timeout "); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z timeout "); } } else if (_zTimeout) { - mavlink_log_info(_mavlink_fd, "[lpe] z resume "); - warnx("[lpe] z resume "); + 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_log_info(&_mavlink_log_pub, "[lpe] tz timeout "); - warnx("[lpe] tz timeout "); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz timeout "); } } else if (_tzTimeout) { - mavlink_log_info(_mavlink_fd, "[lpe] tz resume "); - warnx("[lpe] tz resume "); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz resume "); _tzTimeout = false; } - if (_timeStamp - _time_last_baro > BARO_TIMEOUT) { - if (_baroInitialized) { - _baroInitialized = false; - mavlink_log_info(&_mavlink_log_pub, "[lpe] baro timeout "); - warnx("[lpe] baro timeout "); - } - } + lidarCheckTimeout(); + sonarCheckTimeout(); + baroCheckTimeout(); + gpsCheckTimeout(); + flowCheckTimeout(); + visionCheckTimeout(); + mocapCheckTimeout(); +} - if (_timeStamp - _time_last_gps > GPS_TIMEOUT) { - if (_gpsInitialized) { - _gpsInitialized = false; - mavlink_log_info(&_mavlink_log_pub, "[lpe] GPS timeout "); - warnx("[lpe] GPS timeout "); - } - } +float BlockLocalPositionEstimator::agl() +{ + return _x(X_tz) - _x(X_z); +} - if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) { - if (_flowInitialized) { - _flowInitialized = false; - mavlink_log_info(&_mavlink_log_pub, "[lpe] flow timeout "); - warnx("[lpe] flow timeout "); - } - } +void BlockLocalPositionEstimator::detectDistanceSensors() +{ + for (int i = 0; i < N_DIST_SUBS; i++) { + uORB::Subscription *s = _dist_subs[i]; - if (_timeStamp - _time_last_sonar > RANGER_TIMEOUT) { - if (_sonarInitialized) { - _sonarInitialized = false; - mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar timeout "); - warnx("[lpe] sonar timeout "); - } - } + if (s == _sub_lidar || s == _sub_sonar) { continue; } - if (_timeStamp - _time_last_lidar > RANGER_TIMEOUT) { - if (_lidarInitialized) { - _lidarInitialized = false; - mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar timeout "); - warnx("[lpe] lidar timeout "); - } - } + if (s->updated()) { + s->update(); - if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) { - if (_visionInitialized) { - _visionInitialized = false; - mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position timeout "); - warnx("[lpe] vision position timeout "); - } - } + if (s->get().timestamp == 0) { continue; } - if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) { - if (_mocapInitialized) { - _mocapInitialized = false; - mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap timeout "); - warnx("[lpe] mocap timeout "); + 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); + } } } } @@ -571,12 +534,9 @@ void BlockLocalPositionEstimator::updateHome() // like gps and baro to be off, need to allow it // to reset by resetting covariance initP(); - mavlink_log_info(&_mavlink_log_pub, "[lpe] home " - "lat %6.2f lon %6.2f alt %5.1f m", - lat, lon, double(alt)); - warnx("[lpe] home " - "lat %6.2f lon %6.2f alt %5.1f m", - lat, lon, double(alt)); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] home " + "lat %6.2f lon %6.2f alt %5.1f m", + lat, lon, double(alt)); map_projection_init(&_map_ref, lat, lon); float delta_alt = alt - _altHome; _altHomeInitialized = true; @@ -587,267 +547,6 @@ void BlockLocalPositionEstimator::updateHome() _mocapHome(2) += delta_alt; } -void BlockLocalPositionEstimator::initBaro() -{ - // collect baro data - _baroStats.update(Scalarf(_sub_sensor.get().baro_alt_meter[0])); - _time_last_baro = _timeStamp; - - if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { - _baroAltHome = _baroStats.getMean()(0); - mavlink_log_info(&_mavlink_log_pub, - "[lpe] baro init %d m std %d cm", - (int)_baroStats.getMean()(0), - (int)(100 * _baroStats.getStdDev()(0))); - warnx("[lpe] baro init %d m std %d cm", - (int)_baroStats.getMean()(0), - (int)(100 * _baroStats.getStdDev()(0))); - _baroInitialized = true; - _baroStats.reset(); - - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _baroAltHome; - } - } -} - - -void BlockLocalPositionEstimator::initGps() -{ - // check for good gps signal - uint8_t nSat = _sub_gps.get().satellites_used; - float eph = _sub_gps.get().eph; - - if (nSat < 6 || eph > _gps_eph_max.get()) { - _gpsStats.reset(); - return; - } - - // collect gps data - Vector3 p( - _sub_gps.get().lat * 1e-7, - _sub_gps.get().lon * 1e-7, - _sub_gps.get().alt * 1e-3); - - // increament sums for mean - _gpsStats.update(p); - _time_last_gps = _timeStamp; - - if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) { - _gpsLatHome = _gpsStats.getMean()(0); - _gpsLonHome = _gpsStats.getMean()(1); - _gpsAltHome = _gpsStats.getMean()(2); - map_projection_init(&_map_ref, - _gpsLatHome, _gpsLonHome); - mavlink_log_info(&_mavlink_log_pub, "[lpe] gps init " - "lat %6.2f lon %6.2f alt %5.1f m", - _gpsLatHome, - _gpsLonHome, - double(_gpsAltHome)); - warnx("[lpe] gps init " - "lat %6.2f lon %6.2f alt %5.1f m", - _gpsLatHome, - _gpsLonHome, - double(_gpsAltHome)); - _gpsInitialized = true; - _canEstimateGlobal = true; - _gpsStats.reset(); - - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _gpsAltHome; - } - } -} - -void BlockLocalPositionEstimator::initLidar() -{ - // measure - float d = _sub_lidar->get().current_distance + _lidar_z_offset.get(); - float eps = 0.01f; - float min_dist = _sub_lidar->get().min_distance + eps; - float max_dist = _sub_lidar->get().max_distance - eps; - - // check for bad data - if (d > max_dist || d < min_dist) { - _lidarStats.reset(); - return; - } - - // update stats - _lidarStats.update(Scalarf(d)); - _time_last_lidar = _timeStamp; - - // if finished - if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) { - // if stddev too high, retry - if (_lidarStats.getStdDev()(0) > 0.1f) { - _lidarStats.reset(); - return; - } - - // not, might want to hard code this to zero - mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar init: " - "mean %d cm stddev %d cm", - int(100 * _lidarStats.getMean()(0)), - int(100 * _lidarStats.getStdDev()(0))); - warnx("[lpe] lidar init: " - "mean %d cm std %d cm", - int(100 * _lidarStats.getMean()(0)), - int(100 * _lidarStats.getStdDev()(0))); - _lidarInitialized = true; - _lidarStats.reset(); - } -} - -void BlockLocalPositionEstimator::initSonar() -{ - // measure - float d = _sub_sonar->get().current_distance + _sonar_z_offset.get(); - float eps = 0.01f; - float min_dist = _sub_sonar->get().min_distance + eps; - float max_dist = _sub_sonar->get().max_distance - eps; - - // check for bad data - if (d < min_dist || d > max_dist) { - _sonarStats.reset(); - return; - } - - // update stats - _sonarStats.update(Scalarf(d)); - _time_last_sonar = _timeStamp; - - // if finished - if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) { - // if stddev too high, retry - if (_sonarStats.getStdDev()(0) > 0.1f) { - _sonarStats.reset(); - return; - } - - // not, might want to hard code this to zero - mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar init " - "mean %d cm std %d cm", - int(100 * _sonarStats.getMean()(0)), - int(100 * _sonarStats.getStdDev()(0))); - warnx("[lpe] sonar init " - "mean %d cm std %d cm", - int(100 * _sonarStats.getMean()(0)), - int(100 * _sonarStats.getStdDev()(0))); - _sonarInitialized = true; - } -} - -void BlockLocalPositionEstimator::initFlow() -{ - // increament sums for mean - float qual = _sub_flow.get().quality; - - // check for bad data - if (qual < _flow_min_q.get()) { - _flowQStats.reset(); - return; - } - - _flowQStats.update(Scalarf(_sub_flow.get().quality)); - _time_last_flow = _timeStamp; - - if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] flow init: " - "quality %d std %d", - int(_flowQStats.getMean()(0)), - int(_flowQStats.getStdDev()(0))); - warnx("[lpe] flow init: " - "quality %d std %d", - int(_flowQStats.getMean()(0)), - int(_flowQStats.getStdDev()(0))); - _flowInitialized = true; - _flowQStats.reset(); - } -} - -void BlockLocalPositionEstimator::initVision() -{ - // collect vision position data - Vector3f pos; - pos(0) = _sub_vision_pos.get().x; - pos(1) = _sub_vision_pos.get().y; - pos(2) = _sub_vision_pos.get().z; - - // increament sums for mean - _visionStats.update(pos); - _time_last_vision_p = _timeStamp; - - if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { - _visionHome = _visionStats.getMean(); - mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position init: " - "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", - double(_visionStats.getMean()(0)), - double(_visionStats.getMean()(1)), - double(_visionStats.getMean()(2)), - double(_visionStats.getStdDev()(0)), - double(_visionStats.getStdDev()(1)), - double(_visionStats.getStdDev()(2))); - warnx("[lpe] vision position init: " - "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", - double(_visionStats.getMean()(0)), - double(_visionStats.getMean()(1)), - double(_visionStats.getMean()(2)), - double(_visionStats.getStdDev()(0)), - double(_visionStats.getStdDev()(1)), - double(_visionStats.getStdDev()(2))); - _visionInitialized = true; - _visionStats.reset(); - - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _visionHome(2); - } - } -} - -void BlockLocalPositionEstimator::initMocap() -{ - // collect mocap data - Vector3f pos; - pos(0) = _sub_mocap.get().x; - pos(1) = _sub_mocap.get().y; - pos(2) = _sub_mocap.get().z; - - // increament sums for mean - _mocapStats.update(pos); - _time_last_mocap = _timeStamp; - - if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { - _mocapHome = _mocapStats.getMean(); - mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap position init: " - "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m", - double(_mocapStats.getMean()(0)), - double(_mocapStats.getMean()(1)), - double(_mocapStats.getMean()(2)), - double(_mocapStats.getStdDev()(0)), - double(_mocapStats.getStdDev()(1)), - double(_mocapStats.getStdDev()(2))); - warnx("[lpe] mocap position init: " - "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", - double(_mocapStats.getMean()(0)), - double(_mocapStats.getMean()(1)), - double(_mocapStats.getMean()(2)), - double(_mocapStats.getStdDev()(0)), - double(_mocapStats.getStdDev()(1)), - double(_mocapStats.getStdDev()(2))); - _mocapInitialized = true; - _mocapStats.reset(); - - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _mocapHome(2); - } - } -} - void BlockLocalPositionEstimator::publishLocalPos() { // publish local position @@ -861,7 +560,7 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().v_z_valid = _canEstimateZ; _pub_lpos.get().x = _x(X_x); // north _pub_lpos.get().y = _x(X_y); // east - _pub_lpos.get().z = _x(X_z) - _x(X_tz); // down, AGL + _pub_lpos.get().z = _x(X_z); // down _pub_lpos.get().vx = _x(X_vx); // north _pub_lpos.get().vy = _x(X_vy); // east _pub_lpos.get().vz = _x(X_vz); // down @@ -872,10 +571,10 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI; _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; _pub_lpos.get().ref_alt = _sub_home.get().alt; - _pub_lpos.get().dist_bottom = -_x(X_tz); + _pub_lpos.get().dist_bottom = agl(); _pub_lpos.get().dist_bottom_rate = -_x(X_vz); _pub_lpos.get().surface_bottom_timestamp = _timeStamp; - _pub_lpos.get().dist_bottom_valid = _sonarInitialized || _lidarInitialized; + _pub_lpos.get().dist_bottom_valid = _canEstimateZ; _pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); _pub_lpos.get().epv = sqrtf(_P(X_z, X_z)); _pub_lpos.update(); @@ -884,7 +583,8 @@ void BlockLocalPositionEstimator::publishLocalPos() void BlockLocalPositionEstimator::publishEstimatorStatus() { - if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) && + if (PX4_ISFINITE(_x(X_x)) && + PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) && PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) && PX4_ISFINITE(_x(X_vz))) { _pub_est_status.get().timestamp = _timeStamp; @@ -938,8 +638,8 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().yaw = _sub_att.get().yaw; _pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); _pub_gpos.get().epv = sqrtf(_P(X_z, X_z)); - _pub_gpos.get().terrain_alt = alt - _x(X_tz); // TODO assuming this is ASL? - _pub_gpos.get().terrain_alt_valid = _lidarInitialized || _sonarInitialized; + _pub_gpos.get().terrain_alt = _altHome - _x(X_tz); + _pub_gpos.get().terrain_alt_valid = _canEstimateT; _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0]; _pub_gpos.update(); @@ -1060,630 +760,3 @@ void BlockLocalPositionEstimator::predict() _P += (A * _P + _P * A.transpose() + B * R * B.transpose() + Q) * getDt(); } - -void BlockLocalPositionEstimator::correctFlow() -{ - // check quality - float qual = _sub_flow.get().quality; - - if (qual < _flow_min_q.get()) { - if (_flowFault < FAULT_SEVERE) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] low flow quality %d", int(qual)); - warnx("[lpe] low flow quality %d", int(qual)); - _flowFault = FAULT_SEVERE; - } - - return; - } - - // imporant to timestamp flow even if distance is bad - _time_last_flow = _sub_flow.get().timestamp; - - // calculate range to center of image for flow - float d = 0; - - if (_lidarInitialized && _lidarFault < fault_lvl_disable) { - d = _sub_lidar->get().current_distance - + (_lidar_z_offset.get() - _flow_z_offset.get()); - - } else if (_sonarInitialized && _sonarFault < fault_lvl_disable) { - d = _sub_sonar->get().current_distance - + (_sonar_z_offset.get() - _flow_z_offset.get()); - - } else { - // no valid distance sensor, so return - if (_flowFault < FAULT_SEVERE) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] no distance for flow"); - warnx("[lpe] no distance for flow"); - _flowFault = FAULT_SEVERE; - } - - return; - } - - // flow measurement matrix and noise matrix - Matrix C; - C.setZero(); - C(Y_flow_x, X_x) = 1; - C(Y_flow_y, X_y) = 1; - - Matrix R; - R.setZero(); - R(Y_flow_x, Y_flow_x) = - _flow_xy_stddev.get() * _flow_xy_stddev.get(); - R(Y_flow_y, Y_flow_y) = - _flow_xy_stddev.get() * _flow_xy_stddev.get(); - - // calc dt between flow timestamps - // ignore first flow msg - if (_time_last_flow == 0) { - _time_last_flow = _sub_flow.get().timestamp; - return; - } - - // 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); - - // compute velocities in camera frame using ground distance - // assume camera frame is body frame - // TODO account for frame where flow is mounted - 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 - Vector y; - y(0) = _flowX; - y(1) = _flowY; - - // residual - Vector r = y - C * _x; - - // residual covariance, (inverse) - Matrix S_I = - inv(C * _P * C.transpose() + R); - - // fault detection - float beta = (r.transpose() * (S_I * r))(0, 0); - - if (beta > BETA_TABLE[n_y_flow]) { - if (_flowFault < FAULT_MINOR) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta)); - warnx("[lpe] flow fault, beta %5.2f", double(beta)); - _flowFault = FAULT_MINOR; - } - - } else if (_flowFault) { - _flowFault = FAULT_NONE; - mavlink_log_info(&_mavlink_log_pub, "[lpe] flow OK"); - warnx("[lpe] flow OK"); - } - - if (_flowFault < fault_lvl_disable) { - Matrix K = - _P * C.transpose() * S_I; - _x += K * r; - _P -= K * C * _P; - - } else { - // reset flow integral to current estimate of position - // if a fault occurred - _flowX = _x(X_x); - _flowY = _x(X_y); - } - -} - -void BlockLocalPositionEstimator::correctSonar() -{ - // measure - float d = _sub_sonar->get().current_distance + _sonar_z_offset.get(); - float eps = 0.01f; - float min_dist = _sub_sonar->get().min_distance + eps; - float max_dist = _sub_sonar->get().max_distance - eps; - - if (d < min_dist) { - // can't correct, so return - return; - - } else if (d > max_dist) { - if (_sonarFault < FAULT_SEVERE) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar max distance"); - warnx("[lpe] sonar max distance"); - _sonarFault = FAULT_SEVERE; - } - - // can't correct, so return - return; - } - - _time_last_sonar = _timeStamp; - - // do not use sonar if lidar is active - if (_lidarInitialized && _lidarFault < fault_lvl_disable) { return; } - - // calculate covariance - float cov = _sub_sonar->get().covariance; - - if (cov < 1.0e-3f) { - // use sensor value if reasoanble - cov = _sonar_z_stddev.get() * _sonar_z_stddev.get(); - } - - // sonar measurement matrix and noise matrix - Matrix C; - C.setZero(); - // y = -(z - tz) - // TODO could add trig to make this an EKF correction - C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir. - C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir. - - // covariance matrix - Matrix R; - R.setZero(); - R(0, 0) = cov; - - // measurement - Vector y; - y(0) = d * - cosf(_sub_att.get().roll) * - cosf(_sub_att.get().pitch); - - // residual - Vector r = y - C * _x; - - // residual covariance, (inverse) - Matrix S_I = - inv(C * _P * C.transpose() + R); - - // fault detection - float beta = (r.transpose() * (S_I * r))(0, 0); - - if (beta > BETA_TABLE[n_y_sonar]) { - if (_sonarFault < FAULT_MINOR) { - // avoid printing messages near ground - if (_x(X_tz) > 1.0f) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta)); - warnx("[lpe] sonar fault, beta %5.2f", double(beta)); - } - - _sonarFault = FAULT_MINOR; - } - - } else if (_sonarFault) { - _sonarFault = FAULT_NONE; - - // avoid printing messages near ground - if (_x(X_tz) > 1.0f) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar OK"); - warnx("[lpe] sonar OK"); - } - } - - // kalman filter correction if no fault - if (_sonarFault < fault_lvl_disable) { - Matrix K = - _P * C.transpose() * S_I; - Vector dx = K * r; - - if (!_canEstimateXY) { - dx(X_x) = 0; - dx(X_y) = 0; - dx(X_vx) = 0; - dx(X_vy) = 0; - } - - _x += dx; - _P -= K * C * _P; - } - -} - -void BlockLocalPositionEstimator::correctBaro() -{ - // measure - Vector y; - y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome; - _time_last_baro = _timeStamp; - - // baro measurement matrix - Matrix C; - C.setZero(); - C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. - - Matrix R; - R.setZero(); - R(0, 0) = _baro_stddev.get() * _baro_stddev.get(); - - // residual - Matrix S_I = - inv((C * _P * C.transpose()) + R); - Vector r = y - (C * _x); - - // fault detection - float beta = (r.transpose() * (S_I * r))(0, 0); - - if (beta > BETA_TABLE[n_y_baro]) { - if (_baroFault < FAULT_MINOR) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", - double(r(0)), double(beta)); - warnx("[lpe] baro fault, r %5.2f m, beta %5.2f", - double(r(0)), double(beta)); - _baroFault = FAULT_MINOR; - } - - } else if (_baroFault) { - _baroFault = FAULT_NONE; - mavlink_log_info(&_mavlink_log_pub, "[lpe] baro OK"); - warnx("[lpe] baro OK"); - } - - // kalman filter correction if no fault - if (_baroFault < fault_lvl_disable) { - Matrix K = _P * C.transpose() * S_I; - Vector dx = K * r; - - if (!_canEstimateXY) { - dx(X_x) = 0; - dx(X_y) = 0; - dx(X_vx) = 0; - dx(X_vy) = 0; - } - - _x += dx; - _P -= K * C * _P; - } -} - -void BlockLocalPositionEstimator::correctLidar() -{ - // measure - float d = _sub_lidar->get().current_distance + _lidar_z_offset.get(); - float eps = 0.01f; - float min_dist = _sub_lidar->get().min_distance + eps; - float max_dist = _sub_lidar->get().max_distance - eps; - - // if out of range, this is an error - if (d < min_dist) { - // can't correct, so return - return; - - } else if (d > max_dist) { - if (_lidarFault < FAULT_SEVERE) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar out of range"); - warnx("[lpe] lidar out of range"); - _lidarFault = FAULT_SEVERE; - } - - return; - } - - _time_last_lidar = _timeStamp; - - Matrix C; - C.setZero(); - // y = -(z - tz) - // TODO could add trig to make this an EKF correction - C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir. - C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir. - - // use parameter covariance unless sensor provides reasonable value - Matrix R; - R.setZero(); - float cov = _sub_lidar->get().covariance; - - if (cov < 1.0e-3f) { - R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get(); - - } else { - R(0, 0) = cov; - } - - Vector y; - y.setZero(); - y(0) = d * - cosf(_sub_att.get().roll) * - cosf(_sub_att.get().pitch); - - // residual - Matrix S_I = inv((C * _P * C.transpose()) + R); - Vector r = y - C * _x; - - // fault detection - float beta = (r.transpose() * (S_I * r))(0, 0); - - if (beta > BETA_TABLE[n_y_lidar]) { - if (_lidarFault < FAULT_MINOR) { - // only print message if above 1 meter, avoids - // message clutter when on ground - if (_x(X_tz) > 1.0f) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar fault, r %5.2f m, beta %5.2f", - double(r(0)), double(beta)); - warnx("[lpe] lidar fault, r %5.2f m, beta %5.2f", - double(r(0)), double(beta)); - } - - _lidarFault = FAULT_MINOR; - } - - } else if (_lidarFault) { // disable fault if ok - _lidarFault = FAULT_NONE; - - // only print message if above 1 meter, avoids - // message clutter when on ground - if (_x(X_tz) > 1.0f) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar OK"); - warnx("[lpe] lidar OK"); - } - } - - // kalman filter correction if no fault - if (_lidarFault < fault_lvl_disable) { - Matrix K = _P * C.transpose() * S_I; - Vector dx = K * r; - - if (!_canEstimateXY) { - dx(X_x) = 0; - dx(X_y) = 0; - dx(X_vx) = 0; - dx(X_vy) = 0; - } - - _x += dx; - _P -= K * C * _P; - } -} - -void BlockLocalPositionEstimator::correctGps() -{ - // check for good gps signal - uint8_t nSat = _sub_gps.get().satellites_used; - float eph = _sub_gps.get().eph; - - if (nSat < 6 || eph > _gps_eph_max.get()) { - if (_gpsFault < FAULT_SEVERE) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); - warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); - _gpsFault = FAULT_SEVERE; - } - - return; - } - - // gps measurement in local frame - _time_last_gps = _timeStamp; - double lat = _sub_gps.get().lat * 1.0e-7; - double lon = _sub_gps.get().lon * 1.0e-7; - float alt = _sub_gps.get().alt * 1.0e-3; - float px = 0; - float py = 0; - float pz = -(alt - _gpsAltHome); - map_projection_project(&_map_ref, lat, lon, &px, &py); - Vector y; - y.setZero(); - y(0) = px; - y(1) = py; - y(2) = pz; - y(3) = _sub_gps.get().vel_n_m_s; - y(4) = _sub_gps.get().vel_e_m_s; - y(5) = _sub_gps.get().vel_d_m_s; - - // gps measurement matrix, measures position and velocity - Matrix C; - C.setZero(); - C(Y_gps_x, X_x) = 1; - C(Y_gps_y, X_y) = 1; - C(Y_gps_z, X_z) = 1; - C(Y_gps_vx, X_vx) = 1; - C(Y_gps_vy, X_vy) = 1; - C(Y_gps_vz, X_vz) = 1; - - // gps covariance matrix - Matrix R; - R.setZero(); - - // default to parameter, use gps cov if provided - float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get(); - float var_z = _gps_z_stddev.get() * _gps_z_stddev.get(); - float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get(); - float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get(); - - // if field is not zero, set it to the value provided - if (_sub_gps.get().eph > 1e-3f) { - var_xy = _sub_gps.get().eph * _sub_gps.get().eph; - } - - if (_sub_gps.get().epv > 1e-3f) { - var_z = _sub_gps.get().epv * _sub_gps.get().epv; - } - - R(0, 0) = var_xy; - R(1, 1) = var_xy; - R(2, 2) = var_z; - R(3, 3) = var_vxy; - R(4, 4) = var_vxy; - R(5, 5) = var_vz; - - // get delayed x and P - float t_delay = 0; - int i = 0; - - for (i = 1; i < HIST_LEN; i++) { - t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0)); - - if (t_delay > _gps_delay.get()) { - break; - } - } - - // if you are 3 steps past the delay you wanted, this - // data is probably too old to use - if (t_delay > GPS_DELAY_MAX) { - warnx("[lpe] gps delayed data too old: %8.4f", double(t_delay)); - mavlink_log_info(_mavlink_fd, "[lpe] gps delayed data too old: %8.4f", double(t_delay)); - return; - } - - Vector x0 = _xDelay.get(i); - - // residual - Vector r = y - C * x0; - Matrix S_I = inv(C * _P * C.transpose() + R); - - // fault detection - float beta = (r.transpose() * (S_I * r))(0, 0); - - if (beta > BETA_TABLE[n_y_gps]) { - if (_gpsFault < FAULT_MINOR) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta)); - warnx("[lpe] gps fault, beta: %5.2f", double(beta)); - mavlink_log_info(&_mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", - double(r(0)), double(r(1)), double(r(2)), - double(r(3)), double(r(4)), double(r(5))); - mavlink_log_info(&_mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", - double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), - double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); - warnx("[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", - double(r(0)), double(r(1)), double(r(2)), - double(r(3)), double(r(4)), double(r(5))); - warnx("[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", - double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), - double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); - _gpsFault = FAULT_MINOR; - } - - } else if (_gpsFault) { - _gpsFault = FAULT_NONE; - mavlink_log_info(&_mavlink_log_pub, "[lpe] GPS OK"); - warnx("[lpe] GPS OK"); - } - - // kalman filter correction if no hard fault - if (_gpsFault < fault_lvl_disable) { - Matrix K = _P * C.transpose() * S_I; - _x += K * r; - _P -= K * C * _P; - } -} - -void BlockLocalPositionEstimator::correctVision() -{ - - Vector y; - y.setZero(); - y(0) = _sub_vision_pos.get().x - _visionHome(0); - y(1) = _sub_vision_pos.get().y - _visionHome(1); - y(2) = _sub_vision_pos.get().z - _visionHome(2); - _time_last_vision_p = _sub_vision_pos.get().timestamp_boot; - - // vision measurement matrix, measures position - Matrix C; - C.setZero(); - C(Y_vision_x, X_x) = 1; - C(Y_vision_y, X_y) = 1; - C(Y_vision_z, X_z) = 1; - - // noise matrix - Matrix R; - R.setZero(); - R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); - R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); - R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); - - // residual - Matrix S_I = inv((C * _P * C.transpose()) + R); - Matrix r = y - C * _x; - - // fault detection - float beta = (r.transpose() * (S_I * r))(0, 0); - - if (beta > BETA_TABLE[n_y_vision]) { - if (_visionFault < FAULT_MINOR) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta)); - warnx("[lpe] vision position fault, beta %5.2f", double(beta)); - _visionFault = FAULT_MINOR; - } - - } else if (_visionFault) { - _visionFault = FAULT_NONE; - mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position OK"); - warnx("[lpe] vision position OK"); - } - - // kalman filter correction if no fault - if (_visionFault < fault_lvl_disable) { - Matrix K = _P * C.transpose() * S_I; - _x += K * r; - _P -= K * C * _P; - } -} - -void BlockLocalPositionEstimator::correctMocap() -{ - // measure - Vector y; - y.setZero(); - y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0); - y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1); - y(Y_mocap_z) = _sub_mocap.get().z - _mocapHome(2); - _time_last_mocap = _sub_mocap.get().timestamp_boot; - - // mocap measurement matrix, measures position - Matrix C; - C.setZero(); - C(Y_mocap_x, X_x) = 1; - C(Y_mocap_y, X_y) = 1; - C(Y_mocap_z, X_z) = 1; - - // noise matrix - Matrix R; - R.setZero(); - float mocap_p_var = _mocap_p_stddev.get()* \ - _mocap_p_stddev.get(); - R(Y_mocap_x, Y_mocap_x) = mocap_p_var; - R(Y_mocap_y, Y_mocap_y) = mocap_p_var; - R(Y_mocap_z, Y_mocap_z) = mocap_p_var; - - // residual - Matrix S_I = inv((C * _P * C.transpose()) + R); - Matrix r = y - C * _x; - - // fault detection - float beta = (r.transpose() * (S_I * r))(0, 0); - - if (beta > BETA_TABLE[n_y_mocap]) { - if (_mocapFault < FAULT_MINOR) { - mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta)); - warnx("[lpe] mocap fault, beta %5.2f", double(beta)); - _mocapFault = FAULT_MINOR; - } - - } else if (_mocapFault) { - _mocapFault = FAULT_NONE; - mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap OK"); - warnx("[lpe] mocap OK"); - } - - // kalman filter correction if no fault - if (_mocapFault < fault_lvl_disable) { - Matrix K = _P * C.transpose() * S_I; - _x += K * r; - _P -= K * C * _P; - } -} diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 20489665b1..4ccdb329f4 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -5,14 +5,7 @@ #include #include #include - -#ifdef USE_MATRIX_LIB -#include "matrix/Matrix.hpp" -using namespace matrix; -#else -#include -using namespace Eigen; -#endif +#include // uORB Subscriptions #include @@ -37,13 +30,13 @@ using namespace Eigen; #include #include -#define CBRK_NO_VISION_KEY 328754 - +using namespace matrix; using namespace control; static const float GPS_DELAY_MAX = 0.5; // seconds static const float HIST_STEP = 0.05; // 20 hz -static const size_t HIST_LEN = (GPS_DELAY_MAX / HIST_STEP); +static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP; +static const size_t N_DIST_SUBS = 4; enum fault_t { FAULT_NONE = 0, @@ -61,12 +54,26 @@ enum sensor_t { SENSOR_MOCAP }; +// 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 +// note skip 0 index so we can use degree of freedom as index +static const float BETA_TABLE[7] = {0, + 8.82050518214, + 12.094592431, + 13.9876612368, + 16.0875642296, + 17.8797700658, + 19.6465647819, + }; + class BlockLocalPositionEstimator : public control::SuperBlock { -// -// The purpose of this estimator is to provide a robust solution for -// indoor flight. -// // dynamics: // // x(+) = A * x(-) + B * u(+) @@ -92,8 +99,8 @@ class BlockLocalPositionEstimator : public control::SuperBlock // states: // px, py, pz , ( position NED) // vx, vy, vz ( vel NED), -// bx, by, bz ( TODO accelerometer bias) -// tz (TODO terrain altitude) +// bx, by, bz ( accel bias) +// tz (terrain altitude, ASL) // // measurements: // @@ -141,27 +148,55 @@ private: // predict the next state void predict(); - // correct the state prediction with a measurement - void correctBaro(); - void correctGps(); - void correctLidar(); - void correctFlow(); - void correctSonar(); - void correctVision(); - void correctMocap(); + // lidar + int lidarMeasure(Vector &y); + void lidarCorrect(); + void lidarInit(); + void lidarCheckTimeout(); - // sensor timeout checks + // sonar + int sonarMeasure(Vector &y); + void sonarCorrect(); + void sonarInit(); + void sonarCheckTimeout(); + + // baro + int baroMeasure(Vector &y); + void baroCorrect(); + void baroInit(); + void baroCheckTimeout(); + + // gps + int gpsMeasure(Vector &y); + void gpsCorrect(); + void gpsInit(); + void gpsCheckTimeout(); + + // flow + int flowMeasure(Vector &y); + void flowCorrect(); + void flowInit(); + void flowCheckTimeout(); + + // vision + int visionMeasure(Vector &y); + void visionCorrect(); + void visionInit(); + void visionCheckTimeout(); + + // mocap + int mocapMeasure(Vector &y); + void mocapCorrect(); + void mocapInit(); + void mocapCheckTimeout(); + + // timeouts void checkTimeouts(); - // sensor initialization + // misc + float agl(); + void detectDistanceSensors(); void updateHome(); - void initBaro(); - void initGps(); - void initLidar(); - void initSonar(); - void initFlow(); - void initVision(); - void initMocap(); // publications void publishLocalPos(); @@ -185,7 +220,11 @@ private: uORB::Subscription _sub_gps; uORB::Subscription _sub_vision_pos; uORB::Subscription _sub_mocap; - uORB::Subscription *_distance_subs[ORB_MULTI_MAX_INSTANCES]; + uORB::Subscription _sub_dist0; + uORB::Subscription _sub_dist1; + uORB::Subscription _sub_dist2; + uORB::Subscription _sub_dist3; + uORB::Subscription *_dist_subs[N_DIST_SUBS]; uORB::Subscription *_sub_lidar; uORB::Subscription *_sub_sonar; @@ -216,6 +255,7 @@ private: BlockParamFloat _baro_stddev; // gps parameters + BlockParamInt _gps_on; BlockParamFloat _gps_delay; BlockParamFloat _gps_xy_stddev; BlockParamFloat _gps_z_stddev; @@ -226,7 +266,7 @@ private: // vision parameters BlockParamFloat _vision_xy_stddev; BlockParamFloat _vision_z_stddev; - BlockParamInt _no_vision; + BlockParamInt _vision_on; // mocap parameters BlockParamFloat _mocap_p_stddev; @@ -244,18 +284,22 @@ private: BlockParamFloat _pn_b_noise_density; BlockParamFloat _pn_t_noise_density; + // init home + BlockParamFloat _init_home_lat; + BlockParamFloat _init_home_lon; + // flow gyro filter BlockHighPass _flow_gyro_x_high_pass; BlockHighPass _flow_gyro_y_high_pass; // stats - BlockStats _baroStats; - BlockStats _sonarStats; - BlockStats _lidarStats; + BlockStats _baroStats; + BlockStats _sonarStats; + BlockStats _lidarStats; BlockStats _flowQStats; - BlockStats _visionStats; - BlockStats _mocapStats; - BlockStats _gpsStats; + BlockStats _visionStats; + BlockStats _mocapStats; + BlockStats _gpsStats; // delay blocks BlockDelay _xDelay; @@ -275,9 +319,9 @@ private: uint64_t _time_last_sonar; uint64_t _time_last_vision_p; uint64_t _time_last_mocap; - orb_advert_t _mavlink_log_pub; // initialization flags + bool _receivedGps; bool _baroInitialized; bool _gpsInitialized; bool _lidarInitialized; @@ -299,18 +343,14 @@ private: float _flowY; float _flowMeanQual; - // referene lat/lon - double _gpsLatHome; - double _gpsLonHome; - // status bool _canEstimateXY; bool _canEstimateZ; bool _canEstimateT; - bool _canEstimateGlobal; bool _xyTimeout; bool _zTimeout; bool _tzTimeout; + bool _lastArmedState; // sensor faults fault_t _baroFault; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt index 3702d10aa1..56ed858d57 100644 --- a/src/modules/local_position_estimator/CMakeLists.txt +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -39,6 +39,13 @@ px4_add_module( SRCS local_position_estimator_main.cpp BlockLocalPositionEstimator.cpp + sensors/flow.cpp + sensors/lidar.cpp + sensors/sonar.cpp + sensors/gps.cpp + sensors/baro.cpp + sensors/vision.cpp + sensors/mocap.cpp params.c DEPENDS platforms__common diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp index 6928dfeb1f..27256fc8e7 100644 --- a/src/modules/local_position_estimator/local_position_estimator_main.cpp +++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp @@ -112,7 +112,7 @@ int local_position_estimator_main(int argc, char *argv[]) deamon_task = px4_task_spawn_cmd("lp_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 11264, + 12000, local_position_estimator_thread_main, (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); return 0; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index be8008d8ad..18edfa7492 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -3,14 +3,6 @@ // 16 is max name length -/** - * Enable local position estimator. - * - * @boolean - * @group Local Position Estimator - */ -PARAM_DEFINE_INT32(LPE_ENABLED, 1); - /** * Enable accelerometer integration for prediction. * @@ -136,6 +128,13 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); */ PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); +/** + * Enable GPS + * + * @group Local Position Estimator + * @boolean + */ +PARAM_DEFINE_INT32(LPE_GPS_ON, 1); /** * GPS delay compensaton @@ -227,16 +226,12 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f); PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); /** - * Circuit breaker to disable vision input. - * - * Set to the appropriate key (328754) to disable vision input. + * Enabled vision correction * * @group Local Position Estimator - * @min 0 - * @max 1 - * @decimal 0 + * @boolean */ -PARAM_DEFINE_INT32(LPE_NO_VISION, 0); +PARAM_DEFINE_INT32(LPE_VIS_ON, 1); /** * Vicon position standard deviation. @@ -303,3 +298,26 @@ PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f); * @decimal 3 */ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f); + +/** + * Home latitude for nav w/o GPS + * + * @group Local Position Estimator + * @unit deg + * @min -90 + * @max 90 + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f); + +/** + * Home longitude for nav w/o GPS + * + * @group Local Position Estimator + * @unit deg + * @min -180 + * @max 180 + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(LPE_LON, -86.929); + diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp new file mode 100644 index 0000000000..f3de04856e --- /dev/null +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -0,0 +1,114 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_BARO_INIT_COUNT = 100; +static const uint32_t BARO_TIMEOUT = 100000; // 0.1 s + +void BlockLocalPositionEstimator::baroInit() +{ + // measure + Vector y; + + if (baroMeasure(y) != OK) { + _baroStats.reset(); + return; + } + + // if finished + if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { + _baroAltHome = _baroStats.getMean()(0); + mavlink_and_console_log_info(&mavlink_log_pub, + "[lpe] baro init %d m std %d cm", + (int)_baroStats.getMean()(0), + (int)(100 * _baroStats.getStdDev()(0))); + _baroInitialized = true; + _baroFault = FAULT_NONE; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _baroAltHome; + } + } +} + +int BlockLocalPositionEstimator::baroMeasure(Vector &y) +{ + //measure + y.setZero(); + y(0) = _sub_sensor.get().baro_alt_meter[0]; + _baroStats.update(y); + _time_last_baro = _timeStamp; + return OK; +} + +void BlockLocalPositionEstimator::baroCorrect() +{ + // measure + Vector y; + + if (baroMeasure(y) != OK) { return; } + + // subtract baro home alt + y -= _baroAltHome; + + // baro measurement matrix + Matrix C; + C.setZero(); + C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. + + Matrix R; + R.setZero(); + R(0, 0) = _baro_stddev.get() * _baro_stddev.get(); + + // residual + Matrix S_I = + inv((C * _P * C.transpose()) + R); + Vector r = y - (C * _x); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_baro]) { + if (_baroFault < FAULT_MINOR) { + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", + //double(r(0)), double(beta)); + _baroFault = FAULT_MINOR; + } + + } else if (_baroFault) { + _baroFault = FAULT_NONE; + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK"); + } + + // kalman filter correction if no fault + if (_baroFault < fault_lvl_disable) { + Matrix K = _P * C.transpose() * S_I; + Vector dx = K * r; + + if (!_canEstimateXY) { + dx(X_x) = 0; + dx(X_y) = 0; + dx(X_vx) = 0; + dx(X_vy) = 0; + } + + _x += dx; + _P -= K * C * _P; + } +} + +void BlockLocalPositionEstimator::baroCheckTimeout() +{ + if (_timeStamp - _time_last_baro > BARO_TIMEOUT) { + if (_baroInitialized) { + _baroInitialized = false; + _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 new file mode 100644 index 0000000000..331123ab2f --- /dev/null +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -0,0 +1,186 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +// mavlink pub +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_FLOW_INIT_COUNT = 10; +static const uint32_t FLOW_TIMEOUT = 1000000; // 1 s + +// minimum flow altitude +static const float flow_min_agl = 0.3; + +void BlockLocalPositionEstimator::flowInit() +{ + // measure + Vector y; + + if (flowMeasure(y) != OK) { + _flowQStats.reset(); + return; + } + + // if finished + if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow init: " + "quality %d std %d", + int(_flowQStats.getMean()(0)), + int(_flowQStats.getStdDev()(0))); + _flowInitialized = true; + _flowFault = FAULT_NONE; + } +} + +int BlockLocalPositionEstimator::flowMeasure(Vector &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 + float d = 0; + + if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { + d = _sub_lidar->get().current_distance + + (_lidar_z_offset.get() - _flow_z_offset.get()); + + } else if (_sonarInitialized && (_sonarFault < fault_lvl_disable)) { + d = _sub_sonar->get().current_distance + + (_sonar_z_offset.get() - _flow_z_offset.get()); + + } else { + // no valid distance data + return -1; + } + + // 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, disabled"); + _flowInitialized = false; + 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); + + // 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; +} + +void BlockLocalPositionEstimator::flowCorrect() +{ + // measure flow + Vector y; + + if (flowMeasure(y) != OK) { return; } + + // flow measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_flow_x, X_x) = 1; + C(Y_flow_y, X_y) = 1; + + Matrix R; + R.setZero(); + R(Y_flow_x, Y_flow_x) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + R(Y_flow_y, Y_flow_y) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + + // residual + Vector r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + inv(C * _P * C.transpose() + R); + + // fault detection + 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; + } + + } else if (_flowFault) { + _flowFault = FAULT_NONE; + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK"); + } + + if (_flowFault < fault_lvl_disable) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + + } else { + // reset flow integral to current estimate of position + // if a fault occurred + _flowX = _x(X_x); + _flowY = _x(X_y); + } + +} + +void BlockLocalPositionEstimator::flowCheckTimeout() +{ + if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) { + if (_flowInitialized) { + _flowInitialized = false; + _flowQStats.reset(); + mavlink_and_console_log_info(&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 new file mode 100644 index 0000000000..5bff337a55 --- /dev/null +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -0,0 +1,196 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_GPS_INIT_COUNT = 10; +static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s + +void BlockLocalPositionEstimator::gpsInit() +{ + // measure + Vector y; + + if (gpsMeasure(y) != OK) { + _gpsStats.reset(); + return; + } + + // if finished + if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) { + double gpsLatHome = _gpsStats.getMean()(0); + double gpsLonHome = _gpsStats.getMean()(1); + + if (!_receivedGps) { + _receivedGps = true; + map_projection_init(&_map_ref, gpsLatHome, gpsLonHome); + } + + _gpsAltHome = _gpsStats.getMean()(2); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init " + "lat %6.2f lon %6.2f alt %5.1f m", + gpsLatHome, + gpsLonHome, + double(_gpsAltHome)); + _gpsInitialized = true; + _gpsFault = FAULT_NONE; + _gpsStats.reset(); + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _gpsAltHome; + } + } +} + +int BlockLocalPositionEstimator::gpsMeasure(Vector &y) +{ + // check for good gps signal + uint8_t nSat = _sub_gps.get().satellites_used; + float eph = _sub_gps.get().eph; + + if (nSat < 6 || eph > _gps_eph_max.get()) { + return -1; + } + + // gps measurement + y.setZero(); + y(0) = _sub_gps.get().lat * 1e-7; + y(1) = _sub_gps.get().lon * 1e-7; + y(2) = _sub_gps.get().alt * 1e-3; + y(3) = _sub_gps.get().vel_n_m_s; + y(4) = _sub_gps.get().vel_e_m_s; + y(5) = _sub_gps.get().vel_d_m_s; + + // increament sums for mean + _gpsStats.update(y); + _time_last_gps = _timeStamp; + return OK; +} + +void BlockLocalPositionEstimator::gpsCorrect() +{ + // measure + Vector y_global; + + if (gpsMeasure(y_global) != OK) { return; } + + // gps measurement in local frame + double lat = y_global(0); + double lon = y_global(1); + float alt = y_global(2); + float px = 0; + float py = 0; + float pz = -(alt - _gpsAltHome); + map_projection_project(&_map_ref, lat, lon, &px, &py); + Vector y; + y.setZero(); + y(0) = px; + y(1) = py; + y(2) = pz; + y(3) = y_global(3); + y(4) = y_global(4); + y(5) = y_global(5); + + // gps measurement matrix, measures position and velocity + Matrix C; + C.setZero(); + C(Y_gps_x, X_x) = 1; + C(Y_gps_y, X_y) = 1; + C(Y_gps_z, X_z) = 1; + C(Y_gps_vx, X_vx) = 1; + C(Y_gps_vy, X_vy) = 1; + C(Y_gps_vz, X_vz) = 1; + + // gps covariance matrix + Matrix R; + R.setZero(); + + // default to parameter, use gps cov if provided + float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get(); + float var_z = _gps_z_stddev.get() * _gps_z_stddev.get(); + float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get(); + float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get(); + + // if field is not zero, set it to the value provided + if (_sub_gps.get().eph > 1e-3f) { + var_xy = _sub_gps.get().eph * _sub_gps.get().eph; + } + + if (_sub_gps.get().epv > 1e-3f) { + var_z = _sub_gps.get().epv * _sub_gps.get().epv; + } + + R(0, 0) = var_xy; + R(1, 1) = var_xy; + R(2, 2) = var_z; + R(3, 3) = var_vxy; + R(4, 4) = var_vxy; + R(5, 5) = var_vz; + + // get delayed x and P + float t_delay = 0; + int i = 0; + + for (i = 1; i < HIST_LEN; i++) { + t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0)); + + if (t_delay > _gps_delay.get()) { + break; + } + } + + // if you are 3 steps past the delay you wanted, this + // data is probably too old to use + if (t_delay > GPS_DELAY_MAX) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps delayed data too old: %8.4f", double(t_delay)); + return; + } + + Vector x0 = _xDelay.get(i); + + // residual + Vector r = y - C * x0; + Matrix S_I = inv(C * _P * C.transpose() + R); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_gps]) { + if (_gpsFault < FAULT_MINOR) { + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta)); + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + //double(r(0)), double(r(1)), double(r(2)), + //double(r(3)), double(r(4)), double(r(5))); + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + //double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), + //double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); + _gpsFault = FAULT_MINOR; + } + + } else if (_gpsFault) { + _gpsFault = FAULT_NONE; + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK"); + } + + // kalman filter correction if no hard fault + if (_gpsFault < fault_lvl_disable) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } +} + +void BlockLocalPositionEstimator::gpsCheckTimeout() +{ + if (_timeStamp - _time_last_gps > GPS_TIMEOUT) { + if (_gpsInitialized) { + _gpsInitialized = false; + _gpsStats.reset(); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS timeout "); + } + } +} diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp new file mode 100644 index 0000000000..ecbdb4d733 --- /dev/null +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -0,0 +1,134 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +// +static const uint32_t REQ_LIDAR_INIT_COUNT = 10; +static const uint32_t LIDAR_TIMEOUT = 1000000; // 1.0 s + +void BlockLocalPositionEstimator::lidarInit() +{ + // measure + Vector y; + + if (lidarMeasure(y) != OK) { + _lidarStats.reset(); + } + + // if finished + if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar init: " + "mean %d cm stddev %d cm", + int(100 * _lidarStats.getMean()(0)), + int(100 * _lidarStats.getStdDev()(0))); + _lidarInitialized = true; + _lidarFault = FAULT_NONE; + } +} + +int BlockLocalPositionEstimator::lidarMeasure(Vector &y) +{ + // measure + float d = _sub_lidar->get().current_distance + _lidar_z_offset.get(); + warnx("d %10.2g, lidar z offset %10.2g\n", double(d), double(_lidar_z_offset.get())); + float eps = 0.01f; + float min_dist = _sub_lidar->get().min_distance + eps; + float max_dist = _sub_lidar->get().max_distance - eps; + + // check for bad data + if (d > max_dist || d < min_dist) { + return -1; + } + + // update stats + _lidarStats.update(Scalarf(d)); + _time_last_lidar = _timeStamp; + y.setZero(); + y(0) = d; + return OK; +} + +void BlockLocalPositionEstimator::lidarCorrect() +{ + // measure lidar + Vector y; + + if (lidarMeasure(y) != OK) { return; } + + // account for leaning + y(0) = y(0) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // measurement matrix + Matrix C; + C.setZero(); + // y = -(z - tz) + // TODO could add trig to make this an EKF correction + C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir. + C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir. + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_lidar->get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + // residual + Matrix S_I = inv((C * _P * C.transpose()) + R); + Vector r = y - C * _x; + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_lidar]) { + if (_lidarFault < FAULT_MINOR) { + _lidarFault = FAULT_MINOR; + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta)); + } + + // abort correction + return; + + } else if (_lidarFault) { // disable fault if ok + _lidarFault = FAULT_NONE; + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK"); + } + + // kalman filter correction if no fault + if (_lidarFault < fault_lvl_disable) { + Matrix K = _P * C.transpose() * S_I; + Vector dx = K * r; + + if (!_canEstimateXY) { + dx(X_x) = 0; + dx(X_y) = 0; + dx(X_vx) = 0; + dx(X_vy) = 0; + } + + _x += dx; + _P -= K * C * _P; + } +} + +void BlockLocalPositionEstimator::lidarCheckTimeout() +{ + if (_timeStamp - _time_last_lidar > LIDAR_TIMEOUT) { + if (_lidarInitialized) { + _lidarInitialized = false; + _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 new file mode 100644 index 0000000000..f3c26e4fde --- /dev/null +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -0,0 +1,115 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_MOCAP_INIT_COUNT = 20; +static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s + +void BlockLocalPositionEstimator::mocapInit() +{ + // measure + Vector y; + + if (mocapMeasure(y) != OK) { + _mocapStats.reset(); + return; + } + + // if finished + if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { + _mocapHome = _mocapStats.getMean(); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: " + "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m", + double(_mocapStats.getMean()(0)), + double(_mocapStats.getMean()(1)), + double(_mocapStats.getMean()(2)), + double(_mocapStats.getStdDev()(0)), + double(_mocapStats.getStdDev()(1)), + double(_mocapStats.getStdDev()(2))); + _mocapInitialized = true; + _mocapFault = FAULT_NONE; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _mocapHome(2); + } + } +} + +int BlockLocalPositionEstimator::mocapMeasure(Vector &y) +{ + y.setZero(); + y(Y_mocap_x) = _sub_mocap.get().x; + y(Y_mocap_y) = _sub_mocap.get().y; + y(Y_mocap_z) = _sub_mocap.get().z; + _mocapStats.update(y); + _time_last_mocap = _sub_mocap.get().timestamp_boot; + return OK; +} + +void BlockLocalPositionEstimator::mocapCorrect() +{ + // measure + Vector y; + + if (mocapMeasure(y) != OK) { return; } + + // make measurement relative to home + y -= _mocapHome; + + // mocap measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_mocap_x, X_x) = 1; + C(Y_mocap_y, X_y) = 1; + C(Y_mocap_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + float mocap_p_var = _mocap_p_stddev.get()* \ + _mocap_p_stddev.get(); + R(Y_mocap_x, Y_mocap_x) = mocap_p_var; + R(Y_mocap_y, Y_mocap_y) = mocap_p_var; + R(Y_mocap_z, Y_mocap_z) = mocap_p_var; + + // residual + Matrix S_I = inv((C * _P * C.transpose()) + R); + Matrix r = y - C * _x; + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_mocap]) { + if (_mocapFault < FAULT_MINOR) { + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta)); + _mocapFault = FAULT_MINOR; + } + + } else if (_mocapFault) { + _mocapFault = FAULT_NONE; + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK"); + } + + // kalman filter correction if no fault + if (_mocapFault < fault_lvl_disable) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } +} + +void BlockLocalPositionEstimator::mocapCheckTimeout() +{ + if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) { + if (_mocapInitialized) { + _mocapInitialized = false; + _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 new file mode 100644 index 0000000000..5e6349b671 --- /dev/null +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -0,0 +1,141 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const int REQ_SONAR_INIT_COUNT = 10; +static const uint32_t SONAR_TIMEOUT = 1000000; // 1.0 s + +void BlockLocalPositionEstimator::sonarInit() +{ + // measure + Vector y; + + if (sonarMeasure(y) != OK) { + _sonarStats.reset(); + return; + } + + // if finished + if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init " + "mean %d cm std %d cm", + int(100 * _sonarStats.getMean()(0)), + int(100 * _sonarStats.getStdDev()(0))); + _sonarInitialized = true; + _sonarFault = FAULT_NONE; + } +} + +int BlockLocalPositionEstimator::sonarMeasure(Vector &y) +{ + // measure + float d = _sub_sonar->get().current_distance + _sonar_z_offset.get(); + float eps = 0.01f; + float min_dist = _sub_sonar->get().min_distance + eps; + float max_dist = _sub_sonar->get().max_distance - eps; + + // check for bad data + if (d > max_dist || d < min_dist) { + return -1; + } + + // update stats + _sonarStats.update(Scalarf(d)); + _time_last_sonar = _timeStamp; + y.setZero(); + y(0) = d * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + return OK; +} + +void BlockLocalPositionEstimator::sonarCorrect() +{ + // measure + Vector y; + + if (sonarMeasure(y) != OK) { return; } + + // do not use sonar if lidar is active + //if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { return; } + + // calculate covariance + float cov = _sub_sonar->get().covariance; + + if (cov < 1.0e-3f) { + // use sensor value if reasoanble + cov = _sonar_z_stddev.get() * _sonar_z_stddev.get(); + } + + // sonar measurement matrix and noise matrix + Matrix C; + C.setZero(); + // y = -(z - tz) + // TODO could add trig to make this an EKF correction + C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir. + C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir. + + // covariance matrix + Matrix R; + R.setZero(); + R(0, 0) = cov; + + // residual + Vector r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + inv(C * _P * C.transpose() + R); + + // fault detection + 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)); + } + + // abort correction + return; + + } else if (_sonarFault) { + _sonarFault = FAULT_NONE; + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK"); + } + + // kalman filter correction if no fault + if (_sonarFault < fault_lvl_disable) { + Matrix K = + _P * C.transpose() * S_I; + Vector dx = K * r; + + if (!_canEstimateXY) { + dx(X_x) = 0; + dx(X_y) = 0; + dx(X_vx) = 0; + dx(X_vy) = 0; + } + + _x += dx; + _P -= K * C * _P; + } + +} + +void BlockLocalPositionEstimator::sonarCheckTimeout() +{ + if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) { + if (_sonarInitialized) { + _sonarInitialized = false; + _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 new file mode 100644 index 0000000000..1b3c7ddbca --- /dev/null +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -0,0 +1,113 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_VISION_INIT_COUNT = 20; +static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s + +void BlockLocalPositionEstimator::visionInit() +{ + // measure + Vector y; + + if (visionMeasure(y) != OK) { + _visionStats.reset(); + return; + } + + // increament sums for mean + if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { + _visionHome = _visionStats.getMean(); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: " + "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", + double(_visionStats.getMean()(0)), + double(_visionStats.getMean()(1)), + double(_visionStats.getMean()(2)), + double(_visionStats.getStdDev()(0)), + double(_visionStats.getStdDev()(1)), + double(_visionStats.getStdDev()(2))); + _visionInitialized = true; + _visionFault = FAULT_NONE; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _visionHome(2); + } + } +} + +int BlockLocalPositionEstimator::visionMeasure(Vector &y) +{ + y.setZero(); + y(Y_vision_x) = _sub_vision_pos.get().x; + y(Y_vision_y) = _sub_vision_pos.get().y; + y(Y_vision_z) = _sub_vision_pos.get().z; + _visionStats.update(y); + _time_last_vision_p = _sub_vision_pos.get().timestamp_boot; + return OK; +} + +void BlockLocalPositionEstimator::visionCorrect() +{ + // measure + Vector y; + + if (visionMeasure(y) != OK) { return; } + + // make measurement relative to home + y -= _visionHome; + + // vision measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_vision_x, X_x) = 1; + C(Y_vision_y, X_y) = 1; + C(Y_vision_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); + + // residual + Matrix S_I = inv((C * _P * C.transpose()) + R); + Matrix r = y - C * _x; + + // fault detection + 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; + } + + } else if (_visionFault) { + _visionFault = FAULT_NONE; + //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK"); + } + + // kalman filter correction if no fault + if (_visionFault < fault_lvl_disable) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } +} + +void BlockLocalPositionEstimator::visionCheckTimeout() +{ + if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) { + if (_visionInitialized) { + _visionInitialized = false; + _visionStats.reset(); + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position timeout "); + } + } +}