src/imu.c

   1 #include "board.h"
   2 #include "mw.h"
   3
   4 int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
   5 int32_t accSum[3];
   6 uint32_t accTimeSum = 0;        // keep track for integration of acc
   7 int accSumCount = 0;
   8 int16_t smallAngle = 0;
   9 int32_t baroPressure = 0;
  10 int32_t baroTemperature = 0;
  11 uint32_t baroPressureSum = 0;
  12 int32_t BaroAlt = 0;
  13 int32_t sonarAlt;              // to think about the unit
  14 int32_t EstAlt;                // in cm
  15 int32_t BaroPID = 0;
  16 int32_t AltHold;
  17 int32_t errorAltitudeI = 0;
  18 int32_t vario = 0;                      // variometer in cm/s
  19 int16_t throttleAngleCorrection = 0;    // correction of throttle in lateral wind,
  20 float magneticDeclination = 0.0f;       // calculated at startup from config
  21 float accVelScale;
  22 float throttleAngleScale;
  23
  24 // **************
  25 // gyro+acc IMU
  26 // **************
  27 int16_t gyroData[3] = { 0, 0, 0 };
  28 int16_t gyroZero[3] = { 0, 0, 0 };
  29 int16_t angle[2] = { 0, 0 };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
  30 float anglerad[2] = { 0.0f, 0.0f };    // absolute angle inclination in radians
  31
  32 static void getEstimatedAttitude(void);
  33
  34 void imuInit(void)
  35 {
  36     smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f));
  37     accVelScale = 9.80665f / acc_1G / 10000.0f;
  38     throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle);
  39
  40 #ifdef MAG
  41     // if mag sensor is enabled, use it
  42     if (sensors(SENSOR_MAG))
  43         Mag_init();
  44 #endif
  45 }
  46
  47 void computeIMU(void)
  48 {
  49     uint32_t axis;
  50     static int16_t gyroYawSmooth = 0;
  51
  52     Gyro_getADC();
  53     if (sensors(SENSOR_ACC)) {
  54         ACC_getADC();
  55         getEstimatedAttitude();
  56     } else {
  57         accADC[X] = 0;
  58         accADC[Y] = 0;
  59         accADC[Z] = 0;
  60     }
  61
  62     if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
  63         gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3;
  64         gyroYawSmooth = gyroData[YAW];
  65         gyroData[ROLL] = gyroADC[ROLL];
  66         gyroData[PITCH] = gyroADC[PITCH];
  67     } else {
  68         for (axis = 0; axis < 3; axis++)
  69             gyroData[axis] = gyroADC[axis];
  70     }
  71 }
  72
  73 // **************************************************
  74 // Simplified IMU based on "Complementary Filter"
  75 // Inspired by http://starlino.com/imu_guide.html
  76 //
  77 // adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
  78 //
  79 // The following ideas was used in this project:
  80 // 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
  81 //
  82 // Currently Magnetometer uses separate CF which is used only
  83 // for heading approximation.
  84 //
  85 // **************************************************
  86
  87 #define INV_GYR_CMPF_FACTOR   (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
  88 #define INV_GYR_CMPFM_FACTOR  (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
  89
  90 typedef struct fp_vector {
  91     float X;
  92     float Y;
  93     float Z;
  94 } t_fp_vector_def;
  95
  96 typedef union {
  97     float A[3];
  98     t_fp_vector_def V;
  99 } t_fp_vector;
 100
 101 t_fp_vector EstG;
 102
 103 // Normalize a vector
 104 void normalizeV(struct fp_vector *src, struct fp_vector *dest)
 105 {
 106     float length;
 107
 108     length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
 109     if (length != 0) {
 110         dest->X = src->X / length;
 111         dest->Y = src->Y / length;
 112         dest->Z = src->Z / length;
 113     }
 114 }
 115
 116 // Rotate Estimated vector(s) with small angle approximation, according to the gyro data
 117 void rotateV(struct fp_vector *v, float *delta)
 118 {
 119     struct fp_vector v_tmp = *v;
 120
 121     // This does a  "proper" matrix rotation using gyro deltas without small-angle approximation
 122     float mat[3][3];
 123     float cosx, sinx, cosy, siny, cosz, sinz;
 124     float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
 125
 126     cosx = cosf(delta[ROLL]);
 127     sinx = sinf(delta[ROLL]);
 128     cosy = cosf(delta[PITCH]);
 129     siny = sinf(delta[PITCH]);
 130     cosz = cosf(delta[YAW]);
 131     sinz = sinf(delta[YAW]);
 132
 133     coszcosx = cosz * cosx;
 134     coszcosy = cosz * cosy;
 135     sinzcosx = sinz * cosx;
 136     coszsinx = sinx * cosz;
 137     sinzsinx = sinx * sinz;
 138
 139     mat[0][0] = coszcosy;
 140     mat[0][1] = -cosy * sinz;
 141     mat[0][2] = siny;
 142     mat[1][0] = sinzcosx + (coszsinx * siny);
 143     mat[1][1] = coszcosx - (sinzsinx * siny);
 144     mat[1][2] = -sinx * cosy;
 145     mat[2][0] = (sinzsinx) - (coszcosx * siny);
 146     mat[2][1] = (coszsinx) + (sinzcosx * siny);
 147     mat[2][2] = cosy * cosx;
 148
 149     v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
 150     v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
 151     v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
 152 }
 153
 154 int32_t applyDeadband(int32_t value, int32_t deadband)
 155 {
 156     if (abs(value) < deadband) {
 157         value = 0;
 158     } else if (value > 0) {
 159         value -= deadband;
 160     } else if (value < 0) {
 161         value += deadband;
 162     }
 163     return value;
 164 }
 165
 166 #define F_CUT_ACCZ 10.0f // 10Hz should still be fast enough
 167 static const float fc_acc = 0.5f / (M_PI * F_CUT_ACCZ);
 168
 169 // rotate acc into Earth frame and calculate acceleration in it
 170 void acc_calc(uint32_t deltaT)
 171 {
 172     static int32_t accZoffset = 0;
 173     static float accz_smooth;
 174     float rpy[3];
 175     t_fp_vector accel_ned;
 176
 177     // the accel values have to be rotated into the earth frame
 178     rpy[0] = -(float)anglerad[ROLL];
 179     rpy[1] = -(float)anglerad[PITCH];
 180     rpy[2] = -(float)heading * RAD;
 181
 182     accel_ned.V.X = accSmooth[0];
 183     accel_ned.V.Y = accSmooth[1];
 184     accel_ned.V.Z = accSmooth[2];
 185
 186     rotateV(&accel_ned.V, rpy);
 187
 188     if (cfg.acc_unarmedcal == 1) {
 189         if (!f.ARMED) {
 190             accZoffset -= accZoffset / 64;
 191             accZoffset += accel_ned.V.Z;
 192         }
 193         accel_ned.V.Z -= accZoffset / 64;  // compensate for gravitation on z-axis
 194     } else
 195         accel_ned.V.Z -= acc_1G;
 196
 197     accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
 198
 199     // apply Deadband to reduce integration drift and vibration influence
 200     accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
 201     accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
 202     accSum[Z] += applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
 203
 204     // sum up Values for later integration to get velocity and distance
 205     accTimeSum += deltaT;
 206     accSumCount++;
 207 }
 208
 209 void accSum_reset(void)
 210 {
 211     accSum[0] = 0;
 212     accSum[1] = 0;
 213     accSum[2] = 0;
 214     accSumCount = 0;
 215     accTimeSum = 0;
 216 }
 217
 218 // baseflight calculation by Luggi09 originates from arducopter
 219 static int16_t calculateHeading(t_fp_vector *vec)
 220 {
 221     int16_t head;
 222
 223     float cosineRoll = cosf(anglerad[ROLL]);
 224     float sineRoll = sinf(anglerad[ROLL]);
 225     float cosinePitch = cosf(anglerad[PITCH]);
 226     float sinePitch = sinf(anglerad[PITCH]);
 227     float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
 228     float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
 229     float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
 230     head = lrintf(hd);
 231     if (head < 0)
 232         head += 360;
 233
 234     return head;
 235 }
 236
 237 static void getEstimatedAttitude(void)
 238 {
 239     int32_t axis;
 240     int32_t accMag = 0;
 241     static t_fp_vector EstM;
 242     static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
 243     static float accLPF[3];
 244     static uint32_t previousT;
 245     uint32_t currentT = micros();
 246     uint32_t deltaT;
 247     float scale, deltaGyroAngle[3];
 248     deltaT = currentT - previousT;
 249     scale = deltaT * gyro.scale;
 250     previousT = currentT;
 251
 252     // Initialization
 253     for (axis = 0; axis < 3; axis++) {
 254         deltaGyroAngle[axis] = gyroADC[axis] * scale;
 255         if (cfg.acc_lpf_factor > 0) {
 256             accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
 257             accSmooth[axis] = accLPF[axis];
 258         } else {
 259             accSmooth[axis] = accADC[axis];
 260         }
 261         accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
 262     }
 263     accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
 264
 265     rotateV(&EstG.V, deltaGyroAngle);
 266     if (sensors(SENSOR_MAG)) {
 267         rotateV(&EstM.V, deltaGyroAngle);
 268     } else {
 269         rotateV(&EstN.V, deltaGyroAngle);
 270         normalizeV(&EstN.V, &EstN.V);
 271     }
 272
 273     // Apply complimentary filter (Gyro drift correction)
 274     // 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.
 275     // To do that, we just skip filter, as EstV already rotated by Gyro
 276     if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
 277         for (axis = 0; axis < 3; axis++)
 278             EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
 279     }
 280
 281     if (sensors(SENSOR_MAG)) {
 282         for (axis = 0; axis < 3; axis++)
 283             EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
 284     }
 285
 286     f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
 287
 288     // Attitude of the estimated vector
 289     anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
 290     anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
 291     angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
 292     angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));
 293
 294     if (sensors(SENSOR_MAG))
 295         heading = calculateHeading(&EstM);
 296     else
 297         heading = calculateHeading(&EstN);
 298
 299     acc_calc(deltaT); // rotate acc vector into earth frame
 300
 301     if (cfg.throttle_correction_value) {
 302
 303         float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
 304
 305         if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
 306             throttleAngleCorrection = 0;
 307         } else {
 308             int angle = lrintf(acosf(cosZ) * throttleAngleScale);
 309             if (angle > 900)
 310                 angle = 900;
 311             throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ;
 312         }
 313
 314     }
 315 }
 316
 317 #ifdef BARO
 318 #define UPDATE_INTERVAL 25000   // 40hz update rate (20hz LPF on acc)
 319
 320 int getEstimatedAltitude(void)
 321 {
 322     static uint32_t previousT;
 323     uint32_t currentT = micros();
 324     uint32_t dTime;
 325     int32_t error;
 326     int32_t baroVel;
 327     int32_t vel_tmp;
 328     int32_t BaroAlt_tmp;
 329     int32_t setVel;
 330     float dt;
 331     float vel_acc;
 332     float accZ_tmp;
 333     static float accZ_old = 0.0f;
 334     static float vel = 0.0f;
 335     static float accAlt = 0.0f;
 336     static int32_t lastBaroAlt;
 337     static int32_t baroGroundAltitude = 0;
 338     static int32_t baroGroundPressure = 0;
 339
 340     dTime = currentT - previousT;
 341     if (dTime < UPDATE_INTERVAL)
 342         return 0;
 343     previousT = currentT;
 344
 345     if (calibratingB > 0) {
 346         baroGroundPressure -= baroGroundPressure / 8;
 347         baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1);
 348         baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
 349
 350         vel = 0;
 351         accAlt = 0;
 352         calibratingB--;
 353     }
 354
 355     // calculates height from ground via baro readings
 356     // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
 357     BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
 358     BaroAlt_tmp -= baroGroundAltitude;
 359     BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise
 360
 361     dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
 362
 363     // Integrator - velocity, cm/sec
 364     accZ_tmp = (float)accSum[2] / (float)accSumCount;
 365     vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
 366
 367     // Integrator - Altitude in cm
 368     accAlt += (vel_acc * 0.5f) * dt + vel * dt;                                         // integrate velocity to get distance (x= a/2 * t^2)
 369     accAlt = accAlt * cfg.baro_cf_alt + (float)BaroAlt * (1.0f - cfg.baro_cf_alt);      // complementary filter for Altitude estimation (baro & acc)
 370     EstAlt = accAlt;
 371     vel += vel_acc;
 372
 373 #if 0
 374     debug[0] = accSum[2] / accSumCount; // acceleration
 375     debug[1] = vel;                     // velocity
 376     debug[2] = accAlt;                  // height
 377 #endif
 378
 379     accSum_reset();
 380
 381     baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
 382     lastBaroAlt = BaroAlt;
 383
 384     baroVel = constrain(baroVel, -300, 300);    // constrain baro velocity +/- 300cm/s
 385     baroVel = applyDeadband(baroVel, 10);       // to reduce noise near zero
 386
 387     // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
 388     // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
 389     vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
 390     vel_tmp = lrintf(vel);
 391
 392     // set vario
 393     vario = applyDeadband(vel_tmp, 5);
 394
 395     if (abs(angle[ROLL]) < 800 && abs(angle[PITCH]) < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg)
 396         // Altitude P-Controller
 397         error = constrain(AltHold - EstAlt, -500, 500);
 398         error = applyDeadband(error, 10);       // remove small P parametr to reduce noise near zero position
 399         setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
 400
 401         // Velocity PID-Controller
 402         // P
 403         error = setVel - vel_tmp;
 404         BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
 405
 406         // I
 407         errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8;
 408         errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
 409         BaroPID += errorAltitudeI / 1024;     // I in range +/-200
 410
 411         // D
 412         BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
 413
 414     } else {
 415         BaroPID = 0;
 416     }
 417
 418     accZ_old = accZ_tmp;
 419
 420     return 1;
 421 }
 422 #endif /* BARO */