From e63747ec2f52761ed9b9d795a2f6eeadf44e93cf Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Sat, 21 Jan 2017 14:30:22 +0530 Subject: [PATCH] lpe : changes to allow hybrid GPS-denied navigation --- .../BlockLocalPositionEstimator.cpp | 11 +++++--- .../BlockLocalPositionEstimator.hpp | 1 + src/modules/local_position_estimator/params.c | 12 ++++++++- .../local_position_estimator/sensors/gps.cpp | 5 +++- .../sensors/vision.cpp | 25 ++++++++++++------- 5 files changed, 40 insertions(+), 14 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 5a16932f79..0fea08902b 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -94,6 +94,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _t_max_grade(this, "T_MAX_GRADE"), // init origin + _fake_origin(this, "FAKE_ORIGIN"), _init_origin_lat(this, "LAT"), _init_origin_lon(this, "LON"), @@ -374,11 +375,15 @@ void BlockLocalPositionEstimator::update() // check timeouts checkTimeouts(); - // if we have no lat, lon initialize projection at 0,0 - if ((_estimatorInitialized & EST_XY) && !_map_ref.init_done) { + // if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters + if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get()) { map_projection_init(&_map_ref, _init_origin_lat.get(), _init_origin_lon.get()); + + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m", + double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin)); + } // reinitialize x if necessary @@ -520,7 +525,7 @@ void BlockLocalPositionEstimator::update() publishEstimatorStatus(); _pub_innov.update(); - if ((_estimatorInitialized & EST_XY)) { + if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) { publishGlobalPos(); } } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 21bc0e921a..19cad86f48 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -316,6 +316,7 @@ private: BlockParamFloat _t_max_grade; // init origin + BlockParamInt _fake_origin; BlockParamFloat _init_origin_lat; BlockParamFloat _init_origin_lon; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 72ed4b624c..94b10a0f04 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -337,6 +337,16 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f); */ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f); +/** + * Enable publishing of a fake global position (e.g for AUTO missions using Optical Flow) + * by initializing the estimator to the LPE_LAT/LON parameters when global information is unavailable + * + * @group Local Position Estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(LPE_FAKE_ORIGIN, 1); + /** * Local origin latitude for nav w/o GPS * @@ -441,4 +451,4 @@ PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f); * @bit 6 flow gyro compensation * @bit 7 fuse baro */ -PARAM_DEFINE_INT32(LPE_FUSION, 247); +PARAM_DEFINE_INT32(LPE_FUSION, 247); \ No newline at end of file diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index d7beab2fb2..921a7095df 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -57,7 +57,7 @@ void BlockLocalPositionEstimator::gpsInit() // find lat, lon of current origin by subtracting x and y // if not using vision position since vision will // have it's own origin, not necessarily where vehicle starts - if (!(_fusion.get() & FUSE_VIS_POS)) { + if (!_map_ref.init_done && !(_fusion.get() & FUSE_VIS_POS)) { double gpsLatOrigin = 0; double gpsLonOrigin = 0; // reproject at current coordinates @@ -71,6 +71,9 @@ void BlockLocalPositionEstimator::gpsInit() // possible baro offset in global altitude at init _altOrigin = _gpsAltOrigin; _altOriginInitialized = true; + + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m", + gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin)); } PX4_INFO("[lpe] gps init " diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 8193c33257..911328a42d 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -4,15 +4,15 @@ extern orb_advert_t mavlink_log_pub; -// required number of samples for sensor -// to initialize - -// this is a vision based position measurement so we assume as soon as we get one -// measurement it is initialized, we also don't want to deinitialize it because -// this will throw away a correction before it starts using the data so we -// set the timeout to 10 seconds +// required number of samples for sensor to initialize. +// This is a vision based position measurement so we assume +// as soon as we get one measurement it is initialized. static const uint32_t REQ_VISION_INIT_COUNT = 1; -static const uint32_t VISION_TIMEOUT = 10000000; // 10 s + +// We don't want to deinitialize it because +// this will throw away a correction before it starts using the data so we +// set the timeout to 0.5 seconds +static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s void BlockLocalPositionEstimator::visionInit() { @@ -37,9 +37,16 @@ void BlockLocalPositionEstimator::visionInit() _sensorTimeout &= ~SENSOR_VISION; _sensorFault &= ~SENSOR_VISION; + if (!_map_ref.init_done && _sub_vision_pos.get().xy_global) { + // initialize global origin using the visual estimator reference + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m", + double(_sub_vision_pos.get().ref_lat), double(_sub_vision_pos.get().ref_lon), double(_sub_vision_pos.get().ref_alt)); + map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon); + } + if (!_altOriginInitialized) { _altOriginInitialized = true; - _altOrigin = 0; + _altOrigin = _sub_vision_pos.get().z_global ? _sub_vision_pos.get().ref_alt : 0.0f; } } }