Skip to content

Merging DMP logging into release_candidate. Requires v1.2.6 of the ICM-20948 library for correct operation. #84

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
May 28, 2021
Merged

Merging DMP logging into release_candidate. Requires v1.2.6 of the ICM-20948 library for correct operation. #84

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
190 changes: 169 additions & 21 deletions Firmware/OpenLog_Artemis/OpenLog_Artemis.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -196,6 +196,7 @@ int charsReceived = 0; //Used for verifying/debugging serial reception
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
ICM_20948_SPI myICM;
icm_20948_DMP_data_t dmpData; // Global storage for the DMP data - extracted from the FIFO
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-

//Header files for all compatible Qwiic sensors
Expand Down Expand Up @@ -897,35 +898,182 @@ void beginIMU()
delay(1);
}

//Update the full scale and DLPF settings
ICM_20948_Status_e retval = myICM.enableDLPF(ICM_20948_Internal_Acc, settings.imuAccDLPF);
if (retval != ICM_20948_Stat_Ok)
bool success = true;

//Check if we are using the DMP
if (settings.imuUseDMP == false)
{
SerialPrintln(F("Error: Could not configure the IMU Accelerometer DLPF!"));
//Perform a full startup (not minimal) for non-DMP mode
ICM_20948_Status_e retval = myICM.startupDefault(false);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not startup the IMU in non-DMP mode!"));
success = false;
}
//Update the full scale and DLPF settings
retval = myICM.enableDLPF(ICM_20948_Internal_Acc, settings.imuAccDLPF);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not configure the IMU Accelerometer DLPF!"));
success = false;
}
retval = myICM.enableDLPF(ICM_20948_Internal_Gyr, settings.imuGyroDLPF);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not configure the IMU Gyro DLPF!"));
success = false;
}
ICM_20948_dlpcfg_t dlpcfg;
dlpcfg.a = settings.imuAccDLPFBW;
dlpcfg.g = settings.imuGyroDLPFBW;
retval = myICM.setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not configure the IMU DLPF BW!"));
success = false;
}
ICM_20948_fss_t FSS;
FSS.a = settings.imuAccFSS;
FSS.g = settings.imuGyroFSS;
retval = myICM.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not configure the IMU Full Scale!"));
success = false;
}
}
retval = myICM.enableDLPF(ICM_20948_Internal_Gyr, settings.imuGyroDLPF);
if (retval != ICM_20948_Stat_Ok)
else
{
SerialPrintln(F("Error: Could not configure the IMU Gyro DLPF!"));
// Initialize the DMP
ICM_20948_Status_e retval = myICM.initializeDMP();
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not startup the IMU in DMP mode!"));
success = false;
}
if (settings.imuLogDMPQuat6)
{
retval = myICM.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not enable the Game Rotation Vector (Quat6)!"));
success = false;
}
retval = myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0); // Set ODR to 55Hz
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not set the Quat6 ODR!"));
success = false;
}
}
if (settings.imuLogDMPQuat9)
{
retval = myICM.enableDMPSensor(INV_ICM20948_SENSOR_ROTATION_VECTOR);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not enable the Rotation Vector (Quat9)!"));
success = false;
}
retval = myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0); // Set ODR to 55Hz
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not set the Quat9 ODR!"));
success = false;
}
}
if (settings.imuLogDMPAccel)
{
retval = myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not enable the DMP Accelerometer!"));
success = false;
}
retval = myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0); // Set ODR to 55Hz
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not set the Accel ODR!"));
success = false;
}
}
if (settings.imuLogDMPGyro)
{
retval = myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not enable the DMP Gyroscope!"));
success = false;
}
retval = myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0); // Set ODR to 55Hz
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not set the Gyro ODR!"));
success = false;
}
retval = myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0); // Set ODR to 55Hz
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not set the Gyro Calibr ODR!"));
success = false;
}
}
if (settings.imuLogDMPCpass)
{
retval = myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED);
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not enable the DMP Compass!"));
success = false;
}
retval = myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0); // Set ODR to 55Hz
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not set the Compass ODR!"));
success = false;
}
retval = myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0); // Set ODR to 55Hz
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not set the Compass Calibr ODR!"));
success = false;
}
}
retval = myICM.enableFIFO(); // Enable the FIFO
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not enable the FIFO!"));
success = false;
}
retval = myICM.enableDMP(); // Enable the DMP
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not enable the DMP!"));
success = false;
}
retval = myICM.resetDMP(); // Reset the DMP
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not reset the DMP!"));
success = false;
}
retval = myICM.resetFIFO(); // Reset the FIFO
if (retval != ICM_20948_Stat_Ok)
{
SerialPrintln(F("Error: Could not reset the FIFO!"));
success = false;
}
}
ICM_20948_dlpcfg_t dlpcfg;
dlpcfg.a = settings.imuAccDLPFBW;
dlpcfg.g = settings.imuGyroDLPFBW;
retval = myICM.setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg);
if (retval != ICM_20948_Stat_Ok)

if (success)
{
SerialPrintln(F("Error: Could not configure the IMU DLPF BW!"));
online.IMU = true;
}
ICM_20948_fss_t FSS;
FSS.a = settings.imuAccFSS;
FSS.g = settings.imuGyroFSS;
retval = myICM.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS);
if (retval != ICM_20948_Stat_Ok)
else
{
SerialPrintln(F("Error: Could not configure the IMU Full Scale!"));
//Power down IMU
imuPowerOff();
online.IMU = false;
}

