fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 11:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

added px4flow integral frame, adjusted px4flow i2c driver, adjusted postition_estimator_inav

Browse Source
This commit is contained in:
dominiho
2014-10-28 12:35:20 +01:00
parent 1ff9e4d665
commit 276dc7fbbc
3 changed files with 319 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+6 -5
View File
@@ -298,7 +298,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 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)
@@ -491,8 +491,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
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;
// 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) &&
@@ -550,8 +550,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
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;
//todo check direction of x und y axis
flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//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;