diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 091f62c424..7dc60051c0 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -45,6 +45,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _dist_subs(), _sub_lidar(nullptr), _sub_sonar(nullptr), + _sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()), // publications _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), @@ -92,6 +93,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _pn_t_noise_density(this, "PN_T"), _t_max_grade(this, "T_MAX_GRADE"), + // landing target + _target_min_cov(this, "LT_COV"), + _target_mode(this, "LTEST_MODE", false), + // init origin _fake_origin(this, "FAKE_ORIGIN"), _init_origin_lat(this, "LAT"), @@ -134,6 +139,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _time_last_vision_p(0), _time_last_mocap(0), _time_last_land(0), + _time_last_target(0), // reference altitudes _altOrigin(0), @@ -313,6 +319,7 @@ void BlockLocalPositionEstimator::update() _lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated(); _sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated(); _landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate + bool targetPositionUpdated = _sub_landing_target_pose.updated(); // get new data updateSubscriptions(); @@ -528,10 +535,20 @@ void BlockLocalPositionEstimator::update() } } + if (targetPositionUpdated) { + if (_sensorTimeout & SENSOR_LAND_TARGET) { + landingTargetInit(); + + } else { + landingTargetCorrect(); + } + } + if (_altOriginInitialized) { // update all publications if possible publishLocalPos(); publishEstimatorStatus(); + _pub_innov.get().timestamp = _timeStamp; _pub_innov.update(); if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) { @@ -562,6 +579,7 @@ void BlockLocalPositionEstimator::checkTimeouts() visionCheckTimeout(); mocapCheckTimeout(); landCheckTimeout(); + landingTargetCheckTimeout(); } bool BlockLocalPositionEstimator::landed() diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 1699a1fd1c..e1f2c929aa 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -21,6 +21,7 @@ #include #include #include +#include // uORB Publications #include @@ -112,6 +113,7 @@ public: 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 = 0, Y_land_vy, Y_land_agl, n_y_land}; + enum {Y_target_x = 0, Y_target_y, n_y_target}; enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll}; enum { FUSE_GPS = 1 << 0, @@ -215,6 +217,12 @@ private: void landInit(); void landCheckTimeout(); + // landing target + int landingTargetMeasure(Vector &y); + void landingTargetCorrect(); + void landingTargetInit(); + void landingTargetCheckTimeout(); + // timeouts void checkTimeouts(); @@ -251,6 +259,7 @@ private: uORB::Subscription *_dist_subs[N_DIST_SUBS]; uORB::Subscription *_sub_lidar; uORB::Subscription *_sub_sonar; + uORB::Subscription _sub_landing_target_pose; // publications uORB::Publication _pub_lpos; @@ -318,6 +327,14 @@ private: BlockParamFloat _pn_t_noise_density; BlockParamFloat _t_max_grade; + // target mode paramters from landing_target_estimator module + enum TargetMode { + Target_Moving = 0, + Target_Stationary = 1 + }; + BlockParamFloat _target_min_cov; + BlockParamInt _target_mode; + // init origin BlockParamInt _fake_origin; BlockParamFloat _init_origin_lat; @@ -360,6 +377,7 @@ private: uint64_t _time_last_vision_p; uint64_t _time_last_mocap; uint64_t _time_last_land; + uint64_t _time_last_target; // reference altitudes float _altOrigin; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt index 9b97116090..93db848e28 100644 --- a/src/modules/local_position_estimator/CMakeLists.txt +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_module( sensors/vision.cpp sensors/mocap.cpp sensors/land.cpp + sensors/landing_target.cpp DEPENDS platforms__common ) diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 96ad57e801..740cabd514 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -424,6 +424,17 @@ PARAM_DEFINE_FLOAT(LPE_LAND_Z, 0.03f); */ PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f); +/** + * Minimum landing target standard covariance, uses reported covariance if greater. + * + * @group Local Position Estimator + * @unit m^2 + * @min 0.0 + * @max 10 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_LT_COV, 0.0001f); + /** * Integer bitmask controlling data fusion * diff --git a/src/modules/local_position_estimator/sensors/landing_target.cpp b/src/modules/local_position_estimator/sensors/landing_target.cpp new file mode 100644 index 0000000000..a2fbacfec0 --- /dev/null +++ b/src/modules/local_position_estimator/sensors/landing_target.cpp @@ -0,0 +1,121 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +static const uint64_t TARGET_TIMEOUT = 2000000; // [us] + +void BlockLocalPositionEstimator::landingTargetInit() +{ + if (_target_mode.get() == Target_Moving) { + // target is in moving mode, do not initialize + return; + } + + Vector y; + + if (landingTargetMeasure(y) == OK) { + mavlink_and_console_log_info(&mavlink_log_pub, "Landing target init"); + _sensorTimeout &= ~SENSOR_LAND_TARGET; + _sensorFault &= ~SENSOR_LAND_TARGET; + } +} + +int BlockLocalPositionEstimator::landingTargetMeasure(Vector &y) +{ + if (_target_mode.get() == Target_Stationary) { + if (_sub_landing_target_pose.get().rel_vel_valid) { + y(0) = _sub_landing_target_pose.get().vx_rel; + y(1) = _sub_landing_target_pose.get().vy_rel; + _time_last_target = _timeStamp; + + } else { + return -1; + } + + return OK; + + } + + return -1; +} + +void BlockLocalPositionEstimator::landingTargetCorrect() +{ + if (_target_mode.get() == Target_Moving) { + // nothing to do in this mode + return; + } + + // measure + Vector y; + + if (landingTargetMeasure(y) != OK) { return; } + + // calculate covariance + float cov_vx = _sub_landing_target_pose.get().cov_vx_rel; + float cov_vy = _sub_landing_target_pose.get().cov_vy_rel; + + // use sensor value only if reasoanble + if (cov_vx < _target_min_cov.get() || cov_vy < _target_min_cov.get()) { + cov_vx = _target_min_cov.get(); + cov_vy = _target_min_cov.get(); + } + + // target measurement matrix and noise matrix + Matrix C; + C.setZero(); + // residual = (y + vehicle velocity) + // sign change because target velocitiy is -vehicle velocity + C(Y_target_x, X_vx) = -1; + C(Y_target_y, X_vy) = -1; + + // covariance matrix + SquareMatrix R; + R.setZero(); + R(0, 0) = cov_vx; + R(1, 1) = cov_vy; + + // 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_target]) { + if (!(_sensorFault & SENSOR_LAND_TARGET)) { + mavlink_and_console_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta)); + _sensorFault |= SENSOR_LAND_TARGET; + } + + // abort correction + return; + + } else if (_sensorFault & SENSOR_LAND_TARGET) { + _sensorFault &= ~SENSOR_LAND_TARGET; + mavlink_and_console_log_info(&mavlink_log_pub, "Landing target OK"); + } + + // kalman filter correction + Matrix K = + _P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + _P -= K * C * _P; + +} + +void BlockLocalPositionEstimator::landingTargetCheckTimeout() +{ + if (_timeStamp - _time_last_target > TARGET_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_LAND_TARGET)) { + _sensorTimeout |= SENSOR_LAND_TARGET; + mavlink_and_console_log_info(&mavlink_log_pub, "Landing target timeout"); + } + } +}