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: don’t use GPS vertical speed

Browse Source
This commit is contained in:
Anton Babushkin 2013-11-22 22:23:27 +04:00
parent e4f0c7d26a
commit 8c3e67993e
3 changed files with 0 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -656,7 +656,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
float w_z_gps_p = params.w_z_gps_p * w_gps_z;
float w_z_gps_v = params.w_z_gps_v * w_gps_z;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
@ -682,7 +681,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (use_gps_z) {
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
}
if (use_flow) {
@ -709,7 +707,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter correction for altitude */
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
if (can_estimate_xy) {

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

@ -42,7 +42,6 @@
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
@ -63,7 +62,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->w_z_baro = param_find("INAV_W_Z_BARO");
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
h->w_z_acc = param_find("INAV_W_Z_ACC");
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
@ -87,7 +85,6 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
{
param_get(h->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
param_get(h->w_z_gps_v, &(p->w_z_gps_v));
param_get(h->w_z_acc, &(p->w_z_acc));
param_get(h->w_z_sonar, &(p->w_z_sonar));
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));

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

@ -43,7 +43,6 @@
struct position_estimator_inav_params {
float w_z_baro;
float w_z_gps_p;
float w_z_gps_v;
float w_z_acc;
float w_z_sonar;
float w_xy_gps_p;
@ -64,7 +63,6 @@ struct position_estimator_inav_params {
struct position_estimator_inav_param_handles {
param_t w_z_baro;
param_t w_z_gps_p;
param_t w_z_gps_v;
param_t w_z_acc;
param_t w_z_sonar;
param_t w_xy_gps_p;