Hello all, l am using RTIMULib which can be found here if required.
I am trying unsuccessfully to get Gyro Z biased corrected data into my loop below, you can see l have got the raw gyro info.
void loop()
{
unsigned long now = millis();
unsigned long delta;
int loopCount = 1;
while (imu->IMURead()) { // get the latest data if ready yet
// this flushes remaining data in case we are falling behind
if (++loopCount >= 10)
continue;
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
sampleCount++;
if ((delta = now - lastRate) >= 1000) {
Serial.print("Sample rate: "); Serial.print(sampleCount);
if (imu->IMUGyroBiasValid())
Serial.println(", gyro bias valid");
else
Serial.println(", calculating gyro bias");
sampleCount = 0;
lastRate = now;
}
RTVector3 mag = imu->getCompass();
float mx = mag.x();
float my = mag.y();
float mz = mag.z();
// Serial.print("mpu 9255 Mag Raw Data X,Y,Z "); Serial.print( mx);Serial.print( my); Serial.println(mz);
//float mzAV= m_compassAverage.z();
RTVector3 m_compassAverage;
//RTIMU::calibrateAverageCompass();
//float calmz = m_compassAverage.setZ;
Serial.print("mag raw x,y,z "); Serial.print(mx);Serial.print(" "); Serial.print(my); Serial.print(" ");Serial.println(mz);//prints in real time
RTVector3 vec = fusion.getFusionPose();
float MPU9255_heading[3];
MPU9255_heading[2] = (vec.z() * RTMATH_RAD_TO_DEGREE);
Serial.print("mpu 9255 heading "); Serial.println( MPU9255_heading[2]);//prints in real time
RTVector3 gyro = imu->getGyro();
float gx = gyro.x();
float gy = gyro.y();
float gz = gyro.z();
gz *= 57.2958F ; // convert RAD to DPS
Serial.print(" gz "); Serial.println(gz);//prints in real time
float MPU9255_gyro[2];
MPU9255_gyro[2] = imu->getGyro().z();
MPU9255_gyro[2] *= 57.2958F ; // convert RAD to DPS
Serial.print("mpu 9255 gz "); Serial.println(MPU9255_gyro[2]);//prints in real time
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
lastDisplay = now;
// RTMath::display("Gyro:", (RTVector3&)imu->getGyro()); // gyro data
// RTMath::display("Accel:", (RTVector3&)imu->getAccel()); // accel data
// RTMath::display("Mag:", (RTVector3&)imu->getCompass()); // compass data
// RTMath::displayRollPitchYaw("Pose:", (RTVector3&)fusion.getFusionPose()); // fused output
Serial.println();
// Serial.print("mpu 9255 gz "); Serial.println(MPU9255_gyro[2]);//prints 3 hz
// Serial.print("mpu 9255 heading "); Serial.println( MPU9255_heading[2]);//prints 3 hz
}
}
}
Below is the RTIMU.cpp were l believe the corrected gyro data is ( m_gyroBias.setZ), which l wish to put into above loop. But l have been unsuccessful in getting it into the loop due to my limited coding skills.
if (!m_gyroBiasValid) {
RTVector3 deltaAccel = m_previousAccel;
deltaAccel -= m_accel; // compute difference
m_previousAccel = m_accel;
if ((deltaAccel.squareLength() < RTIMU_FUZZY_ACCEL_ZERO_SQUARED) &&
(m_gyro.squareLength() < RTIMU_FUZZY_GYRO_ZERO_SQUARED)) {
// what we are seeing on the gyros should be bias only so learn from this
m_gyroBias.setX((1.0 - m_gyroAlpha) * m_gyroBias.x() + m_gyroAlpha * m_gyro.x());
m_gyroBias.setY((1.0 - m_gyroAlpha) * m_gyroBias.y() + m_gyroAlpha * m_gyro.y());
m_gyroBias.setZ((1.0 - m_gyroAlpha) * m_gyroBias.z() + m_gyroAlpha * m_gyro.z());
Any help would be appreciated.
Regards
Kevin
Your indentation is all over the place. Run the auto-format tool in the Arduino IDE. It's very good. If it appears to scramble your code then the code was wrong, not the tool.
You've given us a snippet of a program. If we wanted to run it to test it out, we can't. The other code which you think is unimportant isn't. Often the problem is in the code you didn't post because you didn't understand it.
It appears you may be using some kind of sensor fusion library. This will estimate the biases itself. It may or may not let you see the current bias estimate. But even if it doesn't, its output is corrected with the bias removed.
Thanks for the response, but an answer would be helpful.
I am aware of auto format and just hadn't used it then, sorry.
The other part of the code wouldn't be required but here it is. I gave the download as all the library would have to be added to get it to compile and it is an example sketch.
I am aware the fusion would use biased corrected gyro data, but l wish to use rate of turn indication in a sketch.
[code
#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include "CalLib.h"
#include <EEPROM.h>
RTIMU *imu; // the IMU object
RTFusionRTQF fusion; // the fusion object
RTIMUSettings settings; // the settings object
// DISPLAY_INTERVAL sets the rate at which results are displayed
#define DISPLAY_INTERVAL 300 // interval between pose displays
// SERIAL_PORT_SPEED defines the speed to use for the debug serial port
#define SERIAL_PORT_SPEED 115200
unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;
void setup()
{
int errcode;
Serial.begin(SERIAL_PORT_SPEED);
Wire.begin();
imu = RTIMU::createIMU(&settings); // create the imu object
Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
if ((errcode = imu->IMUInit()) < 0) {
Serial.print("Failed to init IMU: "); Serial.println(errcode);
}
if (imu->getCalibrationValid())
Serial.println("Using compass calibration");
else
Serial.println("No valid compass calibration data");
lastDisplay = lastRate = millis();
sampleCount = 0;
// Slerp power controls the fusion and can be between 0 and 1
// 0 means that only gyros are used, 1 means that only accels/compass are used
// In-between gives the fusion mix.
fusion.setSlerpPower(0.02);//-----------------------------------------------------------------------------default
//fusion.setSlerpPower(0.99);
// use of sensors in the fusion algorithm can be controlled here
// change any of these to false to disable that sensor
fusion.setGyroEnable(true);
fusion.setAccelEnable(true);
fusion.setCompassEnable(true);
}
void loop()
{
unsigned long now = millis();
unsigned long delta;
int loopCount = 1;
while (imu->IMURead()) { // get the latest data if ready yet
// this flushes remaining data in case we are falling behind
if (++loopCount >= 10)
continue;
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
sampleCount++;
if ((delta = now - lastRate) >= 1000) {
Serial.print("Sample rate: "); Serial.print(sampleCount);
if (imu->IMUGyroBiasValid())
Serial.println(", gyro bias valid");
else
Serial.println(", calculating gyro bias");
sampleCount = 0;
lastRate = now;
}
RTVector3 mag = imu->getCompass();
float mx = mag.x();
float my = mag.y();
float mz = mag.z();
// Serial.print("mpu 9255 Mag Raw Data X,Y,Z "); Serial.print( mx);Serial.print( my); Serial.println(mz);
float mx_bias = 0;//bias offsets from Maagneto
float my_bias = 0;
float mz_bias = 0;
float mx_scale_a = 0;//hard/soft iron offsets from Magneto
float mx_scale_b = 0;
float mx_scale_c = 0;
float my_scale_a = 0;
float my_scale_b = 0;
float my_scale_c = 0;
float mz_scale_a = 0;
float mz_scale_b = 0;
float mz_scale_c = 0;
// Mag correction via Magneto software
float mx_off = mx - mx_bias;//Bias correct raw data
float my_off = my - my_bias;
float mz_off = mz - mz_bias;
float MX_cal = mx_scale_a * mx_off + mx_scale_b * my_off + mx_scale_c * mz_off;//X axis corrected
float MY_cal = my_scale_a * mx_off + my_scale_b * my_off + my_scale_c * mz_off;//Y axis corrected
float MZ_cal = mz_scale_a * mx_off + mz_scale_b * my_off + mz_scale_c * mz_off;//Z axis corrected
// Serial.print("mpu 9255 Mag corrected Data MX,MY,MZ "); Serial.print( MX_cal); Serial.print(" ");Serial.print( MY_cal);Serial.print(" "); Serial.println(MZ_cal);
RTVector3 vec = fusion.getFusionPose();
float MPU9255_heading[3];
MPU9255_heading[2] = (vec.z() * RTMATH_RAD_TO_DEGREE);
Serial.print("mpu 9255 heading "); Serial.println( MPU9255_heading[2]);//prints in real time
//Raw gyro data
RTVector3 gyro = imu->getGyro();
float gx = gyro.x();
float gy = gyro.y();
float gz = gyro.z();
gz *= 57.2958F ; // convert RAD to DPS
Serial.print(" gz "); Serial.println(gz);//prints in real time
float MPU9255_gyro[2];
MPU9255_gyro[2] = imu->getGyro().z();
MPU9255_gyro[2] *= 57.2958F ; // convert RAD to DPS
Serial.print("mpu 9255 gz "); Serial.println(MPU9255_gyro[2]);//prints in real time
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
lastDisplay = now;
// RTMath::display("Gyro:", (RTVector3&)imu->getGyro()); // gyro data
// RTMath::display("Accel:", (RTVector3&)imu->getAccel()); // accel data
// RTMath::display("Mag:", (RTVector3&)imu->getCompass()); // compass data
// RTMath::displayRollPitchYaw("Pose:", (RTVector3&)fusion.getFusionPose()); // fused output
Serial.println();
// Serial.print("mpu 9255 gz "); Serial.println(MPU9255_gyro[2]);//prints 3 hz
// Serial.print("mpu 9255 heading "); Serial.println( MPU9255_heading[2]);//prints 3 hz
}
}
}
]
Of course there is more library's required.
Regards Kevin
l wish to use rate of turn indication
The library returns the offset corrected rate of rotation about each axis. If this is not what you want, please explain.
Hi Jim, l was hoping you might answer as you have a great knowledge in IMU's and hopefully RTIMULib as l know you are a convert of it.
Yes l am after the offset corrected gyro data, is that not what the excerpt of the rtimu.cpp is?
While you are looking, you will also see l have added some code in the loop to implement magneto, mag correction.
Again l'm unable to reinsert this into the library so the corrected mag could be used for fusion, have you tried to do this?, or could you show how to do this.
I imagine it would need to be called in setup, then updated back into the library.
Any help appreciated.
Regards
Duckman
Kevin
Yes l am after the offset corrected gyro data
That is exactly what is returned by getGyro() function call.
At least for the sensor I use (LSM303DLHC) the magnetometer corrections are built in to the routine that processes the data. However, the full 3x3 matrix multiplication required to correct off-axis errors requires user input is not implemented.
In the newer version of the library, RTIMUlib2, it is implemented but I have not figured out how to get it to compile with Arduino.
However, if you can, this is the section in routine RTIMU.cpp that you want to activate, by entering the ellipsoid correction matrix data elsewhere and setting m_compassCalEllipsoidValid to true.
if (m_settings->m_compassCalEllipsoidValid) {
RTVector3 ev = m_imuData.compass;
ev -= m_settings->m_compassCalEllipsoidOffset;
m_imuData.compass.setX(ev.x() * m_settings->m_compassCalEllipsoidCorr[0][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[0][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[0][2]);
m_imuData.compass.setY(ev.x() * m_settings->m_compassCalEllipsoidCorr[1][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[1][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[1][2]);
m_imuData.compass.setZ(ev.x() * m_settings->m_compassCalEllipsoidCorr[2][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[2][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[2][2]);
}
Hi Jim, thanks for that, l thought "get gyros" was raw data not corrected so that fixes that issue.
I'm using RTIMULib Arduino and it doesn't have the ellipsoid correction in the .cpp, but l will play with putting it in here, removing offset and scale and see what happens.
void RTIMU::calibrateAverageCompass()
{
// calibrate if required
if (!m_calibrationMode && m_calibrationValid) {
m_compass.setX((m_compass.x() - m_compassCalOffset[0]) * m_compassCalScale[0]);
m_compass.setY((m_compass.y() - m_compassCalOffset[1]) * m_compassCalScale[1]);
m_compass.setZ((m_compass.z() - m_compassCalOffset[2]) * m_compassCalScale[2]);
}
Regards Duckman
Well in the end l may not be as poor at coding as l thought and have worked it out myself.
Attached is RTIMu.cpp with changes to enable the full monty (soft/hard iron correction) calibration instead of min/max Magnetometer.
If using, you will of course require your own IMU correction data.
All IMU's supported by RTIMULib Arduino can be corrected via this .cpp change.
Still to test how much it improves the yaw tilt error, but in testing on my boat, it looks like it is less than the +- 5 degrees which the Chinese MPU9255 showed.
Regards
Kevin
RTIMU.cpp (11.6 KB)
The code you posted does not contain the full 3x3 matrix magnetometer correction. Perhaps you posted the wrong file.
Jim, yes wrong 1, attached corrected .cpp, but l may have been a bit quick to say it is working correctly.
Heading not working as hoped, maybe it's my Magneto implementation or the correction data, hopefully you can see the prob.
The heading looked good before because it was running on the default .cpp
RTIMU.cpp (12.8 KB)
Beware that RTIMUlib in some cases changes the sensor axes (changes signs and/or swaps them).
So, if you calibrated the IMU using the raw data of the magnetometer, that calibration matrix has to be swapped around to match whatever RTIMUlib does.
Or, apply the calibration matrix BEFORE any data manipulations are undertaken by either the sensor-specific code or RTIMU.cpp
Jim, l thought l had put the corrections prior to manipulation.
In some cases there are axis modifications in the specific sensor driver, which precede the code you added.
For example, the following kind of crap, which I have yet to understand, taken from either the MPU9250 and the GD20/LSM303DLHC drivers.
In particular the sign of X acceleration is flipped, which makes the acclerometer coordinate system left handed! This is wrong, but it works, so I assume that the mistake gets corrected by a sign swap elsewhere.
// sort out gyro axes
m_gyro.setY(-m_gyro.y());
m_gyro.setZ(-m_gyro.z());
// sort out accel data;
m_accel.setX(-m_accel.x());
Thanks Jim, l did see that sign change on the gyros, but l'm not changing that.
Re scatter plots, which type of software (free hopefully) do you use, then l can maybe see something in that, as excel is just stupid to use, with it axis scale changes.
Scilab will do 3D scatter plots and it is free.
My recommendation is to have RTIMU.cpp output the final, modified sensor data, and plot those to see if all the axis manipulations and offset/scale operations are correct and make sense.
In my case they don't completely, because of the X-axis issue mentioned above.
Thanks for the info, so l take it you have previously tried to do the full monty calibration without success.
Regards
Kevin
I haven't implemented the off axis correction in RTIMUlib, but probably will for a future project.
For my application using RTIMUlib the built in min/max correction worked very well, as the off axis correction was small for that particular sensor in that environment.
I did implement the off axis correction for a simple tilt compensated compass starting with the Pololu code, described here: https://forum.pololu.com/t/correcting-the-balboa-magnetometer/14315.
In that case it made the difference between "usable" and "not usable".
Jim, l also did the full monty on the Pololu V3 which only lowered the yaw, max tilt induced error from 13 degrees to 11, so it's most probable not worth the effort.
In my case with the V3, you had said it's a very poor performing Mag.
https://forum.pololu.com/t/how-to-use-cal-data-from-magcal-magneto-into-ahrs/8685
Again thanks for your advise.
PS, Scilab is another big learning curve for me, Excel is a easy scatter plotter, albeit not very good.
Kevin 1961
Went back and reconfirmed correction data via rerunning Magneto on corrected data to see if the bias was zeroed and matrix was 0's and 1's.
For some reason my intial correction bias was wrong. Re did the correction data and put into .cpp
I can confirm my implementation of Magneto, full monty correction works and l have a stable yaw with limited tilt error (on a boat so can only do a rough tilt test as boat is moving a bit).
Still to do a complete test and will post results with a comparison of min/max cal V full monty.
As promised, attached 2010 excel spreadsheet.
sheet 1, Full monty calibration nearly halved compass rose linearity error from max 10 degrees to 6.
Tilt error was also slightly improved, so the hard work has been done so you would be mad not to use it.
Sheet 2, shows data taken over 5 minutes with IMU static on 0 degree heading.
Yaw approx <+-0.5 degree drift from 0 . Gyro Z axis drift <0.1 dps drift from 0
Regards Kevin1961
MPU9255 min-max, full monty calibration results.zip (447 KB)