void RTIMU::setCalibrationData()
{
float maxDelta = -1;
float delta;
CALLIB_DATA calData;
m_calibrationValid = false;
if (calLibRead(0, &calData)) {
if (calData.magValid != 1) {
return;
}
// find biggest range
for (int i = 0; i < 3; i++) {
if ((calData.magMax[i] - calData.magMin[i]) > maxDelta)
maxDelta = calData.magMax[i] - calData.magMin[i];
}
if (maxDelta < 0) {
return;
}
maxDelta /= 2.0f; // this is the max +/- range
for (int i = 0; i < 3; i++) {
delta = (calData.magMax[i] - calData.magMin[i]) / 2.0f;
m_compassCalScale[i] = maxDelta / delta; // makes everything the same range
m_compassCalOffset[i] = (calData.magMax[i] + calData.magMin[i]) / 2.0f;
}
m_calibrationValid = true;
}
}
void MPU9150::device_loop(Command command){
if (!DidInit){
if( MPU9150ReInit.elapsed(30000)){
MPU9150::device_setup();
}
return;
}
if (command.cmp("ccal")){
// Compass_Calibrate();
// The IMU needs both Magnatrometer and Acceleromter to be calibrated. This attempts to do them both at the same time
calLibRead(MPUDeviceId, &calData); // pick up existing accel data if there
// calData.accelValid = false;
// calData.accelMinX = 0x7fff; // init accel cal data
// calData.accelMaxX = 0x8000;
// calData.accelMinY = 0x7fff;
// calData.accelMaxY = 0x8000;
// calData.accelMinZ = 0x7fff;
// calData.accelMaxZ = 0x8000;
calData.magValid = false;
calData.magMinX = 0x7fff; // init mag cal data
calData.magMaxX = 0x8000;
calData.magMinY = 0x7fff;
calData.magMaxY = 0x8000;
calData.magMinZ = 0x7fff;
calData.magMaxZ = 0x8000;
// MPU.useAccelCal(false);
MPU.init(MPU_UPDATE_RATE, 5, MAG_UPDATE_RATE);
counter = 359;
InCallibrationMode = true;
calibration_timer.reset();
Serial.println(F("!!!:While the compass counts down from 360 to 0, rotate the ROV slowly in all three axis;"));
}
if (InCallibrationMode){
bool changed = false;
if(counter>0){
if (MPU.read()) { // get the latest data
changed = false;
if (MPU.m_rawAccel[VEC3_X] < calData.accelMinX) {
calData.accelMinX = MPU.m_rawAccel[VEC3_X];
changed = true;
}
if (MPU.m_rawAccel[VEC3_X] > calData.accelMaxX) {
calData.accelMaxX = MPU.m_rawAccel[VEC3_X];
changed = true;
}
if (MPU.m_rawAccel[VEC3_Y] < calData.accelMinY) {
calData.accelMinY = MPU.m_rawAccel[VEC3_Y];
changed = true;
}
if (MPU.m_rawAccel[VEC3_Y] > calData.accelMaxY) {
calData.accelMaxY = MPU.m_rawAccel[VEC3_Y];
changed = true;
}
if (MPU.m_rawAccel[VEC3_Z] < calData.accelMinZ) {
calData.accelMinZ = MPU.m_rawAccel[VEC3_Z];
changed = true;
}
if (MPU.m_rawAccel[VEC3_Z] > calData.accelMaxZ) {
calData.accelMaxZ = MPU.m_rawAccel[VEC3_Z];
changed = true;
}
if (MPU.m_rawMag[VEC3_X] < calData.magMinX) {
calData.magMinX = MPU.m_rawMag[VEC3_X];
changed = true;
}
if (MPU.m_rawMag[VEC3_X] > calData.magMaxX) {
calData.magMaxX = MPU.m_rawMag[VEC3_X];
changed = true;
}
if (MPU.m_rawMag[VEC3_Y] < calData.magMinY) {
calData.magMinY = MPU.m_rawMag[VEC3_Y];
changed = true;
}
if (MPU.m_rawMag[VEC3_Y] > calData.magMaxY) {
calData.magMaxY = MPU.m_rawMag[VEC3_Y];
changed = true;
}
if (MPU.m_rawMag[VEC3_Z] < calData.magMinZ) {
calData.magMinZ = MPU.m_rawMag[VEC3_Z];
changed = true;
}
if (MPU.m_rawMag[VEC3_Z] > calData.magMaxZ) {
calData.magMaxZ = MPU.m_rawMag[VEC3_Z];
changed = true;
}
if (changed) {
Serial.print(F("dia:accel.MinX=")); Serial.print(calData.accelMinX); Serial.println(";");
Serial.print(F("dia:accel.maxX=")); Serial.print(calData.accelMaxX); Serial.println(";");
Serial.print(F("dia:accel.minY=")); Serial.print(calData.accelMinY); Serial.println(";");
Serial.print(F("dia:accel.maxY=")); Serial.print(calData.accelMaxY); Serial.println(";");
Serial.print(F("dia:accel.minZ=")); Serial.print(calData.accelMinZ); Serial.println(";");
Serial.print(F("dia:accel.maxZ=")); Serial.print(calData.accelMaxZ); Serial.println(";");
Serial.print(F("dia:mag.minX=")); Serial.print(calData.magMinX); Serial.println(";");
Serial.print(F("dia:mag.maxX=")); Serial.print(calData.magMaxX); Serial.println(";");
Serial.print(F("dia:mag.minY=")); Serial.print(calData.magMinY); Serial.println(";");
Serial.print(F("dia:mag.maxY=")); Serial.print(calData.magMaxY); Serial.println(";");
Serial.print(F("dia:mag.minZ=")); Serial.print(calData.magMinZ); Serial.println(";");
Serial.print(F("dia:mag.maxZ=")); Serial.print(calData.magMaxZ); Serial.println(";");
}
}
if (calibration_timer.elapsed (100)) {
counter--;
navdata::HDGD = counter;
Serial.print(F("hdgd:"));
Serial.print(navdata::HDGD);
Serial.print(';');
}
}
if (counter <= 0){
// calData.accelValid = true;
calData.magValid = true;
calLibWrite(MPUDeviceId, &calData);
Serial.println(F("log:Accel cal data saved for device;"));
InCallibrationMode = false;
MPU9150::device_setup();
}
return; //prevents the normal read and reporting of IMU data
}
else if (command.cmp("i2cscan")){
// scan();
}
// MPU.selectDevice(MPUDeviceId);
MPU.read();
// {
// Serial.println(F("log:SwappingIMUAddress;"));
// MPUDeviceId = !MPUDeviceId;
// MPU.selectDevice(MPUDeviceId);
// if(!MPU.read()){
// Serial.println(F("log:Failed to read IMU on both addresses. Sleeping for 1 minute"));
// DidInit = false;
// }
// }
navdata::HDGD = MPU.m_fusedEulerPose[VEC3_Z] * RAD_TO_DEGREE;
//To convert to-180/180 to 0/360
if (navdata::HDGD < 0) navdata::HDGD+=360;
navdata::PITC = MPU.m_fusedEulerPose[VEC3_X] * RAD_TO_DEGREE;
navdata::ROLL = MPU.m_fusedEulerPose[VEC3_Y] * RAD_TO_DEGREE;
navdata::YAW = MPU.m_fusedEulerPose[VEC3_Z] * RAD_TO_DEGREE;
}
boolean MPU9150Lib::init(int mpuRate, int magMix)
{
struct int_param_s int_param;
int result;
long accelOffset[3];
m_magMix = magMix;
m_lastDMPYaw = 0;
m_lastYaw = 0;
// get calibration data if it's there
if (calLibRead(&m_calData)) { // use calibration data if it's there and wanted
m_useMagCalibration &= m_calData.magValid;
m_useAccelCalibration &= m_calData.accelValid;
// Process calibration data for runtime
if (m_useMagCalibration) {
m_magXOffset = (short)(((long)m_calData.magMaxX + (long)m_calData.magMinX) / 2);
m_magXRange = m_calData.magMaxX - m_magXOffset;
m_magYOffset = (short)(((long)m_calData.magMaxY + (long)m_calData.magMinY) / 2);
m_magYRange = m_calData.magMaxY - m_magYOffset;
m_magZOffset = (short)(((long)m_calData.magMaxZ + (long)m_calData.magMinZ) / 2);
m_magZRange = m_calData.magMaxZ - m_magZOffset;
}
if (m_useAccelCalibration) {
accelOffset[0] = -((long)m_calData.accelMaxX + (long)m_calData.accelMinX) / 2;
accelOffset[1] = -((long)m_calData.accelMaxY + (long)m_calData.accelMinY) / 2;
accelOffset[2] = -((long)m_calData.accelMaxZ + (long)m_calData.accelMinZ) / 2;
mpu_set_accel_bias(accelOffset);
m_accelXRange = m_calData.accelMaxX + accelOffset[0];
m_accelYRange = m_calData.accelMaxY + accelOffset[1];
m_accelZRange = m_calData.accelMaxZ + accelOffset[2];
}
}
#ifdef MPULIB_DEBUG
if (m_useMagCalibration)
Serial.println("Using mag cal");
if (m_useAccelCalibration)
Serial.println("Using accel cal");
#endif
mpu_init_structures();
// Not using interrupts so set up this structure to keep the driver happy
int_param.cb = NULL;
int_param.pin = 0;
int_param.lp_exit = 0;
int_param.active_low = 1;
result = mpu_init(&int_param);
if (result != 0) {
#ifdef MPULIB_DEBUG
Serial.print("mpu_init failed with code: "); Serial.println(result);
#endif
return false;
}
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // enable all of the sensors
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // get accel and gyro data in the FIFO also
mpu_set_sample_rate(mpuRate); // set the update rate
mpu_set_compass_sample_rate(mpuRate); // set the compass update rate to match
#ifdef MPULIB_DEBUG
Serial.println("Loading firmware");
#endif
if ((result = dmp_load_motion_driver_firmware()) != 0) { // try to load the DMP firmware
#ifdef MPULIB_DEBUG
Serial.print("Failed to load dmp firmware: ");
Serial.println(result);
#endif
return false;
}
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // set up the correct orientation
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL);
dmp_set_fifo_rate(mpuRate);
if (mpu_set_dmp_state(1) != 0) {
#ifdef MPULIB_DEBUG
Serial.println("mpu_set_dmp_state failed");
#endif
return false;
}
return true;
}