else
{
// MPU calibration, hold still mpu still for this event
int BIAS_FIFO_SIZE = FIFO_SIZE;
float ax[BIAS_FIFO_SIZE], ay[BIAS_FIFO_SIZE], az[BIAS_FIFO_SIZE];
float gx[BIAS_FIFO_SIZE], gy[BIAS_FIFO_SIZE], gz[BIAS_FIFO_SIZE];
float mx[BIAS_FIFO_SIZE], my[BIAS_FIFO_SIZE], mz[BIAS_FIFO_SIZE];
// throw out first fifo reading
IMU.enableFifo(true, true, true, false);
vTaskDelay( pdMS_TO_TICKS( 200 ) );
IMU.readFifo();
IMU.enableFifo(true, true, true, false);
// There is and interaction between sample size and accuracy of bias.
// Sample size is determined, with the FIFO read, by the length of time FIFO is allowed to gather data.
// Sample sizes larger then 200mS are greatly inaccurate.
vTaskDelay( pdMS_TO_TICKS( 150 ) );
IMU.readFifo();
IMU.getFifoAccelX_mss(&fifoSize, ax);
IMU.getFifoAccelY_mss(&fifoSize, ay);
IMU.getFifoAccelZ_mss(&fifoSize, az);
IMU.getFifoGyroX_rads(&fifoSize, gx);
IMU.getFifoGyroY_rads(&fifoSize, gy);
IMU.getFifoGyroZ_rads(&fifoSize, gz);
IMU.getFifoMagX_uT(&fifoSize, mx);
IMU.getFifoMagY_uT(&fifoSize, my);
IMU.getFifoMagZ_uT(&fifoSize, mz);
int iCount = 0;
for (size_t i = 1; i < fifoSize; i++)
{
ax[0] += ax[i];
ay[0] += ay[i];
az[0] += az[i];
gx[0] += gx[i];
gy[0] += gy[i];
gz[0] += gz[i];
++iCount;
}
axBias = -(ax[0] / iCount);
ayBias = -(ay[0] / iCount);
azBias = -(az[0] / iCount);
gxBias = -(gx[0] / iCount);
gyBias = -(gy[0] / iCount);
gzBias = -(gz[0] / iCount);
Serial.print( "calibration values ");
Serial.print ( iCount );
Serial.print( ", ax ");
Serial.print ( axBias, 6 );
Serial.print( ", ay ");
Serial.print (ayBias, 6 );
Serial.print( ", az ");
Serial.print ( azBias, 6 );
Serial.print( ", gx ");
Serial.print ( gxBias, 6 );
Serial.print( ", gy ");
Serial.print ( gyBias, 6 );
Serial.print( ", gz ");
Serial.print ( gzBias, 6 );
Serial.println();
DidBias = true;
TimePast = micros();
xEventGroupSetBits( eg, evtLIDAR_ServoAspectChange ); // start the LIDAR scanning
}
} // if ( StartUpCount >= StartUpCountWaitCount )
else
{
++StartUpCount;
}
} // if ( IMU.readFifo() > -1 )
IMU.enableFifo(true, true, true, false);
xSemaphoreGive( sema_GetIMU );
// Serial.print(uxTaskGetStackHighWaterMark( NULL ));
// Serial.println();
// Serial.flush();
} // for (;;)
vTaskDelete( NULL );
}
I use the Euler angles to produce servo counter torque values.
I use the BolderFlightSystem library, which works very well on an ESP32. I read the FIFO buffer and average the buffers readings to get the values to be used in the quaternions calculations.
All of the above code, on an ESP32, takes 260uSeconds - measuring code, to calculate. I read the FIFO every other millisecond.
--> see next post
