From abe0d1a620a3c2d600c100141c530beaff0d0f85 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Mon, 26 Apr 2021 18:27:26 +0100 Subject: [PATCH] Initial commit with DMP support --- Firmware/OpenLog_Artemis/OpenLog_Artemis.ino | 190 ++++++- Firmware/OpenLog_Artemis/Sensors.ino | 104 +++- Firmware/OpenLog_Artemis/menuIMU.ino | 516 +++++++++++-------- Firmware/OpenLog_Artemis/menuMain.ino | 6 +- Firmware/OpenLog_Artemis/nvm.ino | 18 + Firmware/OpenLog_Artemis/settings.h | 6 + 6 files changed, 584 insertions(+), 256 deletions(-) diff --git a/Firmware/OpenLog_Artemis/OpenLog_Artemis.ino b/Firmware/OpenLog_Artemis/OpenLog_Artemis.ino index 9e931be..ef92edd 100644 --- a/Firmware/OpenLog_Artemis/OpenLog_Artemis.ino +++ b/Firmware/OpenLog_Artemis/OpenLog_Artemis.ino @@ -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 @@ -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 { diff --git a/Firmware/OpenLog_Artemis/Sensors.ino b/Firmware/OpenLog_Artemis/Sensors.ino index 891cfe6..99bf2a1 100644 --- a/Firmware/OpenLog_Artemis/Sensors.ino +++ b/Firmware/OpenLog_Artemis/Sensors.ino @@ -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 @@ -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 diff --git a/Firmware/OpenLog_Artemis/menuIMU.ino b/Firmware/OpenLog_Artemis/menuIMU.ino index c91af34..c0330fa 100644 --- a/Firmware/OpenLog_Artemis/menuIMU.ino +++ b/Firmware/OpenLog_Artemis/menuIMU.ino @@ -1,5 +1,7 @@ -void menuIMU() +// Return true if IMU requires a restart +bool menuIMU() { + bool restartIMU = false; while (1) { SerialPrintln(F("")); @@ -11,145 +13,171 @@ void menuIMU() if (settings.enableIMU == true) { - SerialPrint(F("2) Accelerometer Logging: ")); - if (settings.logIMUAccel) SerialPrintln(F("Enabled")); - else SerialPrintln(F("Disabled")); - - SerialPrint(F("3) Gyro Logging: ")); - if (settings.logIMUGyro) SerialPrintln(F("Enabled")); - else SerialPrintln(F("Disabled")); - - SerialPrint(F("4) Magnetometer Logging: ")); - if (settings.logIMUMag) SerialPrintln(F("Enabled")); - else SerialPrintln(F("Disabled")); - - SerialPrint(F("5) Temperature Logging: ")); - if (settings.logIMUTemp) SerialPrintln(F("Enabled")); - else SerialPrintln(F("Disabled")); - - if (online.IMU == true) + if (settings.imuUseDMP == false) { - SerialPrint(F("6) Accelerometer Full Scale: +/- ")); - switch (settings.imuAccFSS) - { - case 0: - SerialPrintln(F("2g")); - break; - case 1: - SerialPrintln(F("4g")); - break; - case 2: - SerialPrintln(F("8g")); - break; - case 3: - SerialPrintln(F("16g")); - break; - default: - SerialPrintln(F("UNKNOWN")); - break; - } - - SerialPrint(F("7) Accelerometer Digital Low Pass Filter: ")); - if (settings.imuAccDLPF) + SerialPrint(F("2) Accelerometer Logging: ")); + if (settings.logIMUAccel) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + + SerialPrint(F("3) Gyro Logging: ")); + if (settings.logIMUGyro) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + + SerialPrint(F("4) Magnetometer Logging: ")); + if (settings.logIMUMag) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + + SerialPrint(F("5) Temperature Logging: ")); + if (settings.logIMUTemp) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + + if (online.IMU == true) { - SerialPrintln(F("Enabled")); - SerialPrint(F("8) Accelerometer DLPF Bandwidth (Hz): ")); - switch (settings.imuAccDLPFBW) + SerialPrint(F("6) Accelerometer Full Scale: +/- ")); + switch (settings.imuAccFSS) { case 0: - SerialPrintln(F("246.0 (3dB) 265.0 (Nyquist)")); + SerialPrintln(F("2g")); break; case 1: - SerialPrintln(F("246.0 (3dB) 265.0 (Nyquist)")); + SerialPrintln(F("4g")); break; case 2: - SerialPrintln(F("111.4 (3dB) 136.0 (Nyquist)")); + SerialPrintln(F("8g")); break; case 3: - SerialPrintln(F("50.4 (3dB) 68.8 (Nyquist)")); - break; - case 4: - SerialPrintln(F("23.9 (3dB) 34.4 (Nyquist)")); - break; - case 5: - SerialPrintln(F("11.5 (3dB) 17.0 (Nyquist)")); - break; - case 6: - SerialPrintln(F("5.7 (3dB) 8.3 (Nyquist)")); - break; - case 7: - SerialPrintln(F("473 (3dB) 499 (Nyquist)")); + SerialPrintln(F("16g")); break; default: SerialPrintln(F("UNKNOWN")); break; } - } - else - { - SerialPrintln(F("Disabled (Bandwidth is 1209 Hz (3dB) 1248 Hz (Nyquist))")); - } - - SerialPrint(F("9) Gyro Full Scale: +/- ")); - switch (settings.imuGyroFSS) - { - case 0: - SerialPrintln(F("250dps")); - break; - case 1: - SerialPrintln(F("500dps")); - break; - case 2: - SerialPrintln(F("1000dps")); - break; - case 3: - SerialPrintln(F("2000dps")); - break; - default: - SerialPrintln(F("UNKNOWN")); - break; - } - - SerialPrint(F("10) Gyro Digital Low Pass Filter: ")); - if (settings.imuGyroDLPF) - { - SerialPrintln(F("Enabled")); - SerialPrint(F("11) Gyro DLPF Bandwidth (Hz): ")); - switch (settings.imuGyroDLPFBW) + + SerialPrint(F("7) Accelerometer Digital Low Pass Filter: ")); + if (settings.imuAccDLPF) + { + SerialPrintln(F("Enabled")); + SerialPrint(F("8) Accelerometer DLPF Bandwidth (Hz): ")); + switch (settings.imuAccDLPFBW) + { + case 0: + SerialPrintln(F("246.0 (3dB) 265.0 (Nyquist)")); + break; + case 1: + SerialPrintln(F("246.0 (3dB) 265.0 (Nyquist)")); + break; + case 2: + SerialPrintln(F("111.4 (3dB) 136.0 (Nyquist)")); + break; + case 3: + SerialPrintln(F("50.4 (3dB) 68.8 (Nyquist)")); + break; + case 4: + SerialPrintln(F("23.9 (3dB) 34.4 (Nyquist)")); + break; + case 5: + SerialPrintln(F("11.5 (3dB) 17.0 (Nyquist)")); + break; + case 6: + SerialPrintln(F("5.7 (3dB) 8.3 (Nyquist)")); + break; + case 7: + SerialPrintln(F("473 (3dB) 499 (Nyquist)")); + break; + default: + SerialPrintln(F("UNKNOWN")); + break; + } + } + else + { + SerialPrintln(F("Disabled (Bandwidth is 1209 Hz (3dB) 1248 Hz (Nyquist))")); + } + + SerialPrint(F("9) Gyro Full Scale: +/- ")); + switch (settings.imuGyroFSS) { case 0: - SerialPrintln(F("196.6 (3dB) 229.8 (Nyquist)")); + SerialPrintln(F("250dps")); break; case 1: - SerialPrintln(F("151.8 (3dB) 187.6 (Nyquist)")); + SerialPrintln(F("500dps")); break; case 2: - SerialPrintln(F("119.5 (3dB) 154.3 (Nyquist)")); + SerialPrintln(F("1000dps")); break; case 3: - SerialPrintln(F("51.2 (3dB) 73.3 (Nyquist)")); - break; - case 4: - SerialPrintln(F("23.9 (3dB) 35.9 (Nyquist)")); - break; - case 5: - SerialPrintln(F("11.6 (3dB) 17.8 (Nyquist)")); - break; - case 6: - SerialPrintln(F("5.7 (3dB) 8.9 (Nyquist)")); - break; - case 7: - SerialPrintln(F("361.4 (3dB) 376.5 (Nyquist)")); + SerialPrintln(F("2000dps")); break; default: SerialPrintln(F("UNKNOWN")); break; } + + SerialPrint(F("10) Gyro Digital Low Pass Filter: ")); + if (settings.imuGyroDLPF) + { + SerialPrintln(F("Enabled")); + SerialPrint(F("11) Gyro DLPF Bandwidth (Hz): ")); + switch (settings.imuGyroDLPFBW) + { + case 0: + SerialPrintln(F("196.6 (3dB) 229.8 (Nyquist)")); + break; + case 1: + SerialPrintln(F("151.8 (3dB) 187.6 (Nyquist)")); + break; + case 2: + SerialPrintln(F("119.5 (3dB) 154.3 (Nyquist)")); + break; + case 3: + SerialPrintln(F("51.2 (3dB) 73.3 (Nyquist)")); + break; + case 4: + SerialPrintln(F("23.9 (3dB) 35.9 (Nyquist)")); + break; + case 5: + SerialPrintln(F("11.6 (3dB) 17.8 (Nyquist)")); + break; + case 6: + SerialPrintln(F("5.7 (3dB) 8.9 (Nyquist)")); + break; + case 7: + SerialPrintln(F("361.4 (3dB) 376.5 (Nyquist)")); + break; + default: + SerialPrintln(F("UNKNOWN")); + break; + } + } + else + { + SerialPrintln(F("Disabled (Bandwidth is 12106 Hz (3dB) 12316 Hz (Nyquist))")); + } } - else - { - SerialPrintln(F("Disabled (Bandwidth is 12106 Hz (3dB) 12316 Hz (Nyquist))")); - } + } + + SerialPrint(F("12) Digital Motion Processor (DMP): ")); + if (settings.imuUseDMP) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + + if (settings.imuUseDMP == true) + { + SerialPrint(F("13) Game Rotation Vector (Quat6) Logging: ")); + if (settings.imuLogDMPQuat6) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + SerialPrint(F("14) Rotation Vector (Quat9) Logging: ")); + if (settings.imuLogDMPQuat9) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + SerialPrint(F("15) Accelerometer Logging: ")); + if (settings.imuLogDMPAccel) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + SerialPrint(F("16) Gyro Logging: ")); + if (settings.imuLogDMPGyro) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); + SerialPrint(F("17) Compass Logging: ")); + if (settings.imuLogDMPCpass) SerialPrintln(F("Enabled")); + else SerialPrintln(F("Disabled")); } } @@ -165,131 +193,200 @@ void menuIMU() } else if (settings.enableIMU == true) { - if (incoming == 2) - settings.logIMUAccel ^= 1; - else if (incoming == 3) - settings.logIMUGyro ^= 1; - else if (incoming == 4) - settings.logIMUMag ^= 1; - else if (incoming == 5) - settings.logIMUTemp ^= 1; - else if ((incoming == 6) && (online.IMU == true)) + if (settings.imuUseDMP == false) { - SerialPrintln(F("Enter Accelerometer Full Scale (0 to 3): ")); - SerialPrintln(F("0: +/- 2g")); - SerialPrintln(F("1: +/- 4g")); - SerialPrintln(F("2: +/- 8g")); - SerialPrintln(F("3: +/- 16g")); - int afs = getNumber(menuTimeout); //x second timeout - if (afs < 0 || afs > 3) - SerialPrintln(F("Error: Out of range")); - else + if (incoming == 2) + settings.logIMUAccel ^= 1; + else if (incoming == 3) + settings.logIMUGyro ^= 1; + else if (incoming == 4) + settings.logIMUMag ^= 1; + else if (incoming == 5) + settings.logIMUTemp ^= 1; + else if ((incoming == 6) && (online.IMU == true)) + { + SerialPrintln(F("Enter Accelerometer Full Scale (0 to 3): ")); + SerialPrintln(F("0: +/- 2g")); + SerialPrintln(F("1: +/- 4g")); + SerialPrintln(F("2: +/- 8g")); + SerialPrintln(F("3: +/- 16g")); + int afs = getNumber(menuTimeout); //x second timeout + if (afs < 0 || afs > 3) + SerialPrintln(F("Error: Out of range")); + else + { + settings.imuAccFSS = afs; + ICM_20948_fss_t FSS; + FSS.a = settings.imuAccFSS; + FSS.g = settings.imuGyroFSS; + ICM_20948_Status_e 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!")); + } + } + } + else if ((incoming == 7) && (online.IMU == true)) { - settings.imuAccFSS = afs; - ICM_20948_fss_t FSS; - FSS.a = settings.imuAccFSS; - FSS.g = settings.imuGyroFSS; - ICM_20948_Status_e retval = myICM.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS); + settings.imuAccDLPF ^= 1; + ICM_20948_Status_e retval = myICM.enableDLPF(ICM_20948_Internal_Acc, settings.imuAccDLPF); if (retval != ICM_20948_Stat_Ok) { SerialPrintln(F("Error: Could not configure the IMU!")); } } - } - else if ((incoming == 7) && (online.IMU == true)) - { - settings.imuAccDLPF ^= 1; - ICM_20948_Status_e retval = myICM.enableDLPF(ICM_20948_Internal_Acc, settings.imuAccDLPF); - if (retval != ICM_20948_Stat_Ok) + else if ((incoming == 8) && (online.IMU == true) && (settings.imuAccDLPF == true)) { - SerialPrintln(F("Error: Could not configure the IMU!")); + SerialPrintln(F("Enter Accelerometer DLPF Bandwidth (0 to 7): ")); + SerialPrintln(F("0: 246.0 (3dB) 265.0 (Nyquist) (Hz)")); + SerialPrintln(F("1: 246.0 (3dB) 265.0 (Nyquist) (Hz)")); + SerialPrintln(F("2: 111.4 (3dB) 136.0 (Nyquist) (Hz)")); + SerialPrintln(F("3: 50.4 (3dB) 68.8 (Nyquist) (Hz)")); + SerialPrintln(F("4: 23.9 (3dB) 34.4 (Nyquist) (Hz)")); + SerialPrintln(F("5: 11.5 (3dB) 17.0 (Nyquist) (Hz)")); + SerialPrintln(F("6: 5.7 (3dB) 8.3 (Nyquist) (Hz)")); + SerialPrintln(F("7: 473 (3dB) 499 (Nyquist) (Hz)")); + int afbw = getNumber(menuTimeout); //x second timeout + if (afbw < 0 || afbw > 7) + SerialPrintln(F("Error: Out of range")); + else + { + settings.imuAccDLPFBW = afbw; + ICM_20948_dlpcfg_t dlpcfg; + dlpcfg.a = settings.imuAccDLPFBW; + dlpcfg.g = settings.imuGyroDLPFBW; + ICM_20948_Status_e 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!")); + } + } } - } - else if ((incoming == 8) && (online.IMU == true) && (settings.imuAccDLPF == true)) - { - SerialPrintln(F("Enter Accelerometer DLPF Bandwidth (0 to 7): ")); - SerialPrintln(F("0: 246.0 (3dB) 265.0 (Nyquist) (Hz)")); - SerialPrintln(F("1: 246.0 (3dB) 265.0 (Nyquist) (Hz)")); - SerialPrintln(F("2: 111.4 (3dB) 136.0 (Nyquist) (Hz)")); - SerialPrintln(F("3: 50.4 (3dB) 68.8 (Nyquist) (Hz)")); - SerialPrintln(F("4: 23.9 (3dB) 34.4 (Nyquist) (Hz)")); - SerialPrintln(F("5: 11.5 (3dB) 17.0 (Nyquist) (Hz)")); - SerialPrintln(F("6: 5.7 (3dB) 8.3 (Nyquist) (Hz)")); - SerialPrintln(F("7: 473 (3dB) 499 (Nyquist) (Hz)")); - int afbw = getNumber(menuTimeout); //x second timeout - if (afbw < 0 || afbw > 7) - SerialPrintln(F("Error: Out of range")); - else + else if ((incoming == 9) && (online.IMU == true)) { - settings.imuAccDLPFBW = afbw; - ICM_20948_dlpcfg_t dlpcfg; - dlpcfg.a = settings.imuAccDLPFBW; - dlpcfg.g = settings.imuGyroDLPFBW; - ICM_20948_Status_e retval = myICM.setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg); - if (retval != ICM_20948_Stat_Ok) + SerialPrintln(F("Enter Gyro Full Scale (0 to 3): ")); + SerialPrintln(F("0: +/- 250dps")); + SerialPrintln(F("1: +/- 500dps")); + SerialPrintln(F("2: +/- 1000dps")); + SerialPrintln(F("3: +/- 2000dps")); + int gfs = getNumber(menuTimeout); //x second timeout + if (gfs < 0 || gfs > 3) + SerialPrintln(F("Error: Out of range")); + else { - SerialPrintln(F("Error: Could not configure the IMU!")); + settings.imuGyroFSS = gfs; + ICM_20948_fss_t FSS; + FSS.a = settings.imuAccFSS; + FSS.g = settings.imuGyroFSS; + ICM_20948_Status_e 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!")); + } } } - } - else if ((incoming == 9) && (online.IMU == true)) - { - SerialPrintln(F("Enter Gyro Full Scale (0 to 3): ")); - SerialPrintln(F("0: +/- 250dps")); - SerialPrintln(F("1: +/- 500dps")); - SerialPrintln(F("2: +/- 1000dps")); - SerialPrintln(F("3: +/- 2000dps")); - int gfs = getNumber(menuTimeout); //x second timeout - if (gfs < 0 || gfs > 3) - SerialPrintln(F("Error: Out of range")); - else + else if ((incoming == 10) && (online.IMU == true)) { - settings.imuGyroFSS = gfs; - ICM_20948_fss_t FSS; - FSS.a = settings.imuAccFSS; - FSS.g = settings.imuGyroFSS; - ICM_20948_Status_e retval = myICM.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS); + settings.imuGyroDLPF ^= 1; + ICM_20948_Status_e retval = myICM.enableDLPF(ICM_20948_Internal_Gyr, settings.imuGyroDLPF); if (retval != ICM_20948_Stat_Ok) { SerialPrintln(F("Error: Could not configure the IMU!")); } } - } - else if ((incoming == 10) && (online.IMU == true)) - { - settings.imuGyroDLPF ^= 1; - ICM_20948_Status_e retval = myICM.enableDLPF(ICM_20948_Internal_Gyr, settings.imuGyroDLPF); - if (retval != ICM_20948_Stat_Ok) + else if ((incoming == 11) && (online.IMU == true) && (settings.imuGyroDLPF == true)) + { + SerialPrintln(F("Enter Gyro DLPF Bandwidth (0 to 7): ")); + SerialPrintln(F("0: 196.6 (3dB) 229.8 (Nyquist) (Hz)")); + SerialPrintln(F("1: 151.8 (3dB) 187.6 (Nyquist) (Hz)")); + SerialPrintln(F("2: 119.5 (3dB) 154.3 (Nyquist) (Hz)")); + SerialPrintln(F("3: 51.2 (3dB) 73.3 (Nyquist) (Hz)")); + SerialPrintln(F("4: 23.9 (3dB) 35.9 (Nyquist) (Hz)")); + SerialPrintln(F("5: 11.6 (3dB) 17.8 (Nyquist) (Hz)")); + SerialPrintln(F("6: 5.7 (3dB) 8.9 (Nyquist) (Hz)")); + SerialPrintln(F("7: 361.4 (3dB) 376.5 (Nyquist) (Hz)")); + int gfbw = getNumber(menuTimeout); //x second timeout + if (gfbw < 0 || gfbw > 7) + SerialPrintln(F("Error: Out of range")); + else + { + settings.imuGyroDLPFBW = gfbw; + ICM_20948_dlpcfg_t dlpcfg; + dlpcfg.a = settings.imuAccDLPFBW; + dlpcfg.g = settings.imuGyroDLPFBW; + ICM_20948_Status_e 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!")); + } + } + } + else if (incoming == 12) { - SerialPrintln(F("Error: Could not configure the IMU!")); + settings.imuUseDMP ^= 1; + restartIMU = true; } + else if (incoming == STATUS_PRESSED_X) + break; + else if (incoming == STATUS_GETNUMBER_TIMEOUT) + break; + else + printUnknown(incoming); } - else if ((incoming == 11) && (online.IMU == true) && (settings.imuGyroDLPF == true)) + else if (settings.imuUseDMP == true) { - SerialPrintln(F("Enter Gyro DLPF Bandwidth (0 to 7): ")); - SerialPrintln(F("0: 196.6 (3dB) 229.8 (Nyquist) (Hz)")); - SerialPrintln(F("1: 151.8 (3dB) 187.6 (Nyquist) (Hz)")); - SerialPrintln(F("2: 119.5 (3dB) 154.3 (Nyquist) (Hz)")); - SerialPrintln(F("3: 51.2 (3dB) 73.3 (Nyquist) (Hz)")); - SerialPrintln(F("4: 23.9 (3dB) 35.9 (Nyquist) (Hz)")); - SerialPrintln(F("5: 11.6 (3dB) 17.8 (Nyquist) (Hz)")); - SerialPrintln(F("6: 5.7 (3dB) 8.9 (Nyquist) (Hz)")); - SerialPrintln(F("7: 361.4 (3dB) 376.5 (Nyquist) (Hz)")); - int gfbw = getNumber(menuTimeout); //x second timeout - if (gfbw < 0 || gfbw > 7) - SerialPrintln(F("Error: Out of range")); - else + if (incoming == 12) { - settings.imuGyroDLPFBW = gfbw; - ICM_20948_dlpcfg_t dlpcfg; - dlpcfg.a = settings.imuAccDLPFBW; - dlpcfg.g = settings.imuGyroDLPFBW; - ICM_20948_Status_e retval = myICM.setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg); - if (retval != ICM_20948_Stat_Ok) + settings.imuUseDMP ^= 1; + restartIMU = true; + } + else if (incoming == 13) + { + if (settings.imuLogDMPQuat6 == true) { - SerialPrintln(F("Error: Could not configure the IMU!")); + settings.imuLogDMPQuat6 = false; + } + else + { + settings.imuLogDMPQuat6 = true; + settings.imuLogDMPQuat9 = false; + } + restartIMU = true; + } + else if (incoming == 14) + { + if (settings.imuLogDMPQuat9 == true) + { + settings.imuLogDMPQuat9 = false; + } + else + { + settings.imuLogDMPQuat9 = true; + settings.imuLogDMPQuat6 = false; } + restartIMU = true; + } + else if (incoming == 15) + { + settings.imuLogDMPAccel ^= 1; + restartIMU = true; } + else if (incoming == 16) + { + settings.imuLogDMPGyro ^= 1; + restartIMU = true; + } + else if (incoming == 17) + { + settings.imuLogDMPCpass ^= 1; + restartIMU = true; + } + else if (incoming == STATUS_PRESSED_X) + break; + else if (incoming == STATUS_GETNUMBER_TIMEOUT) + break; + else + printUnknown(incoming); } else if (incoming == STATUS_PRESSED_X) break; @@ -305,4 +402,5 @@ void menuIMU() else printUnknown(incoming); } + return (restartIMU); } diff --git a/Firmware/OpenLog_Artemis/menuMain.ino b/Firmware/OpenLog_Artemis/menuMain.ino index 0cc2202..5809998 100644 --- a/Firmware/OpenLog_Artemis/menuMain.ino +++ b/Firmware/OpenLog_Artemis/menuMain.ino @@ -2,6 +2,7 @@ //If user doesn't respond within a few seconds, return to main loop void menuMain() { + bool restartIMU = false; while (1) { SerialPrintln(F("")); @@ -42,7 +43,7 @@ void menuMain() else if (incoming == '2') menuTimeStamp(); else if (incoming == '3') - menuIMU(); + restartIMU = menuIMU(); else if ((incoming == '4') && (settings.useTxRxPinsForTerminal == false)) menuSerialLogging(); else if (incoming == '5') @@ -161,6 +162,9 @@ void menuMain() configureQwiicDevices(); //Reconfigure the qwiic devices in case any settings have changed + if (restartIMU == true) + beginIMU(); // Restart the IMU if required + while (Serial.available()) Serial.read(); //Empty buffer of any newline chars if (settings.useTxRxPinsForTerminal == true) diff --git a/Firmware/OpenLog_Artemis/nvm.ino b/Firmware/OpenLog_Artemis/nvm.ino index 1d54337..6aebaa7 100644 --- a/Firmware/OpenLog_Artemis/nvm.ino +++ b/Firmware/OpenLog_Artemis/nvm.ino @@ -164,6 +164,12 @@ void recordSystemSettingsToFile() settingsFile.println("slowLoggingStartMOD=" + (String)settings.slowLoggingStartMOD); settingsFile.println("slowLoggingStopMOD=" + (String)settings.slowLoggingStopMOD); settingsFile.println("resetOnZeroDeviceCount=" + (String)settings.resetOnZeroDeviceCount); + settingsFile.println("imuUseDMP=" + (String)settings.imuUseDMP); + settingsFile.println("imuLogDMPQuat6=" + (String)settings.imuLogDMPQuat6); + settingsFile.println("imuLogDMPQuat9=" + (String)settings.imuLogDMPQuat9); + settingsFile.println("imuLogDMPAccel=" + (String)settings.imuLogDMPAccel); + settingsFile.println("imuLogDMPGyro=" + (String)settings.imuLogDMPGyro); + settingsFile.println("imuLogDMPCpass=" + (String)settings.imuLogDMPCpass); updateDataFileAccess(&settingsFile); // Update the file access time & date settingsFile.close(); } @@ -424,6 +430,18 @@ bool parseLine(char* str) { settings.slowLoggingStopMOD = d; else if (strcmp(settingName, "resetOnZeroDeviceCount") == 0) settings.resetOnZeroDeviceCount = d; + else if (strcmp(settingName, "imuUseDMP") == 0) + settings.imuUseDMP = d; + else if (strcmp(settingName, "imuLogDMPQuat6") == 0) + settings.imuLogDMPQuat6 = d; + else if (strcmp(settingName, "imuLogDMPQuat9") == 0) + settings.imuLogDMPQuat9 = d; + else if (strcmp(settingName, "imuLogDMPAccel") == 0) + settings.imuLogDMPAccel = d; + else if (strcmp(settingName, "imuLogDMPGyro") == 0) + settings.imuLogDMPGyro = d; + else if (strcmp(settingName, "imuLogDMPCpass") == 0) + settings.imuLogDMPCpass = d; else { SerialPrintf2("Unknown setting %s. Ignoring...\r\n", settingName); diff --git a/Firmware/OpenLog_Artemis/settings.h b/Firmware/OpenLog_Artemis/settings.h index 6f7f5a6..8f5ea01 100644 --- a/Firmware/OpenLog_Artemis/settings.h +++ b/Firmware/OpenLog_Artemis/settings.h @@ -392,6 +392,12 @@ struct struct_settings { int slowLoggingStartMOD = 1260; // Start slow logging at this many Minutes Of Day. Default to 21:00 int slowLoggingStopMOD = 420; // Stop slow logging at this many Minutes Of Day. Default to 07:00 bool resetOnZeroDeviceCount = false; // A work-around for I2C bus crashes. Enable this via the debug menu. + bool imuUseDMP = false; // If true, enable the DMP + bool imuLogDMPQuat6 = true; // If true, log INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR (Quat6) + bool imuLogDMPQuat9 = false; // If true, log INV_ICM20948_SENSOR_ROTATION_VECTOR (Quat9 + Heading Accuracy) + bool imuLogDMPAccel = false; // If true, log INV_ICM20948_SENSOR_RAW_ACCELEROMETER + bool imuLogDMPGyro = false; // If true, log INV_ICM20948_SENSOR_RAW_GYROSCOPE + bool imuLogDMPCpass = false; // If true, log INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED } settings; //These are the devices on board OpenLog that may be on or offline.