dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

replaced lidar min/max distances with parameters

This commit is contained in:
ChristophTobler 2016-02-01 16:46:15 +01:00 committed by Lorenz Meier
parent 1a6c4e123c
commit e2c04b7fa7
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -319,6 +319,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float dist_ground = 0.0f; //variables for lidar altitude estimation
float corr_lidar = 0.0f;
float lidar_offset = 0.0f;
float lidar_max_dist = 39.0f;
float lidar_min_dist = 0.1f;
int lidar_offset_count = 0;
bool lidar_first = true;
bool use_lidar = false;
@ -538,10 +540,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
}
if (updated && lidar.current_distance > 0.2f && lidar.current_distance < 10.0f
if (updated && lidar.current_distance > lidar_min_dist && lidar.current_distance < lidar_max_dist
&& (PX4_R(att.R, 2, 2) > 0.7f)) { //check if altitude estimation for lidar is enabled and new sensor data
if (!use_lidar_prev && use_lidar) {
lidar_first = true;
}
@ -588,7 +590,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = lidar.current_distance;
if (dist_bottom > 0.21f && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
if (dist_bottom > lidar_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
/* distance to surface */
//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
float flow_dist = dist_bottom; //use this if using lidar