From 3ffff212e1c8c4aad21e4b21c483921284e5e8a1 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 13 Oct 2016 19:13:28 -0400 Subject: [PATCH] Added landed agl correction for lpe. --- .../BlockLocalPositionEstimator.cpp | 20 ++++ .../BlockLocalPositionEstimator.hpp | 23 +++- .../local_position_estimator/CMakeLists.txt | 1 + src/modules/local_position_estimator/params.c | 11 ++ .../local_position_estimator/sensors/land.cpp | 100 ++++++++++++++++++ 5 files changed, 153 insertions(+), 2 deletions(-) create mode 100644 src/modules/local_position_estimator/sensors/land.cpp diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 3f768b4067..c133027e71 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -18,12 +18,14 @@ 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 BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // this block has no parent, and has name LPE SuperBlock(NULL, "LPE"), // subscriptions, set rate, add to list _sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()), + _sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()), _sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()), // set flow max update rate higher than expected to we don't lose packets _sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()), @@ -86,6 +88,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : //_flow_board_x_offs(NULL, "SENS_FLW_XOFF"), //_flow_board_y_offs(NULL, "SENS_FLW_YOFF"), _flow_min_q(this, "FLW_QMIN"), + _land_z_stddev(this, "LAND_Z"), _pn_p_noise_density(this, "PN_P"), _pn_v_noise_density(this, "PN_V"), _pn_b_noise_density(this, "PN_B"), @@ -133,6 +136,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _time_init_sonar(0), _time_last_vision_p(0), _time_last_mocap(0), + _time_last_land(0), // initialization flags _receivedGps(false), @@ -143,6 +147,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _flowInitialized(false), _visionInitialized(false), _mocapInitialized(false), + _landInitialized(false), // reference altitudes _altOrigin(0), @@ -167,6 +172,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _sonarFault(FAULT_NONE), _visionFault(FAULT_NONE), _mocapFault(FAULT_NONE), + _landFault(FAULT_NONE), // loop performance _loop_perf(), @@ -286,6 +292,11 @@ void BlockLocalPositionEstimator::update() 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)); // get new data updateSubscriptions(); @@ -493,6 +504,15 @@ void BlockLocalPositionEstimator::update() } } + if (landUpdated) { + if (!_landInitialized) { + landInit(); + + } else { + landCorrect(); + } + } + if (_altOriginInitialized) { // update all publications if possible publishLocalPos(); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index cc7c0932bb..57f8a3fd4e 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -52,7 +53,8 @@ enum sensor_t { SENSOR_FLOW, SENSOR_SONAR, SENSOR_VISION, - SENSOR_MOCAP + SENSOR_MOCAP, + SENSOR_LAND, }; // change this to set when @@ -113,12 +115,14 @@ class BlockLocalPositionEstimator : public control::SuperBlock // // gps: px, py, pz, vx, vy, vz (flow is in body x, y frame) // -// lidar: px (actual measured d*cos(phi)*cos(theta)) +// lidar: pz (actual measured d*cos(phi)*cos(theta)) // // vision: px, py, pz, vx, vy, vz // // mocap: px, py, pz // +// land (detects when landed)): pz (always measures agl = 0) +// public: // constants @@ -131,6 +135,7 @@ 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_z = 0, n_y_land}; enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll}; BlockLocalPositionEstimator(); @@ -199,6 +204,12 @@ private: void mocapInit(); void mocapCheckTimeout(); + // land + int landMeasure(Vector &y); + void landCorrect(); + void landInit(); + void landCheckTimeout(); + // timeouts void checkTimeouts(); @@ -219,6 +230,7 @@ private: // subscriptions uORB::Subscription _sub_armed; + uORB::Subscription _sub_land; uORB::Subscription _sub_att; uORB::Subscription _sub_flow; uORB::Subscription _sub_sensor; @@ -291,6 +303,9 @@ private: //BlockParamFloat _flow_board_y_offs; BlockParamInt _flow_min_q; + // land parameters + BlockParamFloat _land_z_stddev; + // process noise BlockParamFloat _pn_p_noise_density; BlockParamFloat _pn_v_noise_density; @@ -314,6 +329,7 @@ private: BlockStats _visionStats; BlockStats _mocapStats; BlockStats _gpsStats; + uint16_t _landCount; // low pass BlockLowPassVector _xLowPass; @@ -338,6 +354,7 @@ private: uint64_t _time_init_sonar; uint64_t _time_last_vision_p; uint64_t _time_last_mocap; + uint64_t _time_last_land; // initialization flags bool _receivedGps; @@ -348,6 +365,7 @@ private: bool _flowInitialized; bool _visionInitialized; bool _mocapInitialized; + bool _landInitialized; // reference altitudes float _altOrigin; @@ -372,6 +390,7 @@ private: fault_t _sonarFault; fault_t _visionFault; fault_t _mocapFault; + fault_t _landFault; // performance counters perf_counter_t _loop_perf; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt index 49d78b1a63..5c8d874c6f 100644 --- a/src/modules/local_position_estimator/CMakeLists.txt +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -46,6 +46,7 @@ px4_add_module( sensors/baro.cpp sensors/vision.cpp sensors/mocap.cpp + sensors/land.cpp DEPENDS platforms__common ) diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 56bda8579f..5949d8bfa9 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -402,3 +402,14 @@ PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.3f); * @decimal 1 */ PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f); + +/** + * Land detector z standard deviation + * + * @group Local Position Estimator + * @unit m + * @min 0.001 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_LAND_Z, 0.03f); diff --git a/src/modules/local_position_estimator/sensors/land.cpp b/src/modules/local_position_estimator/sensors/land.cpp new file mode 100644 index 0000000000..df720ba533 --- /dev/null +++ b/src/modules/local_position_estimator/sensors/land.cpp @@ -0,0 +1,100 @@ +#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_LAND_INIT_COUNT = 1; +static const uint32_t LAND_TIMEOUT = 1000000; // 1.0 s + +void BlockLocalPositionEstimator::landInit() +{ + // measure + Vector y; + + if (landMeasure(y) != OK) { + _landCount = 0; + } + + // if finished + if (_landCount > REQ_LAND_INIT_COUNT) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land init"); + _landInitialized = true; + _landFault = FAULT_NONE; + } +} + +int BlockLocalPositionEstimator::landMeasure(Vector &y) +{ + _time_last_land = _timeStamp; + y.setZero(); + _landCount += 1; + return OK; +} + +void BlockLocalPositionEstimator::landCorrect() +{ + // measure land + Vector y; + + if (landMeasure(y) != OK) { return; } + + // measurement matrix + Matrix C; + C.setZero(); + // y = -(z - tz) + // TODO could add trig to make this an EKF correction + C(Y_land_z, X_z) = -1; // measured altitude, negative down dir. + C(Y_land_z, X_tz) = 1; // measured altitude, negative down dir. + + // use parameter covariance + SquareMatrix R; + R.setZero(); + R(0, 0) = _land_z_stddev.get() * _land_z_stddev.get(); + + // residual + Matrix S_I = inv((C * _P * C.transpose()) + R); + Vector r = y - C * _x; + _pub_innov.get().hagl_innov = r(0); + _pub_innov.get().hagl_innov_var = R(0, 0); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_land]) { + if (_landFault < FAULT_MINOR) { + _landFault = FAULT_MINOR; + 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"); + } + + // kalman filter correction if no fault + if (_landFault < fault_lvl_disable) { + Matrix K = _P * C.transpose() * S_I; + Vector dx = K * r; + correctionLogic(dx); + _x += dx; + _P -= K * C * _P; + } +} + +void BlockLocalPositionEstimator::landCheckTimeout() +{ + if (_timeStamp - _time_last_land > LAND_TIMEOUT) { + if (_landInitialized) { + _landInitialized = false; + _landCount = 0; + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout "); + } + } +}