diff options
Diffstat (limited to 'src/modules/position_estimator_inav/position_estimator_inav_main.c')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 15 |
1 files changed, 9 insertions, 6 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2f972fc9f..b418e3a76 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -246,9 +246,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - float alt_avg = 0.0f; - bool landed = true; - hrt_abstime landed_time = 0; + //float alt_avg = 0.0f; + //bool landed = true; + //hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -1068,12 +1068,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - /* detect land */ + +/* + // detect land 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 */ + // get actual thrust output float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { @@ -1097,6 +1099,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed_time = 0; } } +*/ if (verbose_mode) { /* print updates rate */ @@ -1148,7 +1151,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = landed; + local_pos.landed = false; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; |