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

position_estimator_inav: bug fixed, allow to disable GPS by setting INAV_W_POS_GPS_P = 0

Browse Source
This commit is contained in:
Anton Babushkin 2013-10-11 20:36:40 +02:00
parent 74af8d2c45
commit 2b1a11b16d
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -454,10 +454,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (z_est[0] < - 0.31f && flow_q > params.flow_q_min && t < sonar_valid_time + sonar_valid_timeout && att.R[2][2] > 0.7) {
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) {
/* distance to surface */
float flow_dist = (- z_est[0] - surface_offset) / att.R[2][2];
float flow_dist = dist_bottom / att.R[2][2];
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
@ -612,7 +613,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
t_prev = t;
/* use GPS if it's valid and reference position initialized */
bool use_gps = ref_xy_inited && gps_valid;
bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f;
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps);
/* try to estimate xy even if no absolute position source available,