C++ (Cpp) filterUpdate Examples

Example #1

File: Params.cpp Project: KoteSoft/Quadrocopter-flight-controller

void ModbusEEPROMLoader()
{
	uint8_t change_flag = 0;

	
	for (uint8_t i = 0; i < coeff_list_SIZE; i++)
	{
		if (!Uint32Comparrer(usRegHoldingBuf[2 * i + MB_OFFSET + MB_Kp_Oz], usRegHoldingBuf[2 * i + MB_OFFSET + MB_Kp_Oz + 1], coeffs[i].array[0], coeffs[i].array[1]))
		{
			coeffs[i].array[0]=usRegHoldingBuf[2 * i + MB_OFFSET + MB_Kp_Oz];
			coeffs[i].array[1]=usRegHoldingBuf[2 * i + MB_OFFSET + MB_Kp_Oz + 1];
			eeprom_write_float((float*)(i * 4), coeffs[i].value);
			change_flag = 1;
		}
	}	
	
	if (change_flag)
	{
		filterUpdate();
		Sound_On();
		_delay_ms(100);
		Sound_Off();		
	}
	
	ModbusInitValues(); 
}

Example #2

File: IMU_driver.c Project: AlexMechatronic/driver-imu-6500

uint32_t update_and_get_Data_IMU(void)
{
	gyro_data_t gyro;
	accel_data_t accel;
	
	
	//obtener por comunicacion spi los datos
	
	//				= getTime()
	uint32_t timeNow;
	float deltat = timeNow - lastUpdate;//es el diferencial del tiempo transcurrido entre cada actualizacion de los datos
	lastUpdate = timeNow;
	QuaternionStruct quaternion = filterUpdate(deltat, gyro.g_x, gyro.g_y, gyro.g_z, accel.a_x, accel.a_y, accel.a_z);
}

Example #3

File: filter3.c Project: EranSegal/ek-lm4f230H5QR

void AHRS_Step_c() // Attitude heading reference system 
{
  // correlation between x/y/z axis and index in sensor readings
  Axm= (float)BMA180values[0];
  Aym= (float)BMA180values[1];
  Azm= (float)BMA180values[2];

  Gxm= (float)Measurements[3+1]-ITG3200AtRest[1];
  Gym= (float)Measurements[3+0]-ITG3200AtRest[0];
  Gzm= (float)Measurements[3+2]-ITG3200AtRest[2];

  filterUpdate(Axm,Aym,Azm,Gxm,Gym,Gzm);
  //Calibration Accelerometerx
  Ax=cAx+cAxx*Axm+cAxy*Aym+cAxz*Azm; //Ax = -0.585   +  (-1.0/548.0*Axm)
  Ay=cAy+cAyx*Axm+cAyy*Aym+cAyz*Azm; //Ay = 0.3 	    +  (1.0/558.0*Aym)
  Az=cAz+cAzx*Axm+cAzy*Aym+cAzz*Azm; //Az  = -0.5624 +  (1.0/567.0*Azm)
  Gx=cGx+cGxx*Gxm+cGxy*Gym+cGxz*Gzm;
  Gy=cGy+cGyx*Gxm+cGyy*Gym+cGyz*Gzm;
  Gz=cGz+cGzx*Gxm+cGzy*Gym+cGzz*Gzm;

  if(Init)
  {
    g=9.8f;
    Eps=0.01f;
    Gain=1-expf(-dt/Tau);
    Theta=-asinf(Ax/g);
    Phi=asinf((Ay/cosf(Theta))/g);  // DH
    Psi = 0;
    Init = false;
  }
  
  //Acceleration & Magnetometer Euler Angles
  g = sqrt(Ax * Ax + Ay * Ay + Az * Az) + 0.0001; // Add 0.0001 to g is not to  get Non while we 
  sThetac = -(Ax/g);      
  cThetac=sqrtf(1-sThetac*sThetac);
  
  sPhic     =  ((Ay/cThetac)/g);   //DH

  Phic   = asinf(sPhic);
  Thetac = asinf(sThetac);
  return;   // just Phic instead of Phi etc.
}