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

position_estimator_inav: avoid triggering land detector on altitude reference changes, don't reset altitude on arming if sonar is valid

Browse Source
This commit is contained in:
Anton Babushkin
2013-10-08 16:38:49 +02:00
parent 1da6565fc6
commit 52cf8836fe
1 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+15 -15
View File
@@ -363,6 +363,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* reset ground level on arm if sonar is invalid */
if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) {
flag_armed = armed.armed;
baro_alt0 -= z_est[0];
z_est[0] = 0.0f;
alt_avg = 0.0f;
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
}
}
/* sensor combined */
@@ -433,6 +442,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
z_est[0] += sonar_corr;
alt_avg -= sonar_corr;
sonar_corr = 0.0f;
sonar_corr_filtered = 0.0f;
sonar_valid_time = t;
@@ -587,14 +597,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
t_prev = t;
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
baro_alt0 -= z_est[0];
z_est[0] = 0.0f;
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
}
bool use_gps = ref_xy_inited && gps_valid;
bool use_flow = flow_valid;
@@ -687,21 +689,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* detect land */
alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
float alt_disp = z_est[0] - alt_avg;
alt_disp = alt_disp * alt_disp;
alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
float alt_disp2 = - z_est[0] - alt_avg;
alt_disp2 = alt_disp2 * alt_disp2;
float land_disp2 = params.land_disp * params.land_disp;
/* get actual thrust output */
float thrust = armed.armed ? actuator.control[3] : 0.0f;
if (landed) {
if (alt_disp > land_disp2 && thrust > params.land_thr) {
if (alt_disp2 > land_disp2 && thrust > params.land_thr) {
landed = false;
landed_time = 0;
}
} else {
if (alt_disp < land_disp2 && thrust < params.land_thr) {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
if (landed_time == 0) {
landed_time = t; // land detected first time
@@ -791,8 +793,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
flag_armed = armed.armed;
}
warnx("stopped");