This evening I did just that but the results did more to uncover a separate issue than the one I was trying to address. What I did was use the Examples->Adafruit_AHRS->calibration sketch to capture the raw LSM6DSOX+LIS3MDL 9DOF sensor output data. I used the output from one single data collection instance and hardcoded those 9 values into the Examples->Adafruit_AHRS->calibrated_orientation sketch. Doing so would represent an immobile sensor so that I could determine if there really is a code-caused 'drift.'
I then rotated the sensor and repeated this procedure in order to capture a second set of data and hardcoded that into the sketch. Doing so enables the illustration of the Mahoney filter's response to a heavily controlled sensor orientation change. See the code where I highlighted my changes.
Code: Select all
// This sketch is based upon the Arduino Examples-> Adafruit AHRS-> calibrated_orientation sketch.
// The purpose of this modification is to capture data to explore the effects of Mahoney filter calculations.
// Two instances of simulated 9DOF sensor data are injected into the data stream in order to illustrate the output.
// The output data was formatted as a .csv in order to utilize spreadsheet graphical tools.
// This sketch injects 'hardcoded' static LSM6DSOX and LIS3MDL sensor data.
// Doing so will reveal whether observed Heading 'drift' is due to the sensor or calculations.
// Sensor data was collected using the
//
// Full orientation sensing using NXP/Madgwick/Mahony and a range of 9-DoF
// sensor sets.
// You *must* perform a magnetic calibration before this code will work.
//
// To view this data, use the Arduino Serial Monitor to watch the
// scrolling angles, or run the OrientationVisualiser example in Processing.
// Based on https://github.com/PaulStoffregen/NXPMotionSense with adjustments
// to Adafruit Unified Sensor interface
#include <Adafruit_Sensor_Calibration.h>
#include <Adafruit_AHRS.h>
Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
// uncomment one combo 9-DoF!
#include "LSM6DS_LIS3MDL.h" // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
//#include "LSM9DS.h" // LSM9DS1 or LSM9DS0
//#include "NXP_FXOS_FXAS.h" // NXP 9-DoF breakout
// pick your filter! slower == better quality output
//Adafruit_NXPSensorFusion filter; // slowest
//Adafruit_Madgwick filter; // faster than NXP
Adafruit_Mahony filter; // fastest/smalleset
#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)
Adafruit_Sensor_Calibration_EEPROM cal;
#else
Adafruit_Sensor_Calibration_SDFat cal;
#endif
#define FILTER_UPDATE_RATE_HZ 100
#define PRINT_EVERY_N_UPDATES 10
//#define AHRS_DEBUG_OUTPUT
uint32_t timestamp;
unsigned long StartTime; // Added this to record elapsed time
float ElapsedTime; // Added this to record elapsed time
void setup() {
Serial.begin(115200);
while (!Serial) yield();
if (!cal.begin()) {
Serial.println("Failed to initialize calibration helper");
} else if (! cal.loadCalibration()) {
Serial.println("No calibration loaded/found");
}
/*******************************************************************************************************************/
// MEMORANDUM:
// The Examples->Adafruit Sensor Calibration->sensor_calibration_read sketch was used to capture
// the calibration data stored in the Arduino Uno's EEPROM
/*
Calibration filesys test
Has EEPROM: 1
Has FLASH: 0
------------
0x75, 0x54, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB8, 0x1E, 0x24, 0xC2, 0xB8, 0x1E,
0xF3, 0x41, 0x9A, 0x99, 0xED, 0xC1, 0xF6, 0x28, 0x35, 0x42, 0x23, 0xDB, 0x79, 0x3F, 0x60, 0xE5,
0x80, 0x3F, 0x98, 0x6E, 0x82, 0x3F, 0x0A, 0xD7, 0x23, 0x3D, 0x6F, 0x12, 0x03, 0x3C, 0xA6, 0x9B,
0x44, 0xBC, 0xE3, 0x07,
------------
Calibrations found:
Magnetic Hard Offset: -41.03, 30.39, -29.70
Magnetic Soft Offset: 0.98, 0.04, 0.01, 0.04, 1.01, -0.01, 0.01, -0.01, 1.02
Magnetic Field Magnitude: 45.29
Gyro Zero Rate Offset: 0.00, 0.00, 0.00
Accel Zero G Offset: 0.00, 0.00, 0.00
*/
/*******************************************************************************************************************/
if (!init_sensors()) {
Serial.println("Failed to find sensors");
while (1) delay(10);
}
accelerometer->printSensorDetails();
gyroscope->printSensorDetails();
magnetometer->printSensorDetails();
setup_sensors();
filter.begin(FILTER_UPDATE_RATE_HZ);
timestamp = millis();
Wire.setClock(400000); // 400KHz
StartTime = millis(); // Added this in order to produce time-stamped output.
}
int PrintCounter = 0; // Added this to print heading only once
void loop() {
float roll, pitch, heading;
float gx, gy, gz;
static uint8_t counter = 0;
if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
return;
}
timestamp = millis();
// Read the motion sensors
sensors_event_t accel, gyro, mag;
accelerometer->getEvent(&accel);
gyroscope->getEvent(&gyro);
magnetometer->getEvent(&mag);
/*******************************************************************************************************************/
// START OF DEBUGGING MODIFICATIONS:
// OBSERVATION: This sketch using the Mahoney filter produces Heading data that 'drifts' linearly
// HYPOTHESIS: Filter calculations introduce errors due to the Arduino Uno's floating point limitations
// TEST: Replace received sensor data with static values, conduct filtering calculations, and observe Heading output
// EXPECTATION: Filtering calculations using static input data should similarly produce static Heading output data
// INPUT DATA: The Examples->Adafruit AHRS->calibration sketch was used to capture the following data:
/*
Adafruit AHRS - IMU Calibration!
Calibration filesys test
Loaded existing calibration
------------------------------------
Sensor: LSM6DS_A
Type: Acceleration (m/s2)
Driver Ver: 1
Unique ID: 1745
Min Value: -156.91
Max Value: 156.91
Resolution: 0.06
------------------------------------
------------------------------------
Sensor: LSM6DS_G
Type: Gyroscopic (rad/s)
Driver Ver: 1
Unique ID: 1746
Min Value: -34.91
Max Value: 34.91
Resolution: 0.00
------------------------------------
------------------------------------
Sensor: LIS3MDL
Type: Magnetic (uT)
Driver Ver: 1
Unique ID: 0
Min Value: -1600.00
Max Value: 1600.00
Resolution: 0.01
------------------------------------
Raw:-579,1057,8177,10,-2,-13,-310,191,-678
Uni:-0.69,1.27,9.78,0.0115,-0.0027,-0.0150,-31.04,19.12,-67.83
*/
// The following replaces the received sensor data with defined data
// For the first 600 seconds, use the data obtained where X faces North
// For the second 600 seconds, use the data obtained where X faces West
// Terminate at 1200 seconds (20 minutes)
ElapsedTime = (float(millis()) - float(StartTime)) / 1000.0;
if(ElapsedTime <= 600.0)
{
// Data has been captured in one orientation.
accel.acceleration.x = -0.69;
accel.acceleration.y = 1.27;
accel.acceleration.z = 9.78;
gx = 0.0115;
gy = -0.0027;
gz = -0.0150;
mag.magnetic.x = -31.04;
mag.magnetic.y = 19.12;
mag.magnetic.z = -67.83;
}
else
{
if(ElapsedTime <= 1200.00)
{
// Data has been captured from a completely different orientation.
accel.acceleration.x = 0.22;
accel.acceleration.y = 0.26;
accel.acceleration.z = 9.87;
gx = 0.0012;
gy = -0.0017;
gz = -0.0044;
mag.magnetic.x = -78.43;
mag.magnetic.y = 19.56;
mag.magnetic.z = -110.63;
}
else
{
Serial.println("\n\n\nProgram execution complete.\n\n");
while(1)
{
;
}
}
}
// END OF DEBUGGING MODIFICATIONS
/*******************************************************************************************************************/
#if defined(AHRS_DEBUG_OUTPUT)
Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif
cal.calibrate(mag);
cal.calibrate(accel);
cal.calibrate(gyro);
// Gyroscope needs to be converted from Rad/s to Degree/s
// the rest are not unit-important
gx = gyro.gyro.x * SENSORS_RADS_TO_DPS;
gy = gyro.gyro.y * SENSORS_RADS_TO_DPS;
gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;
// Update the SensorFusion filter
filter.update(gx, gy, gz,
accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
#if defined(AHRS_DEBUG_OUTPUT)
Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif
// only print the calculated output once in a while
if (counter++ <= PRINT_EVERY_N_UPDATES) {
return;
}
// reset the counter
counter = 0;
#if defined(AHRS_DEBUG_OUTPUT)
Serial.print("Raw: ");
Serial.print(accel.acceleration.x, 4); Serial.print(", ");
Serial.print(accel.acceleration.y, 4); Serial.print(", ");
Serial.print(accel.acceleration.z, 4); Serial.print(", ");
Serial.print(gx, 4); Serial.print(", ");
Serial.print(gy, 4); Serial.print(", ");
Serial.print(gz, 4); Serial.print(", ");
Serial.print(mag.magnetic.x, 4); Serial.print(", ");
Serial.print(mag.magnetic.y, 4); Serial.print(", ");
Serial.print(mag.magnetic.z, 4); Serial.println("");
#endif
/*******************************************************************************************************************/
// Add the following in order to print CSV header data
PrintCounter = PrintCounter + 1;
if(PrintCounter == 1)
{
Serial.println("Calibrated orientation output\n");
Serial.println("Time, Orientation, , , , Quaternion");
Serial.println("(sec), Heading, Pitch, Roll, , Qw, Qx, Qy, Qz");
}
// Add the following in order to record the elapsed time
ElapsedTime = (float(millis()) - float(StartTime)) / 1000.0;
Serial.print(ElapsedTime);
Serial.print(", ");
/*******************************************************************************************************************/
// print the heading, pitch and roll
roll = filter.getRoll();
pitch = filter.getPitch();
heading = filter.getYaw();
// Below provides slightly modified output more suited to CSV data presentation
// Serial.print("Orientation: ");
Serial.print(heading);
Serial.print(", ");
Serial.print(pitch);
Serial.print(", ");
Serial.print(roll);
Serial.print(", ,");
float qw, qx, qy, qz;
filter.getQuaternion(&qw, &qx, &qy, &qz);
// Serial.print("Quaternion: ");
Serial.print(qw, 4);
Serial.print(", ");
Serial.print(qx, 4);
Serial.print(", ");
Serial.print(qy, 4);
Serial.print(", ");
Serial.println(qz, 4);
#if defined(AHRS_DEBUG_OUTPUT)
Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif
}
The output was then collected into .csv format, put into a spreadsheet, and I was able to display the response.
While my intent was to further explore the issue of filter output data 'drift' I was more surprised with the time required to respond to any position change. Requiring three minutes for the Mahoney filter to perform the calculations in response to a large Heading change is completely unsatisfactory no matter how accurate the result may be. Yes, there is a warning embedded within the libraries indicating a performance issue... but I didn't think the response would be quite so lethargic.
Armed with this understanding I believe it is time that I take your other suggestion and dive into the thecavepearlproject.org's material to learn more. I am also going to repeat today's test on a Mega and a Due in order to compare the Mahoney filter's calculation performance. There may be some control system variables (proportion and integral gains?) that may be covered in your recommended reading that enables performance enhancement. I may even get some answers about the perceived 'drift' problem.
Tomorrow is New Year's Eve. I hope you have a Happy New Year. And Thank You for the help.