Skip to content

Instantly share code, notes, and snippets.

@vladinator1000
Last active March 3, 2018 22:36
Show Gist options
  • Save vladinator1000/cbe998c3dfea711c3413cb23b6244cd9 to your computer and use it in GitHub Desktop.
Save vladinator1000/cbe998c3dfea711c3413cb23b6244cd9 to your computer and use it in GitHub Desktop.
How to use tcleg's complementary filter with an Arduino and the LSM6DS3 motion sensor.
// This gist uses this sensor https://learn.sparkfun.com/tutorials/lsm6ds3-breakout-hookup-guide
// with this filter https://github.com/tcleg/Six_Axis_Complementary_Filter
// to give you angles according to the X and Y axes
/******************************************************************************
Filter Arduino hookup guide by Vlady Veselinov,
******************************************************************************/
#include "SparkFunLSM6DS3.h"
#include "Wire.h"
#include "SPI.h"
#include "six_axis_comp_filter.h"
LSM6DS3 myIMU; // Default constructor for the motion sensor
// (below) Constructor for the Complementary Filter CompSixAxis(deltaTime, tau)
// deltaTime - The time delta update period expressed in seconds. This value
// should be set to how often the filter is updated with new values,
// which should be on a regular interval. The smaller this value is,
// the less noise there will be on the comp. filter's output.
//
// tau - Max allowable time until gyro drifts too far and comp. filter
// shifts its weight to the accelerometer expressed in seconds. This
// value is usually based on the drift rate of the gyro. For example,
// if the gyro drifts at a rate of 0.5 degrees per second and the
// tolerance of max allowable drift is 1 degree, then tau should be set
// to 2 seconds as it will take 2 seconds to drift 1 degree. The larger
// this value is, the less noise there will be on the comp. filter's
// output. In exchange, it will drift further from the correct angular
// position.
CompSixAxis CompFilter(0.1, 2);
float accelX, accelY, accelZ, // variables to store sensor values
gyroX, gyroY, gyroZ,
xAngle, yAngle; // variables for angles calculated by filter
void setup() {
Serial.begin(9600);
//Call .begin() to configure the IMU (Inertial Measurement Unit)
myIMU.begin();
}
void loop() {
// Get all motion sensor (in this case LSM6DS3) parameters,
// If you're using a different sensor you'll have to replace the values
accelX = myIMU.readFloatAccelX();
accelY = myIMU.readFloatAccelY();
accelZ = myIMU.readFloatAccelZ();
gyroX = myIMU.readFloatGyroX();
gyroY = myIMU.readFloatGyroY();
gyroZ = myIMU.readFloatGyroZ();
// Convert these values into angles using the Complementary Filter
CompFilter.CompAccelUpdate(accelX, accelY, accelZ); // takes arguments in m/s^2
CompFilter.CompGyroUpdate(gyroX, gyroY, gyroZ); // takes arguments un rad/s
CompFilter.CompUpdate();
CompFilter.CompStart();
// Get angle relative to X and Y axes and write them to the variables in the arguments
CompAnglesGet(&xAngle, &yAngle);
Serial.write(xAngle);
Serial.write(yAngle);
}
@DavidSanf0rd
Copy link

thanks!

@carlosbegar
Copy link

hello, let's see if you can help me compile the code give me the following error
'CompAnglesGet' was not declared in this scope

@lucalcin
Copy link

lucalcin commented Mar 3, 2018

Solved. Must be:
CompFilter.CompAnglesGet (&xAngle, &yAngle);

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment