diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 89dae8cd99..566f0cfbde 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -32,7 +32,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // status updates 2 hz _sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()), _sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()), - _sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()), // gps 10 hz _sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()), // vision 5 hz @@ -87,9 +86,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _pn_b_noise_density(this, "PN_B"), _t_max_grade(this, "T_MAX_GRADE"), - // init home - _init_home_lat(this, "LAT"), - _init_home_lon(this, "LON"), + // init origin + _init_origin_lat(this, "LAT"), + _init_origin_lon(this, "LON"), // flow gyro _flow_gyro_x_high_pass(this, "FGYRO_HP"), @@ -140,12 +139,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _mocapInitialized(false), // reference altitudes - _altHome(0), - _altHomeInitialized(false), - _baroAltHome(0), - _gpsAltHome(0), - _visionHome(), - _mocapHome(), + _altOrigin(0), + _altOriginInitialized(false), + _baroAltOrigin(0), + _gpsAltOrigin(0), + _visionOrigin(), + _mocapOrigin(), // flow integration _flowX(0), @@ -256,12 +255,10 @@ void BlockLocalPositionEstimator::update() // reset pos, vel, and terrain on arming if (!_lastArmedState && armedState) { - // we just armed, we are at home position on the ground + // we just armed, we are at origin 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 + _x(X_z) = 0; // reset flow integral _flowX = 0; @@ -287,7 +284,6 @@ void BlockLocalPositionEstimator::update() bool paramsUpdated = _sub_param_update.updated(); bool baroUpdated = _sub_sensor.updated(); bool gpsUpdated = _gps_on.get() && _sub_gps.updated(); - bool homeUpdated = _sub_home.updated(); bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated(); bool mocapUpdated = _sub_mocap.updated(); bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated(); @@ -302,11 +298,6 @@ void BlockLocalPositionEstimator::update() updateSSParams(); } - // update home position projection - if (homeUpdated) { - updateHome(); - } - // is xy valid? bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get(); @@ -370,8 +361,8 @@ void BlockLocalPositionEstimator::update() // if we have no lat, lon initialize projection at 0,0 if (_validXY && !_map_ref.init_done) { map_projection_init(&_map_ref, - _init_home_lat.get(), - _init_home_lon.get()); + _init_origin_lat.get(), + _init_origin_lon.get()); } // reinitialize x if necessary @@ -484,7 +475,7 @@ void BlockLocalPositionEstimator::update() } } - if (_altHomeInitialized) { + if (_altOriginInitialized) { // update all publications if possible publishLocalPos(); publishEstimatorStatus(); @@ -570,6 +561,7 @@ void BlockLocalPositionEstimator::correctionLogic(Vector &dx) dx(X_bz) = 0; } + // if xy not valid, stop estimating if (!_validXY) { dx(X_x) = 0; dx(X_y) = 0; @@ -579,12 +571,14 @@ void BlockLocalPositionEstimator::correctionLogic(Vector &dx) dx(X_by) = 0; } + // if z not valid, stop estimating if (!_validZ) { dx(X_z) = 0; dx(X_vz) = 0; dx(X_bz) = 0; } + // if terrain not valid, stop estimating if (!_validTZ) { dx(X_tz) = 0; } @@ -629,28 +623,6 @@ void BlockLocalPositionEstimator::detectDistanceSensors() } } -void BlockLocalPositionEstimator::updateHome() -{ - double lat = _sub_home.get().lat; - double lon = _sub_home.get().lon; - float alt = _sub_home.get().alt; - - // updating home causes absolute measurements - // like gps and baro to be off, need to allow it - // to reset by resetting covariance - initP(); - 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; - _altHome = alt; - _gpsAltHome += delta_alt; - _baroAltHome += delta_alt; - _visionHome(2) += delta_alt; - _mocapHome(2) += delta_alt; -} void BlockLocalPositionEstimator::publishLocalPos() { @@ -672,12 +644,12 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().vy = xLP(X_vy); // east _pub_lpos.get().vz = xLP(X_vz); // down _pub_lpos.get().yaw = _sub_att.get().yaw; - _pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference + _pub_lpos.get().xy_global = _validXY; _pub_lpos.get().z_global = _baroInitialized; - _pub_lpos.get().ref_timestamp = _sub_home.get().timestamp; + _pub_lpos.get().ref_timestamp = _timeStamp; _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI; _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; - _pub_lpos.get().ref_alt = _sub_home.get().alt; + _pub_lpos.get().ref_alt = _altOrigin; _pub_lpos.get().dist_bottom = _aglLowPass.getState(); _pub_lpos.get().dist_bottom_rate = - xLP(X_vz); _pub_lpos.get().surface_bottom_timestamp = _timeStamp; @@ -725,7 +697,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() double lon = 0; const Vector &xLP = _xLowPass.getState(); map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon); - float alt = -xLP(X_z) + _altHome; + float alt = -xLP(X_z) + _altOrigin; if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) && @@ -741,7 +713,7 @@ 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 = _altHome - xLP(X_tz); + _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); _pub_gpos.get().terrain_alt_valid = _validTZ; _pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout; _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index abf858aa6a..4a612182b1 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include @@ -205,7 +204,6 @@ private: float agl(); void correctionLogic(Vector &dx); void detectDistanceSensors(); - void updateHome(); // publications void publishLocalPos(); @@ -222,7 +220,6 @@ private: uORB::Subscription _sub_sensor; uORB::Subscription _sub_param_update; uORB::Subscription _sub_manual; - uORB::Subscription _sub_home; uORB::Subscription _sub_gps; uORB::Subscription _sub_vision_pos; uORB::Subscription _sub_mocap; @@ -292,9 +289,9 @@ private: BlockParamFloat _pn_b_noise_density; BlockParamFloat _t_max_grade; - // init home - BlockParamFloat _init_home_lat; - BlockParamFloat _init_home_lon; + // init origin + BlockParamFloat _init_origin_lat; + BlockParamFloat _init_origin_lon; // flow gyro filter BlockHighPass _flow_gyro_x_high_pass; @@ -344,12 +341,12 @@ private: bool _mocapInitialized; // reference altitudes - float _altHome; - bool _altHomeInitialized; - float _baroAltHome; - float _gpsAltHome; - Vector3f _visionHome; - Vector3f _mocapHome; + float _altOrigin; + bool _altOriginInitialized; + float _baroAltOrigin; + float _gpsAltOrigin; + Vector3f _visionOrigin; + Vector3f _mocapOrigin; // flow integration float _flowX; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 361147235e..3543377a10 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -308,7 +308,7 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f); PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f); /** - * Home latitude for nav w/o GPS + * Local origin latitude for nav w/o GPS * * @group Local Position Estimator * @unit deg @@ -319,7 +319,7 @@ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f); PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f); /** - * Home longitude for nav w/o GPS + * Local origin longitude for nav w/o GPS * * @group Local Position Estimator * @unit deg diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index ffe8e51611..1d0624a8c8 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::baroInit() // if finished if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { - _baroAltHome = _baroStats.getMean()(0); + _baroAltOrigin = _baroStats.getMean()(0); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro init %d m std %d cm", (int)_baroStats.getMean()(0), @@ -29,10 +29,10 @@ void BlockLocalPositionEstimator::baroInit() _baroInitialized = true; _baroFault = FAULT_NONE; - // only initialize alt home with baro if gps is disabled - if (!_altHomeInitialized && !_gps_on.get()) { - _altHomeInitialized = true; - _altHome = _baroAltHome; + // only initialize alt origin with baro and no gps + if (!_altOriginInitialized && !_gps_on.get()) { + _altOriginInitialized = true; + _altOrigin = _baroAltOrigin; } } } @@ -54,8 +54,8 @@ void BlockLocalPositionEstimator::baroCorrect() if (baroMeasure(y) != OK) { return; } - // subtract baro home alt - y -= _baroAltHome; + // subtract baro origin alt + y -= _baroAltOrigin; // baro measurement matrix Matrix C; diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index e08119dddb..c003faf7ca 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -37,27 +37,27 @@ void BlockLocalPositionEstimator::gpsInit() // if finished if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) { - double gpsLatHome = _gpsStats.getMean()(0); - double gpsLonHome = _gpsStats.getMean()(1); + double gpsLatOrigin = _gpsStats.getMean()(0); + double gpsLonOrigin = _gpsStats.getMean()(1); if (!_receivedGps) { _receivedGps = true; - map_projection_init(&_map_ref, gpsLatHome, gpsLonHome); + map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin); } - _gpsAltHome = _gpsStats.getMean()(2); + _gpsAltOrigin = _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)); + gpsLatOrigin, + gpsLonOrigin, + double(_gpsAltOrigin)); _gpsInitialized = true; _gpsFault = FAULT_NONE; _gpsStats.reset(); - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _gpsAltHome; + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOrigin = _gpsAltOrigin; } } } @@ -92,7 +92,7 @@ void BlockLocalPositionEstimator::gpsCorrect() float alt = y_global(2); float px = 0; float py = 0; - float pz = -(alt - _gpsAltHome); + float pz = -(alt - _gpsAltOrigin); map_projection_project(&_map_ref, lat, lon, &px, &py); Vector y; y.setZero(); diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index c629b3f17b..d87f94593d 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::mocapInit() // if finished if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { - _mocapHome = _mocapStats.getMean(); + _mocapOrigin = _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)), @@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::mocapInit() _mocapInitialized = true; _mocapFault = FAULT_NONE; - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _mocapHome(2); + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOrigin = _mocapOrigin(2); } } } @@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::mocapCorrect() if (mocapMeasure(y) != OK) { return; } - // make measurement relative to home - y -= _mocapHome; + // make measurement relative to origin + y -= _mocapOrigin; // mocap measurement matrix, measures position Matrix C; diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index bb5f58e332..f0d9f8c0db 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::visionInit() // increament sums for mean if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { - _visionHome = _visionStats.getMean(); + _visionOrigin = _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)), @@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::visionInit() _visionInitialized = true; _visionFault = FAULT_NONE; - if (!_altHomeInitialized) { - _altHomeInitialized = true; - _altHome = _visionHome(2); + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOrigin = _visionOrigin(2); } } } @@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::visionCorrect() if (visionMeasure(y) != OK) { return; } - // make measurement relative to home - y -= _visionHome; + // make measurement relative to origin + y -= _visionOrigin; // vision measurement matrix, measures position Matrix C; diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index b01efef7f1..071254c80e 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -63,6 +63,7 @@ #include "topics/att_pos_mocap.h" #include "topics/vision_position_estimate.h" #include "topics/control_state.h" +#include "topics/vehicle_land_detected.h" #include @@ -179,5 +180,6 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB