Been completed most of the tasks in the Tiva TM4C Workshop that TI had kindly provided, controlled some motors using PWM and PID control and read sensors so I decided to jump in something "big" (for me apparently) - design a gimbaled system using IMU sensors.
The microcontroller I'm using is EK-TM4123GXL Evaluation Launchpad, which I use one I2C module to interface with the MPU-9250 breakout board which I purchased from this link: world.tmall.com/.../42322982187.htm
The MPU-9250 (develops from 9150 model) from Invensense is a 9-DOF (accelerometer, gyrometer, magnetometer XYZ) IMU sensor for orientation measure. Actually it consists of a MPU-6050 which is known as a popular 6-DOF IMU sensor and an AK8963 3-axis magnetometer so the configuration work in fact can be split into 2 ICs separately. The only interface standard that MPU9250 can communicate with other controller is I2C. The connection is shown below:
Tiva MPU-9250
3v3 VCC
GND GND
PA6 (I2C1-SCL) SCL
PA7 (I2C1-SDA) SDA
After hours of aquiring knowledge about measuring orientation angle (Roll, Pitch) using accelerometer data, correcting and de-noising these values using Kalman filter with gyrometer data I have achieved desired Roll and Pitch values from the MPU-6050 IC inside of MPU-9250. Next challenge will be acquiring raw magnetometer data from magnetometer and use another Kalman filter to estimate the Yaw angle (which, I've learned from internet, is such a pain because of noises).
First thing to do, like with MPU-6050, is to understand the register map of AK8963, which is kindly provided by Invensense at this link: store.invensense.com/ProductDetail/MPU9250-InvenSense-Inc/487537/
I then define the necessary registers as shown below:
// Read-only Reg
#define AK8963_I2C_ADDR 0x0C // slave address for the AK8963
#define AK8963_DEVICE_ID 0x48
#define AK8963_WIA 0x00
#define AK8963_INFO 0x01
#define AK8963_ST1 0x02 // data ready status bit 0
#define AK8963_XOUT_L 0x03 // data
#define AK8963_XOUT_H 0x04
#define AK8963_YOUT_L 0x05
#define AK8963_YOUT_H 0x06
#define AK8963_ZOUT_L 0x07
#define AK8963_ZOUT_H 0x08
#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC 0x0C // Self test control
#define AK8963_I2CDIS 0x0F // I2C disable
#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
// Write/Read Reg
#define AK8963_CNTL1 0x0A
#define AK8963_CNTL2 0x0B
#define AK8963_ASTC 0x0C
#define AK8963_TS1 0x0D
#define AK8963_TS2 0x0E
#define AK8963_I2CDIS 0x0F
// Read-only Reg ( ROM )
#define AK8963_ASAX 0x10
#define AK8963_ASAY 0x11
#define AK8963_ASAZ 0x12
It is noticeable that the address of AKB8963 is 0x0C (discriminate from MPU-6050 which is 0x68)
Then I reuse some of my old I2C read and write handlers (they're proofed to work well with the MPU-6050 config)
void i2cWriteData(uint8_t addr, uint8_t regAddr, uint8_t *data, uint8_t length) // write data with arbitrary length
{
I2CMasterSlaveAddrSet(I2C_PORT, addr, false); // Set to write mode
I2CMasterDataPut(I2C_PORT, regAddr); // Place address into data register // The first to be sent
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_BURST_SEND_START); // Send start condition
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
uint8_t i = 0;
for (i = 0; i < length - 1; i++) { // Just send n-1 data after
I2CMasterDataPut(I2C_PORT, data[i]); // Place data into data register
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_BURST_SEND_CONT); // Send continues condition
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
}
I2CMasterDataPut(I2C_PORT, data[length - 1]); // Place data into data register // The last to be sent
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_BURST_SEND_FINISH); // Send finish condition
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
}
void i2cWrite(uint8_t addr, uint8_t regAddr, uint8_t data) // write 1-byte data (Read handlers are the same)
{
i2cWriteData(addr, regAddr, &data, 1);
}
uint8_t i2cRead(uint8_t addr, uint8_t regAddr)
{
I2CMasterSlaveAddrSet(I2C_PORT, addr, false); // Set to write mode
I2CMasterDataPut(I2C_PORT, regAddr); // Place address into data register
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_SINGLE_SEND); // Send data - just the regAddr
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
I2CMasterSlaveAddrSet(I2C_PORT, addr, true); // Set to read mode
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_SINGLE_RECEIVE); // Tell master to read data
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
return I2CMasterDataGet(I2C_PORT); // Read data
}
void i2cReadData(uint8_t addr, uint8_t regAddr, uint8_t *data, uint8_t length)
{
I2CMasterSlaveAddrSet(I2C_PORT, addr, false); // Set to write mode
I2CMasterDataPut(I2C_PORT, regAddr); // Place address into data register
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_SINGLE_SEND); // Send data
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
I2CMasterSlaveAddrSet(I2C_PORT, addr, true); // Set to read mode
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_BURST_RECEIVE_START); // Send start condition
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
data[0] = I2CMasterDataGet(I2C_PORT); // Place data into data register
uint8_t i = 1;
for (i = 1; i < length - 1; i++) {
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_BURST_RECEIVE_CONT); // Send continues condition
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
data[i] = I2CMasterDataGet(I2C_PORT); // Place data into data register
}
I2CMasterControl(I2C_PORT, I2C_MASTER_CMD_BURST_RECEIVE_FINISH); // Send finish condition
while (I2CMasterBusy(I2C_PORT)); // Wait until transfer is done
data[length - 1] = I2CMasterDataGet(I2C_PORT); // Place data into data register
}
So now I'm ready to config the AK8963. The "destination" array variable contains the constant factory-calibrated values that can be use to correct raw data from magnetometer). the AK8963_MAGNET_RES macro is defined 0 if using 14-bit ADC and 1 if using 16-bit ADC (in this case = 1) and the AK8963_MAGNET_FREQ = 0x02 shown that the frequency of magnetic measurement is 8 Hz.
void initAK8963(float * destination)
{
// First extract the factory calibration for each magnetometer axis
uint8_t rawData[3]; // x/y/z gyro calibration data stored here
i2cWrite(AK8963_I2C_ADDR, AK8963_CNTL, 0x00); // Power down magnetometer
SysCtlDelay(8000);
i2cWrite(AK8963_I2C_ADDR, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
SysCtlDelay(8000);
i2cReadData(AK8963_I2C_ADDR, AK8963_ASAX, &rawData[0], 3); // Read the x-, y-, and z-axis calibration values
destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc.
destination[1] = (float)(rawData[1] - 128)/256. + 1.;
destination[2] = (float)(rawData[2] - 128)/256. + 1.;
i2cWrite(AK8963_I2C_ADDR, AK8963_CNTL, 0x00); // Power down magnetometer
SysCtlDelay(8000);
// Configure the magnetometer for continuous read and highest resolution
// set bit 4 of AK8963_CNTL reg to 1 (0) to enable 16 (14) bit resolution in CNTL register,
// and enable continuous mode data acquisition frequency (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
i2cWrite(AK8963_I2C_ADDR, AK8963_CNTL, (AK8963_MAGNET_RES << 4) | AK8963_MAGNET_FREQ); // Set magnetometer data resolution and sample ODR
SysCtlDelay(8000);
}
Depend on the choice of AK8963_MAGNET_RES (14 or 16 bits) I calculate the resolution (like the "destination" above, the "Mres" variable should unchange).
void GetMres()
{
switch (AK8963_MAGNET_RES)
{
// Possible magnetometer scales (and their register bit settings) are:
// 14 bit resolution (0) and 16 bit resolution (1)
case 0:
mRes = 10.*4912./8190.; // Proper scale to return milliGauss
break;
case 1:
mRes = 10.*4912./32760.0; // Proper scale to return milliGauss
break;
}
}
Now it's the time to get the raw data (really the same as accelerometer and gyrometer):
void ak8963_Read_MagnetXYZ(int16_t *x, int16_t *y, int16_t *z)
{
uint8_t m[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
if(i2cRead(AK8963_I2C_ADDR, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
i2cReadData(AK8963_I2C_ADDR, AK8963_XOUT_L, &m[0], 7); // Read the six raw data and ST2 registers sequentially into data array
int8_t c = m[6]; // End data read by reading ST2 register
if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
*x = ((int16_t)m[1] << 8) | m[0] ; // Turn the MSB and LSB into a signed 16-bit value
*y = ((int16_t)m[3] << 8) | m[2] ; // Data stored as little Endian
*z = ((int16_t)m[5] << 8) | m[4] ;
}
}
}
Now back to the main.c, I create the variables for the data and factory-calibrated value array
int16_t MagnetX, MagnetY, MagnetZ;
float magCalibration[3] = {0, 0, 0};
Then call the function (store the factory-calibrated values into magCalibration array):
initAK8963(magCalibration);
I create a 150ms interval timer to calculate the 'real' data after each time-out interrupt. These are the codes inside the interrupt handler (which is used to calculate Roll and Pitch Kalman filter at the same time)
int16_t MagnetX, MagnetY, MagnetZ;
extern float mRes;
extern float magCalibration[3];
double magX, magY, magZ;
ak8963_Read_MagnetXYZ(&MagnetX, &MagnetY, &MagnetZ);
GetMres();
magX = (double)MagnetX*mRes*magCalibration[0];
magY = (double)MagnetY*mRes*magCalibration[1];
magZ = (double)MagnetZ*mRes*magCalibration[2];
And the result is:
One of my friend used another scheme on this same module (to assure that none of them are malfunction) and produced non-zero value (which weren't proofed to be right at all). This is his result (notice that the factory-calibrated values, which should be the same, also differ from mine) (The "magCount" array values are equivalent to my MagnetX,Y,Z)
In case if needed, I provide the 1) Source code of 6 DOF Kalman filtered data (which worked); 2) The developing code to 9 DOF from 1); 3) The source code from my friend (with his approval to use) which produces 9 unfiltered data.
1)
2)
3)
Pardon me for any grammar issues, thank you very much.
