From e6efb5ec0a7a857aa16f614eca65e96561059ec8 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 2 Nov 2017 15:42:35 +0000 Subject: [PATCH] lpe: update _sensorTimeout and _sensorFault to handle support of more sensors --- .../BlockLocalPositionEstimator.cpp | 7 +++++-- .../BlockLocalPositionEstimator.hpp | 6 ++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 33a746102c..a1a3b1adff 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -180,14 +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\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_FLOW) != 0, (_fusion.get() & FUSE_VIS_POS) != 0, (_fusion.get() & FUSE_VIS_YAW) != 0, (_fusion.get() & FUSE_LAND) != 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() @@ -335,6 +337,7 @@ void BlockLocalPositionEstimator::update() || !(_sensorTimeout & SENSOR_VISION) || !(_sensorTimeout & SENSOR_MOCAP) || !(_sensorTimeout & SENSOR_LAND) + || !(_sensorTimeout & SENSOR_LAND_TARGET) ) { _estimatorInitialized |= EST_XY; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 69eaf5d874..5edde1756a 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -122,6 +122,7 @@ public: FUSE_PUB_AGL_Z = 1 << 5, FUSE_FLOW_GYRO_COMP = 1 << 6, FUSE_BARO = 1 << 7, + FUSE_LAND_TARGET = 1 << 8 }; enum sensor_t { @@ -133,6 +134,7 @@ public: SENSOR_VISION = 1 << 5, SENSOR_MOCAP = 1 << 6, SENSOR_LAND = 1 << 7, + SENSOR_LAND_TARGET = 1 << 8, }; enum estimate_t { @@ -369,8 +371,8 @@ private: bool _lastArmedState; // masks - uint8_t _sensorTimeout; - uint8_t _sensorFault; + uint16_t _sensorTimeout; + uint16_t _sensorFault; uint8_t _estimatorInitialized; // state space