void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
float scale, deltaGyroAngle[3];
static uint16_t previousT;
uint16_t currentT = micros();
///Вычисляем шаг. (считаем время от прошлого просчета)
scale = (currentT - previousT) * GYRO_SCALE;
previousT = currentT;
// Initialization
///По каждой из трех осей.
for (axis = 0; axis < 3; axis++) {
///Получаем дельты по углам поворотов.
deltaGyroAngle[axis] = gyroADC[axis] * scale;
/// Следующее выражение приводится к accSmooth[i]=1/16*accADC[i]+15/16*accSmooth[i-1]
/// Это фильтр нижних частот, причем Т=16*h, где h - шаг дискретизации в секундах.
/// Эксперимент показывает как минимум совпадение порядков (h=50мс. Выход на полку примерно 2с.).
accLPF32[axis] -= accLPF32[axis]>>ACC_LPF_FACTOR;
accLPF32[axis] += accADC[axis];
accSmooth[axis] = accLPF32[axis]>>ACC_LPF_FACTOR;
///Считаем модуль расчитанного вектора.
accMag += (int32_t)accSmooth[axis]*accSmooth[axis] ;
}
/// Масштабируем модуль (g=100).
accMag = accMag*100/((int32_t)acc_1G*acc_1G);
///Используя уравнение Пуассона, правим платформенные вектора G и M.
rotateV(&EstG.V,deltaGyroAngle);
rotateV(&EstM.V,deltaGyroAngle);
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if (correct==1){
if ((72 < accMag) && (accMag < 133)){
// digitalWrite(30,1);
/// Коррекция платформенного вектора G по показаниям акселлерометров. GYR_CMPF_FACTOR отвечает за чувствительность.
for (axis = 0; axis < 3; axis++) {
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
/// Коррекция платформенного вектора M по показаниям магнетометра. GYR_CMPFM_FACTOR отвечает за чувствительность.
for (axis = 0; axis < 3; axis++) {
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
}
}
}
for (axis=0;axis<3;axis++)
{
EstG32.A[axis] = EstG.A[axis];
EstM32.A[axis] = EstM.A[axis];
}
}
void GetEstimatedAttitude(imu_t* imu_ptr)
{
uint8_t axis;
int32_t accMag = 0;
float scale, deltaGyroAngle[3];
uint8_t validAcc;
static uint16_t previousT;
uint16_t currentT = micros();
scale = (currentT - previousT) * GYRO_SCALE; // GYRO_SCALE unit: radian/microsecond
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++)
{
deltaGyroAngle[axis] = imu_ptr->gyroADC[axis] * scale; // radian
accLPF32[axis] -= accLPF32[axis]>>ACC_LPF_FACTOR;
accLPF32[axis] += imu_ptr->accADC[axis];
imu_ptr->accSmooth[axis] = accLPF32[axis]>>ACC_LPF_FACTOR;
accMag += (int32_t)imu_ptr->accSmooth[axis]*imu_ptr->accSmooth[axis] ;
}
rotateV(&EstG.V,deltaGyroAngle);
rotateV(&EstM.V,deltaGyroAngle);
accMag = accMag*100/((int32_t)ACC_1G*ACC_1G);
validAcc = 72 < (uint16_t)accMag && (uint16_t)accMag < 133;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
for (axis = 0; axis < 3; axis++)
{
if ( validAcc )
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu_ptr->accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + imu_ptr->magADC[axis]) * INV_GYR_CMPFM_FACTOR;
EstM32.A[axis] = EstM.A[axis];
}
// Attitude of the estimated vector
int32_t sqGX_sqGZ = sq(EstG32.V.X) + sq(EstG32.V.Z);
invG = InvSqrt(sqGX_sqGZ + sq(EstG32.V.Y));
att.angle[ROLL] = _atan2(EstG32.V.X , EstG32.V.Z);
att.angle[PITCH] = _atan2(EstG32.V.Y , InvSqrt(sqGX_sqGZ)*sqGX_sqGZ);
att.heading = _atan2(
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
(EstM.V.Y * sqGX_sqGZ - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * EstG.V.Y)*invG );
att.heading /= 10;
}
// rotate acc into Earth frame and calculate acceleration in it
void imuCalculateAcceleration(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
float dT;
fp_angles_t rpy;
t_fp_vector accel_ned;
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
// the accel values have to be rotated into the earth frame
rpy.angles.roll = -(float)anglerad[AI_ROLL];
rpy.angles.pitch = -(float)anglerad[AI_PITCH];
rpy.angles.yaw = -(float)heading * RAD;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2];
rotateV(&accel_ned.V, &rpy);
if (imuRuntimeConfig->acc_unarmedcal == 1) {
if (!ARMING_FLAG(ARMED)) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
accSumCount++;
}
glm::mat4 OverviewCamera::getViewMatrix()
{
// The two rotation matrices.
glm::mat4x4 rotateH(1.0f);
glm::mat4x4 rotateV(1.0f);
// Rotate around the Y-axis.
rotateH = glm::rotate(rotateH, m_hrotate, glm::vec3(0.0f, 1.0f, 0.0f));
// Rotate around the Z-axis.
rotateV = glm::rotate(rotateV, m_vrotate, glm::vec3(1.0f, 0.0f, 0.0f));
// Position the camera "behind" the origin, multiplied with the two rotations.
glm::vec4 newEye = rotateH * rotateV * glm::vec4(0.0f, 0.0f, -m_radius, 1.0f);
// Set the eye at the new position.
glm::vec3 eyePos = glm::vec3(newEye.x, newEye.y, newEye.z);
m_viewMatrix = lookAt(eyePos, m_atPos, m_upVector);
return m_viewMatrix;
}
// rotate acc into Earth frame and calculate acceleration in it
void acc_calc(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth;
float rpy[3];
t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame
rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float)heading * RAD;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2];
rotateV(&accel_ned.V, rpy);
if (cfg.acc_unarmedcal == 1) {
if (!f.ARMED) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
accSum[Z] += applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
accSumCount++;
}
static void getEstimatedAttitude(void)
{
int32_t axis;
int32_t accMag = 0;
static t_fp_vector EstM;
static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
uint32_t deltaT;
float scale, deltaGyroAngle[3];
deltaT = currentT - previousT;
scale = deltaT * gyro.scale;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale;
if (cfg.acc_lpf_factor > 0) {
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
accSmooth[axis] = accLPF[axis];
} else {
accSmooth[axis] = accADC[axis];
}
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
}
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle);
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
// Attitude of the estimated vector
anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, deltaGyroAngle);
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
heading = calculateHeading(&EstM);
} else {
rotateV(&EstN.V, deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V);
heading = calculateHeading(&EstN);
}
acc_calc(deltaT); // rotate acc vector into earth frame
if (cfg.throttle_correction_value) {
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
throttleAngleCorrection = 0;
} else {
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;
throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ;
}
}
}
void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
#ifndef MAYHONY
static t_fp_vector EstG;
#if MAG
static t_fp_vector EstM;
#endif
#endif
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
#if defined(ACC_LPF_FACTOR)
static float accLPF[3];
#endif
static uint32_t previousT;
uint32_t currentT = micros();
#ifndef MAYHONY
float scale;
float deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
#else
float GyroRate[3];
samplePeriod = (currentT-previousT)/1000000.0f;
#endif
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
#ifndef MAYHONY
deltaGyroAngle[axis] = gyroADC[axis] * scale;
#endif
#if defined(ACC_LPF_FACTOR)
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
accSmooth[axis] = accLPF[axis];
#define ACC_VALUE accSmooth[axis]
#else
accSmooth[axis] = accADC[axis];
#define ACC_VALUE accADC[axis]
#endif
accMag += (ACC_VALUE * 10 / (int16_t)acc_1G) * (ACC_VALUE * 10 / (int16_t)acc_1G);
accMag += (int32_t)ACC_VALUE*ACC_VALUE ;
#if MAG
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
#endif
}
accMag = accMag*100/((int32_t)acc_1G*acc_1G);
#if defined(MAYHONY)
if(calibratingA == 0 && calibratingG == 0)
{
for(axis = 0; axis < 3; axis++)
{
GyroRate[axis] = gyroAHRS[axis]*GYRO_SCALE_MAYHONY;
}
#ifdef MAG
MayhonyAHRSupdate(GyroRate[Xaxis], GyroRate[Yaxis], GyroRate[Zaxis], (float)accSmooth[Xaxis], (float)accSmooth[Yaxis], (float)accSmooth[Zaxis], (float)magADC[Xaxis], (float)magADC[Yaxis], (float)magADC[Zaxis]);
#else
MayhonyAHRSupdateIMU(GyroRate[Xaxis], GyroRate[Yaxis], GyroRate[Zaxis], (float)accSmooth[Xaxis], (float)accSmooth[Yaxis], (float)accSmooth[Zaxis]);
#endif
}
#else
rotateV(&EstG.V,deltaGyroAngle);
#if MAG
rotateV(&EstM.V,deltaGyroAngle);
#endif
if ( fabs(accSmooth[ROLL])<acc_25deg && fabs(accSmooth[PITCH])<acc_25deg && accSmooth[YAW]>0) {
f.SMALL_ANGLES_25 = 1;
} else {
f.SMALL_ANGLES_25 = 0;
}
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if ( ( 36 < accMag && accMag < 196 ) || f.SMALL_ANGLES_25 )
for (axis = 0; axis < 3; axis++) {
int16_t acc = ACC_VALUE;
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;
}
#if MAG
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
#endif
// Attitude of the estimated vector
angle[ROLL] = _atan2(EstG.V.X , EstG.V.Z) ;
angle[PITCH] = _atan2(EstG.V.Y , EstG.V.Z) ;
#if MAG
// Attitude of the cross product vector GxM
heading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
if ( heading > 180) heading = heading - 360;
else if (heading < -180) heading = heading + 360;
#endif
#endif//multiwii fusion
}
void IMU::update(uint32_t currentTime, bool armed, uint16_t & calibratingA, uint16_t & calibratingG)
{
static float accelLPF[3];
static int32_t accelZoffset;
static float accz_smooth;
static int16_t accelZero[3];
static int32_t a[3];
static int16_t accelSmooth[3];
static float EstG[3];
static float EstN[3] = { 1.0f, 0.0f, 0.0f };
static int16_t gyroZero[3];
static uint32_t previousTime;
int32_t accMag = 0;
float dT = 0;
float rpy[3];
float accel_ned[3];
float deltaGyroAngle[3];
uint32_t deltaT = currentTime - previousTime;
float scale = deltaT * this->gyroScale;
int16_t accelADC[3];
float anglerad[3];
previousTime = currentTime;
this->_board->imuRead(accelADC, this->gyroADC);
if (calibratingA > 0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == this->calibratingAccCycles)
a[axis] = 0;
// Sum up this->calibratingAccCycles readings
a[axis] += accelADC[axis];
// Clear global variables for next reading
accelADC[axis] = 0;
accelZero[axis] = 0;
}
// Calculate average, shift Z down by acc1G
if (calibratingA == 1) {
accelZero[ROLL] = (a[ROLL] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles;
accelZero[PITCH] = (a[PITCH] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles;
accelZero[YAW] = (a[YAW] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles - this->acc1G;
}
calibratingA--;
}
accelADC[ROLL] -= accelZero[ROLL];
accelADC[PITCH] -= accelZero[PITCH];
accelADC[YAW] -= accelZero[YAW];
// range: +/- 8192; +/- 2000 deg/sec
static int32_t g[3];
static stdev_t var[3];
if (calibratingG > 0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == this->calibratingGyroCycles) {
g[axis] = 0;
devClear(&var[axis]);
}
// Sum up 1000 readings
g[axis] += this->gyroADC[axis];
devPush(&var[axis], this->gyroADC[axis]);
// Clear global variables for next reading
this->gyroADC[axis] = 0;
gyroZero[axis] = 0;
if (calibratingG == 1) {
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model
if (CONFIG_MORON_THRESHOLD && dev > CONFIG_MORON_THRESHOLD) {
calibratingG = this->calibratingGyroCycles;
devClear(&var[0]);
devClear(&var[1]);
devClear(&var[2]);
g[0] = g[1] = g[2] = 0;
continue;
}
gyroZero[axis] = (g[axis] + (this->calibratingGyroCycles / 2)) / this->calibratingGyroCycles;
}
}
calibratingG--;
}
for (uint8_t axis = 0; axis < 3; axis++)
this->gyroADC[axis] -= gyroZero[axis];
// Initialization
for (uint8_t axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = this->gyroADC[axis] * scale;
if (CONFIG_ACC_LPF_FACTOR > 0) {
accelLPF[axis] = accelLPF[axis] * (1.0f - (1.0f / CONFIG_ACC_LPF_FACTOR)) + accelADC[axis] *
(1.0f / CONFIG_ACC_LPF_FACTOR);
accelSmooth[axis] = (int16_t)accelLPF[axis];
} else {
accelSmooth[axis] = accelADC[axis];
}
accMag += (int32_t)accelSmooth[axis] * accelSmooth[axis];
}
accMag = accMag * 100 / ((int32_t)this->acc1G * this->acc1G);
rotateV(EstG, deltaGyroAngle);
// Apply complementary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit
// range => we neutralize the effect of accelerometers in the angle
// estimation. To do that, we just skip filter, as EstV already rotated by Gyro
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
for (uint8_t axis = 0; axis < 3; axis++)
EstG[axis] = (EstG[axis] * (float)CONFIG_GYRO_CMPF_FACTOR + accelSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
// Attitude of the estimated vector
anglerad[ROLL] = atan2f(EstG[Y], EstG[Z]);
anglerad[PITCH] = atan2f(-EstG[X], sqrtf(EstG[Y] * EstG[Y] + EstG[Z] * EstG[Z]));
rotateV(EstN, deltaGyroAngle);
normalizeV(EstN, EstN);
// Calculate heading
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = EstN[X] * cosinePitch + EstN[Y] * sineRoll * sinePitch + EstN[Z] * sinePitch * cosineRoll;
float Yh = EstN[Y] * cosineRoll - EstN[Z] * sineRoll;
anglerad[YAW] = atan2f(Yh, Xh);
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
// the accel values have to be rotated into the earth frame
rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float)anglerad[YAW];
accel_ned[X] = accelSmooth[0];
accel_ned[Y] = accelSmooth[1];
accel_ned[Z] = accelSmooth[2];
rotateV(accel_ned, rpy);
if (!armed) {
accelZoffset -= accelZoffset / 64;
accelZoffset += (int32_t)accel_ned[Z];
}
accel_ned[Z] -= accelZoffset / 64; // compensate for gravitation on z-axis
accz_smooth = accz_smooth + (dT / (fcAcc + dT)) * (accel_ned[Z] - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence and
// sum up Values for later integration to get velocity and distance
this->accelSum[X] += deadbandFilter(lrintf(accel_ned[X]), CONFIG_ACCXY_DEADBAND);
this->accelSum[Y] += deadbandFilter(lrintf(accel_ned[Y]), CONFIG_ACCXY_DEADBAND);
this->accelSum[Z] += deadbandFilter(lrintf(accz_smooth), CONFIG_ACCZ_DEADBAND);
this->accelTimeSum += deltaT;
this->accelSumCount++;
// Convert angles from radians to tenths of a degrees
this->angle[ROLL] = (int16_t)lrintf(anglerad[ROLL] * (1800.0f / M_PI));
this->angle[PITCH] = (int16_t)lrintf(anglerad[PITCH] * (1800.0f / M_PI));
this->angle[YAW] = (int16_t)(lrintf(anglerad[YAW] * 1800.0f / M_PI + CONFIG_MAGNETIC_DECLINATION) / 10.0f);
// Convert heading from [-180,+180] to [0,360]
if (this->angle[YAW] < 0)
this->angle[YAW] += 360;
}
int main() {
LocalI2CBus bus(1);
ADXL345 acc(&bus);
L3G4200D gyro(&bus);
acc.setEnabled(false);
acc.setDataRate(ADXL345::DATARATE_100_HZ);
acc.setRange(ADXL345::RANGE_16G);
acc.setFullResolutionEnabled(true);
acc.setEnabled(true);
gyro.setEnabled(false);
gyro.setOutputDataRate(L3G4200D::DATARATE_800_HZ);
//gyro.setBlockDataUpdateEnabled(false);
gyro.setEnabled(true);
timespec ts, ts2;
clock_gettime(CLOCK_REALTIME, &ts);
for (int i=0; i<1000; i++)
clock_gettime(CLOCK_REALTIME, &ts2);
int gettime_delay_ns = ((ts2.tv_sec-ts.tv_sec)*1000000000 + ts2.tv_nsec-ts.tv_nsec) / 1000;
printf("gettime delay: %d\n", gettime_delay_ns);
usleep(1000000);
double accv[32][3];
double rotv[100][3];
double rota[3], rot[3];
for (int i=0; i<10000; i++) {
//acc.getLinearAcceleration(accv[i][0], accv[i][1], accv[i][2]);
//gyro.getAngularVelocity(rotv[i][0], rotv[i][1], rotv[i][2]);
gyro.getAngularVelocity(rot[0], rot[1], rot[2]);
rota[0] += rot[0];
rota[1] += rot[1];
rota[2] += rot[2];
clock_gettime(CLOCK_REALTIME, &ts2);
//usleep(10000);
}
rota[0] /= 10000;
rota[1] /= 10000;
rota[2] /= 10000;
/*
for (int i=0; i<100; i++) {
//printf("\t%.2lf\t%.2lf\t%.2lf\n", accv[i][0], accv[i][1], accv[i][2]);
//printf("\t%.2lf\t%.2lf\t%.2lf\n", rotv[i][0], rotv[i][1], rotv[i][2]);
}
printf("\n\n");
printf("\t%.2lf\t%.2lf\t%.2lf\n", rota[0]/32, rota[1]/32, rota[2]/32);
printf("\n\n");
usleep(10000000);
*/
acc.calibrate(100);
int8 ox, oy, oz;
acc.getOffset(ox, oy, oz);
printf("%d\t%d\t%d\n", ox, oy, oz);
usleep(1000000);
int16 x, y, z;
double acc_x = 0, acc_y = 0, acc_z = 0, mag;
double lacc_x, lacc_y, lacc_z;
double rot_x, rot_y, rot_z;
double ang_x = 0, ang_y = 0, ang_z = 0, interval, dang[3], gvec[3] = {0, 0, 0};
clock_gettime(CLOCK_REALTIME, &ts);
while (true) {
acc.getLinearAcceleration(lacc_x, lacc_y, lacc_z);
gyro.getAngularVelocity(rot_x, rot_y, rot_z);
acc_x = (15 * acc_x + lacc_x) / 16;
acc_y = (15 * acc_y + lacc_y) / 16;
acc_z = (15 * acc_z + lacc_z) / 16;
mag = acc_x*acc_x+acc_y*acc_y+acc_z*acc_z;
//printf("\t%.2lf\t%.2lf\t%.2lf\t%.2lf\t|\t%.2lf\t%.2lf\t%.2lf", acc_x, acc_y, acc_z, mag, rot_x, rot_y, rot_z);
//usleep(10000);
clock_gettime(CLOCK_REALTIME, &ts2);
interval = ((ts2.tv_sec-ts.tv_sec)*1000000000 + ts2.tv_nsec-ts.tv_nsec - gettime_delay_ns)/1000.0;
ts = ts2;
dang[0] = - (rot_x - rota[0]) * interval / 1000000;
dang[1] = - (rot_y - rota[1]) * interval / 1000000;
dang[2] = - (rot_z - rota[2]) * interval / 1000000;
ang_x += dang[0];
ang_y += dang[1];
ang_z += dang[2];
dang[0] *= 3.14 / 180;
dang[1] *= 3.14 / 180;
dang[2] *= 3.14 / 180;
rotateV(gvec, dang);
mag = mag * 100 / (9.81 * 9.81);
if (72.0 < mag && mag < 133.0) {
gvec[0] = (gvec[0] * 600 + acc_x) / 601;
gvec[1] = (gvec[1] * 600 + acc_y) / 601;
gvec[2] = (gvec[2] * 600 + acc_z) / 601;
}
printf("\t%.2lf\t%.2lf\t%.2lf\t|\t%.2lf\t%.2lf\t|\t%.2lf\t%.2lf\t%.2lf\t[%.2lf]\n", ang_x, ang_y, ang_z,
atan2(gvec[1], sqrt(gvec[0]*gvec[0] + gvec[2]*gvec[2])) * 180/ 3.14,
atan2(gvec[0], gvec[2]) * 180/ 3.14, gvec[0], gvec[1], gvec[2], interval);
}
}
static void getEstimatedAttitude(void)
{
int32_t axis;
int32_t accMag = 0;
static t_fp_vector EstM;
static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
uint32_t deltaT;
float scale;
fp_angles_t deltaGyroAngle;
deltaT = currentT - previousT;
scale = deltaT * gyroScaleRad;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle.raw[axis] = gyroADC[axis] * scale;
if (imuRuntimeConfig->acc_lpf_factor > 0) {
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / imuRuntimeConfig->acc_lpf_factor)) + accADC[axis] * (1.0f / imuRuntimeConfig->acc_lpf_factor);
accSmooth[axis] = accLPF[axis];
} else {
accSmooth[axis] = accADC[axis];
}
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
}
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, &deltaGyroAngle);
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
float invGyroComplimentaryFilterFactor = (1.0f / (imuRuntimeConfig->gyro_cmpf_factor + 1.0f));
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
}
f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
// Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, &deltaGyroAngle);
// FIXME what does the _M_ mean?
float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f));
for (axis = 0; axis < 3; axis++) {
EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
}
heading = calculateHeading(&EstM);
} else {
rotateV(&EstN.V, &deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V);
heading = calculateHeading(&EstN);
}
acc_calc(deltaT); // rotate acc vector into earth frame
}
