fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 17:49:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1397 from dyeldandi/vz

Browse Source 
Adding vertical (Z) velocity to inav estimator
This commit is contained in:
Lorenz Meier 2015-01-23 20:26:50 +01:00
commit ba54deefc9
3 changed files with 17 additions and 0 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

@ -872,6 +872,7 @@ 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;
float w_xy_vision_p = params.w_xy_vision_p;
float w_xy_vision_v = params.w_xy_vision_v;
@ -902,6 +903,7 @@ 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;
}
/* transform error vector from NED frame to body frame */
@ -986,6 +988,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
epv = fminf(epv, gps.epv);
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);
}
if (use_vision_z) {

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

@ -63,6 +63,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
/**
* Z velocity weight for GPS
*
* Weight (cutoff frequency) for GPS altitude velocity measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);
/**
* Z axis weight for vision
*
@ -281,6 +292,7 @@ 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_vision_p = param_find("INAV_W_Z_VIS_P");
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");

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

@ -44,6 +44,7 @@
struct position_estimator_inav_params {
float w_z_baro;
float w_z_gps_p;
float w_z_gps_v;
float w_z_vision_p;
float w_z_sonar;
float w_xy_gps_p;
@ -68,6 +69,7 @@ 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_vision_p;
param_t w_z_sonar;
param_t w_xy_gps_p;