online.IMU = true;
}
else
{
Expand Down
104 changes: 79 additions & 25 deletions Firmware/OpenLog_Artemis/Sensors.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,37 +86,75 @@ void getData()
{
//printDebug("getData: online.IMU = " + (String)online.IMU + "\r\n");

if (myICM.dataReady())
if (settings.imuUseDMP == false)
{
//printDebug("getData: myICM.dataReady = " + (String)myICM.dataReady() + "\r\n");

myICM.getAGMT(); //Update values

if (settings.logIMUAccel)
if (myICM.dataReady())
{
//printDebug("getData: myICM.dataReady = " + (String)myICM.dataReady() + "\r\n");

myICM.getAGMT(); //Update values

if (settings.logIMUAccel)
{
sprintf(tempData, "%.2f,%.2f,%.2f,", myICM.accX(), myICM.accY(), myICM.accZ());
strcat(outputData, tempData);
}
if (settings.logIMUGyro)
{
sprintf(tempData, "%.2f,%.2f,%.2f,", myICM.gyrX(), myICM.gyrY(), myICM.gyrZ());
strcat(outputData, tempData);
}
if (settings.logIMUMag)
{
sprintf(tempData, "%.2f,%.2f,%.2f,", myICM.magX(), myICM.magY(), myICM.magZ());
strcat(outputData, tempData);
}
if (settings.logIMUTemp)
{
sprintf(tempData, "%.2f,", myICM.temp());
strcat(outputData, tempData);
}
}
//else
//{
// printDebug("getData: myICM.dataReady = " + (String)myICM.dataReady() + "\r\n");
//}
}
else
{
myICM.readDMPdataFromFIFO(&dmpData);
while (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)
{
myICM.readDMPdataFromFIFO(&dmpData); // Empty the FIFO - make sure data contains the most recent data
}
if (settings.imuLogDMPQuat6)
{
sprintf(tempData, "%.2f,%.2f,%.2f,", myICM.accX(), myICM.accY(), myICM.accZ());
sprintf(tempData, "%.3f,%.3f,%.3f,", ((double)dmpData.Quat6.Data.Q1) / 1073741824.0,
((double)dmpData.Quat6.Data.Q2) / 1073741824.0, ((double)dmpData.Quat6.Data.Q3) / 1073741824.0);
strcat(outputData, tempData);
}
if (settings.logIMUGyro)
if (settings.imuLogDMPQuat9)
{
sprintf(tempData, "%.2f,%.2f,%.2f,", myICM.gyrX(), myICM.gyrY(), myICM.gyrZ());
sprintf(tempData, "%.3f,%.3f,%.3f,%d,", ((double)dmpData.Quat9.Data.Q1) / 1073741824.0,
((double)dmpData.Quat9.Data.Q2) / 1073741824.0, ((double)dmpData.Quat9.Data.Q3) / 1073741824.0, dmpData.Quat9.Data.Accuracy);
strcat(outputData, tempData);
}
if (settings.logIMUMag)
if (settings.imuLogDMPAccel)
{
sprintf(tempData, "%.2f,%.2f,%.2f,", myICM.magX(), myICM.magY(), myICM.magZ());
sprintf(tempData, "%d,%d,%d,", dmpData.Raw_Accel.Data.X, dmpData.Raw_Accel.Data.Y, dmpData.Raw_Accel.Data.Z);
strcat(outputData, tempData);
}
if (settings.logIMUTemp)
if (settings.imuLogDMPGyro)
{
sprintf(tempData, "%.2f,", myICM.temp());
sprintf(tempData, "%d,%d,%d,", dmpData.Raw_Gyro.Data.X, dmpData.Raw_Gyro.Data.Y, dmpData.Raw_Gyro.Data.Z);
strcat(outputData, tempData);
}
if (settings.imuLogDMPCpass)
{
sprintf(tempData, "%d,%d,%d,", dmpData.Compass.Data.X, dmpData.Compass.Data.Y, dmpData.Compass.Data.Z);
strcat(outputData, tempData);
}
}
//else
//{
// printDebug("getData: myICM.dataReady = " + (String)myICM.dataReady() + "\r\n");
//}
}

//Append all external sensor data on linked list to outputData
Expand Down Expand Up @@ -887,14 +925,30 @@ void printHelperText(bool terminalOnly)

if (online.IMU)
{
if (settings.logIMUAccel)
strcat(helperText, "aX,aY,aZ,");
if (settings.logIMUGyro)
strcat(helperText, "gX,gY,gZ,");
if (settings.logIMUMag)
strcat(helperText, "mX,mY,mZ,");
if (settings.logIMUTemp)
strcat(helperText, "imu_degC,");
if (settings.imuUseDMP == false)
{
if (settings.logIMUAccel)
strcat(helperText, "aX,aY,aZ,");
if (settings.logIMUGyro)
strcat(helperText, "gX,gY,gZ,");
if (settings.logIMUMag)
strcat(helperText, "mX,mY,mZ,");
if (settings.logIMUTemp)
strcat(helperText, "imu_degC,");
}
else
{
if (settings.imuLogDMPQuat6)
strcat(helperText, "Q6_1,Q6_2,Q6_3,");
if (settings.imuLogDMPQuat9)
strcat(helperText, "Q9_1,Q9_2,Q9_3,HeadAcc,");
if (settings.imuLogDMPAccel)
strcat(helperText, "RawAX,RawAY,RawAZ,");
if (settings.imuLogDMPGyro)
strcat(helperText, "RawGX,RawGY,RawGZ,");
if (settings.imuLogDMPCpass)
strcat(helperText, "RawMX,RawMY,RawMZ,");
}
}

//Step through list, printing values as we go
Expand Down
Loading