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

Make LPE sonar reading more robust. (#4806)

Browse Source
This commit is contained in:
James Goppert
2016-06-15 12:37:52 -04:00
committed by GitHub
parent a76eab367c
commit 2a26611cf5
5 changed files with 34 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+3 -12
View File
@@ -49,21 +49,12 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
}
// calculate range to center of image for flow
float d = 0;
if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) {
d = _sub_lidar->get().current_distance
+ (_lidar_z_offset.get() - _flow_z_offset.get());
} else if (_sonarInitialized && (_sonarFault < fault_lvl_disable)) {
d = _sub_sonar->get().current_distance
+ (_sonar_z_offset.get() - _flow_z_offset.get());
} else {
// no valid distance data
if (!_canEstimateT) {
return -1;
}
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
// check for global accuracy
if (_gpsInitialized) {
double lat = _sub_gps.get().lat * 1.0e-7;