Skip to content

Added gyro high pass filter for lpos output. #72

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jgoppert wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Added gyro high pass filter for lpos output. #72

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ccab8fa
Added gyro high pass filter for lpos output.
jgoppert Feb 10, 2016
dc50413
Fixed hp filter coefficient.
jgoppert Feb 10, 2016
0ff973e
Changed highpass cut freq based on experimental testing.
jgoppert Feb 10, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 16 additions & 2 deletions src/modules/flow/main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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 = 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;
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;
Expand Down