我试图使用Madgwick过滤算法在arduino mega board中使用LSM6DS3找到物体的倾斜角度。但是加速度计值突然下降到零读数，因此滤波器会产生错误输出。用这篇文章附上我的代码。
/* Sensor SDI/SDA - Arduino Digital Pin 51 or MOSI (Master Out Slave In)
* Sensor SCK/SCL - Arduino Digital Pin 52 or SCK (Serial Clock)
* Sensor SDO/ADR - Arduino Digital Pin 50 or MISO (Master In Slave Out)
* Sensor CS - Arduino Digital Pin 10 or 53 (Slave Select or Chip Select)
*/
#include <SPI.h> // Include Arduino SPI library
#include <math.h>
#define OUT_X_L_G 0x22 // Register addresses from sensor datasheet.
#define OUT_X_H_G 0x23 // Only the registers that are used
#define OUT_Y_L_G 0x24 // in this program are defined here.
#define OUT_Y_H_G 0x25
#define OUT_Z_L_G 0x26
#define OUT_Z_H_G 0x27
#define CTRL1_A 0x10
#define CTRL2_G 0x11
#define CTRL3_C 0x12
#define CTRL4_C 0x13
#define CTRL9_A 0x18
#define CTRL10_C 0x19
#define OUT_X_L_A 0x28
#define OUT_X_H_A 0x29
#define OUT_Y_L_A 0x2A
#define OUT_Y_H_A 0x2B
#define OUT_Z_L_A 0x2C
#define OUT_Z_H_A 0x2D
#define WHO_AM_I 0x0F
#define CTRL7_G 0x16 // setting hpf for gyro
#define STATUS_REG 0x1E
#define sampleFreq 512.0f // sample frequency in Hz 512
#define betaDef 0.1f // 2 * proportional gain 0.1
#define M_PI 3.14
int8_t readData = 0x80;
int8_t writeData = 0x00;
int8_t k ;
int16_t Ax, Ay, Az; // 16-bit variables to hold raw data from sensor
int16_t Gx, Gy, Gz;
float Aax, Aay, Aaz;
float Ggx, Ggy, Ggz;
const int CS = 53; // Chip Select pin for SPI
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
volatile float beta = betaDef;
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float invSqrt(float x);
float roll,pitch;
void setup()
{
Serial.begin(57600);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV4);
SPI.setBitOrder(MSBFIRST);
SPI.setDataMode(SPI_MODE3);
pinMode(CS, OUTPUT);
writeReg(CTRL3_C, 0x40); // BDU UPDATE
delay(100);
writeReg(CTRL9_A, 0x38); // Enable x,y,z axes of ACCEL
writeReg(CTRL10_C, 0x38); //Enable x,y,z of GYRO
writeReg(CTRL1_A, 0x7C); // ODR=833, scale = 8g
writeReg(CTRL2_G, 0x7C); // scale= =2000dps, ODR = 833
writeReg(CTRL4_C, 0x04); // SPI only, I2C disabled
writeReg(CTRL7_G, 0x70); // setting high pass filter for gyro
delay(100);
}
void loop()
{
Gx = (int16_t) readReg(OUT_X_H_G) <<8 | readReg(OUT_X_L_G); // typecast as
Gy = (int16_t) readReg(OUT_Y_H_G) <<8 | readReg(OUT_Y_L_G); // 16-bit
Gz = (int16_t) readReg(OUT_Z_H_G) <<8 | readReg(OUT_Z_L_G);
Ax = (int16_t) readReg(OUT_X_H_A) <<8 | readReg(OUT_X_L_A);
Ay = (int16_t) readReg(OUT_Y_H_A) <<8 | readReg(OUT_Y_L_A);
Az = (int16_t) readReg(OUT_Z_H_A) <<8 | readReg(OUT_Z_L_A);
Aax = (Ax*0.244)/1000;
Aay = (Ay*0.244)/1000;
Aaz = (Az*0.244)/1000;
Ggx = (Gx*0.07)*(M_PI/180);
Ggy = (Gy*0.07)*(M_PI/180);
Ggz = (Gz*0.07)*(M_PI/180);
MadgwickAHRSupdateIMU( Ggx, Ggy, Ggz, Aax,Aay,Aaz);
roll=atan2(2*q2*q3-2*q0*q1,(2*q0*q0)+(2*q3*q3)-1)*180/M_PI;
pitch=-1*asin(2*q1*q3+2*q0*q2)*180/M_PI;
Serial.println(roll);
Serial.println(pitch);
}
int8_t readReg(int8_t address) {
int8_t buffer = 0;
digitalWrite(CS, LOW);
SPI.transfer(readData | address);
buffer = SPI.transfer(writeData);
digitalWrite(CS, HIGH);
return(buffer);
}
void writeReg(int8_t address, int8_t val)
{
digitalWrite(CS, LOW);
SPI.transfer(writeData | address);
SPI.transfer(val);
digitalWrite(CS, HIGH);
}
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
{
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}