MultiWii

Code: Select all

heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z);

Code: Select all

        #define RADX10 M_PI/1800.0f                  // 0.001745329252f
        float rollRAD = (float)angle[ROLL] * RADX10;
        float pitchRAD = -(float)angle[PITCH] * RADX10;
        float magX = EstM.A[1];                         // Swap X/Y
        float magY = EstM.A[0];                         // Swap X/Y
        float magZ = EstM.A[2];
        float cr = cosf(rollRAD);
        float sr = sinf(rollRAD);
        float cp = cosf(pitchRAD);
        float sp = sinf(pitchRAD);
        float Xh;
        float Yh;
        Xh = magX*cp+magY*sr*sp + magZ*cr*sp;
        Yh = magY*cr-magZ*sr;    
        heading = _atan2f(-Yh,Xh);                      // magnetic heading * 10

Code: Select all

 #define RADX10 M_PI/1800.0f                  // if not already defined in board.h

    if (sensors(SENSOR_MAG)) {
        float rollRAD = (float)angle[ROLL] * RADX10;    // Crashpilot CORRECT MAG TILT COMPENSATION now you can fly on your back with correct heading
        float pitchRAD = -(float)angle[PITCH] * RADX10;
        float magX = EstM.A[1];                         // Swap X/Y
        float magY = EstM.A[0];                         // Swap X/Y
        float magZ = EstM.A[2];
        float cr = cosf(rollRAD);
        float sr = sinf(rollRAD);
        float cp = cosf(pitchRAD);
        float sp = sinf(pitchRAD);
        float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
        float Yh = magY * cr - magZ * sr;
        float flthead = (atan2f(-Yh,Xh) * 1800.0f / M_PI + magneticDeclination)/10;
        if (flthead > 180.0f) flthead = flthead - 360.0f;
        else if (flthead < -180.0f) flthead = flthead + 360.0f;
        heading = flthead;
    }

Code: Select all

altered ms5611Detect:
...
    baro->get_up = ms5611_get_up;
    baro->calculate = ms5611_calculate;

    delay(50);
    for (i = 0; i < 10; i++) ms5611_reset();                     // Reset the damn thing a few times
    delay(50);
    return true;
}
...

Code: Select all

...
     float VelNorth = 0, VelEast = 0;      This is "global" for IMU.c and GPS.c
...
    accVelScale = 9.80665f / acc_1G / 10000.0f;
...
    velfactor = accVelScale * (float)ACCDeltaTime;  This is calculated right at acc readout to get the correct timebase
... 
    cfg.gps_ins_cf           This is a new CLI Variable to set the complementary filter for the gps/acc fusion. 0 disables the acc stuff. The higher it gets, the more acc influence you will get.
... 

Code: Select all

#define RADX M_PI / 180.0f
#ifdef MAG
    if (sensors(SENSOR_MAG)) {
    float rollRAD, pitchRAD, cr, sr, cp, sp, Xh, Yh, flthead;
    float cy, sy, spcy, spsy, acc_south, acc_west;
    rollRAD = (float)angle[ROLL] * RADX10;                        // CORRECT MAG TILT COMPENSATION
        pitchRAD = -(float)angle[PITCH] * RADX10;
        cr = cosf(rollRAD);
        sr = sinf(rollRAD);
        cp = cosf(pitchRAD);
        sp = sinf(pitchRAD);
        Xh = EstM.A[1] * cp + EstM.A[0] * sr * sp + EstM.A[2] * cr * sp;
        Yh = EstM.A[0] * cr - EstM.A[2] * sr;
          flthead = (atan2f(-Yh,Xh) * 1800.0f / M_PI + magneticDeclination)/10;
        if (flthead > 180.0f) flthead = flthead - 360.0f;
        else if (flthead < -180.0f) flthead = flthead + 360.0f;
        heading = flthead;                                            // Heading in Degrees
            if (sensors(SENSOR_GPS) && cfg.gps_ins_cf !=0) {              // Do GPS INS rotate ACC X/Y to earthframe no centrifugal comp. yet
                flthead = flthead * RADX;                                 // Floatheading to RAD
               cy = cosf(flthead);
            sy = sinf(flthead);
               spcy = sp * cy;
               spsy = sp * sy;
            acc_south = (cp * cy) * accLPFVel[1] + (sr * spcy - cr * sy) * accLPFVel[0] + (sr * sy + cr * spcy) * accLPFVel[2];
            acc_west  = (cp * sy) * accLPFVel[1] + (cr * cy + sr * spsy) * accLPFVel[0] + (-sr * cy + cr * spsy) * accLPFVel[2];
            if (abs(acc_south) < 80) acc_south = 0;                   // Cancel out some acc noise maybe 80?
            if (abs(acc_west) < 80) acc_west = 0;                     // Cancel out some acc noise maybe 80?
            VelNorth  = VelNorth - acc_south * velfactor;             // Positive when moving North cm/sec 
            VelEast   = VelEast - acc_west * velfactor;               // Positive when moving East cm/sec
           }
    }
#endif

Code: Select all

static void GPS_calc_velocity(void)
{
    static int16_t speed_old[2] = { 0, 0 };
    static int32_t last[2] = { 0, 0 };
    static uint8_t init = 0;
    static uint32_t lasttime = 0;
    uint32_t temp;
    // actual_speed[GPS_Y] y_GPS_speed positve = Up (NORTH)
    // actual_speed[GPS_X] x_GPS_speed positve = Right (EAST)
    if (init){
        float tmp = 1.0f / dTnav;
        actual_speed[GPS_X] = (float) (GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
        actual_speed[GPS_Y] = (float) (GPS_coord[LAT] - last[LAT]) * tmp;
        actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
        actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
        speed_old[GPS_X] = actual_speed[GPS_X];
        speed_old[GPS_Y] = actual_speed[GPS_Y];
           temp = micros();                                          // Crashpilot do CF with acc vel START
           if ((temp - lasttime) < 500000 && cfg.gps_ins_cf !=0) {   // Only accept summation if last correction is not older than 0,5s
               VelNorth = VelNorth * cfg.gps_ins_cf + (float)actual_speed[GPS_Y] * (1.0f - cfg.gps_ins_cf);  // CF: GPS Correction
                VelEast  = VelEast  * cfg.gps_ins_cf + (float)actual_speed[GPS_X] * (1.0f - cfg.gps_ins_cf);  // CF: GPS Correction
               actual_speed[GPS_Y] = VelNorth;
               actual_speed[GPS_X] = VelEast;
            } else {                                                  // INS Correction not possible: Timeout or turned off
                VelNorth = 0;
                 VelEast = 0;
            }
            lasttime = temp;                                          // Crashpilot do CF with acc vel END
    }
    init = 1;
    last[LON] = GPS_coord[LON];
    last[LAT] = GPS_coord[LAT];
}

Code: Select all

errorAngle = errorAngle*(float)cycleTime/BasePIDtime;

Code: Select all

ITermACC = ((float)errorAngleI[axis] * (float)cfg.I8[PIDLEVEL] * (float)cycleTime/BasePIDtime )/4096.0f;

Code: Select all

DTerm = ((float)deltaSum * (float)dynD8[axis] * (float)BasePIDtime/cycleTime )/32.0f;