From ccab8fae6d6978d666215374e463f613a48a26ea Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 10 Feb 2016 16:17:36 -0500 Subject: [PATCH 1/3] Added gyro high pass filter for lpos output. --- src/modules/flow/main.c | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index fb11e9f..f82e3b1 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -332,6 +332,9 @@ int main(void) int valid_frame_count = 0; int pixel_flow_count = 0; + float rot_flow_x_lp = 0.0f; + float rot_flow_y_lp = 0.0f; + static float accumulated_flow_x = 0; static float accumulated_flow_y = 0; static float accumulated_gyro_x = 0; @@ -573,9 +576,20 @@ int main(void) /* send approximate local position estimate without heading */ if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS])) { + float pi = M_PI; + float fcut = 0.1; // cut all freq. below 0.1 hz + float dt = integration_timespan; + float alpha = 2*pi*dt*fcut/(2*pi*dt*fcut+1); + float rot_flow_x = ground_distance*accumulated_gyro_x; + float rot_flow_y = ground_distance*accumulated_gyro_y; + rot_flow_x_lp = rot_flow_x_lp*(1-alpha) + rot_flow_x*alpha; + rot_flow_y_lp = rot_flow_y_lp*(1-alpha) + rot_flow_y*alpha; + float rot_flow_x_hp = rot_flow_x - rot_flow_x_lp; + float rot_flow_y_hp = rot_flow_y - rot_flow_y_lp; + /* rough local position estimate for unit testing */ - lpos.x += ground_distance*accumulated_flow_x; - lpos.y += ground_distance*accumulated_flow_y; + lpos.x += ground_distance*(accumulated_flow_x) - rot_flow_x_hp; + lpos.y += ground_distance*(accumulated_flow_y) - rot_flow_y_hp; lpos.z = -ground_distance; /* velocity not directly measured and not important for testing */ lpos.vx = 0; From dc50413649a580c5da7e8062a644d98ae8c9b0a0 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 10 Feb 2016 17:03:36 -0500 Subject: [PATCH 2/3] Fixed hp filter coefficient. --- src/modules/flow/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index f82e3b1..88cab80 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -577,8 +577,8 @@ int main(void) if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS])) { float pi = M_PI; - float fcut = 0.1; // cut all freq. below 0.1 hz - float dt = integration_timespan; + float fcut = 0.01; // cut all freq. below 0.1 hz + float dt = integration_timespan/1000000.0f; float alpha = 2*pi*dt*fcut/(2*pi*dt*fcut+1); float rot_flow_x = ground_distance*accumulated_gyro_x; float rot_flow_y = ground_distance*accumulated_gyro_y; From 0ff973e84bc8fac85c6d9441c0e45967470287d3 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 10 Feb 2016 17:15:14 -0500 Subject: [PATCH 3/3] Changed highpass cut freq based on experimental testing. --- src/modules/flow/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 88cab80..583dbeb 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -577,7 +577,7 @@ int main(void) if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS])) { float pi = M_PI; - float fcut = 0.01; // cut all freq. below 0.1 hz + float fcut = 1.0; // cut all freq. below 1 hz float dt = integration_timespan/1000000.0f; float alpha = 2*pi*dt*fcut/(2*pi*dt*fcut+1); float rot_flow_x = ground_distance*accumulated_gyro_x;