Chris LU / Mbed 2 deprecated Nucleo_ZTC_20160607_oldest

code of robot bike

Dependencies:   SDFileSystem mbed

Fork of Robot_Bicycle by Chris LU

SPI_9dSensor.cpp@10:3b952ea7fad4, 2016-07-18 (annotated)

Committer:
YCTung
Date:
Mon Jul 18 12:12:50 2016 +0000
Revision:
10:3b952ea7fad4
Parent:
8:79ca11e0129d
Child:
11:45a641da473d
modified some files. "interrupt" and "pc" declared in main.cpp

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YCTung 0:830ddddc129f 1 #include "SPI_9dSensor.h"
YCTung 0:830ddddc129f 2 #include "SensorFusion.h"
YCTung 0:830ddddc129f 3
YCTung 4:b0967990e390 4 SPI sensor_LSM9D(PB_15,PB_14,PB_13);
YCTung 0:830ddddc129f 5 DigitalOut sensor_CSG(PB_2);
YCTung 0:830ddddc129f 6 DigitalOut sensor_CSXM(PB_1);
YCTung 0:830ddddc129f 7
YCTung 0:830ddddc129f 8 unsigned char sensorG_CTRL_REG[6];
YCTung 0:830ddddc129f 9 unsigned char sensorXM_CTRL_REG[9];
YCTung 0:830ddddc129f 10
YCTung 0:830ddddc129f 11 short int Gyro_axis_data[3]; // X, Y, Z axis
YCTung 0:830ddddc129f 12 short int Gyro_axis_zero[3] = {GX_offset, GY_offset, GZ_offset};///new{-57,-10,34};///{32, -28, 138};///{25, -25, 120};
YCTung 0:830ddddc129f 13 float Gyro_scale[3];
YCTung 0:830ddddc129f 14
YCTung 0:830ddddc129f 15 short int Acce_axis_data[3]; // X, Y, Z axis
YCTung 0:830ddddc129f 16 short int Acce_axis_zero[3] = {AX_offset, AY_offset, AZ_offset};///new{-847,-420,186};///{-855, -590, 186};///{-855, -504, 186};///{-855, -454, 186};///
YCTung 0:830ddddc129f 17 float Acce_scale[3];
YCTung 0:830ddddc129f 18
YCTung 0:830ddddc129f 19 short int Magn_axis_data[3]; //X,Y,Z axis
YCTung 0:830ddddc129f 20 short int Magn_axis_zero[3] = {MX_offset, MY_offset, MZ_offset};///{-55, -21, 77};//{-32,-25,80};
YCTung 0:830ddddc129f 21 float Magn_scale[3];
YCTung 0:830ddddc129f 22
YCTung 0:830ddddc129f 23 float B_x = 1015.0f*Magn_gain;
YCTung 0:830ddddc129f 24 float B_y = 0.0f*Magn_gain;
YCTung 0:830ddddc129f 25 float B_z = -580.0f*Magn_gain;
YCTung 0:830ddddc129f 26 float B_total = 0.0f;
YCTung 0:830ddddc129f 27
YCTung 0:830ddddc129f 28 float u_cali[9] = {0,0,0,0,0,0,0,0,0};
YCTung 0:830ddddc129f 29 float u_old[9] = {GX_offset, GY_offset, GZ_offset, AX_offset, AY_offset, AZ_offset, MX_offset, MY_offset, MZ_offset};
YCTung 0:830ddddc129f 30
YCTung 10:3b952ea7fad4 31 bool setup_spi_sensor(void)
YCTung 0:830ddddc129f 32 {
YCTung 0:830ddddc129f 33 sensor_CSG = 1;
YCTung 0:830ddddc129f 34 sensor_CSXM = 1;
YCTung 8:79ca11e0129d 35 sensor_LSM9D.frequency(10500000);
YCTung 4:b0967990e390 36 sensor_LSM9D.format(8, 3);
YCTung 10:3b952ea7fad4 37 // pc.printf("SPI sensor ready.\r\n");
YCTung 0:830ddddc129f 38 wait_ms(50);
YCTung 10:3b952ea7fad4 39 return 1;
YCTung 0:830ddddc129f 40 }
YCTung 0:830ddddc129f 41
YCTung 0:830ddddc129f 42 void init_Sensors(void)
YCTung 0:830ddddc129f 43 {
YCTung 0:830ddddc129f 44 Gyro_axis_data[0] = 0; // X
YCTung 0:830ddddc129f 45 Gyro_axis_data[1] = 0; // Y
YCTung 0:830ddddc129f 46 Gyro_axis_data[2] = 0; // Z
YCTung 0:830ddddc129f 47 Acce_axis_data[0] = 0;
YCTung 0:830ddddc129f 48 Acce_axis_data[1] = 0;
YCTung 0:830ddddc129f 49 Acce_axis_data[2] = 0;
YCTung 0:830ddddc129f 50 Magn_axis_data[0] = 0;
YCTung 0:830ddddc129f 51 Magn_axis_data[1] = 0;
YCTung 0:830ddddc129f 52 Magn_axis_data[2] = 0;
YCTung 0:830ddddc129f 53 sensorG_setup();
YCTung 0:830ddddc129f 54 sensorXM_setup();
YCTung 0:830ddddc129f 55 }
YCTung 0:830ddddc129f 56
YCTung 0:830ddddc129f 57 void sensorG_setup(void)
YCTung 0:830ddddc129f 58 {
YCTung 0:830ddddc129f 59 sensorG_CTRL_REG[1] = 0b11111111; // x, y, z enabled
YCTung 0:830ddddc129f 60 sensorG_CTRL_REG[2] = 0b00001001; // ODR = 800Hz, LPfc = 110Hz
YCTung 0:830ddddc129f 61 sensorG_CTRL_REG[3] = 0b00000000; // output circuit: push- pull
YCTung 0:830ddddc129f 62 sensorG_CTRL_REG[4] = 0b00110000; // 2000dps
YCTung 0:830ddddc129f 63 sensorG_CTRL_REG[5] = 0b00000000; // HPen = 0, LPFen = 0
YCTung 0:830ddddc129f 64
YCTung 0:830ddddc129f 65 // write mode (R/W = 0)
YCTung 0:830ddddc129f 66 // Auto increase address (M/S = 1)
YCTung 0:830ddddc129f 67 //sensorG_data_write = 0x20;
YCTung 0:830ddddc129f 68 sensorG_CTRL_REG[0] = (sensorG_CTRL_REG1_address & 0b00111111) | 0b01000000; // mask first two bits, write first two bits
YCTung 0:830ddddc129f 69
YCTung 0:830ddddc129f 70 // start
YCTung 0:830ddddc129f 71 sensor_CSG = 0;
YCTung 0:830ddddc129f 72
YCTung 4:b0967990e390 73 sensor_LSM9D.write(sensorG_CTRL_REG[0]);
YCTung 4:b0967990e390 74 sensor_LSM9D.write(sensorG_CTRL_REG[1]);
YCTung 4:b0967990e390 75 sensor_LSM9D.write(sensorG_CTRL_REG[2]);
YCTung 4:b0967990e390 76 sensor_LSM9D.write(sensorG_CTRL_REG[3]);
YCTung 4:b0967990e390 77 sensor_LSM9D.write(sensorG_CTRL_REG[4]);
YCTung 4:b0967990e390 78 sensor_LSM9D.write(sensorG_CTRL_REG[5]);
YCTung 0:830ddddc129f 79
YCTung 0:830ddddc129f 80 sensor_CSG = 1;
YCTung 0:830ddddc129f 81 // end
YCTung 0:830ddddc129f 82 }
YCTung 0:830ddddc129f 83
YCTung 0:830ddddc129f 84 void sensorXM_setup(void)
YCTung 0:830ddddc129f 85 {
YCTung 0:830ddddc129f 86 sensorXM_CTRL_REG[1] = 0b00000000;
YCTung 0:830ddddc129f 87 sensorXM_CTRL_REG[2] = 0b10010111; // ODR = 800Hz, x, y, z enabled
YCTung 0:830ddddc129f 88 sensorXM_CTRL_REG[3] = 0b11011000; // LPfc = 110Hz, +-8g
YCTung 0:830ddddc129f 89 sensorXM_CTRL_REG[4] = 0b00000000; // output circuit: push- pull
YCTung 0:830ddddc129f 90 sensorXM_CTRL_REG[5] = 0b00000000;
YCTung 0:830ddddc129f 91 sensorXM_CTRL_REG[6] = 0b01110100; //Magnetic data rate 100Hz
YCTung 0:830ddddc129f 92 sensorXM_CTRL_REG[7] = 0b01000000; //mag full scale +-8 gauss
YCTung 0:830ddddc129f 93 sensorXM_CTRL_REG[8] = 0b00000000; // HPen = 0, LPFen = 0
YCTung 0:830ddddc129f 94 // write mode (R/W = 0)
YCTung 0:830ddddc129f 95 // Auto increase address (M/S = 1)
YCTung 0:830ddddc129f 96 //sensorXM_data_write = sensorXM_CTRL_REG0_address;
YCTung 0:830ddddc129f 97 sensorXM_CTRL_REG[0] = (sensorXM_CTRL_REG0_address & 0b00111111) | 0b01000000; // mask first two bits, write first two bits
YCTung 0:830ddddc129f 98
YCTung 0:830ddddc129f 99 // start
YCTung 0:830ddddc129f 100 sensor_CSXM = 0;
YCTung 0:830ddddc129f 101
YCTung 4:b0967990e390 102 sensor_LSM9D.write(sensorXM_CTRL_REG[0]);
YCTung 4:b0967990e390 103 sensor_LSM9D.write(sensorXM_CTRL_REG[1]);
YCTung 4:b0967990e390 104 sensor_LSM9D.write(sensorXM_CTRL_REG[2]);
YCTung 4:b0967990e390 105 sensor_LSM9D.write(sensorXM_CTRL_REG[3]);
YCTung 4:b0967990e390 106 sensor_LSM9D.write(sensorXM_CTRL_REG[4]);
YCTung 4:b0967990e390 107 sensor_LSM9D.write(sensorXM_CTRL_REG[5]);
YCTung 4:b0967990e390 108 sensor_LSM9D.write(sensorXM_CTRL_REG[6]);
YCTung 4:b0967990e390 109 sensor_LSM9D.write(sensorXM_CTRL_REG[7]);
YCTung 4:b0967990e390 110 sensor_LSM9D.write(sensorXM_CTRL_REG[8]);
YCTung 0:830ddddc129f 111
YCTung 0:830ddddc129f 112 sensor_CSXM = 1;
YCTung 0:830ddddc129f 113 // end
YCTung 0:830ddddc129f 114 }
YCTung 0:830ddddc129f 115
YCTung 0:830ddddc129f 116 void sensorG_read_3axis(void)
YCTung 0:830ddddc129f 117 {
YCTung 0:830ddddc129f 118 static unsigned char sensorG_READ_REG[7];
YCTung 0:830ddddc129f 119 // read mode (R/W = 1)
YCTung 0:830ddddc129f 120 // Auto increase address (M/S = 1)
YCTung 0:830ddddc129f 121 //sensorG_data_write = sensorG_OUT_X_L_address;
YCTung 0:830ddddc129f 122 sensorG_READ_REG[0] = 0x28;
YCTung 0:830ddddc129f 123 sensorG_READ_REG[0] = (sensorG_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
YCTung 0:830ddddc129f 124 sensorG_READ_REG[1] = 0x00;
YCTung 0:830ddddc129f 125 sensorG_READ_REG[2] = 0x00;
YCTung 0:830ddddc129f 126 sensorG_READ_REG[3] = 0x00;
YCTung 0:830ddddc129f 127 sensorG_READ_REG[4] = 0x00;
YCTung 0:830ddddc129f 128 sensorG_READ_REG[5] = 0x00;
YCTung 0:830ddddc129f 129 sensorG_READ_REG[6] = 0x00;
YCTung 0:830ddddc129f 130
YCTung 0:830ddddc129f 131 // start
YCTung 0:830ddddc129f 132 sensor_CSG = 0;
YCTung 0:830ddddc129f 133
YCTung 4:b0967990e390 134 sensor_LSM9D.write(sensorG_READ_REG[0]);
YCTung 4:b0967990e390 135 sensorG_READ_REG[1] = sensor_LSM9D.write(0x00); // XL
YCTung 4:b0967990e390 136 sensorG_READ_REG[2] = sensor_LSM9D.write(0x00); // XH
YCTung 4:b0967990e390 137 sensorG_READ_REG[3] = sensor_LSM9D.write(0x00); // YL
YCTung 4:b0967990e390 138 sensorG_READ_REG[4] = sensor_LSM9D.write(0x00); // YH
YCTung 4:b0967990e390 139 sensorG_READ_REG[5] = sensor_LSM9D.write(0x00); // ZL
YCTung 4:b0967990e390 140 sensorG_READ_REG[6] = sensor_LSM9D.write(0x00); // YH
YCTung 0:830ddddc129f 141
YCTung 0:830ddddc129f 142 sensor_CSG = 1;
YCTung 0:830ddddc129f 143 // end
YCTung 0:830ddddc129f 144
YCTung 0:830ddddc129f 145 // Data reconstruction
YCTung 0:830ddddc129f 146 Gyro_axis_data[0] = (short int)((sensorG_READ_REG[2]<<8) | sensorG_READ_REG[1]);
YCTung 0:830ddddc129f 147 Gyro_axis_data[1] = (short int)((sensorG_READ_REG[4]<<8) | sensorG_READ_REG[3]);
YCTung 0:830ddddc129f 148 Gyro_axis_data[2] = (short int)((sensorG_READ_REG[6]<<8) | sensorG_READ_REG[5]);
YCTung 0:830ddddc129f 149 }
YCTung 0:830ddddc129f 150
YCTung 0:830ddddc129f 151 void sensorX_read_3axis(void)
YCTung 0:830ddddc129f 152 {
YCTung 0:830ddddc129f 153 static unsigned char sensorX_READ_REG[7];
YCTung 0:830ddddc129f 154
YCTung 0:830ddddc129f 155 // read mode (R/W = 1)
YCTung 0:830ddddc129f 156 // Auto increase address (M/S = 1)
YCTung 0:830ddddc129f 157 sensorX_READ_REG[0] = 0x28;//accelerator's first address
YCTung 0:830ddddc129f 158 sensorX_READ_REG[0] = (sensorX_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
YCTung 0:830ddddc129f 159 sensorX_READ_REG[1] = 0x00;
YCTung 0:830ddddc129f 160 sensorX_READ_REG[2] = 0x00;
YCTung 0:830ddddc129f 161 sensorX_READ_REG[3] = 0x00;
YCTung 0:830ddddc129f 162 sensorX_READ_REG[4] = 0x00;
YCTung 0:830ddddc129f 163 sensorX_READ_REG[5] = 0x00;
YCTung 0:830ddddc129f 164 sensorX_READ_REG[6] = 0x00;
YCTung 0:830ddddc129f 165 // start
YCTung 0:830ddddc129f 166 sensor_CSXM = 0;
YCTung 0:830ddddc129f 167
YCTung 4:b0967990e390 168 sensor_LSM9D.write(sensorX_READ_REG[0]);
YCTung 4:b0967990e390 169 sensorX_READ_REG[1] = sensor_LSM9D.write(0x00); // XL
YCTung 4:b0967990e390 170 sensorX_READ_REG[2] = sensor_LSM9D.write(0x00); // XH
YCTung 4:b0967990e390 171 sensorX_READ_REG[3] = sensor_LSM9D.write(0x00); // YL
YCTung 4:b0967990e390 172 sensorX_READ_REG[4] = sensor_LSM9D.write(0x00); // YH
YCTung 4:b0967990e390 173 sensorX_READ_REG[5] = sensor_LSM9D.write(0x00); // ZL
YCTung 4:b0967990e390 174 sensorX_READ_REG[6] = sensor_LSM9D.write(0x00); // YH
YCTung 0:830ddddc129f 175
YCTung 0:830ddddc129f 176 sensor_CSXM = 1;
YCTung 0:830ddddc129f 177 // end
YCTung 0:830ddddc129f 178
YCTung 0:830ddddc129f 179 // Data reconstruction
YCTung 0:830ddddc129f 180 Acce_axis_data[0] = (short int)((sensorX_READ_REG[2]<<8) | sensorX_READ_REG[1]); // X
YCTung 0:830ddddc129f 181 Acce_axis_data[1] = (short int)((sensorX_READ_REG[4]<<8) | sensorX_READ_REG[3]); // Y
YCTung 0:830ddddc129f 182 Acce_axis_data[2] = (short int)((sensorX_READ_REG[6]<<8) | sensorX_READ_REG[5]); // Z
YCTung 0:830ddddc129f 183 }
YCTung 0:830ddddc129f 184
YCTung 0:830ddddc129f 185 void sensorM_read_3axis(void)
YCTung 0:830ddddc129f 186 {
YCTung 0:830ddddc129f 187 static unsigned char sensorM_READ_REG[7];
YCTung 0:830ddddc129f 188
YCTung 0:830ddddc129f 189 // read mode (R/W = 1)
YCTung 0:830ddddc129f 190 // Auto increase address (M/S = 1)
YCTung 0:830ddddc129f 191 sensorM_READ_REG[0] = 0x08;//magnetometer's first address
YCTung 0:830ddddc129f 192 sensorM_READ_REG[0] = (sensorM_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
YCTung 0:830ddddc129f 193 sensorM_READ_REG[1] = 0x00;
YCTung 0:830ddddc129f 194 sensorM_READ_REG[2] = 0x00;
YCTung 0:830ddddc129f 195 sensorM_READ_REG[3] = 0x00;
YCTung 0:830ddddc129f 196 sensorM_READ_REG[4] = 0x00;
YCTung 0:830ddddc129f 197 sensorM_READ_REG[5] = 0x00;
YCTung 0:830ddddc129f 198 sensorM_READ_REG[6] = 0x00;
YCTung 0:830ddddc129f 199
YCTung 0:830ddddc129f 200 // start
YCTung 0:830ddddc129f 201 sensor_CSXM = 0;
YCTung 0:830ddddc129f 202
YCTung 4:b0967990e390 203 sensor_LSM9D.write(sensorM_READ_REG[0]);
YCTung 4:b0967990e390 204 sensorM_READ_REG[1] = sensor_LSM9D.write(0x00); // XL
YCTung 4:b0967990e390 205 sensorM_READ_REG[2] = sensor_LSM9D.write(0x00); // XH
YCTung 4:b0967990e390 206 sensorM_READ_REG[3] = sensor_LSM9D.write(0x00); // YL
YCTung 4:b0967990e390 207 sensorM_READ_REG[4] = sensor_LSM9D.write(0x00); // YH
YCTung 4:b0967990e390 208 sensorM_READ_REG[5] = sensor_LSM9D.write(0x00); // ZL
YCTung 4:b0967990e390 209 sensorM_READ_REG[6] = sensor_LSM9D.write(0x00); // YH
YCTung 0:830ddddc129f 210
YCTung 0:830ddddc129f 211 sensor_CSXM = 1;
YCTung 0:830ddddc129f 212 // end
YCTung 0:830ddddc129f 213
YCTung 0:830ddddc129f 214 // Data reconstruction
YCTung 0:830ddddc129f 215 Magn_axis_data[0] = (short int)((sensorM_READ_REG[2]<<8) | sensorM_READ_REG[1]); // X
YCTung 0:830ddddc129f 216 Magn_axis_data[1] = (short int)((sensorM_READ_REG[4]<<8) | sensorM_READ_REG[3]); // Y
YCTung 0:830ddddc129f 217 Magn_axis_data[2] = (short int)((sensorM_READ_REG[6]<<8) | sensorM_READ_REG[5]); // Z
YCTung 0:830ddddc129f 218 }
YCTung 0:830ddddc129f 219
YCTung 0:830ddddc129f 220 short int filted_sensor_data(unsigned char axis_index, float freq)
YCTung 0:830ddddc129f 221 {
YCTung 0:830ddddc129f 222 float data_to_filt = 0.0;
YCTung 0:830ddddc129f 223
YCTung 0:830ddddc129f 224 switch(axis_index)
YCTung 0:830ddddc129f 225 {
YCTung 0:830ddddc129f 226 case 1: data_to_filt = (float)Gyro_axis_data[0]; break;
YCTung 0:830ddddc129f 227 case 2: data_to_filt = (float)Gyro_axis_data[1]; break;
YCTung 0:830ddddc129f 228 case 3: data_to_filt = (float)Gyro_axis_data[2]; break;
YCTung 0:830ddddc129f 229 case 4: data_to_filt = (float)Acce_axis_data[0]; break;
YCTung 0:830ddddc129f 230 case 5: data_to_filt = (float)Acce_axis_data[1]; break;
YCTung 0:830ddddc129f 231 case 6: data_to_filt = (float)Acce_axis_data[2]; break;
YCTung 0:830ddddc129f 232 case 7: data_to_filt = (float)Magn_axis_data[0]; break;
YCTung 0:830ddddc129f 233 case 8: data_to_filt = (float)Magn_axis_data[1]; break;
YCTung 0:830ddddc129f 234 case 9: data_to_filt = (float)Magn_axis_data[2]; break;
YCTung 0:830ddddc129f 235 default: break;
YCTung 0:830ddddc129f 236 }
YCTung 0:830ddddc129f 237
YCTung 0:830ddddc129f 238 u_cali[axis_index - 1] = lpf(data_to_filt, u_old[axis_index - 1], freq);
YCTung 0:830ddddc129f 239 u_old[axis_index - 1] = u_cali[axis_index - 1];
YCTung 0:830ddddc129f 240 return (short int)u_cali[axis_index - 1];
YCTung 0:830ddddc129f 241 }
YCTung 0:830ddddc129f 242
YCTung 0:830ddddc129f 243 void reset_gyro_offset(void)
YCTung 0:830ddddc129f 244 {
YCTung 0:830ddddc129f 245 sensorG_read_3axis();
YCTung 0:830ddddc129f 246 Gyro_axis_zero[0] = filted_sensor_data(INDEX_GYRO_X, 1.8);
YCTung 0:830ddddc129f 247 Gyro_axis_zero[1] = filted_sensor_data(INDEX_GYRO_Y, 1.8);
YCTung 0:830ddddc129f 248 Gyro_axis_zero[2] = filted_sensor_data(INDEX_GYRO_Z, 1.8);
YCTung 0:830ddddc129f 249 }
YCTung 0:830ddddc129f 250
YCTung 0:830ddddc129f 251 void reset_acceX_offset(void)
YCTung 0:830ddddc129f 252 {
YCTung 0:830ddddc129f 253 sensorX_read_3axis();
YCTung 0:830ddddc129f 254 Acce_axis_zero[0] = filted_sensor_data(INDEX_ACCE_X, 1.8);
YCTung 0:830ddddc129f 255 }
YCTung 0:830ddddc129f 256
YCTung 0:830ddddc129f 257 void get_9axis_scale(void)
YCTung 0:830ddddc129f 258 {
YCTung 0:830ddddc129f 259 Gyro_scale[0] = ((float)Gyro_axis_data[0]-Gyro_axis_zero[0])*Gyro_gainx;
YCTung 0:830ddddc129f 260 Gyro_scale[1] = ((float)Gyro_axis_data[1]-Gyro_axis_zero[1])*Gyro_gainy;
YCTung 0:830ddddc129f 261 Gyro_scale[2] = ((float)Gyro_axis_data[2]-Gyro_axis_zero[2])*Gyro_gainz;
YCTung 0:830ddddc129f 262 Acce_scale[0] = ((float)(Acce_axis_data[0]-Acce_axis_zero[0]))*Acce_gainx;
YCTung 0:830ddddc129f 263 Acce_scale[1] = ((float)(Acce_axis_data[1]-Acce_axis_zero[1]))*Acce_gainy;
YCTung 0:830ddddc129f 264 Acce_scale[2] = ((float)(Acce_axis_data[2]-Acce_axis_zero[2]))*Acce_gainz;
YCTung 0:830ddddc129f 265 Magn_scale[0] = ((float)(Magn_axis_data[0]-Magn_axis_zero[0]))*Magn_gain;
YCTung 0:830ddddc129f 266 Magn_scale[1] = ((float)(Magn_axis_data[1]-Magn_axis_zero[1]))*Magn_gain;
YCTung 0:830ddddc129f 267 Magn_scale[2] = ((float)(Magn_axis_data[2]-Magn_axis_zero[2]))*Magn_gain;
YCTung 0:830ddddc129f 268 }
YCTung 4:b0967990e390 269
YCTung 10:3b952ea7fad4 270 unsigned char get_WhoAmI_G(void)
YCTung 4:b0967990e390 271 {
YCTung 4:b0967990e390 272 static unsigned char WhoAmI_G = 0;
YCTung 4:b0967990e390 273 sensor_CSG = 0;
YCTung 4:b0967990e390 274 sensor_LSM9D.write((0x0F & 0x3F) | 0x80);
YCTung 4:b0967990e390 275 WhoAmI_G = sensor_LSM9D.write(0x00);
YCTung 4:b0967990e390 276 sensor_CSG = 1;
YCTung 10:3b952ea7fad4 277 return WhoAmI_G;
YCTung 4:b0967990e390 278 }
YCTung 4:b0967990e390 279
YCTung 10:3b952ea7fad4 280 unsigned char get_WhoAmI_XM(void)
YCTung 4:b0967990e390 281 {
YCTung 4:b0967990e390 282 static unsigned char WhoAmI_XM = 0;
YCTung 4:b0967990e390 283 sensor_CSXM = 0;
YCTung 4:b0967990e390 284 sensor_LSM9D.write((0x0F & 0x3F) | 0x80);
YCTung 4:b0967990e390 285 WhoAmI_XM = sensor_LSM9D.write(0x00);
YCTung 4:b0967990e390 286 sensor_CSXM = 1;
YCTung 10:3b952ea7fad4 287 return WhoAmI_XM;
YCTung 4:b0967990e390 288 }