I am asking a question to the community of experts.
How do I properly scale the gyroscope and accelerometer axes to use with the example code?
I have a board with sensors lsm303dlh and l3gd20.
I also have the MPU9250, BMI160 and LSM6DS3 sensors, but I will return to them after I manage to run the sensors described in the question header with the example code.
To work with these sensors I use libraries
#include <Adafruit_LSM303.h>
#include <L3G.h>
I didn’t like the
#include <LSM303.h>
library, but I’ll tell you about that a little later, why I don’t use it to work with the accelerometer and magnetometer.
It was not easy to determine the model of my sensors, but from indirect evidence I realized that a gyroscope is l3gd20, and an accelerometer with magnetometer is lsm303dlh.
I have previously used these sensors in other implementations of the Madgwick and Mahony filters and obtained the raw values as follows:
L3G gyro;
Adafruit_LSM303 accel_mag;
// gyro
gyro.init();
gyro.enableDefault();
gyro.read();
gx = gyro.g.x;
gy = gyro.g.y;
gz = gyro.g.z;
// accel + mag
accel_mag.begin();
accel_mag.read();
ax = accel_mag.accelData.x;
ay = accel_mag.accelData.y;
az = accel_mag.accelData.z;
mx = accel_mag.magData.x;
my = accel_mag.magData.y;
mz = accel_mag.magData.z;
The code is not complete and is not code at all, but just to understand how I get the data.
Initially I took the code from the example, but it did not compile out of the box and it was decided to rewrite the code for other sensors and also get rid of so many *ino files.
And so, I get raw data from the gyroscope for each X, Y, Z axis.
uint8_t xlg = Wire.read();
uint8_t xhg = Wire.read();
uint8_t ylg = Wire.read();
uint8_t yhg = Wire.read();
uint8_t zlg = Wire.read();
uint8_t zhg = Wire.read();
// combine high and low bytes
g.x = (int16_t)(xhg << 8 | xlg);
g.y = (int16_t)(yhg << 8 | ylg);
g.z = (int16_t)(zhg << 8 | zlg);
Using the library, I can change what angular velocity the sensor will measure in terms of the entire scale, which is +32752 and -32736, although according to the library datasheet it should be 32768.
And because the ADC is 16-bit, it’s actually 65536 - but I didn’t delve into this issue for the time being, because with a different code the orientation of the corners was determined and that suited me.
I entered a definition to assign the desired rotation speed, for example:
#define GYRO_RANGE 250.0f
Received the raw value of angular velocity
gx = gyro.g.x;
And he simply performed mathematical calculations and received a scaled value of angular velocity.
gxScaled = (gx - gyroOffSets[X_]) * GYRO_RANGE / 32768.0;
If I register the value of the angular velocity, for example
// 0x00 = 0b00000000
// FS = 00 (+/- 250 dps full scale)
//writeReg(CTRL_REG4, 0x00);
// FS = 01 (+/- 500 dps full scale)
writeReg(CTRL_REG4, 0x01);
// FS = 10 (+/- 2000 dps full scale)
// writeReg(CTRL_REG4, 0x10);
500 or 2000 DPS also changed
#define GYRO_RANGE 500.0f
or
#define GYRO_RANGE 2000.0f
I received approximately the same acceleration values from the accelerometer.
Also introduced define
#define ACCEL_RANGE 2.0f
Also changed registers in the library
write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x27);
// Y - OFF 0b100101
// write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x25);
// 0b100111
// LSM303_REGISTER_ACCEL_CTRL_REG1_A - 0x20
// 0100
// ODR3 ODR2 ODR1 ODR0 Power mode and ODR selection
// 0 0 0 0 Power-down mode
// 0 0 0 1 Normal / low-power mode (1 Hz) - 0x17
// 0 0 1 0 Normal / low-power mode (10 Hz) - 0x27
// 0 0 1 1 Normal / low-power mode (25 Hz) - 0x37
// 0 1 0 0 Normal / low-power mode (50 Hz) - 0x47
// 0 1 0 1 Normal / low-power mode (100 Hz) - 0x57
// 0 1 1 0 Normal / low-power mode (200 Hz)
// 0 1 1 1 Normal / low-power mode (400 Hz)
// 1 0 0 0 Low-power mode (1.620 kHz)
// 1 0 0 1 Normal (1.344 kHz) / low-power mode (5.376 kHz)
// 0111
// LPen
// Low-power mode enable. Default value: 0
// (0: normal mode, 1: low-power mode)
// Zen
// Z-axis enable. Default value: 1
// (0: Z-axis disabled, 1: Z-axis enabled)
// Yen
// Y-axis enable. Default value: 1
// (0: Y-axis disabled, 1: Y-axis enabled)
//Xen
// X-axis enable. Default value: 1
// (0: X-axis disabled, 1: X-axis enabled)
And then I scaled the resulting raw acceleration value.
In the example code in a file* I get gyroscope, accelerometer and magnetometer data.
And here the incomprehensible moments begin.
In the source code of the example, defines are assigned
// #define Gyro_Scaled_X(x) (x*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
which did not want to be compiled in the Arduino environment.
I had to replace them with simple defines, without math
#define DEG_TO_RAD (M_PI / 180.0)
#define RAD_TO_DEG (180.0 / M_PI)
And now to get the values of the vector of the gyro and accelerometer I write like this
Gyro_Vector[0]=gyro_x * (GYRO_RANGE / 32768.0) * DEG_TO_RAD; //gyro x roll
The second problem is the strange scaling of the accelerometer axes.
For some reason, the authors of the example coarsened the values of a 16-bit ADC by bit shifting.
Introduced a strange variable GRAVITY with an even stranger value of 256!
Why do such strange things in a place where there is already no physical connection to the entity, except for the ADC bit capacity.
Maybe I don’t understand something?
In general, I assembled the code and it started working.
If I bet
#define OUTPUTMODE 1
There is practically no drift, but over time, nevertheless, the angles move slightly away from the initial values.
After start
32
42
-46
-534
-124
14361
Orientation: 0.00 -0.00 -360.00
Orientation: 0.01 -0.01 -0.00
Orientation: 0.01 -0.01 -0.00
After moving
I tried calibrating accelerometers with gyroscopes, as well as a strange magnetometer calibration based on min and max values.
By the way, what kind of strange calibration is this?
Is this calibration of soft iron and that’s all?
Result with this code
#include <Wire.h>
#include <LSM303.h>
LSM303 compass;
LSM303::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32768, -32768, -32768};
char report[80];
void setup() {
Serial.begin(115200);
Wire.begin();
compass.init();
compass.enableDefault();
}
void loop() {
compass.read();
running_min.x = min(running_min.x, compass.m.x);
running_min.y = min(running_min.y, compass.m.y);
running_min.z = min(running_min.z, compass.m.z);
running_max.x = max(running_max.x, compass.m.x);
running_max.y = max(running_max.y, compass.m.y);
running_max.z = max(running_max.z, compass.m.z);
snprintf(report, sizeof(report), "min: {%+6d, %+6d, %+6d} max: {%+6d, %+6d, %+6d}",
running_min.x, running_min.y, running_min.z,
running_max.x, running_max.y, running_max.z);
Serial.println(report);
delay(100);
}
min: { +198, -377, -270} max: { +206, -375, -77}
min: { +198, -377, -270} max: { +206, -375, -77}
min: { +198, -377, -270} max: { +206, -374, -77}
min: { +198, -378, -270} max: { +207, -374, -77}
min: { +198, -378, -270} max: { +207, -374, -77}
min: { +198, -378, -270} max: { +207, -374, -77}
The result of the same code when I don’t bit shift the results of the magnetometer values.
min: {-14592, -32001, +26878} max: {-11520, +29951, +29438}
min: {-14592, -32001, +26878} max: {-11520, +29951, +29438}
min: {-14592, -32001, +26878} max: {-11520, +29951, +29438}
min: {-14592, -32001, +26878} max: {-11520, +29951, +29438}
min: {-14592, -32001, +26878} max: {-11520, +29951, +29438}
min: {-14592, -32001, +26878} max: {-11520, +29951, +29694}
If you rotate the board, the values will change.
min: {-32258, -32513, -32514} max: {+32767, +32767, +32766}
min: {-32258, -32513, -32514} max: {+32767, +32767, +32766}
min: {-32258, -32513, -32514} max: {+32767, +32767, +32766}
min: {-32258, -32513, -32514} max: {+32767, +32767, +32766}
min: {-32258, -32513, -32514} max: {+32767, +32767, +32766}
For me, it doesn’t make any difference in the work… but this is my purely personal opinion.
This moment is of greater interest - for some reason, when setting the gyroscope to 250 degrees per second, and turning the board with sensors by - 90 degrees, the angle calculated by the example code turns by -80, then I return the board to the horizon and the angle shows ± 0 degrees, then I change the board’s roll angle to +90 degrees and the example code calculates +100 degrees for me.
At the same time, the calibration of the axes of gyroscopes and accelerometers is the same.
Then there is another strange thing - when I change the roll angle, the pitch angle does not change.
But if I start changing the pitch angle, for example from 0 to -60, then after -60 the roll angle begins to change and becomes -30 degrees.
And vice versa, if I change the pitch angle from 0 to +60, then after +60 degrees the roll angle will begin to change and become +30 degrees.
Also, when the pitch angle changes, for some reason the yaw angle also changes.
Well, the worst thing about all this is that if, after turning on the board and starting the program, you take the board from a horizontal position and smoothly begin to change the roll and pitch angles, then after returning to the horizontal position, the angles will be ±0 degrees.
If I change the roll or pitch angle sharply, that is, I quickly tilt the board to the left and then sharply to the right, then when I leave the board alone after such roll or pitch jerks, the angle will no longer be 0 and it never returns to zeros.
It feels like the example code doesn’t use the I values of the accelerometers and magnetometers to correct for drift and serve as reference vectors to return the board to zero positions.
I have attached the code of the redesigned example, although the original code of the example is quite old, but I hope someone can help me figure out the problems and tell me what I’m doing wrong.
I’m weak in python and therefore use Processing to visually display the position of the board with sensors.
minimu-9-ahrs-arduino.rar (22.0 KB)
The archive contains example code and a GUI with a program for processing 4.2
Why didn’t you use the #include <LSM303.h> library?
Because when using it, the magnetometer values are only three-digit, because the library defines my sensor as
device_DLHC
// Reads the 3 magnetometer channels and stores them in vector m
void LSM303::readMag(void)
{
Wire.beginTransmission(mag_address);
// If LSM303D, assert MSB to enable subaddress updating
// OUT_X_L_M comes first on D, OUT_X_H_M on others
Wire.write((_device == device_D) ? translated_regs[-OUT_X_L_M] | (1 << 7) : translated_regs[-OUT_X_H_M]);
last_status = Wire.endTransmission();
Wire.requestFrom(mag_address, (byte)6);
unsigned int millis_start = millis();
while (Wire.available() < 6) {
if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)
{
did_timeout = true;
return;
}
}
byte xlm, xhm, ylm, yhm, zlm, zhm;
if (_device == device_D)
{
// D: X_L, X_H, Y_L, Y_H, Z_L, Z_H
xlm = Wire.read();
xhm = Wire.read();
ylm = Wire.read();
yhm = Wire.read();
zlm = Wire.read();
zhm = Wire.read();
}
else
{
// DLHC, DLM, DLH: X_H, X_L...
xhm = Wire.read();
xlm = Wire.read();
if (_device == device_DLH)
{
// DLH: ...Y_H, Y_L, Z_H, Z_L
yhm = Wire.read();
ylm = Wire.read();
zhm = Wire.read();
zlm = Wire.read();
}
else
{
// DLM, DLHC: ...Z_H, Z_L, Y_H, Y_L
zhm = Wire.read();
zlm = Wire.read();
yhm = Wire.read();
ylm = Wire.read();
}
}
// combine high and low bytes
m.x = (int16_t)(xhm << 8 | xlm);
m.y = (int16_t)(yhm << 8 | ylm);
m.z = (int16_t)(zhm << 8 | zlm);
}
While the library
#include <Adafruit_LSM303.h>
Gives the full value of raw magnetometer data
65320,65204,65453
