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@4:b0967990e390, 2016-07-06 (annotated)

Committer:
YCTung
Date:
Wed Jul 06 06:54:15 2016 +0000
Revision:
4:b0967990e390
Parent:
2:ec30613f2b2b
Child:
8:79ca11e0129d
update program

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