diff options
Diffstat (limited to 'src/modules/position_estimator_inav/position_estimator_inav_params.c')
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_params.c | 186 |
1 files changed, 180 insertions, 6 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 2e4f26661..98b31d17b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,34 +40,207 @@ #include "position_estimator_inav_params.h" +/** + * Z axis weight for barometer + * + * Weight (cutoff frequency) for barometer altitude measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); + +/** + * Z axis weight for GPS + * + * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); -PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); + +/** + * Z axis weight for sonar + * + * Weight (cutoff frequency) for sonar measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); + +/** + * XY axis weight for GPS position + * + * Weight (cutoff frequency) for GPS position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); + +/** + * XY axis weight for GPS velocity + * + * Weight (cutoff frequency) for GPS velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); + +/** + * XY axis weight for optical flow + * + * Weight (cutoff frequency) for optical flow (velocity) measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); + +/** + * XY axis weight for resetting velocity + * + * When velocity sources lost slowly decrease estimated horizontal velocity with this weight. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); + +/** + * XY axis weight factor for GPS when optical flow available + * + * When optical flow data available, multiply GPS weights (for position and velocity) by this factor. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); + +/** + * Accelerometer bias estimation weight + * + * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + * + * @min 0.0 + * @max 0.1 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); + +/** + * Optical flow scale factor + * + * Factor to convert raw optical flow (in pixels) to radians [rad/px]. + * + * @min 0.0 + * @max 1.0 + * @unit rad/px + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); + +/** + * Minimal acceptable optical flow quality + * + * 0 - lowest quality, 1 - best quality. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); + +/** + * Weight for sonar filter + * + * Sonar filter detects spikes on sonar measurements and used to detect new surface level. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); + +/** + * Sonar maximal error for new surface + * + * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). + * + * @min 0.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); + +/** + * Land detector time + * + * Vehicle assumed landed if no altitude changes happened during this time on low throttle. + * + * @min 0.0 + * @max 10.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); + +/** + * Land detector altitude dispersion threshold + * + * Dispersion threshold for triggering land detector. + * + * @min 0.0 + * @max 10.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); + +/** + * Land detector throttle threshold + * + * Value should be lower than minimal hovering thrust. Half of it is good choice. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); +/** + * GPS delay + * + * GPS delay compensation + * + * @min 0.0 + * @max 1.0 + * @unit s + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); + 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_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"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); - h->w_xy_acc = param_find("INAV_W_XY_ACC"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); + h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); 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"); @@ -77,6 +250,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); + h->delay_gps = param_find("INAV_DELAY_GPS"); return OK; } @@ -85,12 +259,11 @@ 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_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)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); - param_get(h->w_xy_acc, &(p->w_xy_acc)); param_get(h->w_xy_flow, &(p->w_xy_flow)); + param_get(h->w_xy_res_v, &(p->w_xy_res_v)); 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)); @@ -100,6 +273,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); + param_get(h->delay_gps, &(p->delay_gps)); return OK; } |