コード例 #1
0
void Compass_ReadMagAvg(float *v, int n)
{
    float vals[3] = {};
    int i = n, x;
    memset(v, 0, sizeof(float[3]));
    while (i--) {
        Compass_ReadMag(vals);
        for (x = 0; x < 3; ++x)
            v[x] += vals[x];
    }
    for (x = 0; x < 3; ++x)
        v[x] /= n;
}
コード例 #2
0
void calibrate()
{
    // TODO: wait for things to stabilise
    //printf("Calibrating\n");
    Compass_ReadAccAvg(Zaxis, 1000);
    vecNorm(Zaxis);
    //printf("Z: %9.3f %9.3f %9.3f\n", Zaxis[0], Zaxis[1], Zaxis[2]);

    Compass_ReadMag(Xaxis);
    vecNorm(Xaxis);
    //printf("X: %9.3f %9.3f %9.3f\n", Xaxis[0], Xaxis[1], Xaxis[2]);
    vecCross(Yaxis, Zaxis, Xaxis);
    vecNorm(Yaxis);
    vecCross(Xaxis, Yaxis, Zaxis);
    vecNorm(Xaxis);

    Gyro_ReadAngRateAvg(zeroAngRate, 100);

    //printf("X: %9.3f %9.3f %9.3f\n", Xaxis[0], Xaxis[1], Xaxis[2]);
    //printf("Y: %9.3f %9.3f %9.3f\n", Yaxis[0], Yaxis[1], Yaxis[2]);
    //printf("Z: %9.3f %9.3f %9.3f\n", Zaxis[0], Zaxis[1], Zaxis[2]);
}
コード例 #3
0
ファイル: imu.c プロジェクト: kumencz/fiscopter
void read_imu(void)
{   
	//repair gyro -> set gyro to acc value
	//opravy offset/rozjezd gyroskopu od prave hodnoty akcelerometru
	if(gyro_cnt > reload_gyro)
	{
		gyrXangle = Xrot - 1;
		gyrYangle = Yrot - 1;
		gyro_cnt = 0;
	}

	/*--------------- Read Acc ---------------*/
	Acc_ReadAcc(Acc_Buffer);

	Acc_Buffer[0] /= 100.0f;
	Acc_Buffer[1] /= 100.0f;
	Acc_Buffer[2] /= 100.0f;

	fNormAcc = sqrt((Acc_Buffer[0]*Acc_Buffer[0])+(Acc_Buffer[1]*Acc_Buffer[1])+(Acc_Buffer[2]*Acc_Buffer[2])); //vector length of acceleration
	
	accXangle = -(atan2((Acc_Buffer[0]/fNormAcc),Acc_Buffer[2]/fNormAcc)*180/PI);
	accYangle = -(atan2((Acc_Buffer[1]/fNormAcc),Acc_Buffer[2]/fNormAcc)*180/PI);			

	/*--------------- Read Gyro ---------------*/
	Gyro_ReadAngRate(Gyro_Buffer);
		 
	gyrXangle = gyrXangle + ((Gyro_Buffer[0]*time));
	gyrYangle = gyrYangle + ((Gyro_Buffer[1]*time));
	gyrZangle = gyrZangle + ((Gyro_Buffer[2]*time));

	/*--------------- Read Mag ---------------*/
	Compass_ReadMag(Mag_Buffer);
	
	Mag_angle = (atan2(Mag_Buffer[1],Mag_Buffer[0])*180/PI)-Mag_correction;
	if (Mag_angle < 0) Mag_angle += 360;
	
	/* Filter and combine acc + gyro (+ mag)*/

	PitchAng = 0.98f *(PitchAng+Gyro_Buffer[0]*time) + 0.02f*accXangle;
	RollAng = 0.98f *(RollAng+Gyro_Buffer[1]*time) + 0.02f*accYangle;
	YawAng = 0.98f *(YawAng-Gyro_Buffer[2]*time) + 0.02f*Mag_angle;



#ifdef SEND_PROCESING_DATA
	sprintf(send, "0|%f|%f|%f|%f|%f|0|\n",PitchAng,RollAng,YawAng,time,Mag_angle);
	//sprintf(send, "0|%f|%f|%f|0|0|0|\n",YawAng,RollAng,PitchAng);
	USART_puts(USART3, send); 
#endif


	//counter pocita tiky timeru
	//TIM4->CNT obsahuje soucasny stav citace tiku timeru 4
	counter = TIM4->CNT;	
	if(counter > counter_old)
	{
		time = (counter-counter_old)*0.0000005; //pocet tiku od minule * doba tiku
	}else if(counter < counter_old)  //pokud counter napocita 50000 pocita znova od 0 -> counter bude mensi nez minule
	{
		time = (50000-counter_old+counter)*0.0000005;
	}
	//Delay(5);
	counter_old = TIM4->CNT; //ulozit soucasny stav counteru do counter_old

	gyro_cnt++;	//pocitani cyklu pro obnoveni hodnoty gyroskopu			
}
コード例 #4
0
int main()
{
    setvbuf(stdout, NULL, _IONBF, 0);
    setvbuf(stderr, NULL, _IONBF, 0);

    /*!< At this stage the microcontroller clock setting is already configured, 
      this is done through SystemInit() function which is called from startup
      file (startup_stm32f30x.s) before to branch to application main.
      To reconfigure the default setting of SystemInit() function, refer to
      system_stm32f30x.c file
      */ 

    /* SysTick end of count event each 10ms */
    RCC_GetClocksFreq(&RCC_Clocks);
    SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);

    /* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */
    USART1_Init();

    //printf("Starting\n");
    USART1_flush();

    /*
    printf("Initialising USB\n");
    USBHID_Init();
    printf("Initialising USB HID\n");
    Joystick_init();
    */
    
    /* Initialise LEDs */
    //printf("Initialising LEDs\n");
    int i;
    for (i = 0; i < 8; ++i) {
        STM_EVAL_LEDInit(leds[i]);
        STM_EVAL_LEDOff(leds[i]);
    }

    /* Initialise gyro */
    //printf("Initialising gyroscope\n");
    Gyro_Init();

    /* Initialise compass */
    //printf("Initialising compass\n");
    Compass_Init();

    Delay(100);
    calibrate();

    int C = 0, noAccelCount = 0;

    while (1) {
        float *acc = accs[C&1],
              *prevAcc = accs[(C&1)^1],
              *vel = vels[C&1],
              *prevVel = vels[(C&1)^1],
              *pos = poss[C&1],
              *prevPos = poss[(C&1)^1],
              *angRate = angRates[C&1],
              *prevAngRate = angRates[(C&1)^1],
              *ang = angs[C&1],
              *prevAng = angs[(C&1)^1],
              *mag = mags[C&1],
              *prevmag = mags[(C&1)^1];

        /* Wait for data ready */

#if 0
        Compass_ReadAccAvg(acc, 10);
        vecMul(axes, acc);
        //printf("X: %9.3f Y: %9.3f Z: %9.3f\n", acc[0], acc[1], acc[2]);
        
        float grav = acc[2];
        acc[2] = 0;
        
        if (noAccelCount++ > 50) {
            for (i = 0; i < 2; ++i) {
                vel[i] = 0;
                prevVel[i] = 0;
            }
            noAccelCount = 0;
        }

        if (vecLen(acc) > 50.f) {
            for (i = 0; i < 2; ++i) {
                vel[i] += prevAcc[i] + (acc[i]-prevAcc[i])/2.f;
                pos[i] += prevVel[i] + (vel[i]-prevVel[i])/2.f;
            }
            noAccelCount = 0;
        }

        C += 1;
        if (((C) & 0x7F) == 0) {
            printf("%9.3f %9.3f %9.3f %9.3f %9.3f\n", vel[0], vel[1], pos[0], pos[1], grav);
            //printf("%3.1f%% %d %5.1f %6.3f\n", (float) timeReadI2C*100.f / totalTime, C, (float) C*100.f / (totalTime), grav);
        }
#endif

        Compass_ReadMagAvg(mag, 2);
        vecMul(axes, mag);
        float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI;
        if (compassAngle > 180.f) compassAngle -= 360.f;
        //vecNorm(mag);
        

        Gyro_ReadAngRateAvg(mag, 2);
        printf("%6.3f:%6.3f,%6.3f,%6.3f\n", compassAngle, mag[0], mag[1], mag[2]);

#if 0
        Gyro_ReadAngRateAvg(angRate, 2);
        angRate[0] *= 180.f / PI;
        angRate[1] *= 180.f / PI;
        angRate[2] *= 180.f / PI;
        float s[3] = {sin(angRate[0]), sin(angRate[1]), sin(angRate[2])};
        float c[3] = {cos(angRate[0]), cos(angRate[1]), cos(angRate[2])};
        float gyroMat[3][3] = {
            {c[0]*c[1], c[0]*s[1], -s[1]},
            {c[0]*s[1]*s[2]-s[0]*c[2], c[0]*c[2]+s[0]*s[1]*s[2], c[1]*s[2]},
            {c[0]*s[1]*c[2]+s[0]*s[2], -c[0]*s[2]+s[0]*s[1]*c[2], c[1]*c[2]}};
        /*
        float gyroWorldMat[3][3];
        vecMulMatTrans(gyroWorldMat, axes, gyroMat);
        *ang = gyroWorldMat[2][0];
        *ang += gyroWorldMat[2][1];
        *ang += gyroWorldMat[2][2];
        *ang /= 300.f;
        static const float ANGALPHA = 0.0f;
        *ang += ANGALPHA*(compassAngle - *ang);
        */
        float rotObsVec[3];
        memcpy(rotObsVec, axes[0], sizeof(rotObsVec));
        vecMul(gyroMat, rotObsVec);
        vecMul(axes, rotObsVec);
        rotObsVec[2] = 0.f;
        vecNorm(rotObsVec);
        float angDelta = acos(rotObsVec[0]);

        if (((++C) & 0x7) == 0) {
            printf("%6.3f %6.3f %6.3f %6.3f\n", rotObsVec[0], rotObsVec[1], rotObsVec[2], angDelta);
        }
#endif


#if 0
        float angRate[3];
      
      /* Read Gyro Angular data */
      Gyro_ReadAngRate(angRate);

      printf("X: %f Y: %f Z: %f\n", angRate[0], angRate[1], angRate[2]);

        float MagBuffer[3] = {0.0f}, AccBuffer[3] = {0.0f};
        float fNormAcc,fSinRoll,fCosRoll,fSinPitch,fCosPitch = 0.0f, RollAng = 0.0f, PitchAng = 0.0f;
        float fTiltedX,fTiltedY = 0.0f;


      Compass_ReadMag(MagBuffer);
      Compass_ReadAcc(AccBuffer);
      
      for(i=0;i<3;i++)
        AccBuffer[i] /= 100.0f;
      
      fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
      
      fSinRoll = -AccBuffer[1]/fNormAcc;
      fCosRoll = sqrt(1.0-(fSinRoll * fSinRoll));
      fSinPitch = AccBuffer[0]/fNormAcc;
      fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch));
     if ( fSinRoll >0)
     {
       if (fCosRoll>0)
       {
         RollAng = acos(fCosRoll)*180/PI;
       }
       else
       {
         RollAng = acos(fCosRoll)*180/PI + 180;
       }
     }
     else
     {
       if (fCosRoll>0)
       {
         RollAng = acos(fCosRoll)*180/PI + 360;
       }
       else
       {
         RollAng = acos(fCosRoll)*180/PI + 180;
       }
     }
     
      if ( fSinPitch >0)
     {
       if (fCosPitch>0)
       {
            PitchAng = acos(fCosPitch)*180/PI;
       }
       else
       {
          PitchAng = acos(fCosPitch)*180/PI + 180;
       }
     }
     else
     {
       if (fCosPitch>0)
       {
            PitchAng = acos(fCosPitch)*180/PI + 360;
       }
       else
       {
          PitchAng = acos(fCosPitch)*180/PI + 180;
       }
     }

      if (RollAng >=360)
      {
        RollAng = RollAng - 360;
      }
      
      if (PitchAng >=360)
      {
        PitchAng = PitchAng - 360;
      }
      
      fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
      fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;
      
      HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI;
 
        printf("Compass heading: %f\n", HeadingValue);
#endif
    }

    return 1;
}