dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Resets XY velocities when we can't estimate them

This commit is contained in:
Antonio Sanniravong 2014-06-24 07:47:16 -04:00
parent 68442e31ac
commit 179bace35a
3 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -916,6 +916,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est)); memcpy(y_est_prev, y_est, sizeof(y_est));
} }
} else {
/* gradually reset xy velocity estimates */
inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_reset_v);
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_reset_v);
} }
/* detect land */ /* detect land */
@ -931,6 +935,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
landed = false; landed = false;
landed_time = 0; landed_time = 0;
} }
/* reset xy velocity estimates when landed */
x_est[1] = 0.0f;
y_est[1] = 0.0f;
} else { } else {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) { if (alt_disp2 < land_disp2 && thrust < params.land_thr) {

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

@ -46,6 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_RESET_V, 0.2f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_xy_reset_v = param_find("INAV_W_XY_RESET_V");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K"); h->flow_k = param_find("INAV_FLOW_K");
@ -87,6 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_xy_reset_v, &(p->w_xy_reset_v));
param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_gps_flow, &(p->w_gps_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k)); param_get(h->flow_k, &(p->flow_k));

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

@ -47,6 +47,7 @@ struct position_estimator_inav_params {
float w_xy_gps_p; float w_xy_gps_p;
float w_xy_gps_v; float w_xy_gps_v;
float w_xy_flow; float w_xy_flow;
float w_xy_reset_v;
float w_gps_flow; float w_gps_flow;
float w_acc_bias; float w_acc_bias;
float flow_k; float flow_k;
@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles {
param_t w_xy_gps_p; param_t w_xy_gps_p;
param_t w_xy_gps_v; param_t w_xy_gps_v;
param_t w_xy_flow; param_t w_xy_flow;
param_t w_xy_reset_v;
param_t w_gps_flow; param_t w_gps_flow;
param_t w_acc_bias; param_t w_acc_bias;
param_t flow_k; param_t flow_k;