From 2fc011d20c7bd08bf5e076e79ba33a41f8fcbc30 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Oct 2013 11:05:22 +0200 Subject: Remove vehicle_local_position.ref_surface_timestamp field, don't sync baro_offset and local_pos.ref_alt instead --- .../position_estimator_inav_main.c | 42 ++++++++++------------ 1 file changed, 18 insertions(+), 24 deletions(-) (limited to 'src/modules/position_estimator_inav') 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 eaa21f8eb..d37c49696 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -180,7 +180,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_cnt = 0; int baro_init_num = 200; - float baro_alt0 = 0.0f; /* to determine while start up */ + float baro_offset = 0.0f; // baro offset from reference altitude, it's not the same as local_position.ref_alt if sonar available float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; @@ -299,15 +299,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - baro_alt0 += sensor.baro_alt_meter; + baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } else { wait_baro = false; - baro_alt0 /= (float) baro_init_cnt; - warnx("init ref: alt=%.3f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; + baro_offset /= (float) baro_init_cnt; + warnx("init ref: alt=%.3f", baro_offset); + mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_offset); + local_pos.ref_alt = baro_offset; local_pos.ref_timestamp = hrt_absolute_time(); local_pos.z_valid = true; local_pos.v_z_valid = true; @@ -323,10 +323,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = vehicle_attitude_sub, .events = POLLIN }, }; - if (!thread_should_exit) { - warnx("main loop started"); - } - while (!thread_should_exit) { int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); @@ -371,12 +367,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* 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]; + baro_offset -= z_est[0]; z_est[0] = 0.0f; alt_avg = 0.0f; - local_pos.ref_alt = baro_alt0; + local_pos.ref_alt = baro_offset; local_pos.ref_timestamp = t; - local_pos.ref_surface_timestamp = t; } } @@ -415,7 +410,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = - sensor.baro_alt_meter - z_est[0]; + baro_corr = - sensor.baro_alt_meter - z_est[0]; // should be shifted by baro_offset before applying correction baro_counter = sensor.baro_counter; baro_updates++; } @@ -442,9 +437,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { /* new ground level */ - baro_alt0 += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); - local_pos.ref_surface_timestamp = t; + baro_offset += sonar_corr; + mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_offset); + local_pos.ref_alt += sonar_corr; + local_pos.ref_timestamp = t; z_est[0] += sonar_corr; alt_avg -= sonar_corr; sonar_corr = 0.0f; @@ -619,16 +615,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool use_gps = ref_xy_inited && gps_valid; /* 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, * if using optical flow velocity will be valid */ bool can_estimate_xy = use_gps || use_flow || (t < flow_valid_time + flow_valid_timeout); - /* baro offset correction if sonar available */ + /* baro offset correction if sonar available, + * don't touch reference altitude, local_pos.ref_alt != baro_offset after this */ if (sonar_valid) { - baro_alt0 -= (baro_corr + baro_alt0) * params.w_alt_sonar * dt; - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = t; + baro_offset -= (baro_corr + baro_offset) * params.w_alt_sonar * dt; } /* accelerometer bias correction */ @@ -646,7 +640,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow; } - accel_bias_corr[2] -= (baro_corr + baro_alt0) * params.w_alt_baro * params.w_alt_baro; + accel_bias_corr[2] -= (baro_corr + baro_offset) * params.w_alt_baro * params.w_alt_baro; if (sonar_valid) { accel_bias_corr[2] -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar; @@ -671,7 +665,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); } - inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(baro_corr + baro_offset, dt, z_est, 0, params.w_alt_baro); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); if (can_estimate_xy) { -- cgit v1.2.3