From 28a5287a91556ccb7a9c14fdec74b45197b3152a Mon Sep 17 00:00:00 2001 From: stmoon Date: Sun, 7 Jan 2018 20:44:13 +0900 Subject: [PATCH] add FUSE_MOCAP, FUSE_LIDAR, and FUSE_SONAR to select more senors --- .../BlockLocalPositionEstimator.cpp | 14 +++++++++----- .../BlockLocalPositionEstimator.hpp | 5 ++++- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index a32444ce1a..4574af6214 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -181,7 +181,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // print fusion settings to console printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, " "vis_yaw: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, " - "landing_target %d\n", + "baro: %d, landing_target: %d, mocap: %d, lidar: %d, sonar: %d\n", (_fusion.get() & FUSE_GPS) != 0, (_fusion.get() & FUSE_FLOW) != 0, (_fusion.get() & FUSE_VIS_POS) != 0, @@ -189,7 +189,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : (_fusion.get() & FUSE_LAND) != 0, (_fusion.get() & FUSE_PUB_AGL_Z) != 0, (_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0, - (_fusion.get() & FUSE_LAND_TARGET) != 0); + (_fusion.get() & FUSE_BARO) != 0, + (_fusion.get() & FUSE_LAND_TARGET) != 0, + (_fusion.get() & FUSE_MOCAP) != 0, + (_fusion.get() & FUSE_LIDAR) != 0, + (_fusion.get() & FUSE_SONAR) != 0); } BlockLocalPositionEstimator::~BlockLocalPositionEstimator() @@ -299,9 +303,9 @@ void BlockLocalPositionEstimator::update() bool flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated(); bool gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated(); bool visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated(); - bool mocapUpdated = _sub_mocap.updated(); - bool lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated(); - bool sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated(); + bool mocapUpdated = (_fusion.get() & FUSE_MOCAP) && _sub_mocap.updated(); + bool lidarUpdated = (_fusion.get() & FUSE_LIDAR) && (_sub_lidar != nullptr) && _sub_lidar->updated(); + bool sonarUpdated = (_fusion.get() & FUSE_SONAR) && (_sub_sonar != nullptr) && _sub_sonar->updated(); bool landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 44fc1d2124..1bb206d332 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -122,7 +122,10 @@ public: FUSE_PUB_AGL_Z = 1 << 5, FUSE_FLOW_GYRO_COMP = 1 << 6, FUSE_BARO = 1 << 7, - FUSE_LAND_TARGET = 1 << 8 + FUSE_LAND_TARGET = 1 << 8, + FUSE_MOCAP = 1 << 9, + FUSE_LIDAR = 1 << 10, + FUSE_SONAR = 1 << 11, }; enum sensor_t {