fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 23:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

lpe: update _sensorTimeout and _sensorFault to handle support of more sensors

Browse Source
This commit is contained in:
TSC21
2017-11-03 23:45:43 +01:00
committed by Lorenz Meier
parent 392999c2df
commit e6efb5ec0a
2 changed files with 9 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+5 -2
View File
@@ -180,14 +180,16 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// print fusion settings to console // print fusion settings to console
printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, " printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
"vis_yaw: %d, land: %d, pub_agl_z: %d, flow_gyro: %d\n", "vis_yaw: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"landing_target %d\n",
(_fusion.get() & FUSE_GPS) != 0, (_fusion.get() & FUSE_GPS) != 0,
(_fusion.get() & FUSE_FLOW) != 0, (_fusion.get() & FUSE_FLOW) != 0,
(_fusion.get() & FUSE_VIS_POS) != 0, (_fusion.get() & FUSE_VIS_POS) != 0,
(_fusion.get() & FUSE_VIS_YAW) != 0, (_fusion.get() & FUSE_VIS_YAW) != 0,
(_fusion.get() & FUSE_LAND) != 0, (_fusion.get() & FUSE_LAND) != 0,
(_fusion.get() & FUSE_PUB_AGL_Z) != 0, (_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0); (_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
(_fusion.get() & FUSE_LAND_TARGET) != 0);
} }
BlockLocalPositionEstimator::~BlockLocalPositionEstimator() BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
@@ -335,6 +337,7 @@ void BlockLocalPositionEstimator::update()
|| !(_sensorTimeout & SENSOR_VISION) || !(_sensorTimeout & SENSOR_VISION)
|| !(_sensorTimeout & SENSOR_MOCAP) || !(_sensorTimeout & SENSOR_MOCAP)
|| !(_sensorTimeout & SENSOR_LAND) || !(_sensorTimeout & SENSOR_LAND)
|| !(_sensorTimeout & SENSOR_LAND_TARGET)
) { ) {
_estimatorInitialized |= EST_XY; _estimatorInitialized |= EST_XY;
} }
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+4 -2
View File
@@ -122,6 +122,7 @@ public:
FUSE_PUB_AGL_Z = 1 << 5, FUSE_PUB_AGL_Z = 1 << 5,
FUSE_FLOW_GYRO_COMP = 1 << 6, FUSE_FLOW_GYRO_COMP = 1 << 6,
FUSE_BARO = 1 << 7, FUSE_BARO = 1 << 7,
FUSE_LAND_TARGET = 1 << 8
}; };
enum sensor_t { enum sensor_t {
@@ -133,6 +134,7 @@ public:
SENSOR_VISION = 1 << 5, SENSOR_VISION = 1 << 5,
SENSOR_MOCAP = 1 << 6, SENSOR_MOCAP = 1 << 6,
SENSOR_LAND = 1 << 7, SENSOR_LAND = 1 << 7,
SENSOR_LAND_TARGET = 1 << 8,
}; };
enum estimate_t { enum estimate_t {
@@ -369,8 +371,8 @@ private:
bool _lastArmedState; bool _lastArmedState;
// masks // masks
uint8_t _sensorTimeout; uint16_t _sensorTimeout;
uint8_t _sensorFault; uint16_t _sensorFault;
uint8_t _estimatorInitialized; uint8_t _estimatorInitialized;
// state space // state space