diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-21 12:52:27 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-21 12:52:27 +0400 |
| commit | 712c72d25bc595d879c2592e0a750bb0a981120f (patch) | |
| tree | 8ebcfc8522ee85c6f7dc0ff9d8f87a56178a7fa7 /src/modules/position_estimator_inav | |
| parent | 9b71e660ad86cb2ecec4db93795f417a9ba0fddd (diff) | |
| download | px4-firmware-712c72d25bc595d879c2592e0a750bb0a981120f.tar.gz px4-firmware-712c72d25bc595d879c2592e0a750bb0a981120f.tar.bz2 px4-firmware-712c72d25bc595d879c2592e0a750bb0a981120f.zip | |
Optical flow fixes
Diffstat (limited to 'src/modules/position_estimator_inav')
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 11 | ||||
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 |
2 files changed, 9 insertions, 4 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index caf2f840c..15a88066f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -248,6 +248,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_flow = 0.0f; float sonar_prev = 0.0f; + hrt_abstime flow_prev = 0; // time of last flow measurement 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 xy_src_time = 0; // time of last available position data @@ -434,6 +435,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + /* calculate time from previous update */ + float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; + flow_prev = flow.flow_timestamp; + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; @@ -484,10 +489,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - /* convert raw flow to angular flow */ + /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k; - flow_ang[1] = flow.flow_raw_y * params.flow_k; + flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; 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 b71f9472f..6892ac496 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); |