Hi. I'm developing IMU system for multiple purpose using ICM20948 & nRF52832.
But there are problems on TWI communications when using BLE simultaneously.
If not use BLE, all the data comes from IMU sensor is pretty well as shown below:
Small deviation is deliberate data for test purpose.
But after enable BLE, wrong data comes out from ICM20948 sensor as below. It is connected with TWI lines.
Only X-Axis are normal data. I attached source codes which get or send data to ICM20948 sensor.
Fullscreen
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/*
* Roverdyn I2C(TWI) Library
* Created at Jan. 14, 2020
* Author : Seonguk Jeong
* Copyright to 2019 Seonguk Jeong. All rights reserved.
* Contact : jswcomkr@roverdyn.com
*/
#include "Wire.h"
void _Wire::begin(){
// I2C 통신을 활성화 시키는 함수
// 기본 통신 속도는 400kHz
// 기본 설정 핀 : SCL(12번), SDA(11번)
// 설정 핀은 Wire.h 에서 바꿀 수 있음
// I2C 통신 비활성화
NRF_TWI0->ENABLE = 0;
// SCL/SDA핀 설정
NRF_TWI0->PSELSCL = pinSCL;
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
/*
* Roverdyn I2C(TWI) Library
* Created at Jan. 14, 2020
* Author : Seonguk Jeong
* Copyright to 2019 Seonguk Jeong. All rights reserved.
* Contact : jswcomkr@roverdyn.com
*/
#include "Wire.h"
void _Wire::begin(){
// I2C 통신을 활성화 시키는 함수
// 기본 통신 속도는 400kHz
// 기본 설정 핀 : SCL(12번), SDA(11번)
// 설정 핀은 Wire.h 에서 바꿀 수 있음
// I2C 통신 비활성화
NRF_TWI0->ENABLE = 0;
// SCL/SDA핀 설정
NRF_TWI0->PSELSCL = pinSCL;
NRF_TWI0->PSELSDA = pinSDA;
// 통신 주파수 설정(400kHz)
NRF_TWI0->FREQUENCY = 0x06680000;
// 인터럽트 비활성화
NRF_TWI0->INTENCLR = 0x044286;
// I2C 통신 활성화
NRF_TWI0->ENABLE = 5;
// 이벤트 초기화
NRF_TWI0->EVENTS_RXDREADY = 0;
NRF_TWI0->EVENTS_TXDSENT = 0;
}
uint8_t _Wire::requestFrom(uint8_t address, uint8_t bytes){
// Slave device로부터 데이터를 요청하는 함수
uint8_t recvLength = 0;
// 통신 주소 및 Read 신호 송신
NRF_TWI0->ADDRESS = address;
NRF_TWI0->TASKS_STARTRX = 1;
for(uint8_t i=0;i<bytes;i++){
// Byte Boundary 이벤트 확인
while(!NRF_TWI0->EVENTS_BB){}
NRF_TWI0->EVENTS_BB = 0;
// 마지막 데이터가 오면 STOP
if(i == (bytes - 1)){
NRF_TWI0->TASKS_STOP = 1;
}
// 데이터 읽기
while(!NRF_TWI0->EVENTS_RXDREADY){}
NRF_TWI0->EVENTS_RXDREADY = 0;
rxBuffer[i] = NRF_TWI0->RXD;
}
return recvLength;
}
void _Wire::beginTransmission(uint8_t address){
// TWI 데이터 전송을 시작하는 함수
NRF_TWI0->ENABLE = 5;
NRF_TWI0->ADDRESS = address;
NRF_TWI0->TASKS_STARTTX = 1;
memset(&rCount, 0x00, sizeof(rCount));
memset(&rxBuffer, 0x00, sizeof(rxBuffer));
rCount = 0;
}
uint8_t _Wire::endTransmission(bool toggle){
// TWI 통신 종료 함수
uint8_t err_code = 0;
if(toggle){
if(!NRF_TWI0->EVENTS_STOPPED){NRF_TWI0->TASKS_STOP = 1;}
while(!NRF_TWI0->EVENTS_STOPPED){}
NRF_TWI0->EVENTS_STOPPED = 0;
err_code = 1;
}
NRF_TWI0->ENABLE = 0;
return err_code;
}
uint8_t _Wire::write(uint8_t data){
NRF_TWI0->TXD = data;
while(!NRF_TWI0->EVENTS_TXDSENT){}
NRF_TWI0->EVENTS_TXDSENT = 0;
return 0;
}
uint8_t _Wire::available(){
return 0;
}
uint8_t _Wire::read(){
static uint8_t output = 0;
output = rxBuffer[rCount];
rCount++;
return output;
}
void _Wire::setClock(uint32_t frequency){
// I2C 통신 속도를 설정하는 함수
// 별도로 함수를 호출하지 않을 경우 400kHz로 동작
switch(frequency){
case 100000:
// 100Khz로 설정 시
NRF_TWI1->FREQUENCY = 0x01980000;
break;
case 250000:
// 250Khz로 설정 시
NRF_TWI1->FREQUENCY = 0x04000000;
break;
case 400000:
// 400Khz로 설정 시
NRF_TWI1->FREQUENCY = 0x06680000;
break;
default:
NRF_TWI1->FREQUENCY = 0x06680000;
break;
}
}
Fullscreen
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/*
* Roverdyn UART Library
* Created at Mar. 6, 2020
* Author : Seonguk Jeong
* Copyright to 2020 Seonguk Jeong. All rights reserved.
* Contact : jswcomkr@roverdyn.com
*/
#include "ICM20948.h"
#include <SEGGER_RTT.h>
_Wire Wire;
extern _Serial Serial;
uint8_t _ICM20948::Init(){
uint8_t err_code = 0;
// TWI 통신 초기화 및 시작
Wire.begin();
// ICM20948 초기화
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
/*
* Roverdyn UART Library
* Created at Mar. 6, 2020
* Author : Seonguk Jeong
* Copyright to 2020 Seonguk Jeong. All rights reserved.
* Contact : jswcomkr@roverdyn.com
*/
#include "ICM20948.h"
#include <SEGGER_RTT.h>
_Wire Wire;
extern _Serial Serial;
uint8_t _ICM20948::Init(){
uint8_t err_code = 0;
// TWI 통신 초기화 및 시작
Wire.begin();
// ICM20948 초기화
writeData(DEVICE_ID, REG_BANK_SEL, 0x00);
writeData(DEVICE_ID, PWR_MGMT_1, 0x80);
nrf_delay_ms(100);
writeData(DEVICE_ID, PWR_MGMT_1, 0x01);
writeData(DEVICE_ID, PWR_MGMT_2, 0x00);
// 바이패스 모드 설정
writeData(DEVICE_ID, INT_PIN_CFG, (0x01 << 1));
// Device ID 읽기
// 제대로 연결이 되어 있지 않으면 0x01(에러)를 리턴
if(getDeviceId() != 234){
return ROVERDYN_ERROR_NO_DATA;
}
if(getMagId() != 0x09){
return ROVERDYN_ERROR_NO_DATA;
}
/*
// Sample rate
writeData(DEVICE_ID, REG_BANK_SEL, 0x02);
writeData(DEVICE_ID, GYRO_SMPLRT_DIV, 0x01);
writeData(DEVICE_ID, ACCEL_SMPLRT_DIV_2, 0x01);
*/
// 자이로스코프 설정
setCfgGyro(DLPF_GYRO_6HZ, GYRO_RANGE_1000DPS, GYRO_AVG_1X, true);
// 가속도 센서 설정
setCfgAcc(DLPF_ACC_6HZ, ACCEL_RANGE_2G, ACCEL_AVG_4X, true);
// 지자기 센서 설정
setMagMode(MAG_CONTINUOUS_MODE2);
// 온도 센서 설정
setCfgTemp(TEMP_DLPF_34HZ);
return err_code;
}
uint8_t _ICM20948::writeData(uint8_t device, uint8_t addr, uint8_t data){
// 레지스터에 값을 쓰는 함수
uint8_t err_code = 0;
// 통신 시작
Wire.beginTransmission(device);
Wire.write(addr);
Wire.write(data);
Wire.endTransmission(true);
return err_code;
}
uint8_t _ICM20948::getDeviceId(){
// Who AM I 레지스터로부터 데이터를 읽어오는 함수
uint8_t output = 0;
Wire.beginTransmission(DEVICE_ID);
Wire.write(WHO_AM_I);
Wire.requestFrom(DEVICE_ID, 1);
output = Wire.read();
Wire.endTransmission(true);
if(output != 234){
return ROVERDYN_ERROR_NO_DATA;
}
return output;
}
uint8_t _ICM20948::getMagId(){
// 지자기 센서의 WIA 레지스터로부터 데이터를 읽어오는 함수
uint8_t output = 0;
// Get Data
Wire.beginTransmission(AK09916);
Wire.write(WIA2);
Wire.requestFrom(AK09916, 1);
output = Wire.read();
Wire.endTransmission(true);
return output;
}
uint8_t _ICM20948::setCfgAcc(uint8_t ACCEL_DLPF_SEL, uint8_t ACCEL_FS_SEL, uint8_t ACCEL_AVG_SEL, bool Fchoice_b){
// 가속도 센서의 측정 범위와 DLPF 적용 유무 설정 함수
uint8_t err_code = 0;
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x20);
// Set Accel Config 1
uint8_t data = (ACCEL_DLPF_SEL << 3) | (ACCEL_FS_SEL << 1) | (Fchoice_b);
writeData(DEVICE_ID, ACCEL_CONFIG, data);
// Set Accel Config 2
writeData(DEVICE_ID, ACCEL_CONFIG_2, ACCEL_FS_SEL);
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x00);
return err_code;
}
uint8_t _ICM20948::setCfgGyro(uint8_t GYRO_DLPF_SEL, uint8_t GYRO_FS_SEL, uint8_t GYRO_AVG_SEL, bool Fchoice_b){
// 자이로스코프의 측정 범위와 DLPF 적용 유무 설정 함수
uint8_t err_code = 0;
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x20);
// Set Gyro Config 1
uint8_t data = (GYRO_DLPF_SEL << 3) | (GYRO_FS_SEL << 1) | (Fchoice_b);
writeData(DEVICE_ID, GYRO_CONFIG_1, data);
// Set Gyro Config 2
writeData(DEVICE_ID, GYRO_CONFIG_2, GYRO_AVG_SEL);
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x00);
return err_code;
}
uint8_t _ICM20948::setCfgTemp(uint8_t TEMP_DLPF_SET){
// 온도 센서의 DLPF 설정
uint8_t err_code = 0;
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x20);
// Set DLPF
writeData(DEVICE_ID, TEMP_CONFIG, TEMP_DLPF_SET);
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x00);
return err_code;
}
uint8_t _ICM20948::setMagMode(uint8_t MODE){
// 지자기 센서 설정 함수
uint8_t err_code = 0;
// Mode 설정
writeData(AK09916, CNTL2, MODE);
return err_code;
}
int16_t* _ICM20948::getMagASA(){
// 지자기 센서 ASA 데이터 읽기 함수
static int16_t output[3];
return output;
}
int16_t* _ICM20948::getRawAcc(){
// 가속도 센서 Raw 데이터 읽기 함수
static int16_t output[3] = {0};
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x00);
// Get Data
Wire.beginTransmission(DEVICE_ID);
Wire.write(ACCEL_XOUT_H);
Wire.requestFrom(DEVICE_ID, 6);
for(uint8_t i=0;i<3;i++){
output[i] = Wire.read() << 8 | Wire.read();
}
Wire.endTransmission(true);
return output;
}
int16_t* _ICM20948::getRawGyro(){
// 자이로스코프 Raw 데이터 읽기 함수
static int16_t output[3] = {0};
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x00);
// Get Data
Wire.beginTransmission(DEVICE_ID);
Wire.write(GYRO_XOUT_H);
Wire.requestFrom(DEVICE_ID, 6);
for(uint8_t i=0;i<3;i++){
output[i] = Wire.read() << 8 | Wire.read();
}
Wire.endTransmission(true);
return output;
}
int16_t* _ICM20948::getRawMag(){
// 지자기 센서 Raw 데이터 취득 함수
static int16_t output[3] = {0};
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x00);
// Get Data
Wire.beginTransmission(AK09916);
Wire.write(ST1);
Wire.requestFrom(AK09916, 1);
bool st1 = Wire.read() & 0x01;
Wire.endTransmission(true);
if(st1){
// Read Mag Data
Wire.beginTransmission(AK09916);
Wire.write(HXL);
Wire.requestFrom(AK09916, 6);
for(uint8_t i=0;i<3;i++){
output[i] = Wire.read();
output[i] = (Wire.read() << 8) | output[i];
}
Wire.endTransmission(true);
// Read ST2 register
Wire.beginTransmission(AK09916);
Wire.write(ST2);
Wire.requestFrom(AK09916, 1);
bool st2 = (Wire.read() >> 3) & 0x01;
if(st2){
return 0;
}
Wire.endTransmission(true);
}
return output;
}
float _ICM20948::getTemp(float TEMP_OFFSET){
// 온도 데이터 수신 함수
float output = 0;
SEGGER_RTT_printf(0, "TEST\n");
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x00);
// Get Data
Wire.beginTransmission(DEVICE_ID);
Wire.write(TEMP_OUT_H);
Wire.requestFrom(DEVICE_ID, 2);
output = Wire.read() << 8 | Wire.read();
Wire.endTransmission(true);
// 측정한 데이터를 Degree 단위로 변환
output = ((output - TEMP_OFFSET)/TEMP_SENSITIVITY) + 21.0f;
return output;
}
uint8_t _ICM20948::calibrateAccel(){
// 가속도 센서 보정 함수
uint8_t err_code = 0;
uint16_t count = 0, itinary = 2000;
int16_t avg[3] = {0}, accOffsetDefault[3] = {0};
// Low 레지스터 1번 비트 받아옴
// Select Bank
writeData(DEVICE_ID, REG_BANK_SEL, 0x10);
// Get Data
for(uint8_t i=0;i<3;i++){
Wire.beginTransmission(DEVICE_ID);
Wire.write(XA_OFFS_H + i);
Wire.requestFrom(DEVICE_ID, 2);
accOffsetDefault[i] = Wire.read() << 8 | Wire.read();
Wire.endTransmission(true);
}
// 초기값 평가
int16_t *ACCEL;
do{
ACCEL = getRawAcc();
count++;
for(uint8_t i=0;i<3;i++){
avg[i] = (float)((float)( (float)(count - 1)/(float)count ) * (float)avg[i - 1]) + ((float)((float)ACCEL[i] / count));
}
}while(count <= itinary);
while(1){
}
return err_code;
}
uint8_t _ICM20948::calibrateGyro(){
// 자이로스코프 보정 함수
uint8_t err_code = 0;
return err_code;
}
uint8_t _ICM20948::calibrateMag(){
// 지자기센서 보정 함수
uint8_t err_code = 0;
return err_code;
}
Please check it.
Thank you.
