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

Added vehicle_local_position.ref_surface_timestamp to track surface level changes. Reference altitude updated continuosly when sonar available.

Browse Source
This commit is contained in:
Anton Babushkin 2013-10-08 21:18:17 +02:00
parent ff92770305
commit c96de846b3
3 changed files with 54 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
const float pos_ctl_dz = 0.05f;
float ref_alt_prev = 0.0f;
hrt_abstime ref_alt_prev_t = 0;
hrt_abstime ref_surface_prev_t = 0;
uint64_t local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
@ -345,11 +345,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (control_mode.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_prev_t) {
if (ref_alt_prev_t != 0) {
/* reference alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z -= local_pos.ref_alt - ref_alt_prev;
}
if (local_pos.ref_surface_timestamp != ref_surface_prev_t) {
/* reference alt changed, don't follow large ground level changes in manual flight */
mavlink_log_info(mavlink_fd, "[mpc] update z sp %.2f -> %.2f = %.2f", ref_alt_prev, local_pos.ref_alt, local_pos.ref_alt - ref_alt_prev);
local_pos_sp.z += local_pos.ref_alt - ref_alt_prev;
// TODO also correct XY setpoint
}
@ -476,6 +475,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
att_sp.yaw_body = global_pos_sp.yaw;
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
@ -675,7 +675,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
}
/* track local position reference even when not controlling position */
ref_alt_prev_t = local_pos.ref_timestamp;
ref_surface_prev_t = local_pos.ref_surface_timestamp;
ref_alt_prev = local_pos.ref_alt;
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */

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

@ -82,6 +82,7 @@ static const hrt_abstime sonar_valid_timeout = 1000000; // assume that altitude
static const hrt_abstime flow_timeout = 1000000; // optical flow topic timeout = 1s
static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@ -363,6 +364,7 @@ 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;
@ -370,7 +372,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
z_est[0] = 0.0f;
alt_avg = 0.0f;
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
local_pos.ref_timestamp = t;
local_pos.ref_surface_timestamp = t;
}
}
@ -439,8 +442,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* new ground level */
baro_alt0 += sonar_corr;
mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
local_pos.ref_surface_timestamp = t;
z_est[0] += sonar_corr;
alt_avg -= sonar_corr;
sonar_corr = 0.0f;
@ -465,32 +467,49 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
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) {
/* distance to surface */
float flow_dist = -z_est[0] / att.R[2][2];
/* convert raw flow to angular flow */
float flow_ang[2];
flow_ang[0] = flow.flow_raw_x * params.flow_k;
flow_ang[1] = flow.flow_raw_y * params.flow_k;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
flow_m[1] = -flow_ang[1] * flow_dist;
flow_m[2] = z_est[1];
/* velocity in NED */
float flow_v[2] = { 0.0f, 0.0f };
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += att.R[i][j] * flow_m[j];
}
body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
}
/* velocity correction */
flow_corr[0] = flow_v[0] - x_est[1];
flow_corr[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
flow_w = att.R[2][2] * flow_q_weight;
flow_valid = true;
/* use flow only if it is not too large according to estimate */
// TODO add timeout for flow to allow flying without GPS
if (fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow) {
/* convert raw flow to angular flow */
float flow_ang[2];
flow_ang[0] = flow.flow_raw_x * params.flow_k;
flow_ang[1] = flow.flow_raw_y * params.flow_k;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
flow_m[1] = -flow_ang[1] * flow_dist;
flow_m[2] = z_est[1];
/* velocity in NED */
float flow_v[2] = { 0.0f, 0.0f };
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += att.R[i][j] * flow_m[j];
}
}
/* velocity correction */
flow_corr[0] = flow_v[0] - x_est[1];
flow_corr[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
flow_w = att.R[2][2] * flow_q_weight;
flow_valid = true;
} else {
flow_w = 0.0f;
flow_valid = false;
}
} else {
flow_w = 0.0f;
@ -607,6 +626,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* baro offset correction if sonar available */
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;
}
/* accelerometer bias correction */

1
src/modules/uORB/topics/vehicle_local_position.h
View File

@ -73,6 +73,7 @@ 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 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_surface_timestamp; /**< Time when reference surface was set */
int32_t ref_lat; /**< Reference point latitude 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 */