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@0:830ddddc129f, 2016-06-09 (annotated)

Committer:
YCTung
Date:
Thu Jun 09 09:46:38 2016 +0000
Revision:
0:830ddddc129f
Child:
2:ec30613f2b2b
add header files

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 0:830ddddc129f 9 SPI spi2(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 0:830ddddc129f 47 void setup_spi(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 0:830ddddc129f 53 spi2.format(8, 3);
YCTung 0:830ddddc129f 54 pc.printf("SPI ready.\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 0:830ddddc129f 89 spi2.write(sensorG_CTRL_REG[0]);
YCTung 0:830ddddc129f 90 spi2.write(sensorG_CTRL_REG[1]);
YCTung 0:830ddddc129f 91 spi2.write(sensorG_CTRL_REG[2]);
YCTung 0:830ddddc129f 92 spi2.write(sensorG_CTRL_REG[3]);
YCTung 0:830ddddc129f 93 spi2.write(sensorG_CTRL_REG[4]);
YCTung 0:830ddddc129f 94 spi2.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 0:830ddddc129f 118 spi2.write(sensorXM_CTRL_REG[0]);
YCTung 0:830ddddc129f 119 spi2.write(sensorXM_CTRL_REG[1]);
YCTung 0:830ddddc129f 120 spi2.write(sensorXM_CTRL_REG[2]);
YCTung 0:830ddddc129f 121 spi2.write(sensorXM_CTRL_REG[3]);
YCTung 0:830ddddc129f 122 spi2.write(sensorXM_CTRL_REG[4]);
YCTung 0:830ddddc129f 123 spi2.write(sensorXM_CTRL_REG[5]);
YCTung 0:830ddddc129f 124 spi2.write(sensorXM_CTRL_REG[6]);
YCTung 0:830ddddc129f 125 spi2.write(sensorXM_CTRL_REG[7]);
YCTung 0:830ddddc129f 126 spi2.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 0:830ddddc129f 150 spi2.write(sensorG_READ_REG[0]);
YCTung 0:830ddddc129f 151 sensorG_READ_REG[1] = spi2.write(0x00); // XL
YCTung 0:830ddddc129f 152 sensorG_READ_REG[2] = spi2.write(0x00); // XH
YCTung 0:830ddddc129f 153 sensorG_READ_REG[3] = spi2.write(0x00); // YL
YCTung 0:830ddddc129f 154 sensorG_READ_REG[4] = spi2.write(0x00); // YH
YCTung 0:830ddddc129f 155 sensorG_READ_REG[5] = spi2.write(0x00); // ZL
YCTung 0:830ddddc129f 156 sensorG_READ_REG[6] = spi2.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 0:830ddddc129f 184 spi2.write(sensorX_READ_REG[0]);
YCTung 0:830ddddc129f 185 sensorX_READ_REG[1] = spi2.write(0x00); // XL
YCTung 0:830ddddc129f 186 sensorX_READ_REG[2] = spi2.write(0x00); // XH
YCTung 0:830ddddc129f 187 sensorX_READ_REG[3] = spi2.write(0x00); // YL
YCTung 0:830ddddc129f 188 sensorX_READ_REG[4] = spi2.write(0x00); // YH
YCTung 0:830ddddc129f 189 sensorX_READ_REG[5] = spi2.write(0x00); // ZL
YCTung 0:830ddddc129f 190 sensorX_READ_REG[6] = spi2.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 0:830ddddc129f 219 spi2.write(sensorM_READ_REG[0]);
YCTung 0:830ddddc129f 220 sensorM_READ_REG[1] = spi2.write(0x00); // XL
YCTung 0:830ddddc129f 221 sensorM_READ_REG[2] = spi2.write(0x00); // XH
YCTung 0:830ddddc129f 222 sensorM_READ_REG[3] = spi2.write(0x00); // YL
YCTung 0:830ddddc129f 223 sensorM_READ_REG[4] = spi2.write(0x00); // YH
YCTung 0:830ddddc129f 224 sensorM_READ_REG[5] = spi2.write(0x00); // ZL
YCTung 0:830ddddc129f 225 sensorM_READ_REG[6] = spi2.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 }