Idris Al Muhajiri / Mbed 2 deprecated K64F_ADIS16407_AHRS

I'm trying to make an AHRS with ADIS14607 IMU sensor and Freescale K64F processor

Dependencies:   mbed

Fork of SPI_Master_Example_SerialComm by IEEE TAMU MCC

main.cpp@1:939932df321d, 2015-08-27 (annotated)

Committer:
Idoriz
Date:
Thu Aug 27 06:07:46 2015 +0000
Revision:
1:939932df321d
Parent:
0:c48a4735d25f 
Until conversion of RAW data sensor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MoffMade 0:c48a4735d25f 1 #include "mbed.h"
Idoriz 1:939932df321d 2
Idoriz 1:939932df321d 3 // Address untuk masing-masing data dari IMU ADIS16407
Idoriz 1:939932df321d 4 #define SUPPLY_ADD 0x0200
Idoriz 1:939932df321d 5 #define XGYRO_ADD 0x0400
Idoriz 1:939932df321d 6 #define YGYRO_ADD 0x0600
Idoriz 1:939932df321d 7 #define ZGYRO_ADD 0x0800
Idoriz 1:939932df321d 8 #define XACCL_ADD 0x0A00
Idoriz 1:939932df321d 9 #define YACCL_ADD 0x0C00
Idoriz 1:939932df321d 10 #define ZACCL_ADD 0x0E00
Idoriz 1:939932df321d 11 #define XMAGN_ADD 0x1000
Idoriz 1:939932df321d 12 #define YMAGN_ADD 0x1200
Idoriz 1:939932df321d 13 #define ZMAGN_ADD 0x1400
Idoriz 1:939932df321d 14 #define BAROH_ADD 0x1600
Idoriz 1:939932df321d 15 #define BAROL_ADD 0x1800
Idoriz 1:939932df321d 16 #define TEMP_ADD 0x1A00
Idoriz 1:939932df321d 17 #define AUX_ADD 0x1C00
Idoriz 1:939932df321d 18 // Address untuk burst mode
Idoriz 1:939932df321d 19 #define BURST_ADD 0x4200
Idoriz 1:939932df321d 20
Idoriz 1:939932df321d 21 // Nilai sensor per LSB/resolusi
Idoriz 1:939932df321d 22 #define GYRO_LSB 0.05f
Idoriz 1:939932df321d 23 #define ACCL_LSB 0.003333f
Idoriz 1:939932df321d 24 #define MAGN_LSB 0.0005f
Idoriz 1:939932df321d 25 #define SUPPLY_LSB 0.002418f
Idoriz 1:939932df321d 26 #define TEMP_LSB 0.136f
Idoriz 1:939932df321d 27
MoffMade 0:c48a4735d25f 28
Idoriz 1:939932df321d 29 SPI spi(PTD2, PTD3, PTD1); // MOSI, MISO, SCLK
Idoriz 1:939932df321d 30 DigitalOut cs(PTD0); // Chip select
Idoriz 1:939932df321d 31 Serial pc(USBTX, USBRX); // Konfigurasi UART untuk termnial
Idoriz 1:939932df321d 32
Idoriz 1:939932df321d 33 // Deklarasi Fungsi dan Prosedur
Idoriz 1:939932df321d 34 void Akuisisi_RAW_Data_IMU_Normal(void);
Idoriz 1:939932df321d 35 float konversi_data1(unsigned short accl, float resolusi);
Idoriz 1:939932df321d 36 float konversi_data2(unsigned short suhu, float resolusi);
Idoriz 1:939932df321d 37
Idoriz 1:939932df321d 38 // Variabel Global
Idoriz 1:939932df321d 39 unsigned short data_high, data_low, info[14];
Idoriz 1:939932df321d 40 float suhu, dummy, supply, xgyro, ygyro, zgyro, xaccl, yaccl, zaccl, xmagn, ymagn, zmagn;
Idoriz 1:939932df321d 41 int size;
MoffMade 0:c48a4735d25f 42
MoffMade 0:c48a4735d25f 43 int main() {
MoffMade 0:c48a4735d25f 44
Idoriz 1:939932df321d 45 // Konfigurasi baud rate terminal
MoffMade 0:c48a4735d25f 46 pc.baud(9600);
Idoriz 1:939932df321d 47
Idoriz 1:939932df321d 48 // Variabel Lokal
Idoriz 1:939932df321d 49
Idoriz 1:939932df321d 50 // Inisialisasi CS, off
MoffMade 0:c48a4735d25f 51 cs = 1;
Idoriz 1:939932df321d 52
Idoriz 1:939932df321d 53 // Set format komunikasi SPI, 8 bit, mode 3 (CPOL = 1, CPHA = 1)
MoffMade 0:c48a4735d25f 54 spi.format(8,3);
Idoriz 1:939932df321d 55
Idoriz 1:939932df321d 56 // Set frekuensi komunikasi SPI, 1 MHz
MoffMade 0:c48a4735d25f 57 spi.frequency(1000000);
MoffMade 0:c48a4735d25f 58
MoffMade 0:c48a4735d25f 59 while(true) {
Idoriz 1:939932df321d 60
Idoriz 1:939932df321d 61 Akuisisi_RAW_Data_IMU_Normal();
Idoriz 1:939932df321d 62 // dummy=info[0];
Idoriz 1:939932df321d 63 // Data power supply sensor dalam floating point
Idoriz 1:939932df321d 64 supply = (info[1]&0xfff)*SUPPLY_LSB;
Idoriz 1:939932df321d 65
Idoriz 1:939932df321d 66 // Data gyroscope dalam floating point
Idoriz 1:939932df321d 67 xgyro = konversi_data1(info[2],GYRO_LSB);
Idoriz 1:939932df321d 68 ygyro = konversi_data1(info[3],GYRO_LSB);
Idoriz 1:939932df321d 69 zgyro = konversi_data1(info[4],GYRO_LSB);
Idoriz 1:939932df321d 70
Idoriz 1:939932df321d 71 // Data accelerometer dalam floating point
Idoriz 1:939932df321d 72 xaccl = konversi_data1(info[5],ACCL_LSB);
Idoriz 1:939932df321d 73 yaccl = konversi_data1(info[6],ACCL_LSB);
Idoriz 1:939932df321d 74 zaccl = konversi_data1(info[7],ACCL_LSB);
Idoriz 1:939932df321d 75
Idoriz 1:939932df321d 76 // Data magnetometer dalam floating point
Idoriz 1:939932df321d 77 xmagn = konversi_data1(info[8],MAGN_LSB);
Idoriz 1:939932df321d 78 ymagn = konversi_data1(info[9],MAGN_LSB);
Idoriz 1:939932df321d 79 zmagn = konversi_data1(info[10],MAGN_LSB);
Idoriz 1:939932df321d 80
Idoriz 1:939932df321d 81 // Data barometer dalam floating point
Idoriz 1:939932df321d 82
Idoriz 1:939932df321d 83
Idoriz 1:939932df321d 84 // Data suhu internal sensor dalam floating point
Idoriz 1:939932df321d 85 suhu = konversi_data2(info[13],TEMP_LSB);
Idoriz 1:939932df321d 86
Idoriz 1:939932df321d 87
Idoriz 1:939932df321d 88 // pc.printf("test = 0x%x\n\r", test); // Print to PC/Serial
Idoriz 1:939932df321d 89 pc.printf("Supply = %.6f\n\r", supply); // Print to PC/Serial
Idoriz 1:939932df321d 90 pc.printf("xgyro = %.6f ", xgyro); // Print to PC/Serial
Idoriz 1:939932df321d 91 pc.printf("ygyro = %.6f ", ygyro); // Print to PC/Serial
Idoriz 1:939932df321d 92 pc.printf("zgyro = %.6f\n\r", zgyro); // Print to PC/Serial
Idoriz 1:939932df321d 93 pc.printf("xaccl = %.6f ", xaccl); // Print to PC/Serial
Idoriz 1:939932df321d 94 pc.printf("yaccl = %.6f ", yaccl); // Print to PC/Serial
Idoriz 1:939932df321d 95 pc.printf("zaccl = %.6f\n\r", zaccl); // Print to PC/Serial
Idoriz 1:939932df321d 96 pc.printf("xmagn = %.6f ", xmagn); // Print to PC/Serial
Idoriz 1:939932df321d 97 pc.printf("ymagn = %.6f ", ymagn); // Print to PC/Serial
Idoriz 1:939932df321d 98 pc.printf("zmagn = %.6f\n\r", zmagn); // Print to PC/Serial
Idoriz 1:939932df321d 99 pc.printf("suhu = %.6f\n\r", suhu); // Print to PC/Serial
Idoriz 1:939932df321d 100 // pc.printf("size zmagn = %d\n\r", size); // Print to PC/Serial
Idoriz 1:939932df321d 101 wait(0.5f);
MoffMade 0:c48a4735d25f 102 }
Idoriz 1:939932df321d 103 }
Idoriz 1:939932df321d 104
Idoriz 1:939932df321d 105 void Akuisisi_RAW_Data_IMU_Normal()
Idoriz 1:939932df321d 106 {
Idoriz 1:939932df321d 107 int count=0;
Idoriz 1:939932df321d 108 unsigned short int dir, data;
Idoriz 1:939932df321d 109 for (dir=0x02;dir<0x1D;dir+=0x02)
Idoriz 1:939932df321d 110 {
Idoriz 1:939932df321d 111 cs = 0; // Chip Select di set 0 untuk memulai komunikasi
Idoriz 1:939932df321d 112 data_high=spi.write(dir);
Idoriz 1:939932df321d 113 data_low=spi.write(0x00);
Idoriz 1:939932df321d 114 cs = 1; // Chip Select di set 1 untuk menghentikan komunikasi
Idoriz 1:939932df321d 115 data = data_high<<8 | data_low;
Idoriz 1:939932df321d 116 wait_us(10);
Idoriz 1:939932df321d 117 info[count]=(data&0x3fff);
Idoriz 1:939932df321d 118 count++;
Idoriz 1:939932df321d 119 }
Idoriz 1:939932df321d 120 }
Idoriz 1:939932df321d 121
Idoriz 1:939932df321d 122 float konversi_data1(unsigned short accl, float resolusi)
Idoriz 1:939932df321d 123 {
Idoriz 1:939932df321d 124 unsigned short temp1;
Idoriz 1:939932df321d 125 float temp2;
Idoriz 1:939932df321d 126 signed short sign;
Idoriz 1:939932df321d 127 temp1 = accl;
Idoriz 1:939932df321d 128 if ((temp1&0x2000) == 0) sign = 1;
Idoriz 1:939932df321d 129 else
Idoriz 1:939932df321d 130 {
Idoriz 1:939932df321d 131 temp1 = (~temp1)+1;
Idoriz 1:939932df321d 132 temp1 = temp1&0x1fff;
Idoriz 1:939932df321d 133 sign = -1;
Idoriz 1:939932df321d 134 }
Idoriz 1:939932df321d 135 temp2 = (temp1*resolusi*sign);
Idoriz 1:939932df321d 136 return (temp2);
Idoriz 1:939932df321d 137 }
Idoriz 1:939932df321d 138
Idoriz 1:939932df321d 139 float konversi_data2(unsigned short suhu, float resolusi)
Idoriz 1:939932df321d 140 {
Idoriz 1:939932df321d 141 unsigned short temp1;
Idoriz 1:939932df321d 142 float temp2;
Idoriz 1:939932df321d 143 signed short sign;
Idoriz 1:939932df321d 144 temp1 = suhu;
Idoriz 1:939932df321d 145 if ((temp1&0x0800) == 0) sign = 1;
Idoriz 1:939932df321d 146 else
Idoriz 1:939932df321d 147 {
Idoriz 1:939932df321d 148 temp1 = (~temp1)+1;
Idoriz 1:939932df321d 149 temp1 = temp1&0x7ff;
Idoriz 1:939932df321d 150 sign = -1;
Idoriz 1:939932df321d 151 }
Idoriz 1:939932df321d 152 temp2 = 25+(temp1*resolusi*sign);
Idoriz 1:939932df321d 153 return (temp2);
MoffMade 0:c48a4735d25f 154 }