reefwing-software / reefwing-ahrs Goto Github PK
View Code? Open in Web Editor NEWAttitude and Heading Reference System (AHRS)
License: Other
reefwing-ahrs's People
Contributors
Stargazers
Watchers
reefwing-ahrs's Issues
Problem detecting the IMU
Hi David,
first of all thank you very much for all your hard work, I have been maintaining few softwares myself and I know how much energy this can drain.
Now straight to the question (not sure it is the right place where to ask this but I will try, so feel free to ignore it; also full disclosure: I posted this on the Arduino forum but so far nobody replied). Just bought an Arduino Nano 33 BLE Sense Rev. 2 for a pet project I am doing with the kids. I have installed the AHRS libraries and tried to run the testAndCalibrate sketch. It tells me "LSM9DS1 IMU not found". OTOH, if I use the standard Arduino_BMI270_BMM150 I can read data from the accelerometer, gyro and magnetometer.
So at the moment I am a bit clueless on how to proceed: I have installed the libraries one at a time just to be sure that they do not conflict and found that I have failures of detecting the IMU also with other libraries like Femme Verbeeck's one. Am I missing something obvious?
Thanks for any spark in the dark you can provide.
Compensation for centrifugal accelerations
Hello @reefwing, thanks for you work!
I'm trying the AHRS library for experiments.
Before that I used Madgwick filtering.
Everything works fine on the ground.
After testing in flight, centrifugal acceleration changes the level of the horizon and makes it parallel to the airplane wing.
Compiled the code from the example.
Acceleration and magnetic field data are provided by lsm303, and angular velocity data are provided by l3gd20.
I work with the development board.
Due to the design features, I had to rotate the board axes.
As a result, my roll axis is Z, the pitch axis is X, and the yaw axis is Y.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise
The positive direction of rotation of the aircraft axes does not coincide with the positive rotation of the board axes.
But the “minus” was indicated only for the Z axis, inside the library Reefwing-AHRS code there was some kind of perception of the “sign” of the axis rotation.
I submit data from the following units to the input of the ahrs.
Accelerations in G.
Angular velocities in degrees/sec.
Magnetic field strength in uGauss.
I tell the filter like this
ahrs.setFusionAlgorithm(SensorFusion::MADGWICK);
In the code, I swapped the Z and Y axes only for the accelerometer and gyroscope, I did not change the magnetometer axis, because when I change the magnetometer, the pitch angle starts to twitch like crazy.
gyro.read();
lsm303.read();
data.ax = lsm303.accelData.x;// G
data.ay = -lsm303.accelData.z;// G
data.az = lsm303.accelData.y;// G
data.gx = gyro.g.x;// deg/s
data.gy = -gyro.g.z;// deg/s
data.gz = gyro.g.y;// deg/s
data.mx = lsm303.magData.x;// uGauss
data.my = lsm303.magData.y;// uGauss
data.mz = lsm303.magData.z;// uGauss
ahrs.setData(data);
ahrs.update();
roll = ahrs.angles.pitch;
pitch = ahrs.angles.roll;
heading = ahrs.angles.heading;
At the output I get the roll, pitch and heading angles.
Roll: 0.71 Pitch: 87.70 Yaw: 43.96 Heading: 43.96 Loop Frequency: 1 Hz
Accel X: -18.00 Y: 16568.00 Z: 1088.00 Raw
Accel X: -0.00 Y: 1.01 Z: 0.07 G
Accel X: -0.01 Y: 9.92 Z: 0.65 m/s^2
Gyro X: 33.00 Y: 19.00 Z: -20.00 raw
Gyro X: 0.50 Y: 0.29 Z: deg/s
Gyro X: 0.01 Y: 0.01 Z: -0.01 rad/s
Mag X: 65354.00 Y: 64935.00 Z: 7.00 uGauss
Mag X: 653.54 Y: 649.35 Z: 0.07 microtesla (uT)
Roll: -2.16 Pitch: 80.85 Yaw: 45.96 Heading: 45.96 Loop Frequency: 1 Hz
Accel X: 238.00 Y: 16248.00 Z: 1024.00 Raw
Accel X: 0.01 Y: 0.99 Z: 0.06 G
Accel X: 0.14 Y: 9.73 Z: 0.61 m/s^2
Gyro X: 43.00 Y: 28.00 Z: -33.00 raw
Gyro X: 0.66 Y: 0.43 Z: deg/s
Gyro X: 0.01 Y: 0.01 Z: -0.01 rad/s
Mag X: 65354.00 Y: 64935.00 Z: 137.00 uGauss
Mag X: 653.54 Y: 649.35 Z: 1.37 microtesla (uT)
Roll: -0.20 Pitch: 89.22 Yaw: 44.82 Heading: 44.82 Loop Frequency: 1 Hz
Accel X: -18.00 Y: 16440.00 Z: 1024.00 Raw
Accel X: -0.00 Y: 1.00 Z: 0.06 G
Accel X: -0.01 Y: 9.84 Z: 0.61 m/s^2
Gyro X: 29.00 Y: 32.00 Z: -31.00 raw
Gyro X: 0.44 Y: 0.49 Z: deg/s
Gyro X: 0.01 Y: 0.01 Z: -0.01 rad/s
Mag X: 65355.00 Y: 64934.00 Z: 65532.00 uGauss
Mag X: 653.55 Y: 649.34 Z: 655.32 microtesla (uT)
If I move the sensors from side to side, for example along the X axis, then the roll angle also changes, which indicates that the ahrs does not compensate for centrifugal forces.
Have you tried this code on a flight?
yaw and pitch interconnection/tilt-compensation problem.
Hi, I'm integrating reefwing_ahrs with the nano 33 ble and opentrack to create a wired/wireless head tracker for PC games, but this library has been giving me some trouble.
Basically no matter what filter type I choose, angles.pitch affects angles.yaw. Yaw and roll work perfectly alone and combined, but even if I disable output from pitch entirely, somewhere in the math pitch is still affecting yaw. Pitch still affects pitch, but it also bleeds over into yaw calculation.
I spent a day or two assuming it was axis misalignment; trying to verify the orientation of the LSM9DS1 IMU's axis and the order and signs needed in the quaternion update calls (or the filter function in this case) but from what I can find your library is compensating properly for the weird mag orientation of the 33 ble.
Another possibility is that the compass tilt compensation is busted, but this seems unlikely to me because yaw is only affected by one of the other axis not both.
I have mainly been using Fusion filter. It works the best for this application imo, but all of the filter types are broken in the same way: pitch always affects yaw.
So basically I'm at a loss of what's wrong or how to fix it, but I'm not smart enough to implement Fusion on my own. Any ideas or tips would be greatly appreciated. I'm attaching my current ino file, ignore the hat struct and weird serial writes, that is only used when talking with opentrack software. It's currently just outputting strings and ints to serial in test mode.
updateAngles() function is where the angles are updated and printed.
Thanks for this lib by the way!
#include <ArduinoBLE.h>
#include <ReefwingAHRS.h>
// LED Pin definitions
// *RGB PINS ARE INVERTED ON THIS BOARD*
// for RED, BLUE, GREEN, and BUILTIN only: HIGH means LOW and vice versa.
#define PIN_LED (13u)
#define LED_BUILTIN PIN_LED
#define RED (22u)
#define GREEN (23u)
#define BLUE (24u)
#define LED_PWR (25u)
LSM9DS1 imu;
EulerAngles angles;
//TEST MODE ENABLE OR DISABLE
bool TestMode = true; //Set true to output data directly to serial (bypass hatire conversion + centering)
//Variables only required in test mode
int loopFrequency = 0;
const long displayPeriod = 100;
unsigned long previousMillis = 0;
//Centering variables (currently disabled)
int center_yaw = 0;
int center_pitch = 0;
int center_roll = 0;
int loopCount = 0; //Used for centering once filter is calm
//BLE variables + init
bool BLEconnected = false;
const char* deviceServiceUuid = "19b10000-e8f2-537e-4f6c-d104768a1214";
const char* deviceServiceCharacteristicUuid = "19b10001-e8f2-537e-4f6c-d104768a1214";
const char* deviceServiceCharacteristicUuid1 = "19b10002-e8f2-537e-4f6c-d104768a1214";
BLEService opentrackService(deviceServiceUuid);
BLECharacteristic hatireCharacteristic(deviceServiceCharacteristicUuid, BLERead | BLENotify, 30, true);
BLECharacteristic writeCharacteristic(deviceServiceCharacteristicUuid1, BLEWrite, 30, false);
//this structure is needed by hatire
struct {
int16_t Begin; // 2 Debut
uint16_t Cpt; // 2 Compteur trame or Code
float gyro[3]; // 12 [Y, P, R] gyro (actually just the Y,P,R from the fusion filter)
float acc[3]; // 12 [x, y, z] Acc (this is left empty and ignored)
int16_t End; // 2 Fin
} hat;
/////////////////////////////////////////////////////////////////////////////////
// Prints filtered angles to either serial or BLE based on connection. //
// Only runs in test mode and usb mode for now, hatire requires custom struct. //
// *FOR DEBUGGING* //
/////////////////////////////////////////////////////////////////////////////////
void SerialPrint(char msg[]) {
if (TestMode) {
Serial.print(msg);
/* if bluetooth mode
if (BLEconnected) {
hatireCharacteristic.writeValue(msg);
}
// else serial mode
else {
Serial.print(msg);
} */
}
}
///////////////////////////////////////////////////////////////////
// Sends hat struct to hatire using current communication method //
///////////////////////////////////////////////////////////////////
void sendAnglesToHatire() {
// Send HAT Frame to PC base on communication mode
// if bluetooth mode
if (BLEconnected) {
hatireCharacteristic.writeValue((byte*)&hat,30);
}
// else serial mode
else {
Serial.write((byte*)&hat,30);
}
hat.Cpt++;
if (hat.Cpt>999)
{
hat.Cpt=0;
}
}
///////////////////////////////////////////////////////
// Updates the IMU and fusion filter to get new data //
///////////////////////////////////////////////////////
void updateAngles() {
// Check for new IMU data and update angles
angles = imu.update();
// Wait for new sample - 7 ms delay provides a ~100Hz sample rate / loop frequency
delay(6);
loopCount++;
if (TestMode){
// Display sensor data every displayPeriod, non-blocking.
if (millis() - previousMillis >= displayPeriod) {
Serial.print("Roll: ");
Serial.print(angles.roll);
Serial.print("\tPitch: ");
Serial.print(angles.pitch);
Serial.print("\tYaw: ");
Serial.print(angles.yaw);
Serial.print("\tLoop Frequency: ");
Serial.print(loopFrequency);
Serial.println(" Hz");
loopFrequency = 0;
previousMillis = millis();
}
loopFrequency++;
}
else {
if (loopCount == -1) {
center_yaw=angles.yaw;
center_pitch=angles.pitch;
center_roll=angles.roll;
}
//Assign yaw, pitch, and roll in hatire struct
hat.gyro[0]=angles.yaw - center_yaw;
//hat.gyro[1]=angles.pitch - center_pitch; //Roll axis disabled for now
hat.gyro[2]=angles.roll - center_roll;
// Send HAT Frame to PC
sendAnglesToHatire();
}
}
//////////////////////////////////
// Setup function, runs at boot //
//////////////////////////////////
void setup() {
// Set hatire constants //
// header frame
hat.Begin=0xAAAA;
// Frame Number or err code
hat.Cpt=0;
// footer frame
hat.End=0x5555;
// initialize the led digital pins as an output
pinMode(LED_BUILTIN, OUTPUT);
pinMode(RED, OUTPUT);
pinMode(BLUE, OUTPUT);
pinMode(GREEN, OUTPUT);
pinMode(LED_PWR, OUTPUT);
//Disable power LED until setup procedure is complete, Enable purple while booting up
digitalWrite(LED_PWR, LOW);
digitalWrite(RED, LOW);
digitalWrite(GREEN, HIGH);
digitalWrite(BLUE, LOW);
digitalWrite(LED_BUILTIN, LOW);
//Bluetooth Init
BLE.begin();
BLE.setLocalName("Nano 33 Head Tracker");
BLE.setAdvertisedService(opentrackService);
opentrackService.addCharacteristic(hatireCharacteristic);
opentrackService.addCharacteristic(writeCharacteristic);
BLE.addService(opentrackService);
hatireCharacteristic.writeValue((byte*)&hat,30);
BLE.advertise();
//Serial Start
Serial.begin(115200);
//Initialise the LSM9DS1 IMU
imu.begin();
//Set filter settings
imu.setFusionAlgorithm(SensorFusion::FUSION);
imu.setFusionPeriod(0.01f); // Estimated sample period = 0.01 s = 100 Hz
imu.setFusionGain(10); // Lower than 10 caused issues for me but you may be able to go as low as 5.
// *Play around with gain if you have accuracy or slow response issues*
// Lower = More accurate but also more sluggish, Higher = Faster but less accurate.
if (imu.connected()) {
SerialPrint("LSM9DS1 IMU Connected.\n");
//Start IMU
imu.start();
//Set boot up led RED only by disabling blue to indicate device is now running and waiting for connection
digitalWrite(BLUE, HIGH);
}
else {
// LSM9DS1 IMU not found
digitalWrite(BLUE, HIGH);
//Flash Red Light To indicate IMU error
while(1) {
digitalWrite(RED, LOW);
delay(500);
digitalWrite(RED, HIGH);
}
}
}
//////////////////////////////////////////////////////////////
// Attemps connection over BT and usb serial over and over. //
// Once connected, runs updateAngles() until disconnected. //
/////////////////////////////////////////////////////////////
void loop() {
//LED red, pwr LED off while disconnected
digitalWrite(RED, LOW);
digitalWrite(LED_PWR, LOW);
//Attempt Serial and Bluetooth connection
BLEDevice central = BLE.central();
delay(500);
//if connected with BT or serial, else restart loop
if (Serial || central); {
//If bluetooth mode
if (central.connected()) {
BLEconnected = true;
digitalWrite(RED, HIGH);
digitalWrite(LED_PWR, HIGH);
//call main loop, runs while connected
while(central.connected()) {
updateAngles();
}
}
// Else usb serial mode
else {
BLEconnected = false;
digitalWrite(RED, HIGH);
digitalWrite(LED_PWR, HIGH);
//call main loop, runs while connected
while(Serial) {
updateAngles();
}
}
}
}
Modification to add 6dof (lsm3) and 9dof config split
Hello.
I find your repository and it works very well for my nano 33ble sense.
I would like to use it on other mpu 9dof and also on a 6 dof sensor.
Could you split lsm9 config in specific .h and .cpp in aim to replace by other sensor?
Moreover could you add possibility to use 6dof (lsm3) sensor with less function(no bias etc)?
Thank you for your answer about possibility or problem.
If you don't have time could you give me different step to do this?
Thank you
(My aim is to use with xiao nrf52840 sense)
I'm using your repository to track golf swing trajectory motion.
Kalman and pitch
Hi,
I use Nano 33 BLE rev 2
setting in KALMAN mode, but
when I write roll , pitch in serial I only get roll, pitch in nan
void setup() {
ahrs.begin();
ahrs.setFusionAlgorithm(SensorFusion::KALMAN );
ahrs.setDeclination(12.717); // Sydney, Australia
}
void loop() {
if (IMU.gyroscopeAvailable()) { IMU.readGyroscope(data.gx, data.gy, data.gz); }
if (IMU.accelerationAvailable()) { IMU.readAcceleration(data.ax, data.ay, data.az); }
if (IMU.magneticFieldAvailable()) { IMU.readMagneticField(data.mx, data.my, data.mz); }
ahrs.setData(data);
ahrs.update();
Serial.print(ahrs.angles.roll , 2);
Serial.print(",");
Serial.print(ahrs.angles.pitch, 2);
Serial.println();
}
Recovery time after saturating the gyroscope.
During fast rotations, or when knocked, or whipped around, the raw gyro measurements plateau at for a while instead of tracking the board's movement. This is likely due to the maximum sensibility of the gyro. After that, the axis value is offset until the filter corrects the "drift" over a few seconds.
When the gyro saturation is detected, the gain should be raised for a short time to re-center the axis (similarly to the initialization phase).
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.