SonicMEMS@Cornell
/*============================================================================ MBED_MAX21105_2IMU_Burst Read data from 2 MAX21105 IMU Data return by serial command in burst mode Visarute Pinrod SonicMEMS Laboratory, ECE, Cornell University, Ithaca, NY, USA Novemver 16, 2015 ============================================================================*/
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:ee6b1953c19a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jan 25 03:44:30 2016 +0000
@@ -0,0 +1,455 @@
+/*============================================================================
+ MBED_MAX21105_2IMU_Burst
+ Read data from 2 MAX21105 IMU
+ Data return by serial command in burst mode
+ Visarute Pinrod
+ SonicMEMS Laboratory, ECE, Cornell University, Ithaca, NY, USA
+Novemver 16, 2015
+============================================================================*/
+
+// Import library
+#include "mbed.h"
+#include "SPI.h"
+#include "DigitalOut.h"
+
+// Define pinout
+SPI spi1(p5, p6, p7); //mosi, miso, sck
+SPI spi2(p11, p12, p13); //mosi, miso, sck
+DigitalOut cs_MAX21105(p8); //cs for MAX21105 6Dof IMU
+DigitalOut cs_MAX21105_2(p9); //cs for MAX21105 6Dof IMU
+DigitalOut cs_MCP4921(p14); //cs for MCP4921 DAC for heater
+DigitalOut cs_TSYS01st(p15); //cs for TSYS01 Temperature sensor for stage
+
+
+// Object
+Serial pc(USBTX, USBRX); // tx, rx
+Timer t;
+
+// get value from command format
+int getVal(char *cmd){
+ int val=-1;
+ char valC[4];
+ valC[0]=cmd[3];
+ valC[1]=cmd[4];
+ valC[2]=cmd[5];
+ valC[3]=cmd[6];
+ sscanf(valC,"%4x",&val);
+ return val;
+}
+
+// Start SPI connection
+void startSPI(){
+ spi1.format(8,3);
+ spi1.frequency(1000000);
+ spi2.format(8,3);
+ spi2.frequency(1000000);
+ cs_MAX21105 = 1;
+ cs_MAX21105_2 = 1;
+ cs_MCP4921 = 1;
+ cs_TSYS01st = 1;
+}
+
+// SPI write data to MAX21105
+void writeMAX21105(char addr, char value){
+ cs_MAX21105 = 0;
+ spi1.write(addr); // MAX21100 Writing scheme
+ spi1.write(value); // MAX21100 Writing scheme
+ cs_MAX21105 = 1;
+}
+
+// SPI read and return 8 bits data to MBED
+char readMAX21105(char addr){
+ char buf_u8;
+ cs_MAX21105 = 0;
+ spi1.write(0x80|addr); // MAX21100 Reading scheme
+ buf_u8 = spi1.write(0x0); // MAX21100 Reading scheme
+ cs_MAX21105 = 1;
+ return(buf_u8);
+}
+
+// SPI write data to MAX21105
+void writeMAX21105_2(char addr, char value){
+ cs_MAX21105_2 = 0;
+ spi2.write(addr); // MAX21100 Writing scheme
+ spi2.write(value); // MAX21100 Writing scheme
+ cs_MAX21105_2 = 1;
+}
+
+// SPI read and return 8 bits data to MBED
+char readMAX21105_2(char addr){
+ char buf_u8;
+ cs_MAX21105_2 = 0;
+ spi2.write(0x80|addr); // MAX21100 Reading scheme
+ buf_u8 = spi2.write(0x0); // MAX21100 Reading scheme
+ cs_MAX21105_2 = 1;
+ return(buf_u8);
+}
+
+void startMAX21105(){
+ // Check MAX21105 conditions
+ char whoami = readMAX21105(0x20); // WHOAMI register
+ char buf = readMAX21105(0x21); // SILICON_REV_OTP register
+
+ // Set MAX21105 options
+ writeMAX21105(0x00, 0x00); // SET_PWR 0x00 - Power down,
+ writeMAX21105(0x01, 0x03); // SNS_CFG_1 0x03 - LPF = 2kHz, SNS_DOUT_FSC=125 dps
+ writeMAX21105(0x02, 0x22); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=2000Hz
+ // writeMAX21105(0x02, 0x25); // SNS_CFG_2 0x25 - sns_gyr_ois_lpf=on, sns_odr=200Hz
+// writeMAX21105(0x02, 0x06); // SNS_CFG_2 0x06 - sns_odr=100Hz
+ writeMAX21105(0x03, 0x00); // SNS_CFG_3 0x00 - sns_hpf_co = 0.08 Hz
+ writeMAX21105(0x04, 0xC0); // SET_ACC_PWR 0xC0 - sns_acc_fsc=2g
+ writeMAX21105(0x05, 0x30); // ACC_CFG1 0x30 acc_odr = 2000 Hz
+ // writeMAX21105(0x05, 0x33); // ACC_CFG1 0x33 acc_odr = 200 Hz
+ // writeMAX21105(0x05, 0x01); // ACC_CFG1 0x01 - acc_odr = 800 Hz
+ writeMAX21105(0x06, 0x01); // ACC_CFG2 0x01 Unfiltered
+ writeMAX21105(0x00, 0x78); // POWER_CFG 0x78 - accelero,gyro low noise
+
+ writeMAX21105(0x13, 0x01); // SET_TEMP_DR 0x01 - enable temp, fine, dataready all
+ writeMAX21105(0x00, 0x78); // POWER_CFG 0x78 - accelero,gyro low noise
+
+ pc.printf("[IMU_INITIAL_ST]");
+ pc.printf("WHOAMI = 0x%X (MAX21105: B4)", whoami); //WhoAmI MAX21105 = B4
+ pc.printf("REVISION_ID = 0x%X", buf); // Should be 0x00
+ pc.printf("[IMU_INITIAL_ED]\n");
+}
+
+
+void startMAX21105_2(){
+ // Check MAX21105 conditions
+ char whoami = readMAX21105_2(0x20); // WHOAMI register
+ whoami = readMAX21105_2(0x20);
+ char buf = readMAX21105_2(0x21); // SILICON_REV_OTP register
+
+ // Set MAX21105 options
+ writeMAX21105_2(0x00, 0x00); // SET_PWR 0x00 - Power down,
+ writeMAX21105_2(0x01, 0x03); // SNS_CFG_1 0x03 - LPF = 2kHz, SNS_DOUT_FSC=125 dps
+ writeMAX21105_2(0x02, 0x22); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=2000Hz
+ writeMAX21105_2(0x03, 0x00); // SNS_CFG_3 0x00 - sns_hpf_co = 0.08 Hz
+ writeMAX21105_2(0x04, 0xC0); // SET_ACC_PWR 0xC0 - sns_acc_fsc=2g
+ writeMAX21105_2(0x05, 0x30); // ACC_CFG1 0x30 acc_odr = 2000 Hz
+ writeMAX21105_2(0x06, 0x01); // ACC_CFG2 0x01 Unfiltered
+ writeMAX21105_2(0x00, 0x78); // POWER_CFG 0x78 - accelero,gyro low noise
+ writeMAX21105_2(0x13, 0x01); // SET_TEMP_DR 0x01 - enable temp, fine, dataready all
+ writeMAX21105_2(0x00, 0x78); // POWER_CFG 0x78 - accelero,gyro low noise
+
+ pc.printf("[IMU_INITIAL_ST]");
+ pc.printf("WHOAMI = 0x%X (MAX21105: B4)", whoami); //WhoAmI MAX21105 = B4
+ pc.printf("REVISION_ID = 0x%X", buf); // Should be 0x00
+ pc.printf("[IMU_INITIAL_ED]\n");
+}
+
+// Read 6 axis data
+// See MAX21105 User guide for address and bit field description
+void readMAX21105All(char *imuBuf,short imuCnt){
+ // Variables for data from IMU
+ char con; // IMU status
+ short i; // Data point measurement loop variable
+ int time;
+ // Wait for connection and return data
+ t.start();
+ t.reset();
+ for(i=0;i<imuCnt;i++) {
+ while ((con = readMAX21105(0x23))==0); // wait until DATA_READY
+ imuBuf[0+18*i] = con;
+ imuBuf[1+18*i] = readMAX21105(0x24); // wX_H MSB Angular speed X
+ imuBuf[2+18*i] = readMAX21105(0x25); // wX_L LSB Angular speed X
+ imuBuf[3+18*i] = readMAX21105(0x26); // wY_H MSB Angular speed Y
+ imuBuf[4+18*i] = readMAX21105(0x27); // wY_L LSB Angular speed Y
+ imuBuf[5+18*i] = readMAX21105(0x28); // wZ_H MSB Angular speed Z
+ imuBuf[6+18*i] = readMAX21105(0x29); // wZ_L LSB Angular speed Z
+
+ imuBuf[7+18*i] = readMAX21105(0x2A); // aX_H MSB Acceleration X
+ imuBuf[8+18*i] = readMAX21105(0x2B); // aX_L LSB Acceleration X
+ imuBuf[9+18*i] = readMAX21105(0x2C); //aY_H MSB Acceleration Y
+ imuBuf[10+18*i] = readMAX21105(0x2D); //aY_L LSB Acceleration Y
+ imuBuf[11+18*i] = readMAX21105(0x2E); //aZ_H MSB Acceleration Z
+ imuBuf[12+18*i] = readMAX21105(0x2F); //aZ_L LSB Acceleration Z
+ imuBuf[13+18*i] = readMAX21105(0x30); //TEMP_H MSB Temperature
+ imuBuf[14+18*i] = readMAX21105(0x31); //TEMP_L LSB Temperature
+ time=t.read_us();
+ imuBuf[15+18*i] = (char)(time>>16&0xFF); //TEMP_L LSB Temperature
+ imuBuf[16+18*i] = (char)(time>>8&0xFF); //TEMP_L LSB Temperature
+ imuBuf[17+18*i] = (char)(time&0xFF); //TEMP_L LSB Temperature
+ }
+ t.stop();
+}
+
+// Send all axis data to PC
+void sendPCAll(char *imuBuf,short imuCnt){
+ pc.printf("[IMU_READALL_ST]");
+ // Variables for data from IMU
+ char con; // IMU status
+ short i; // Data point measurement loop variable
+
+ uint8_t wX_H, wX_L, wY_H, wY_L, wZ_H, wZ_L; // Angular speed MSB&LSB x,y,z
+ uint8_t aX_H, aX_L, aY_H, aY_L, aZ_H, aZ_L; // Acceleration MSB&LSB x,y,z
+ uint8_t temp_H, temp_L;
+ uint8_t time_H, time_M, time_L;
+ uint16_t wx, wy, wz, ax, ay, az, temp; // Full angular speed & acceleration
+ int time;
+
+ // Wait for connection and return data
+ for(i=0;i<imuCnt;i++) {
+ con=imuBuf[0+18*i];
+ wX_H = imuBuf[1+18*i]; // MSB Angular speed X
+ wX_L = imuBuf[2+18*i]; // LSB Angular speed X
+ wY_H = imuBuf[3+18*i]; // MSB Angular speed Y
+ wY_L = imuBuf[4+18*i]; // LSB Angular speed Y
+ wZ_H = imuBuf[5+18*i]; // MSB Angular speed Z
+ wZ_L = imuBuf[6+18*i]; // LSB Angular speed Z
+
+ aX_H = imuBuf[7+18*i]; // MSB Acceleration X
+ aX_L = imuBuf[8+18*i]; // LSB Acceleration X
+ aY_H = imuBuf[9+18*i]; // MSB Acceleration Y
+ aY_L = imuBuf[10+18*i]; // LSB Acceleration Y
+ aZ_H = imuBuf[11+18*i]; // MSB Acceleration Z
+ aZ_L = imuBuf[12+18*i]; // LSB Acceleration Z
+
+ temp_H = imuBuf[13+18*i]; // MSB Temperature
+ temp_L = imuBuf[14+18*i]; // LSB Temperature
+
+ time_H = imuBuf[15+18*i]; // MSB Time
+ time_M = imuBuf[16+18*i]; // middle Time
+ time_L = imuBuf[17+18*i]; // LSB Time
+
+ // Stack MSB and LSB together and send out thru serialPC
+ wx = (uint16_t)((uint16_t)wX_H<<8|(wX_L));
+ wy = (uint16_t)((uint16_t)wY_H<<8|(wY_L));
+ wz = (uint16_t)((uint16_t)wZ_H<<8|(wZ_L));
+ ax = (uint16_t)((uint16_t)aX_H<<8|(aX_L));
+ ay = (uint16_t)((uint16_t)aY_H<<8|(aY_L));
+ az = (uint16_t)((uint16_t)aZ_H<<8|(aZ_L));
+ temp = (uint16_t)((uint16_t)temp_H<<8|(temp_L));
+ time = (int)((int)time_H<<16|(int)time_M<<8|(time_L));
+ pc.printf("ST%01X%04X%04X%04X%04X%04X%04X%04X%06XED",
+ con, wx, wy, wz, ax, ay, az,temp,time);
+ }
+ pc.printf("[IMU_READALL_ED]");
+}
+
+void burstGMAX21105(){
+ writeMAX21105(0x02, 0x24); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=2000Hz
+ writeMAX21105(0x05, 0x32); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=2000Hz
+ writeMAX21105(0x02, 0x24); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=2000Hz
+ writeMAX21105(0x05, 0x32); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=2000Hz
+ char con; // IMU status
+ char wX_H, wX_L, wY_H, wY_L, wZ_H, wZ_L; // Angular speed MSB&LSB x,y,z
+ char aX_H, aX_L, aY_H, aY_L, aZ_H, aZ_L; // Accel MSB&LSB x,y,z
+ int time;
+ t.start();
+ t.reset();
+ while(1){
+ while ((con = readMAX21105(0x23))==0); // wait until DATA_READY
+ time=t.read_us();
+ wX_H = readMAX21105(0x24); // wX_H MSB Angular speed X
+ wX_L = readMAX21105(0x25); // wX_L LSB Angular speed X
+ wY_H = readMAX21105(0x26); // wY_H MSB Angular speed Y
+ wY_L = readMAX21105(0x27); // wY_L LSB Angular speed Y
+ wZ_H = readMAX21105(0x28); // wZ_H MSB Angular speed Z
+ wZ_L = readMAX21105(0x29); // wZ_L LSB Angular speed Z
+ aX_H = readMAX21105(0x2A); // aX_H MSB Acceleration X
+ aX_L = readMAX21105(0x2B); // aX_L LSB Acceleration X
+ aY_H = readMAX21105(0x2C); //aY_H MSB Acceleration Y
+ aY_L = readMAX21105(0x2D); //aY_L LSB Acceleration Y
+ aZ_H = readMAX21105(0x2E); //aZ_H MSB Acceleration Z
+ aZ_L = readMAX21105(0x2F); //aZ_L LSB Acceleration Z
+ pc.printf("ST%01X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%08XED\n",con,wX_H,wX_L,wY_H,wY_L,wZ_H,wZ_L,aX_H,aX_L,aY_H,aY_L,aZ_H,aZ_L,time);
+ }
+ t.stop();
+}
+
+void burstMAX21105_2(){
+ writeMAX21105(0x02, 0x24); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=400Hz
+ writeMAX21105(0x05, 0x32); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=400Hz
+ writeMAX21105(0x02, 0x24); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=400Hz
+ writeMAX21105(0x05, 0x32); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=400Hz
+ writeMAX21105_2(0x02, 0x24); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=400Hz
+ writeMAX21105_2(0x05, 0x32); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=400Hz
+ writeMAX21105_2(0x02, 0x24); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=400Hz
+ writeMAX21105_2(0x05, 0x32); // SNS_CFG_2 0x22 - sns_gyr_ois_lpf=on, sns_odr=400Hz
+
+ char con; // IMU status
+ char wX_H, wX_L, wY_H, wY_L, wZ_H, wZ_L; // Angular speed MSB&LSB x,y,z
+ char aX_H, aX_L, aY_H, aY_L, aZ_H, aZ_L; // Accel MSB&LSB x,y,z
+ int time;
+ t.start();
+ t.reset();
+ while(1){
+ con = readMAX21105(0x23);
+ if(con!=0){ // wait until DATA_READY
+ time=t.read_us();
+ wX_H = readMAX21105(0x24); // wX_H MSB Angular speed X
+ wX_L = readMAX21105(0x25); // wX_L LSB Angular speed X
+ wY_H = readMAX21105(0x26); // wY_H MSB Angular speed Y
+ wY_L = readMAX21105(0x27); // wY_L LSB Angular speed Y
+ wZ_H = readMAX21105(0x28); // wZ_H MSB Angular speed Z
+ wZ_L = readMAX21105(0x29); // wZ_L LSB Angular speed Z
+ aX_H = readMAX21105(0x2A); // aX_H MSB Acceleration X
+ aX_L = readMAX21105(0x2B); // aX_L LSB Acceleration X
+ aY_H = readMAX21105(0x2C); //aY_H MSB Acceleration Y
+ aY_L = readMAX21105(0x2D); //aY_L LSB Acceleration Y
+ aZ_H = readMAX21105(0x2E); //aZ_H MSB Acceleration Z
+ aZ_L = readMAX21105(0x2F); //aZ_L LSB Acceleration Z
+ pc.printf("ST1%01X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%08XED\n",con,wX_H,wX_L,wY_H,wY_L,wZ_H,wZ_L,aX_H,aX_L,aY_H,aY_L,aZ_H,aZ_L,time);
+ }
+
+ con = readMAX21105_2(0x23);
+ if(con!=0){ // wait until DATA_READY
+ time=t.read_us();
+ wX_H = readMAX21105_2(0x24); // wX_H MSB Angular speed X
+ wX_L = readMAX21105_2(0x25); // wX_L LSB Angular speed X
+ wY_H = readMAX21105_2(0x26); // wY_H MSB Angular speed Y
+ wY_L = readMAX21105_2(0x27); // wY_L LSB Angular speed Y
+ wZ_H = readMAX21105_2(0x28); // wZ_H MSB Angular speed Z
+ wZ_L = readMAX21105_2(0x29); // wZ_L LSB Angular speed Z
+ aX_H = readMAX21105_2(0x2A); // aX_H MSB Acceleration X
+ aX_L = readMAX21105_2(0x2B); // aX_L LSB Acceleration X
+ aY_H = readMAX21105_2(0x2C); //aY_H MSB Acceleration Y
+ aY_L = readMAX21105_2(0x2D); //aY_L LSB Acceleration Y
+ aZ_H = readMAX21105_2(0x2E); //aZ_H MSB Acceleration Z
+ aZ_L = readMAX21105_2(0x2F); //aZ_L LSB Acceleration Z
+ pc.printf("ST2%01X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%08XED\n",con,wX_H,wX_L,wY_H,wY_L,wZ_H,wZ_L,aX_H,aX_L,aY_H,aY_L,aZ_H,aZ_L,time);
+ }
+
+ }
+ t.stop();
+}
+
+void setMCP4921(int val){
+ cs_MCP4921 = 0; // Select the device by seting chip select low
+ spi1.write(0x70|(0xF&(val>>8)));// Set MCP4921 value. 0x7000 prefix
+ spi1.write(0xFF&val); // Set MCP4921 value. 0x7000 prefix
+ cs_MCP4921 = 1; // Deselect the device
+}
+
+void startTSYS01st(){
+ // Select the device by seting chip select low
+ cs_TSYS01st = 0;
+ spi1.write(0x1E); // Reset
+ cs_TSYS01st=1;
+ wait(0.1); //2.8 ms reload
+
+ uint8_t kH, kL, kA[8];
+ char i;
+
+ kA[0]=0xAA;
+ kA[1]=0xA8;
+ kA[2]=0xA6;
+ kA[3]=0xA4;
+ kA[4]=0xA2;
+ kA[5]=0xAC;
+ kA[6]=0xAE;
+ kA[7]=0xA0;
+ pc.printf("[TEMPS_COEFF_ST]");
+ for(i=0;i<8;i++){
+ cs_TSYS01st = 0;
+ spi1.write(kA[i]); // k0 read
+ kH=spi1.write(0x00); // k0_MSB
+ kL=spi1.write(0x00); // k0_LSB
+ pc.printf("%02X%02X",kH,kL);
+ cs_TSYS01st = 1;
+ }
+ pc.printf("[TEMPS_COEFF_ED]");
+}
+
+void readTSYS01st(){
+ uint8_t t[3];
+ cs_TSYS01st = 0;
+ spi1.write(0x48); // k0 read
+ wait(0.1);
+ cs_TSYS01st = 1;
+
+ cs_TSYS01st = 0;
+ spi1.write(0x00); // k0 read
+ t[0]=spi1.write(0x00); // k0 read
+ t[1]=spi1.write(0x00); // k0 read
+ t[2]=spi1.write(0x00); // k0 read
+ cs_TSYS01st = 1;
+ pc.printf("[TEMPS_VALUE_ST]");
+ pc.printf("%02X%02X%02X",t[0],t[1],t[2]);
+ pc.printf("[TEMPS_VALUE_ED]");
+}
+
+// Start connection
+void initialization(){
+ //pc.baud(921600);
+ pc.baud(460800);
+
+ startSPI(); // Start SPI connection
+ startMAX21105(); // Connect to MAX21105
+ startMAX21105_2();
+}
+
+int main(){
+ // Variable for main function
+ char buf[16]; // Buffer for the command from PC
+ char imuBuf[28800]; // Buffer for IMU output
+ short imuCnt=1600; // # of Point read from imu
+ int val; // Value from command
+
+ // Start system
+ initialization();
+ // Infinite loop: listen to serial command
+ while(1) {
+ if (pc.readable()) {
+ //Command format: Start Mode Submode VALUe enD S F F FFFF D
+ pc.gets(buf, 16);
+ //pc.printf("Get buf: %s\n",buf);
+ if ((buf[0] == 'S')&&(buf[7] == 'D')) {
+ //Mode switch
+ switch(buf[1]) {
+ //read all axis mode
+ case 'r':
+ // Submode switch
+ switch(buf[2]) {
+ case 'a':
+ readMAX21105All(imuBuf,imuCnt);
+ sendPCAll(imuBuf,imuCnt);
+ pc.printf("\n");
+ break;
+ case 'm':
+ readMAX21105All(imuBuf,imuCnt);
+ sendPCAll(imuBuf,imuCnt);
+ readTSYS01st();
+ pc.printf("\n");
+ break;
+ case 'b':
+ burstGMAX21105();
+ break;
+ case '2':
+ burstMAX21105_2();
+ break;
+ default:
+ break;
+ }
+ //stage temperature
+ case 's':
+ // Submode switch
+ switch(buf[2]) {
+ case 's':
+ startTSYS01st();
+ pc.printf("\n");
+ break;
+ case 'r':
+ readTSYS01st();
+ pc.printf("\n");
+ break;
+ default:
+ break;
+ }
+ break;
+ //set heater
+ case 'h':
+ val = getVal(buf);
+ if(val!=-1){
+ setMCP4921(val);
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ }
+ return 0;
+}
\ No newline at end of file