fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 12:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove vehicle_local_position.ref_surface_timestamp field, don't sync baro_offset and local_pos.ref_alt instead

Browse Source
This commit is contained in:
Anton Babushkin
2013-10-09 11:05:22 +02:00
parent 9d1027162f
commit 2fc011d20c
3 changed files with 20 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/multirotor_pos_control/multirotor_pos_control.c
+2 -2
View File
@@ -345,7 +345,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (control_mode.flag_control_manual_enabled) { if (control_mode.flag_control_manual_enabled) {
/* manual control */ /* manual control */
/* check for reference point updates and correct setpoint */ /* check for reference point updates and correct setpoint */
if (local_pos.ref_surface_timestamp != ref_surface_prev_t) { if (local_pos.ref_timestamp != ref_surface_prev_t) {
/* reference alt changed, don't follow large ground level changes in manual flight */ /* reference alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; local_pos_sp.z += local_pos.ref_alt - ref_alt_prev;
@@ -674,7 +674,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} }
/* track local position reference even when not controlling position */ /* track local position reference even when not controlling position */
ref_surface_prev_t = local_pos.ref_surface_timestamp; ref_surface_prev_t = local_pos.ref_timestamp;
ref_alt_prev = local_pos.ref_alt; ref_alt_prev = local_pos.ref_alt;
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
src/modules/position_estimator_inav/position_estimator_inav_main.c
+18 -24
View File
@@ -180,7 +180,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int baro_init_cnt = 0; int baro_init_cnt = 0;
int baro_init_num = 200; 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; float alt_avg = 0.0f;
bool landed = true; bool landed = true;
hrt_abstime landed_time = 0; hrt_abstime landed_time = 0;
@@ -299,15 +299,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* mean calculation over several measurements */ /* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) { if (baro_init_cnt < baro_init_num) {
baro_alt0 += sensor.baro_alt_meter; baro_offset += sensor.baro_alt_meter;
baro_init_cnt++; baro_init_cnt++;
} else { } else {
wait_baro = false; wait_baro = false;
baro_alt0 /= (float) baro_init_cnt; baro_offset /= (float) baro_init_cnt;
warnx("init ref: alt=%.3f", baro_alt0); warnx("init ref: alt=%.3f", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_offset);
local_pos.ref_alt = baro_alt0; local_pos.ref_alt = baro_offset;
local_pos.ref_timestamp = hrt_absolute_time(); local_pos.ref_timestamp = hrt_absolute_time();
local_pos.z_valid = true; local_pos.z_valid = true;
local_pos.v_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 }, { .fd = vehicle_attitude_sub, .events = POLLIN },
}; };
if (!thread_should_exit) {
warnx("main loop started");
}
while (!thread_should_exit) { while (!thread_should_exit) {
int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time(); 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 */ /* reset ground level on arm if sonar is invalid */
if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) { if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) {
flag_armed = armed.armed; flag_armed = armed.armed;
baro_alt0 -= z_est[0]; baro_offset -= z_est[0];
z_est[0] = 0.0f; z_est[0] = 0.0f;
alt_avg = 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_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) { 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_counter = sensor.baro_counter;
baro_updates++; baro_updates++;
} }
@@ -442,9 +437,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else { } else {
/* new ground level */ /* new ground level */
baro_alt0 += sonar_corr; baro_offset += sonar_corr;
mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_offset);
local_pos.ref_surface_timestamp = t; local_pos.ref_alt += sonar_corr;
local_pos.ref_timestamp = t;
z_est[0] += sonar_corr; z_est[0] += sonar_corr;
alt_avg -= sonar_corr; alt_avg -= sonar_corr;
sonar_corr = 0.0f; 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; bool use_gps = ref_xy_inited && gps_valid;
/* use flow if it's valid and (accurate or no GPS available) */ /* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps); bool use_flow = flow_valid && (flow_accurate || !use_gps);
/* try to estimate xy even if no absolute position source available, /* try to estimate xy even if no absolute position source available,
* if using optical flow velocity will be valid */ * if using optical flow velocity will be valid */
bool can_estimate_xy = use_gps || use_flow || (t < flow_valid_time + flow_valid_timeout); 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) { if (sonar_valid) {
baro_alt0 -= (baro_corr + baro_alt0) * params.w_alt_sonar * dt; baro_offset -= (baro_corr + baro_offset) * params.w_alt_sonar * dt;
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = t;
} }
/* accelerometer bias correction */ /* 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[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) { if (sonar_valid) {
accel_bias_corr[2] -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar; 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(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); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
if (can_estimate_xy) { if (can_estimate_xy) {
src/modules/uORB/topics/vehicle_local_position.h
-1
View File
@@ -73,7 +73,6 @@ struct vehicle_local_position_s
bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */
uint64_t ref_timestamp; /**< Time when reference position was set */ uint64_t ref_timestamp; /**< Time when reference position was set */
uint64_t ref_surface_timestamp; /**< Time when reference surface was set */
int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */
int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */
float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */