code of robot bike
Dependencies: SDFileSystem mbed
Fork of Robot_Bicycle by
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?
User | Revision | Line number | New 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 | } |