// *Somehow* modified by me..
// Besides mwii credit must go to Sebbi, BRM! Hopefully they condone mentioning them above my trash.
// Sebbi for his rotation of the acc vector and BRM for his normalization ideas.
static void getEstimatedAttitude(void)
{
static t_fp_vector EstG, EstM;
static float accLPFALT[3], accLPFGPS[3], Tilt_25deg, INVsens1G;
static float INV_GYR_CMPF_FACTOR, INV_GYR_CMPFM_FACTOR, ACC_GPS_RC, ACC_ALT_RC, ACC_RC;
static uint32_t previousT, UpsDwnTimer;
static bool init = false;
float tmp[3], scale, deltaGyroAngle[3], ACCALTFac, ACCGPSFac, ACCFac, rollRAD, pitchRAD;
float NormFst = 0.0f, NormScnd, NormR, A, B, cr, sr, cp, sp, cy, sy, spcy, spsy, acc_south, acc_west, acc_up, FAC;
uint8_t i;
uint32_t tmpu32, currentT = micros();
ACCDeltaTimeINS = (float)(currentT - previousT) * 0.000001f;
previousT = currentT;
if(!init) // Setup variables & constants
{
init = true;
INVsens1G = 1.0f / cfg.sens_1G;
Tilt_25deg = cosf(25.0f * RADX);
INV_GYR_CMPF_FACTOR = 1.0f / (float)(cfg.gy_cmpf + 1); // Default 400
INV_GYR_CMPFM_FACTOR = 1.0f / (float)(cfg.gy_cmpfm + 1); // Default 200
ACC_ALT_RC = 0.5f / (M_PI * cfg.acc_altlpfhz); // Default 10 Hz
ACC_RC = 0.5f / (M_PI * cfg.acc_lpfhz); // Default 0,536 Hz
ACC_GPS_RC = 0.5f / (M_PI * cfg.acc_gpslpfhz); // Default 5 Hz
for (i = 0; i < 3; i++) // Preset some values to reduce runup time
{
tmp[0] = accADC[i] * INVsens1G;
accSmooth[i] = tmp[0];
accLPFGPS[i] = tmp[0];
accLPFALT[i] = tmp[0];
EstG.A[i] = tmp[0] * 0.5f;
}
}
ACCALTFac = ACCDeltaTimeINS / (ACC_ALT_RC + ACCDeltaTimeINS); // Adjust LPF to cycle time / do Hz cut off
ACCGPSFac = ACCDeltaTimeINS / (ACC_GPS_RC + ACCDeltaTimeINS); // Adjust LPF to cycle time / do Hz cut off
ACCFac = ACCDeltaTimeINS / (ACC_RC + ACCDeltaTimeINS);
scale = ACCDeltaTimeINS * GyroScale; // SCALE CHANGED TO RAD per SECONDS, DAMN
for (i = 0; i < 3; i++)
{
tmp[0] = accADC[i] * INVsens1G; // Reference all to 1G here
accLPFGPS[i] += ACCGPSFac * (tmp[0] - accLPFGPS[i]); // For GPS
accLPFALT[i] += ACCALTFac * (tmp[0] - accLPFALT[i]); // For Althold
accSmooth[i] += ACCFac * (tmp[0] - accSmooth[i]); // For Gyrodrift correction
NormFst += accSmooth[i] * accSmooth[i];
deltaGyroAngle[i] = gyroADC[i] * scale; // deltaGyroAngle is in RAD
}
RotGravAndMag(&EstG.V, &EstM.V, deltaGyroAngle); // Rotate Grav&Mag together to avoid doublecalculation
NormFst = sqrtf(NormFst);
tmpu32 = (uint32_t)(NormFst * 1000.0f); // Make it "shorter" for comparison in temp variable
NormScnd = sqrtf(EstG.A[0] * EstG.A[0] + EstG.A[1] * EstG.A[1] + EstG.A[2] * EstG.A[2]);
if (NormScnd) NormR = 1.0f / NormScnd;
else NormR = INVsens1G; // Feed vanilla value in that rare case, to let angle calculation always happen
for (i = 0; i < 3; i++) tmp[i] = EstG.A[i] * NormR; // tmp[0..2] contains normalized EstG
if (850 < tmpu32 && tmpu32 < 1150) // Gyro drift correction if ACC between 0.85G and 1.15G else skip filter, as EstV already rotated by Gyro
{
NormR = 1.0f / NormFst; // We just cmp filter together normalized vectors here. Div 0 not possible here.
for (i = 0; i < 3; i++) EstG.A[i] = (tmp[i] * (float)cfg.gy_cmpf + accSmooth[i] * NormR) * INV_GYR_CMPF_FACTOR;
}
rollRAD = atan2f(tmp[0], tmp[2]); // Note: One cycle after successful cmpf is done with old values.
pitchRAD = asinf(-tmp[1]); // Has to have the "wrong sign" relative to angle[PITCH]
angle[ROLL] = rollRAD * RADtoDEG10; // Use rounded values, eliminate jitter for main PID I and D
angle[PITCH] = -pitchRAD * RADtoDEG10;
TiltValue = cosf(rollRAD) * cosf(pitchRAD); // We do this correctly here
if (TiltValue >= 0) UpsDwnTimer = 0;
else if(!UpsDwnTimer) UpsDwnTimer = currentT + 20000; // Use Timer here to make absolutely sure we are upsidedown
if (UpsDwnTimer && currentT > UpsDwnTimer) UpsideDown = true;
else UpsideDown = false;
if (TiltValue > Tilt_25deg) f.SMALL_ANGLES_25 = 1;
else f.SMALL_ANGLES_25 = 0;
cr = cosf(rollRAD);
sr = sinf(rollRAD);
cp = cosf(pitchRAD);
sp = sinf(pitchRAD);
if (cfg.mag_calibrated) // mag_calibrated can just be true if MAG present
{
NormFst = sqrtf(EstM.A[0] * EstM.A[0] + EstM.A[1] * EstM.A[1] + EstM.A[2] * EstM.A[2]);
if (NormFst) NormR = 1.0f / NormFst;
else NormR = 1.0f; // Vanilla value for rare case
for (i = 0; i < 3; i++) tmp[i] = EstM.A[i] * NormR; // tmp[0..2] contains normalized EstM
if(HaveNewMag) // Only do Complementary filter when new MAG data are available
{
HaveNewMag = false;
NormFst = sqrtf(magADCfloat[0] * magADCfloat[0] + magADCfloat[1] * magADCfloat[1] + magADCfloat[2] * magADCfloat[2]);
if (NormFst) NormR = 1.0f / NormFst;
else NormR = 1.0f;
for (i = 0; i < 3; i++) EstM.A[i] = (tmp[i] * (float)cfg.gy_cmpfm + magADCfloat[i] * NormR) * INV_GYR_CMPFM_FACTOR;
}
A = tmp[1] * cp + tmp[0] * sr * sp + tmp[2] * cr * sp;
B = tmp[0] * cr - tmp[2] * sr;
heading = wrap_180(atan2f(-B, A) * RADtoDEG + magneticDeclination); // Get rad to Degree and add declination (note: without *10) // Wrap to -180 0 +180 Degree
} else heading = 0; // if no mag or not calibrated do bodyframe below
tmp[0] = heading * RADX; // Do GPS INS rotate ACC X/Y to earthframe no centrifugal comp. yet
cy = cosf(tmp[0]);
sy = sinf(tmp[0]);
cos_yaw_x = cy; // Store for general use
sin_yaw_y = sy; // Store for general use
spcy = sp * cy;
spsy = sp * sy;
FAC = 980.665f * ACCDeltaTimeINS; // vel factor for normalized output tmp3 = (9.80665f * (float)ACCDeltaTime) / 10000.0f;
acc_up = ((-sp) * accLPFALT[1] + sr * cp * accLPFALT[0] + cp * cr * accLPFALT[2]) - 1;// -1G
if(GroundAltInitialized) vario += acc_up * FAC * constrain(TiltValue + 0.05, 0.5f, 1.0f);// Positive when moving Up. Just do Vario when Baro completely initialized. Empirical hightdrop reduction on tilt.
acc_south = cp * cy * accLPFGPS[1] + (sr * spcy - cr * sy) * accLPFGPS[0] + ( sr * sy + cr * spcy) * accLPFGPS[2];
acc_west = cp * sy * accLPFGPS[1] + (cr * cy + sr * spsy) * accLPFGPS[0] + (-sr * cy + cr * spsy) * accLPFGPS[2];
ACC_speed[LAT] -= acc_south * FAC; // Positive when moving North cm/sec when no MAG this is speed to the front
ACC_speed[LON] -= acc_west * FAC; // Positive when moving East cm/sec when no MAG this is speed to the right
}
static void getEstimatedAttitude(void)
{
static t_fp_vector EstG, EstM;
static float cms[3] = {0.0f, 0.0f, 0.0f}, Tilt_25deg, AccScaleCMSS;
static float INV_GY_CMPF, INV_GY_CMPFM, ACC_GPS_RC, ACC_ALT_RC, ACC_RC;
static uint32_t UpsDwnTimer, SQ1G;
static bool init = false;
float tmp[3], DeltGyRad[3], rollRAD, pitchRAD;
float Norm, A, B, cr, sr, cp, sp, spcy, spsy, accycp, CmsFac;
uint8_t i;
uint32_t tmpu32 = 0;
if(!init) // Setup variables & constants
{
init = true;
AccScaleCMSS = OneGcmss / (float)cfg.sens_1G; // scale to cm/ss
SQ1G = (int32_t)cfg.sens_1G * (int32_t)cfg.sens_1G;
Tilt_25deg = cosf(25.0f * RADX);
INV_GY_CMPF = 1.0f / (float)(cfg.gy_gcmpf + 1); // Default 400
INV_GY_CMPFM = 1.0f / (float)(cfg.gy_mcmpf + 1); // Default 200
tmp[0] = 0.5f / M_PI;
ACC_RC = tmp[0] / cfg.acc_lpfhz; // Default 0,536 Hz
ACC_ALT_RC = tmp[0] / (float)cfg.acc_altlpfhz; // Default 10 Hz
ACC_GPS_RC = tmp[0] / (float)cfg.acc_gpslpfhz; // Default 5 Hz
for (i = 0; i < 3; i++) // Preset some values to reduce runup time
{
accSmooth[i] = accADC[i];
EstG.A[i] = accSmooth[i];
EstM.A[i] = magADCfloat[i]; // Using /2 for more stability
}
}
CmsFac = ACCDeltaTimeINS * AccScaleCMSS; // We need that factor for INS below
tmp[0] = ACCDeltaTimeINS / (ACC_RC + ACCDeltaTimeINS);
for (i = 0; i < 3; i++)
{
accSmooth[i] += tmp[0] * (accADC[i] - accSmooth[i]); // For Gyrodrift correction
DeltGyRad[i] = (ACCDeltaTimeINS * gyroADC[i]) * 0.0625f; // gyroADC delivered in 16 * rad/s
tmpu32 += (int32_t)accSmooth[i] * (int32_t)accSmooth[i]; // Accumulate ACC magnitude there
}
RotGravAndMag(&EstG.V, &EstM.V, DeltGyRad); // Rotate Grav & Mag together to avoid doublecalculation
tmpu32 = (tmpu32 * 100) / SQ1G; // accMag * 100 / ((int32_t)acc_1G * acc_1G);
if (72 < tmpu32 && tmpu32 < 133) // Gyro drift correct between 0.85G - 1.15G
{
for (i = 0; i < 3; i++) EstG.A[i] = (EstG.A[i] * (float)cfg.gy_gcmpf + accSmooth[i]) * INV_GY_CMPF;
}
tmp[0] = EstG.A[0] * EstG.A[0] + EstG.A[2] * EstG.A[2]; // Start Angle Calculation. tmp[0] is used for heading below
Norm = sqrtf(tmp[0] + EstG.A[1] * EstG.A[1]);
if(!Norm) return; // Should never happen but break here to prevent div-by-zero-evil
Norm = 1.0f / Norm;
rollRAD = atan2f(EstG.A[0] * Norm, EstG.A[2] * Norm); // Norm seems to be obsolete, but testing shows different result :)
pitchRAD = -asinf(constrain(EstG.A[1] * Norm, -1.0f, 1.0f)); // Ensure range, eliminate rounding stuff that might occure.
cr = cosf(rollRAD);
sr = sinf(rollRAD);
cp = cosf(pitchRAD);
sp = sinf(pitchRAD);
TiltValue = cr * cp; // We do this correctly here
angle[ROLL] = (float)((int32_t)( rollRAD * RADtoDEG10 + 0.5f)); // Use rounded values, eliminate jitter for main PID I and D
angle[PITCH] = (float)((int32_t)(-pitchRAD * RADtoDEG10 + 0.5f));
if (TiltValue >= 0) UpsDwnTimer = 0;
else if(!UpsDwnTimer) UpsDwnTimer = currentTime + 20000; // Use 20ms Timer here to make absolutely sure we are upsidedown
if (UpsDwnTimer && currentTime > UpsDwnTimer) UpsideDown = true;
else UpsideDown = false;
if (TiltValue > Tilt_25deg) f.SMALL_ANGLES_25 = 1;
else f.SMALL_ANGLES_25 = 0;
if (cfg.mag_calibrated) // mag_calibrated can just be true if MAG present
{
if(HaveNewMag) // Only do Complementary filter when new MAG data are available
{
HaveNewMag = false;
for (i = 0; i < 3; i++) EstM.A[i] = (EstM.A[i] * (float)cfg.gy_mcmpf + magADCfloat[i]) * INV_GY_CMPFM;
}
A = EstM.A[1] * tmp[0] - (EstM.A[0] * EstG.A[0] + EstM.A[2] * EstG.A[2]) * EstG.A[1];// Mwii method is more precise (less rounding errors)
B = EstM.A[2] * EstG.A[0] - EstM.A[0] * EstG.A[2];
heading = wrap_180(atan2f(B, A * Norm) * RADtoDEG + magneticDeclination);
if (sensors(SENSOR_GPS) && !UpsideDown)
{
tmp[0] = heading * RADX; // Do GPS INS rotate ACC X/Y to earthframe no centrifugal comp. yet
cos_yaw_x = cosf(tmp[0]); // Store for general use
sin_yaw_y = sinf(tmp[0]);
spcy = sp * cos_yaw_x;
spsy = sp * sin_yaw_y;
accycp = cp * accADC[1];
tmp[0] = accycp * cos_yaw_x + (sr * spcy - cr * sin_yaw_y) * accADC[0] + ( sr * sin_yaw_y + cr * spcy) * accADC[2]; // Rotate raw acc here
tmp[1] = accycp * sin_yaw_y + (cr * cos_yaw_x + sr * spsy) * accADC[0] + (-sr * cos_yaw_x + cr * spsy) * accADC[2];
tmp[2] = ACCDeltaTimeINS / (ACC_GPS_RC + ACCDeltaTimeINS);
for (i = 0; i < 2; i++)
{
cms[i] += tmp[2] * (tmp[i] * CmsFac - cms[i]);
ACC_speed[i] -= cms[i]; //cm/s N+ E+
}
}
}
if(GroundAltInitialized && !UpsideDown) // GroundAltInitialized can just be true if baro present
{
tmp[0] = ((-sp) * accADC[1] + sr * cp * accADC[0] + cp * cr * accADC[2]) - (float)cfg.sens_1G;
cms[2] += (ACCDeltaTimeINS / (ACC_ALT_RC + ACCDeltaTimeINS)) * (tmp[0] * CmsFac - cms[2]);
vario += cms[2];
}
}
