yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

modules: lpe: refactor fusion bitmask

Browse Source
This commit is contained in:
TSC21
2018-01-14 15:11:49 -05:00
committed by Lorenz Meier
parent 1c74887a68
commit b512759336
3 changed files with 12 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@@ -180,20 +180,16 @@ 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, "
"baro: %d, landing_target: %d, mocap: %d, lidar: %d, sonar: %d\n",
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"baro: %d\n",
(_fusion.get() & FUSE_GPS) != 0,
(_fusion.get() & FUSE_FLOW) != 0,
(_fusion.get() & FUSE_VIS_POS) != 0,
(_fusion.get() & FUSE_VIS_YAW) != 0,
(_fusion.get() & FUSE_LAND_TARGET) != 0,
(_fusion.get() & FUSE_LAND) != 0,
(_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_fusion.get() & FUSE_FLOW_GYRO_COMP) != 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);
(_fusion.get() & FUSE_BARO) != 0);
}
BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
@@ -303,9 +299,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 = (_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 mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
bool landUpdated = landed()
&& ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate

8
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@@ -117,15 +117,11 @@ public:
FUSE_GPS = 1 << 0,
FUSE_FLOW = 1 << 1,
FUSE_VIS_POS = 1 << 2,
FUSE_VIS_YAW = 1 << 3,
FUSE_LAND_TARGET = 1 << 3,
FUSE_LAND = 1 << 4,
FUSE_PUB_AGL_Z = 1 << 5,
FUSE_FLOW_GYRO_COMP = 1 << 6,
FUSE_BARO = 1 << 7,
FUSE_LAND_TARGET = 1 << 8,
FUSE_MOCAP = 1 << 9,
FUSE_LIDAR = 1 << 10,
FUSE_SONAR = 1 << 11,
FUSE_BARO = 1 << 7
};
enum sensor_t {

14
src/modules/local_position_estimator/params.c
View File

@@ -431,17 +431,13 @@ PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f);
* 0 : Set to true to fuse GPS data if available, also requires GPS for altitude init
* 1 : Set to true to fuse optical flow data if available
* 2 : Set to true to fuse vision position
* 3 : Set to true to fuse vision yaw
* 3 : Set to true to enable landing target
* 4 : Set to true to fuse land detector
* 5 : Set to true to publish AGL as local position down component
* 6 : Set to true to enable flow gyro compensation
* 7 : Set to true to enable baro fusion
* 8 : Set to true to enable land target
* 9 : Set to true to enable motion capture fusion
* 10 : Set to true to enable lidar fusion
* 11 : Set to true to enable sonar fusion
*
* default (145 - GPS only)
* default (145 - GPS, baro, land detector)
*
* @group Local Position Estimator
* @min 0
@@ -449,14 +445,10 @@ PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f);
* @bit 0 fuse GPS, requires GPS for alt. init
* @bit 1 fuse optical flow
* @bit 2 fuse vision position
* @bit 3 fuse vision yaw
* @bit 3 fuse landing target
* @bit 4 fuse land detector
* @bit 5 pub agl as lpos down
* @bit 6 flow gyro compensation
* @bit 7 fuse baro
* @bit 8 land_target
* @bit 9 motion capture
* @bit 10 lidar
* @bit 11 sonar
*/
PARAM_DEFINE_INT32(LPE_FUSION, 145);