fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 03:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make LPE est always log. (#4749)

Browse Source 
This is a trivial change so I'm going to merge to help address edge cases in users logs.
This commit is contained in:
James Goppert
2016-06-06 14:12:18 -05:00
parent f9df60919e
commit 5ec7de3a5a
1 changed files with 24 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+24 -29
View File
@@ -583,37 +583,32 @@ void BlockLocalPositionEstimator::publishLocalPos()
void BlockLocalPositionEstimator::publishEstimatorStatus()
{
if (PX4_ISFINITE(_x(X_x)) &&
PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
_pub_est_status.get().timestamp = _timeStamp;
_pub_est_status.get().timestamp = _timeStamp;
for (int i = 0; i < n_x; i++) {
_pub_est_status.get().states[i] = _x(i);
_pub_est_status.get().covariances[i] = _P(i, i);
}
_pub_est_status.get().n_states = n_x;
_pub_est_status.get().nan_flags = 0;
_pub_est_status.get().health_flags =
((_baroFault > fault_lvl_disable) << SENSOR_BARO)
+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
_pub_est_status.get().timeout_flags =
(_baroInitialized << SENSOR_BARO)
+ (_gpsInitialized << SENSOR_GPS)
+ (_flowInitialized << SENSOR_FLOW)
+ (_lidarInitialized << SENSOR_LIDAR)
+ (_sonarInitialized << SENSOR_SONAR)
+ (_visionInitialized << SENSOR_VISION)
+ (_mocapInitialized << SENSOR_MOCAP);
_pub_est_status.update();
for (int i = 0; i < n_x; i++) {
_pub_est_status.get().states[i] = _x(i);
_pub_est_status.get().covariances[i] = _P(i, i);
}
_pub_est_status.get().n_states = n_x;
_pub_est_status.get().nan_flags = 0;
_pub_est_status.get().health_flags =
((_baroFault > fault_lvl_disable) << SENSOR_BARO)
+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
_pub_est_status.get().timeout_flags =
(_baroInitialized << SENSOR_BARO)
+ (_gpsInitialized << SENSOR_GPS)
+ (_flowInitialized << SENSOR_FLOW)
+ (_lidarInitialized << SENSOR_LIDAR)
+ (_sonarInitialized << SENSOR_SONAR)
+ (_visionInitialized << SENSOR_VISION)
+ (_mocapInitialized << SENSOR_MOCAP);
_pub_est_status.update();
}
void BlockLocalPositionEstimator::publishGlobalPos()