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@8:79ca11e0129d, 2016-07-18 (annotated)

Committer:
YCTung
Date:
Mon Jul 18 03:31:39 2016 +0000
Revision:
8:79ca11e0129d
Parent:
4:b0967990e390
Child:
10:3b952ea7fad4
boost the frequency of SD  clock

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