dikuei yen / Mbed 2 deprecated MTI_2_IMU

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:f2d1cdd67064, 2022-05-16 (annotated)

Committer:
dikueiyen
Date:
Mon May 16 13:20:05 2022 +0000
Revision:
1:f2d1cdd67064
Parent:
0:75b96455c9ac 
40>50;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dikueiyen 0:75b96455c9ac 1 #include "MTi2.h"
dikueiyen 0:75b96455c9ac 2 #include <stdio.h>
dikueiyen 0:75b96455c9ac 3 #include "mbed.h"
dikueiyen 0:75b96455c9ac 4 #include <math.h>
dikueiyen 0:75b96455c9ac 5 #include <stdlib.h>
dikueiyen 0:75b96455c9ac 6
dikueiyen 0:75b96455c9ac 7 #define GRAVITYACCELERATION 9.81f
dikueiyen 0:75b96455c9ac 8 #define pi 3.14159265358979323846f
dikueiyen 0:75b96455c9ac 9 SPI spi_MTI(PB_15, PB_14, PB_13);//MOSI MISO SCLK
dikueiyen 0:75b96455c9ac 10 Serial pc(USBTX,USBRX);
dikueiyen 0:75b96455c9ac 11 InterruptIn mybutton(USER_BUTTON);
dikueiyen 0:75b96455c9ac 12 Ticker main_function; //interrupt
dikueiyen 0:75b96455c9ac 13
dikueiyen 0:75b96455c9ac 14 DigitalOut led1(LED1);
dikueiyen 0:75b96455c9ac 15 DigitalOut cs_MTI(PC_4);
dikueiyen 0:75b96455c9ac 16
dikueiyen 0:75b96455c9ac 17 typedef union{
dikueiyen 0:75b96455c9ac 18 uint32_t data1;
dikueiyen 0:75b96455c9ac 19 float data2;
dikueiyen 0:75b96455c9ac 20 }imu_data;
dikueiyen 0:75b96455c9ac 21
dikueiyen 0:75b96455c9ac 22 imu_data eul[3];//euler angle
dikueiyen 0:75b96455c9ac 23 imu_data acc[3];
dikueiyen 0:75b96455c9ac 24 imu_data gry[3];
dikueiyen 0:75b96455c9ac 25
dikueiyen 0:75b96455c9ac 26 bool button_state = false;
dikueiyen 0:75b96455c9ac 27 float dt = 0.01; // sec
dikueiyen 0:75b96455c9ac 28
dikueiyen 0:75b96455c9ac 29 void step_command();
dikueiyen 0:75b96455c9ac 30 void Start_read();
dikueiyen 0:75b96455c9ac 31
dikueiyen 0:75b96455c9ac 32 int i = 1;
dikueiyen 0:75b96455c9ac 33
dikueiyen 0:75b96455c9ac 34
dikueiyen 0:75b96455c9ac 35 int main(void)
dikueiyen 0:75b96455c9ac 36 {
dikueiyen 0:75b96455c9ac 37 spi_MTI.format(8, 3);
dikueiyen 0:75b96455c9ac 38 pc.baud(230400);
dikueiyen 0:75b96455c9ac 39 MTi2_Init();
dikueiyen 0:75b96455c9ac 40 mybutton.fall(&step_command);
dikueiyen 0:75b96455c9ac 41
dikueiyen 0:75b96455c9ac 42 main_function.attach_us(&Start_read,dt*1000000);
dikueiyen 0:75b96455c9ac 43 while (1) {
dikueiyen 0:75b96455c9ac 44 //LL_mDelay(5);
dikueiyen 0:75b96455c9ac 45 //wait_us(5000); // ?
dikueiyen 0:75b96455c9ac 46
dikueiyen 0:75b96455c9ac 47 }
dikueiyen 0:75b96455c9ac 48
dikueiyen 0:75b96455c9ac 49 }
dikueiyen 0:75b96455c9ac 50
dikueiyen 0:75b96455c9ac 51 void Start_read() //interrupt function by TT
dikueiyen 0:75b96455c9ac 52 {
dikueiyen 0:75b96455c9ac 53 if (button_state == true)
dikueiyen 0:75b96455c9ac 54 {
dikueiyen 0:75b96455c9ac 55 ReadData();
dikueiyen 0:75b96455c9ac 56 // printf("euler %f,%f,%f\r\n",euler[0],euler[1],euler[2]);
dikueiyen 0:75b96455c9ac 57 //printf("%f,%f,%f,%f,%f,%f\r\n",accel[0],accel[1],accel[2],omega[0],omega[1],omega[2]);
dikueiyen 0:75b96455c9ac 58 printf("%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\r\n",euler[0],euler[1],euler[2],accel_[0],accel_[1],accel_[2],omega[0],omega[1],omega[2]);
dikueiyen 0:75b96455c9ac 59 //printf("omega %f,%f,%f\r\n",omega[0],omega[1],omega[2]);
dikueiyen 0:75b96455c9ac 60 }
dikueiyen 0:75b96455c9ac 61 }
dikueiyen 0:75b96455c9ac 62
dikueiyen 0:75b96455c9ac 63
dikueiyen 0:75b96455c9ac 64 void step_command(){
dikueiyen 0:75b96455c9ac 65 led1 = !led1;
dikueiyen 0:75b96455c9ac 66 button_state = !button_state;
dikueiyen 0:75b96455c9ac 67 }
dikueiyen 0:75b96455c9ac 68
dikueiyen 0:75b96455c9ac 69 void SendOpcode(uint8_t Opcode)
dikueiyen 0:75b96455c9ac 70 {
dikueiyen 0:75b96455c9ac 71 // printf("SendOpcode \r\n");
dikueiyen 0:75b96455c9ac 72 //LL_SPI_TransmitData8(SPI3, Opcode);
dikueiyen 0:75b96455c9ac 73 //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
dikueiyen 0:75b96455c9ac 74 //FW[0] = LL_SPI_ReceiveData8(SPI3);
dikueiyen 0:75b96455c9ac 75 FW[0] = spi_MTI.write(Opcode);
dikueiyen 0:75b96455c9ac 76
dikueiyen 0:75b96455c9ac 77 for(uint8_t i = 0;i<3;i++){// 3 fillword ?
dikueiyen 0:75b96455c9ac 78 //LL_SPI_TransmitData8(SPI3, i);
dikueiyen 0:75b96455c9ac 79 //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
dikueiyen 0:75b96455c9ac 80 //FW[i+1] = LL_SPI_ReceiveData8(SPI3);
dikueiyen 0:75b96455c9ac 81 FW[i+1] = spi_MTI.write(i);
dikueiyen 0:75b96455c9ac 82 }
dikueiyen 0:75b96455c9ac 83 }
dikueiyen 0:75b96455c9ac 84
dikueiyen 0:75b96455c9ac 85 uint8_t ReadProtInfo(){
dikueiyen 0:75b96455c9ac 86 len = 2;
dikueiyen 0:75b96455c9ac 87 //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
dikueiyen 0:75b96455c9ac 88 cs_MTI = 0;
dikueiyen 0:75b96455c9ac 89 SendOpcode(ProtInfo);//send opcode
dikueiyen 0:75b96455c9ac 90 for(int i = 0;i<len;i++){//read data
dikueiyen 0:75b96455c9ac 91 //LL_SPI_TransmitData8(SPI3, 0x00);
dikueiyen 0:75b96455c9ac 92 //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
dikueiyen 0:75b96455c9ac 93 //buffer[i] = LL_SPI_ReceiveData8(SPI3);
dikueiyen 0:75b96455c9ac 94 buffer[i] = spi_MTI.write(0x00);
dikueiyen 0:75b96455c9ac 95 }
dikueiyen 0:75b96455c9ac 96 //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
dikueiyen 0:75b96455c9ac 97 cs_MTI = 1;
dikueiyen 0:75b96455c9ac 98 if(FW[0]!=0xFA||FW[1]!=0xFF||FW[2]!=0xFF||FW[3]!=0xFF){
dikueiyen 0:75b96455c9ac 99 printf("Error!!\n");
dikueiyen 0:75b96455c9ac 100 }
dikueiyen 0:75b96455c9ac 101 return buffer[1];
dikueiyen 0:75b96455c9ac 102 }
dikueiyen 0:75b96455c9ac 103
dikueiyen 0:75b96455c9ac 104 void ConfigureProt(_Bool M,_Bool N,_Bool O,_Bool P)
dikueiyen 0:75b96455c9ac 105 {
dikueiyen 0:75b96455c9ac 106 // printf("ConfigureProt \r\n");
dikueiyen 0:75b96455c9ac 107 uint8_t config = (M<<3) | (N<<2) | (O<<1) | (P<<0);
dikueiyen 0:75b96455c9ac 108 //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
dikueiyen 0:75b96455c9ac 109 cs_MTI = 0;
dikueiyen 0:75b96455c9ac 110 SendOpcode(ConfigProt);
dikueiyen 0:75b96455c9ac 111 //LL_SPI_TransmitData8(SPI3, config);
dikueiyen 0:75b96455c9ac 112 spi_MTI.write(config);
dikueiyen 0:75b96455c9ac 113 //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
dikueiyen 0:75b96455c9ac 114 //LL_SPI_ReceiveData8(SPI3); ???
dikueiyen 0:75b96455c9ac 115 //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
dikueiyen 0:75b96455c9ac 116 cs_MTI = 1;
dikueiyen 0:75b96455c9ac 117 }
dikueiyen 0:75b96455c9ac 118
dikueiyen 0:75b96455c9ac 119 void PipeStatus(){
dikueiyen 0:75b96455c9ac 120 // printf("PipeStatus \r\n");
dikueiyen 0:75b96455c9ac 121 len = 4;
dikueiyen 0:75b96455c9ac 122 //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
dikueiyen 0:75b96455c9ac 123 cs_MTI = 0;
dikueiyen 0:75b96455c9ac 124 SendOpcode(PipeStat);//send opcode
dikueiyen 0:75b96455c9ac 125 for(int i = 0;i<len;i++){//read data
dikueiyen 0:75b96455c9ac 126 //LL_SPI_TransmitData8(SPI3, 0x00);
dikueiyen 0:75b96455c9ac 127 //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
dikueiyen 0:75b96455c9ac 128 //buffer[i] = LL_SPI_ReceiveData8(SPI3);
dikueiyen 0:75b96455c9ac 129 buffer[i] = spi_MTI.write(0x00);
dikueiyen 0:75b96455c9ac 130 }
dikueiyen 0:75b96455c9ac 131 //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
dikueiyen 0:75b96455c9ac 132 cs_MTI = 1;
dikueiyen 0:75b96455c9ac 133 notificationSize = buffer[0] | (buffer[1]<<8);
dikueiyen 0:75b96455c9ac 134 measurementSize = buffer[2] | (buffer[3]<<8);
dikueiyen 0:75b96455c9ac 135 // printf("AAAAAAAAAAAAAAAAA %d\r\n",measurementSize);
dikueiyen 0:75b96455c9ac 136 // printf("nSize:%d\r\n",notificationSize);
dikueiyen 0:75b96455c9ac 137 // printf("mSize:%d\n",measurementSize);
dikueiyen 0:75b96455c9ac 138 }
dikueiyen 0:75b96455c9ac 139
dikueiyen 0:75b96455c9ac 140 void NotificationPipe(){
dikueiyen 0:75b96455c9ac 141 //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
dikueiyen 0:75b96455c9ac 142 cs_MTI = 0;
dikueiyen 0:75b96455c9ac 143 SendOpcode(NotiPipe);//send opcode
dikueiyen 0:75b96455c9ac 144 for(int i = 0;i<notificationSize;i++){//read data
dikueiyen 0:75b96455c9ac 145 //LL_SPI_TransmitData8(SPI3, 0x00);
dikueiyen 0:75b96455c9ac 146 //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
dikueiyen 0:75b96455c9ac 147 //buffer[i] = LL_SPI_ReceiveData8(SPI3);
dikueiyen 0:75b96455c9ac 148 buffer[i] = spi_MTI.write(0x00);
dikueiyen 0:75b96455c9ac 149 }
dikueiyen 0:75b96455c9ac 150 //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
dikueiyen 0:75b96455c9ac 151 cs_MTI = 1;
dikueiyen 0:75b96455c9ac 152 }
dikueiyen 0:75b96455c9ac 153
dikueiyen 0:75b96455c9ac 154 void MeasurementPipe(){
dikueiyen 0:75b96455c9ac 155 // printf("MeasurementPipe \r\n");
dikueiyen 0:75b96455c9ac 156 //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
dikueiyen 0:75b96455c9ac 157 cs_MTI = 0;
dikueiyen 0:75b96455c9ac 158 SendOpcode(MeasPipe);//send opcode
dikueiyen 0:75b96455c9ac 159 for(int i = 0;i<measurementSize;i++){//read data
dikueiyen 0:75b96455c9ac 160 //LL_SPI_TransmitData8(SPI3, 0x00);
dikueiyen 0:75b96455c9ac 161 //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
dikueiyen 0:75b96455c9ac 162 //buffer[i] = LL_SPI_ReceiveData8(SPI3);
dikueiyen 0:75b96455c9ac 163 buffer[i] = spi_MTI.write(0x00);
dikueiyen 0:75b96455c9ac 164 }
dikueiyen 0:75b96455c9ac 165 //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
dikueiyen 0:75b96455c9ac 166 cs_MTI = 1;
dikueiyen 0:75b96455c9ac 167 }
dikueiyen 0:75b96455c9ac 168
dikueiyen 0:75b96455c9ac 169 void ControlPipe(){
dikueiyen 0:75b96455c9ac 170 //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
dikueiyen 0:75b96455c9ac 171 cs_MTI = 0;
dikueiyen 0:75b96455c9ac 172 SendOpcode(Control);//send opcode
dikueiyen 0:75b96455c9ac 173 for(int i = 0;i<ctrl_len;i++){//read data
dikueiyen 0:75b96455c9ac 174 //LL_SPI_TransmitData8(SPI3, ctrlBuf[i]);
dikueiyen 0:75b96455c9ac 175 //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
dikueiyen 0:75b96455c9ac 176 //buffer[i] = LL_SPI_ReceiveData8(SPI3);
dikueiyen 0:75b96455c9ac 177 buffer[i] = spi_MTI.write(ctrlBuf[i]);
dikueiyen 0:75b96455c9ac 178 }
dikueiyen 0:75b96455c9ac 179 //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
dikueiyen 0:75b96455c9ac 180 cs_MTI = 1;
dikueiyen 0:75b96455c9ac 181 }
dikueiyen 0:75b96455c9ac 182
dikueiyen 0:75b96455c9ac 183 void ReadData(){
dikueiyen 0:75b96455c9ac 184 // printf("ReadData \r\n");
dikueiyen 0:75b96455c9ac 185 PipeStatus();
dikueiyen 0:75b96455c9ac 186 //LL_mDelay(1);
dikueiyen 0:75b96455c9ac 187 wait_us(100);
dikueiyen 0:75b96455c9ac 188 MeasurementPipe();
dikueiyen 0:75b96455c9ac 189 int len1,len2,len3,data_bytes;
dikueiyen 0:75b96455c9ac 190 // printf("Measurement FINISH \r\n");
dikueiyen 0:75b96455c9ac 191 // printf("buffer[0] == %d \r\n",buffer[0]);
dikueiyen 0:75b96455c9ac 192 if(buffer[0] == 0x36){
dikueiyen 0:75b96455c9ac 193 // printf("buffer \r\n");
dikueiyen 0:75b96455c9ac 194 if(buffer[2]== 0x20&&buffer[3]== 0x30){//Read Euler Angle
dikueiyen 0:75b96455c9ac 195 len1 = buffer[4];
dikueiyen 0:75b96455c9ac 196 data_bytes = len1/3;
dikueiyen 0:75b96455c9ac 197 for(int j=0;j<3;j++){
dikueiyen 0:75b96455c9ac 198 uint32_t temp = (buffer[5+j*data_bytes]<<24) | (buffer[6+j*data_bytes]<<16) | (buffer[7+j*data_bytes]<<8) | (buffer[8+j*data_bytes]);
dikueiyen 0:75b96455c9ac 199 // float *ptr = NULL;
dikueiyen 0:75b96455c9ac 200 // ptr = &temp;
dikueiyen 0:75b96455c9ac 201 eul[j].data1 = temp;
dikueiyen 0:75b96455c9ac 202 // euler[j] = *ptr;
dikueiyen 0:75b96455c9ac 203 euler[j] = lpf(eul[j].data2, euler[j], 13.0f);
dikueiyen 0:75b96455c9ac 204 }
dikueiyen 0:75b96455c9ac 205 }
dikueiyen 0:75b96455c9ac 206 if(buffer[4+len1+1]== 0x40&&buffer[4+len1+2]== 0x20){
dikueiyen 0:75b96455c9ac 207 len2 = buffer[4+len1+3];
dikueiyen 0:75b96455c9ac 208 data_bytes = len2/3;
dikueiyen 0:75b96455c9ac 209 for(int j=0;j<3;j++){
dikueiyen 0:75b96455c9ac 210 // printf("A\r\n");
dikueiyen 0:75b96455c9ac 211 uint32_t temp = (buffer[8+len1+j*data_bytes]<<24) | (buffer[9+len1+j*data_bytes]<<16) | (buffer[10+len1+j*data_bytes]<<8) | (buffer[11+len1+j*data_bytes]);
dikueiyen 0:75b96455c9ac 212 // float *ptr = NULL;
dikueiyen 0:75b96455c9ac 213 // ptr = &(float*)temp;
dikueiyen 0:75b96455c9ac 214 acc[j].data1 = temp;
dikueiyen 0:75b96455c9ac 215 //accel[j] = acc[j].data2;
dikueiyen 0:75b96455c9ac 216 accel[j] = lpf(acc[j].data2, accel[j], 13.0f);
dikueiyen 0:75b96455c9ac 217 }
dikueiyen 0:75b96455c9ac 218 }
dikueiyen 0:75b96455c9ac 219 if(buffer[7+len1+len2+1]== 0x80&&buffer[7+len1+len2+2]==0x20){
dikueiyen 0:75b96455c9ac 220 len3 = buffer[7+len1+len2+3];
dikueiyen 0:75b96455c9ac 221 data_bytes = len3/3;
dikueiyen 0:75b96455c9ac 222 for(int j=0;j<3;j++){
dikueiyen 0:75b96455c9ac 223 uint32_t temp = (buffer[11+len1+len2+j*data_bytes]<<24) | (buffer[12+len1+len2+j*data_bytes]<<16) | (buffer[13+len1+len2+j*data_bytes]<<8) | (buffer[14+len1+len2+j*data_bytes]);
dikueiyen 0:75b96455c9ac 224 // float *ptr = NULL;
dikueiyen 0:75b96455c9ac 225 // ptr = (float)temp;
dikueiyen 0:75b96455c9ac 226 gry[j].data1 = temp;
dikueiyen 0:75b96455c9ac 227 //omega[j] = gry[j].data2;
dikueiyen 0:75b96455c9ac 228 omega[j] = lpf(gry[j].data2, omega[j], 13.0f);
dikueiyen 0:75b96455c9ac 229 }
dikueiyen 0:75b96455c9ac 230 }
dikueiyen 0:75b96455c9ac 231
dikueiyen 0:75b96455c9ac 232 }
dikueiyen 0:75b96455c9ac 233 // calculate
dikueiyen 0:75b96455c9ac 234 accel_[0] = (accel[0] + sin(euler[1]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[1]/180.0f*pi) * (-1.0f);//deal with gravity * tilt angle ; *-1 because IMU on robot is 180 degree reverse
dikueiyen 0:75b96455c9ac 235 accel_[1] = (accel[1] - sin(euler[0]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[0]/180.0f*pi) * (-1.0f);
dikueiyen 0:75b96455c9ac 236 accel_[2] = accel[2];
dikueiyen 0:75b96455c9ac 237 // printf("NO ??? \r\n");
dikueiyen 0:75b96455c9ac 238 }
dikueiyen 0:75b96455c9ac 239
dikueiyen 0:75b96455c9ac 240 void MTi2_Init(){
dikueiyen 0:75b96455c9ac 241 //LL_SPI_Enable(SPI3);//Enable SPI
dikueiyen 0:75b96455c9ac 242 // printf("Init \r\n");
dikueiyen 0:75b96455c9ac 243 cs_MTI = 1;///???
dikueiyen 0:75b96455c9ac 244 ConfigureProt(1,0,0,0);//Configure DRDY
dikueiyen 0:75b96455c9ac 245
dikueiyen 0:75b96455c9ac 246 //LL_mDelay(500);//Wait 500ms MTi2 go to measurement mode
dikueiyen 0:75b96455c9ac 247
dikueiyen 0:75b96455c9ac 248 }
dikueiyen 0:75b96455c9ac 249
dikueiyen 0:75b96455c9ac 250 float lpf(float input, float output_old, float frequency)
dikueiyen 0:75b96455c9ac 251 {
dikueiyen 0:75b96455c9ac 252 float output = 0;
dikueiyen 0:75b96455c9ac 253 output = (output_old + frequency * dt * input) / (1 + frequency * dt);
dikueiyen 0:75b96455c9ac 254 return output;
dikueiyen 0:75b96455c9ac 255 }