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

position_estimator_inav: estimate distance to bottom rate, increase time of position estimation on only accelerometer, reduce weight for GPS if flow available

Browse Source
This commit is contained in:
Anton Babushkin 2013-10-12 11:20:20 +02:00
parent 2b1a11b16d
commit c0c366d6ee
4 changed files with 42 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,8 +79,8 @@ static bool verbose_mode = false;
static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
static const hrt_abstime sonar_valid_timeout = 1000000; // assume that altitude == distance to surface during this time
static const hrt_abstime flow_valid_timeout = 1000000; // assume that altitude == distance to surface during this time
static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss
static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss
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
@ -182,6 +182,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int baro_init_num = 200;
float baro_offset = 0.0f; // baro offset for reference altitude, initialized only once
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;
@ -226,7 +227,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float sonar_prev = 0.0f;
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime flow_valid_time = 0; // time of last flow measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
@ -434,6 +435,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
/* spike detected, ignore */
sonar_corr = 0.0f;
surface_offset_rate = 0.0f;
sonar_valid = false;
} else {
@ -441,6 +443,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
surface_offset -= sonar_corr;
sonar_corr = 0.0f;
sonar_corr_filtered = 0.0f;
surface_offset_rate = 0.0f;
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
@ -448,8 +451,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
} else {
/* correction is ok, use it */
sonar_valid_time = t;
sonar_valid = true;
surface_offset_rate = -sonar_corr * params.w_alt_sonar;
}
}
@ -495,13 +500,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
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;
/* if flow is not accurate, lower weight for it */
flow_w = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
if (!flow_accurate)
flow_w *= 0.2f;
flow_w *= 0.05f;
flow_valid = true;
flow_valid_time = t;
} else {
flow_w = 0.0f;
@ -606,6 +610,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* check for sonar measurement timeout */
if (sonar_valid && t > sonar_time + sonar_timeout) {
sonar_corr = 0.0f;
surface_offset_rate = 0.0f;
sonar_valid = false;
}
@ -616,24 +621,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f;
/* 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);
/* try to estimate position during some time after position sources lost */
if (use_gps || use_flow) {
xy_src_time = t;
}
bool can_estimate_xy = (t < xy_src_time + xy_src_timeout);
/* baro offset correction if sonar available,
* don't touch reference altitude, local_pos.ref_alt != baro_offset after this */
if (sonar_valid) {
surface_offset -= sonar_corr * params.w_alt_sonar * dt;
surface_offset += surface_offset_rate * dt;
}
/* accelerometer bias correction */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
float w_pos_gps_p = params.w_pos_gps_p;
float w_pos_gps_v = params.w_pos_gps_v;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
w_pos_gps_p *= params.w_gps_flow;
w_pos_gps_v *= params.w_gps_flow;
}
if (use_gps) {
accel_bias_corr[0] -= gps_corr[0][0] * params.w_pos_gps_p * params.w_pos_gps_p;
accel_bias_corr[0] -= gps_corr[0][1] * params.w_pos_gps_v;
accel_bias_corr[1] -= gps_corr[1][0] * params.w_pos_gps_p * params.w_pos_gps_p;
accel_bias_corr[1] -= gps_corr[1][1] * params.w_pos_gps_v;
accel_bias_corr[0] -= gps_corr[0][0] * w_pos_gps_p * w_pos_gps_p;
accel_bias_corr[0] -= gps_corr[0][1] * w_pos_gps_v;
accel_bias_corr[1] -= gps_corr[1][0] * w_pos_gps_p * w_pos_gps_p;
accel_bias_corr[1] -= gps_corr[1][1] * w_pos_gps_v;
}
if (use_flow) {
@ -681,12 +697,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (use_gps) {
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, w_pos_gps_p);
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, w_pos_gps_p);
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, w_pos_gps_v);
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, w_pos_gps_v);
}
}
@ -765,6 +781,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.dist_bottom_valid = t < sonar_valid_time + sonar_valid_timeout;
if (local_pos.dist_bottom_valid) {
local_pos.dist_bottom = -z_est[0] - surface_offset;
local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
}
local_pos.timestamp = t;

3
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -47,6 +47,7 @@ PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
h->w_pos_acc = param_find("INAV_W_POS_ACC");
h->w_pos_flow = param_find("INAV_W_POS_FLOW");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
@ -86,6 +88,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
param_get(h->w_pos_acc, &(p->w_pos_acc));
param_get(h->w_pos_flow, &(p->w_pos_flow));
param_get(h->w_gps_flow, &(p->w_gps_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));
param_get(h->flow_q_min, &(p->flow_q_min));

2
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -48,6 +48,7 @@ struct position_estimator_inav_params {
float w_pos_gps_v;
float w_pos_acc;
float w_pos_flow;
float w_gps_flow;
float w_acc_bias;
float flow_k;
float flow_q_min;
@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles {
param_t w_pos_gps_v;
param_t w_pos_acc;
param_t w_pos_flow;
param_t w_gps_flow;
param_t w_acc_bias;
param_t flow_k;
param_t flow_q_min;

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

@ -79,7 +79,7 @@ struct vehicle_local_position_s
bool landed; /**< true if vehicle is landed */
/* Distance to surface */
float dist_bottom; /**< Distance to bottom surface (ground) */
float v_dist_bottom; /**< Distance to bottom surface (ground) change rate */
float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */
uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */
bool dist_bottom_valid; /**< true if distance to bottom surface is valid */
};