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(); }
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); }
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. }