Skip to content

Commit

Permalink
Merge pull request #19 from ShimmerResearch/SHIM3-316
Browse files Browse the repository at this point in the history 
SHIM3-316  More efficient read of ICM20948 Magnetometer
  • Loading branch information
@marknolan
marknolan authored Jun 26, 2024
2 parents c838aa0 + e0824d2 commit ef0c3d7
Show file tree
Hide file tree
Showing 3 changed files with 157 additions and 54 deletions.
72 changes: 40 additions & 32 deletions LogAndStream/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -245,6 +245,7 @@ uint8_t dirName[64], expDirName[32], sdHeadText[SDHEAD_LEN],
uint16_t sdBuffLen, blockLen, fileNum, dirCounter, blinkCnt10, blinkCnt20,
blinkCnt50;
uint64_t firstTs, fileLastHour, fileLastMin;
volatile uint8_t currentSampleTsTicks[4];

volatile uint8_t fileBad, fileBadCnt, toggleLedRed;
static FRESULT ff_result;
Expand Down Expand Up @@ -2920,8 +2921,6 @@ uint8_t timeStampLen;
__interrupt void TIMER0_B0_ISR(void)
{
uint16_t timer_b0 = GetTB0();

uint8_t rtc_temp[4];
TB0CCR0 = timer_b0 + *(uint16_t*) (storedConfig + NV_SAMPLING_RATE);

if (!streamDataInProc)
Expand All @@ -2932,26 +2931,18 @@ __interrupt void TIMER0_B0_ISR(void)
{
firstTs = RTC_get64();
firstTsFlag = 2;
*(uint32_t*) rtc_temp = (uint64_t) firstTs;
}
else
{
*(uint32_t*) rtc_temp = RTC_get32();
}
if (currentBuffer)
{
//*((uint16_t *)(txBuff1+2)) = timer_b0; //the first two bytes are packet type bytes. reserved for BTstream
txBuff1[1] = rtc_temp[0];
txBuff1[2] = rtc_temp[1];
txBuff1[3] = rtc_temp[2];
*(uint32_t*) currentSampleTsTicks = (uint64_t) firstTs;
}
else
{
//*((uint16_t *)(txBuff0+2)) = timer_b0;
txBuff0[1] = rtc_temp[0];
txBuff0[2] = rtc_temp[1];
txBuff0[3] = rtc_temp[2];
*(uint32_t*) currentSampleTsTicks = RTC_get32();
}

uint8_t *current_buffer_ptr = currentBuffer ? txBuff1 : txBuff0;
// The first byte is packet type byte when Bluetooth streaming
current_buffer_ptr[1] = currentSampleTsTicks[0];
current_buffer_ptr[2] = currentSampleTsTicks[1];
current_buffer_ptr[3] = currentSampleTsTicks[2];
#else
if(currentBuffer)
{
Expand Down Expand Up @@ -7229,14 +7220,31 @@ void StreamData()
digi_offset += 6;
}

uint8_t icm20948MagBuf[6] = {0};
uint8_t icm20948MagBuf[ICM_MAG_RD_SIZE] = {0};
uint8_t icm20948MagRdy = 0;
if ((storedConfig[NV_SENSORS2] & SENSOR_MPU9X50_ICM20948_MAG)
|| (isWrAccelInUseIcm20948() && (storedConfig[NV_SENSORS0] & SENSOR_LSM303XXXX_MAG)))
{
if(icm20948MagRdy = ICM20948_isMagDataRdy())
if (ICM20948_isMagSampleSkipEnabled())
{
ICM20948_getMag(&icm20948MagBuf[0]);
/* This system tries to avoid lock-up scenario in the ICM20948 Mag
* (AK09916) in-which we see a 0.1 ms worth of repeated data samples
* if the chip was being read from too often. */
if(icm20948MagRdy = ICM20948_hasTimeoutPeriodPassed(*(uint32_t*)currentSampleTsTicks))
{
icm20948MagRdy = ICM20948_getMagAndStatus(*(uint32_t*)currentSampleTsTicks, &icm20948MagBuf[0]);
}
}
else
{
/* Original approach in-which the status 1 register is read first
* before reading the remaining bytes. This approach was found to
* work fine <512Hz but after that it would cause packet loss due
* to the length of time the I2C operations take to finish. */
if(icm20948MagRdy = ICM20948_isMagDataRdy())
{
ICM20948_getMag(&icm20948MagBuf[1]);
}
}
}

Expand Down Expand Up @@ -7276,14 +7284,14 @@ void StreamData()
{
if(icm20948MagRdy)
{
current_buffer_ptr[digi_offset + 0U] = icm20948MagBuf[0U];
current_buffer_ptr[digi_offset + 1U] = icm20948MagBuf[1U];
current_buffer_ptr[digi_offset + 0U] = icm20948MagBuf[ICM_MAG_IDX_XOUT_L];
current_buffer_ptr[digi_offset + 1U] = icm20948MagBuf[ICM_MAG_IDX_XOUT_H];

current_buffer_ptr[digi_offset + 2U] = icm20948MagBuf[2U];
current_buffer_ptr[digi_offset + 3U] = icm20948MagBuf[3U];
current_buffer_ptr[digi_offset + 2U] = icm20948MagBuf[ICM_MAG_IDX_YOUT_L];
current_buffer_ptr[digi_offset + 3U] = icm20948MagBuf[ICM_MAG_IDX_YOUT_H];

//Invert sign of uncalibrated Z-axis to match LSM303 chip placement
int16_t signInvertBuffer = - ((int16_t)((icm20948MagBuf[5U] << 8) | icm20948MagBuf[4U]));
int16_t signInvertBuffer = - ((int16_t)((icm20948MagBuf[ICM_MAG_IDX_ZOUT_H] << 8) | icm20948MagBuf[ICM_MAG_IDX_ZOUT_L]));
current_buffer_ptr[digi_offset + 4U] = signInvertBuffer & 0xFF;
current_buffer_ptr[digi_offset + 5U] = (signInvertBuffer >> 8) & 0xFF;
}
Expand Down Expand Up @@ -7329,12 +7337,12 @@ void StreamData()
{
if(icm20948MagRdy)
{
current_buffer_ptr[digi_offset + 0U] = icm20948MagBuf[0U];
current_buffer_ptr[digi_offset + 1U] = icm20948MagBuf[1U];
current_buffer_ptr[digi_offset + 2U] = icm20948MagBuf[2U];
current_buffer_ptr[digi_offset + 3U] = icm20948MagBuf[3U];
current_buffer_ptr[digi_offset + 4U] = icm20948MagBuf[4U];
current_buffer_ptr[digi_offset + 5U] = icm20948MagBuf[5U];
current_buffer_ptr[digi_offset + 0U] = icm20948MagBuf[ICM_MAG_IDX_XOUT_L];
current_buffer_ptr[digi_offset + 1U] = icm20948MagBuf[ICM_MAG_IDX_XOUT_H];
current_buffer_ptr[digi_offset + 2U] = icm20948MagBuf[ICM_MAG_IDX_YOUT_L];
current_buffer_ptr[digi_offset + 3U] = icm20948MagBuf[ICM_MAG_IDX_YOUT_H];
current_buffer_ptr[digi_offset + 4U] = icm20948MagBuf[ICM_MAG_IDX_ZOUT_L];
current_buffer_ptr[digi_offset + 5U] = icm20948MagBuf[ICM_MAG_IDX_ZOUT_H];
}
else
{
Expand Down
115 changes: 94 additions & 21 deletions LogAndStream/shimmer3_common_source/ICM20948/ICM20948.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,9 @@
#include "ICM20948.h"
#include "math.h"

uint8_t magSampleSkipEnabled;
uint32_t lastMagSampleTsTicks;

void ICM20948_bankSelect(uint8_t addr, uint8_t val)
{
uint8_t selectBank[2];
Expand Down Expand Up @@ -262,24 +265,37 @@ uint8_t ICM20948_getMagId(void)

void ICM20948_setMagSamplingRateFromShimmerRate(uint16_t samplingRateTicks)
{
magSampleSkipEnabled = 0;
lastMagSampleTsTicks = 0;

AK09916_opMode opMode = AK09916_CONT_MODE_100HZ;
// e.g., if Shimmer's sampling rate is faster then 50Hz, choose the next highest rate, i.e., 100Hz
if (samplingRateTicks <= 656) // ceil(32768/50Hz)
/* Choose the next highest sampling rate to the if Shimmer's sampling rate
* (e.g., if Shimmer's sampling rate is 9Hz, choose 10Hz). Note, choosing
* '>=' here as the comparison values are all floored. */
if (samplingRateTicks >= 3277) // ceil(32768/10Hz) = 3277. i.e., if <= 9.9994 Hz
{
opMode = AK09916_CONT_MODE_100HZ;
opMode = AK09916_CONT_MODE_10HZ;
}
else if (samplingRateTicks <= 1639) // ceil(32768/20Hz)
else if (samplingRateTicks >= 1639) // ceil(32768/20Hz) = 1639. i.e., if <= 19.9927 Hz
{
opMode = AK09916_CONT_MODE_50HZ;
opMode = AK09916_CONT_MODE_20HZ;
}
else if (samplingRateTicks <= 3277) // ceil(32768/10Hz)
else if (samplingRateTicks >= 656) // ceil(32768/50Hz) = 656. i.e., if <= 49.9512 Hz
{
opMode = AK09916_CONT_MODE_20HZ;
opMode = AK09916_CONT_MODE_50HZ;
}
else
{
opMode = AK09916_CONT_MODE_10HZ;
opMode = AK09916_CONT_MODE_100HZ;
/* If Shimmer sampling rate is >threshold, enable sample skip feature to
* avoid locking up AK09916 Magnetometer. */
// if (samplingRateTicks < SAMPLING_TIMER_TICKS_100Hz)
if (samplingRateTicks < SAMPLING_TIMER_TICKS_512Hz)
{
magSampleSkipEnabled = 1;
}
}

ICM20948_setMagMode(opMode);
}

Expand Down Expand Up @@ -312,23 +328,80 @@ uint8_t ICM20948_isMagDataRdy(void)
//either due to data read error or magnetic sensor overflow
void ICM20948_getMag(uint8_t *buf)
{
uint8_t status;

I2C_Set_Slave_Address(AK09916_MAG_ADDR);

*buf = ICM_MAG_XOUT_L;
I2C_Read_Packet_From_Sensor(buf, 6);
/* -1 here because we aren't reading the status 1 register in this read operation */
I2C_Read_Packet_From_Sensor(buf, (ICM_MAG_RD_SIZE-1));

//check status register
status = ICM_ST2;
I2C_Read_Packet_From_Sensor(&status, 1);
if (status&0x08)
//check Status 2 register
if (*(buf + ICM_MAG_IDX_ST2) & 0x08)
{
//either a read error or mag sensor overflow occurred
buf[ICM_MAG_IDX_XOUT_L] = 0xFF;
buf[ICM_MAG_IDX_XOUT_H] = 0x7F;
buf[ICM_MAG_IDX_YOUT_L] = 0xFF;
buf[ICM_MAG_IDX_YOUT_H] = 0x7F;
buf[ICM_MAG_IDX_ZOUT_L] = 0xFF;
buf[ICM_MAG_IDX_ZOUT_H] = 0x7F;
}
}

uint8_t ICM20948_isMagSampleSkipEnabled(void)
{
return magSampleSkipEnabled;
}

uint8_t ICM20948_hasTimeoutPeriodPassed(uint32_t currentSampleTsTicks)
{
//TODO make more efficient

/* Mask to 16-bit to simplify calculations */
uint32_t currentSampleTsTicksMasked = currentSampleTsTicks & 0xFFFF;
uint32_t lastMagSampleTsTicksMasked = lastMagSampleTsTicks & 0xFFFF;

// Check if roll-over has occurred
if (lastMagSampleTsTicksMasked > currentSampleTsTicksMasked)
{
currentSampleTsTicksMasked |= 0x10000;
}

uint32_t magSampleTsDiffTicks = currentSampleTsTicksMasked - lastMagSampleTsTicksMasked;
if (magSampleTsDiffTicks < SAMPLING_TIMER_TICKS_100Hz)
{
/* Mag won't have new sample ready yet so don't read from it */
return 0;
}

// Mag should have new sample ready
return 1;
}

uint8_t ICM20948_getMagAndStatus(uint32_t currentSampleTsTicks, uint8_t *buf)
{
I2C_Set_Slave_Address(AK09916_MAG_ADDR);

*buf = ICM_ST1;
I2C_Read_Packet_From_Sensor(buf, ICM_MAG_RD_SIZE);

// Check Status 1 if Data not ready
if (!(*(buf + ICM_MAG_IDX_ST1) & 0x01))
{
return 0;
}

lastMagSampleTsTicks = currentSampleTsTicks;

//check Status 2 register
if (*(buf + ICM_MAG_IDX_ST2) & 0x08)
{
//either a read error or mag sensor overflow occurred
buf[0] = 0xFF;
buf[1] = 0x7F;
buf[2] = 0xFF;
buf[3] = 0x7F;
buf[4] = 0xFF;
buf[5] = 0x7F;
buf[ICM_MAG_IDX_XOUT_L] = 0xFF;
buf[ICM_MAG_IDX_XOUT_H] = 0x7F;
buf[ICM_MAG_IDX_YOUT_L] = 0xFF;
buf[ICM_MAG_IDX_YOUT_H] = 0x7F;
buf[ICM_MAG_IDX_ZOUT_L] = 0xFF;
buf[ICM_MAG_IDX_ZOUT_H] = 0x7F;
}
return 1;
}
24 changes: 23 additions & 1 deletion LogAndStream/shimmer3_common_source/ICM20948/ICM20948.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,25 @@
#define CNTL2 0x31
#define CNTL3 0x32

typedef enum AK09916_OP_MODE {
#define SAMPLING_TIMER_TICKS_100Hz 328 // ceil(32768/100Hz) = 328. i.e., 99.90244Hz
#define SAMPLING_TIMER_TICKS_512Hz 64 // 32768/512Hz = 64

enum ICM_MAG_ALTERNATIVE_READ_IDX
{
ICM_MAG_IDX_ST1,
ICM_MAG_IDX_XOUT_L,
ICM_MAG_IDX_XOUT_H,
ICM_MAG_IDX_YOUT_L,
ICM_MAG_IDX_YOUT_H,
ICM_MAG_IDX_ZOUT_L,
ICM_MAG_IDX_ZOUT_H,
ICM_MAG_IDX_TMPS,
ICM_MAG_IDX_ST2,
ICM_MAG_RD_SIZE
};

typedef enum AK09916_OP_MODE
{
AK09916_PWR_DOWN = 0x00,
AK09916_TRIGGER_MODE = 0x01,
AK09916_CONT_MODE_10HZ = 0x02,
Expand Down Expand Up @@ -177,6 +195,10 @@ uint8_t ICM20948_isMagDataRdy(void);
//either due to data read error or magnetic sensor overflow
void ICM20948_getMag(uint8_t *buf);

uint8_t ICM20948_isMagSampleSkipEnabled(void);
uint8_t ICM20948_hasTimeoutPeriodPassed(uint32_t currentSampleTsTicks);
uint8_t ICM20948_getMagAndStatus(uint32_t currentSampleTsTicks, uint8_t *buf);

//read the x, y and z mag sensitivity adjustment values
void ICM20948_getMagSensitivityAdj(uint8_t *buf);

Expand Down

0 comments on commit ef0c3d7

Please sign in to comment.