z jx / Mbed 2 deprecated IMU_fusion_9DOF

imu_fusion

Dependencies:   ICM20602_I2C IMU_fusion QMC5883L ledControl2 mbed

Fork of IMU_fusion by Baser Kandehir

main.cpp@3:788eecfd5ae9, 2017-07-19 (annotated)

Committer:
sarahbest
Date:
Wed Jul 19 07:59:55 2017 +0000
Revision:
3:788eecfd5ae9
Parent:
2:3881894aaff5 
1\ouput the data of imu and mag.; 2\can calibrate imu easily, but the offsets are not written into flash.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 0:d8d055e8f830 1 /* Calculating Roll, Pitch and Yaw angles from IMU
BaserK 0:d8d055e8f830 2 *
BaserK 0:d8d055e8f830 3 * @author: Baser Kandehir
BaserK 0:d8d055e8f830 4 * @date: August 5, 2015
BaserK 0:d8d055e8f830 5 * @license: MIT license
BaserK 0:d8d055e8f830 6 *
BaserK 0:d8d055e8f830 7 * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
BaserK 0:d8d055e8f830 8 *
BaserK 0:d8d055e8f830 9 * Permission is hereby granted, free of charge, to any person obtaining a copy
BaserK 0:d8d055e8f830 10 * of this software and associated documentation files (the "Software"), to deal
BaserK 0:d8d055e8f830 11 * in the Software without restriction, including without limitation the rights
BaserK 0:d8d055e8f830 12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
BaserK 0:d8d055e8f830 13 * copies of the Software, and to permit persons to whom the Software is
BaserK 0:d8d055e8f830 14 * furnished to do so, subject to the following conditions:
BaserK 0:d8d055e8f830 15 *
BaserK 0:d8d055e8f830 16 * The above copyright notice and this permission notice shall be included in
BaserK 0:d8d055e8f830 17 * all copies or substantial portions of the Software.
BaserK 0:d8d055e8f830 18 *
BaserK 0:d8d055e8f830 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
BaserK 0:d8d055e8f830 20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
BaserK 0:d8d055e8f830 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
BaserK 0:d8d055e8f830 22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
BaserK 0:d8d055e8f830 23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
BaserK 0:d8d055e8f830 24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
BaserK 0:d8d055e8f830 25 * THE SOFTWARE.
BaserK 0:d8d055e8f830 26 *
BaserK 0:d8d055e8f830 27 * @description of the program:
BaserK 0:d8d055e8f830 28 *
BaserK 0:d8d055e8f830 29 * Program can calculate roll, pitch and yaw angles from the raw data that comes
BaserK 0:d8d055e8f830 30 * from IMU. Yaw angle is compensated for tilt. All the angles are sent to the matlab
BaserK 2:3881894aaff5 31 * for further processing.
BaserK 0:d8d055e8f830 32 *
BaserK 0:d8d055e8f830 33 */
BaserK 0:d8d055e8f830 34
BaserK 0:d8d055e8f830 35 #include "mbed.h"
sarahbest 3:788eecfd5ae9 36 #include "QMC5883L.h"
sarahbest 3:788eecfd5ae9 37 #include "icm20602_i2c.h"
BaserK 0:d8d055e8f830 38 #include "ledControl.h"
BaserK 0:d8d055e8f830 39
BaserK 0:d8d055e8f830 40 Serial pc(USBTX,USBRX);
sarahbest 3:788eecfd5ae9 41 ICM20602 icm20602;
sarahbest 3:788eecfd5ae9 42 QMC5883L qmc5883l;
BaserK 0:d8d055e8f830 43 Ticker toggler1;
BaserK 0:d8d055e8f830 44 Ticker filter;
BaserK 0:d8d055e8f830 45 Ticker compass;
BaserK 0:d8d055e8f830 46
sarahbest 3:788eecfd5ae9 47 enum detect_orientation_return
sarahbest 3:788eecfd5ae9 48 {
sarahbest 3:788eecfd5ae9 49 DETECT_ORIENTATION_UPSIDE_DOWN,
sarahbest 3:788eecfd5ae9 50 DETECT_ORIENTATION_RIGHTSIDE_UP,
sarahbest 3:788eecfd5ae9 51 DETECT_ORIENTATION_LEFT,
sarahbest 3:788eecfd5ae9 52 DETECT_ORIENTATION_RIGHT,
sarahbest 3:788eecfd5ae9 53 DETECT_ORIENTATION_TAIL_DOWN,
sarahbest 3:788eecfd5ae9 54 DETECT_ORIENTATION_NOSE_DOWN,
sarahbest 3:788eecfd5ae9 55 DETECT_ORIENTATION_ERROR
sarahbest 3:788eecfd5ae9 56 };
sarahbest 3:788eecfd5ae9 57
BaserK 0:d8d055e8f830 58 void toggle_led1();
BaserK 0:d8d055e8f830 59 void toggle_led2();
sarahbest 3:788eecfd5ae9 60 //void compFilter();
sarahbest 3:788eecfd5ae9 61 void read_IMU_data();
sarahbest 3:788eecfd5ae9 62 void read_MAG_data();
sarahbest 3:788eecfd5ae9 63 void IMU_calibration();
sarahbest 3:788eecfd5ae9 64 void IMU_compensate();
sarahbest 3:788eecfd5ae9 65 char detect_orientation(float acc_dete[3], float gyro_dete[3]);
sarahbest 3:788eecfd5ae9 66 //void tiltCompensatedAngle();
BaserK 0:d8d055e8f830 67
BaserK 0:d8d055e8f830 68 float pitchAngle = 0;
BaserK 0:d8d055e8f830 69 float rollAngle = 0;
BaserK 0:d8d055e8f830 70 float yawAngle = 0;
sarahbest 3:788eecfd5ae9 71 float acc[3];
sarahbest 3:788eecfd5ae9 72 float gyro[3];
sarahbest 3:788eecfd5ae9 73 float acc_comp[3];
sarahbest 3:788eecfd5ae9 74 float gyro_comp[3];
sarahbest 3:788eecfd5ae9 75 float acc_off[3];
sarahbest 3:788eecfd5ae9 76 float gyro_off[3];
sarahbest 3:788eecfd5ae9 77 float mag[3];
sarahbest 3:788eecfd5ae9 78 float IMU_tmp;
sarahbest 3:788eecfd5ae9 79 float mag_tmp;
sarahbest 3:788eecfd5ae9 80 const char cali = 'c';
sarahbest 3:788eecfd5ae9 81 const char coll = ' ';
sarahbest 3:788eecfd5ae9 82 char orientation;
BaserK 0:d8d055e8f830 83
BaserK 0:d8d055e8f830 84 int main()
BaserK 0:d8d055e8f830 85 {
sarahbest 3:788eecfd5ae9 86
BaserK 0:d8d055e8f830 87 pc.baud(9600); // baud rate: 9600
sarahbest 3:788eecfd5ae9 88 icm20602.whoAmI(); // Communication test: WHO_AM_I register reading
sarahbest 3:788eecfd5ae9 89 qmc5883l.ChipID();
sarahbest 3:788eecfd5ae9 90 // icm20602.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
sarahbest 3:788eecfd5ae9 91 // filter.attach(&compFilter, 2); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
sarahbest 3:788eecfd5ae9 92 // compass.attach(&tiltCompensatedAngle, 0.015); // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period)
sarahbest 3:788eecfd5ae9 93
sarahbest 3:788eecfd5ae9 94 icm20602.init();
sarahbest 3:788eecfd5ae9 95 qmc5883l.init();
BaserK 0:d8d055e8f830 96 while(1)
sarahbest 3:788eecfd5ae9 97 {
sarahbest 3:788eecfd5ae9 98 // pc.putc(pc.getc());
sarahbest 3:788eecfd5ae9 99 // pc.printf("%c\n",pc.getc());
sarahbest 3:788eecfd5ae9 100 //if(pc.getc()==cali)
sarahbest 3:788eecfd5ae9 101 // {
sarahbest 3:788eecfd5ae9 102 // pc.printf("calibrate IMU!\n");
sarahbest 3:788eecfd5ae9 103 // IMU_calibration();
sarahbest 3:788eecfd5ae9 104 //// break;
sarahbest 3:788eecfd5ae9 105 // }else{
sarahbest 3:788eecfd5ae9 106 // while(1) {
sarahbest 3:788eecfd5ae9 107 read_IMU_data();
sarahbest 3:788eecfd5ae9 108 read_MAG_data();
sarahbest 3:788eecfd5ae9 109 IMU_compensate();
sarahbest 3:788eecfd5ae9 110 // pc.printf("%.5f,%.5f\r\n",aRes,gRes); // send data to matlab
sarahbest 3:788eecfd5ae9 111 pc.printf("original data:%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2],IMU_tmp,mag[0],mag[1],mag[2],mag_tmp);
sarahbest 3:788eecfd5ae9 112 // pc.printf("original acc:%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 113 // pc.printf("original gyro:%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 114 // pc.printf("compensated acc:%.5f,%.5f,%.5f\r\n",acc_comp[0],acc_comp[1],acc_comp[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 115 // pc.printf("compensated gyro:%.5f,%.5f,%.5f\r\n",gyro_comp[0],gyro_comp[1],gyro_comp[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 116 // pc.printf("mag:%.5f,%.5f,%.5f\r\n",mag[0],mag[1],mag[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 117 // wait_ms(400);
sarahbest 3:788eecfd5ae9 118 // ledToggle(3);
sarahbest 3:788eecfd5ae9 119 // }
sarahbest 3:788eecfd5ae9 120 // }
BaserK 0:d8d055e8f830 121 }
BaserK 0:d8d055e8f830 122 }
BaserK 0:d8d055e8f830 123
BaserK 0:d8d055e8f830 124 void toggle_led1() {ledToggle(1);}
BaserK 0:d8d055e8f830 125 void toggle_led2() {ledToggle(2);}
BaserK 0:d8d055e8f830 126
BaserK 0:d8d055e8f830 127 /* This function is created to avoid address error that caused from Ticker.attach func */
sarahbest 3:788eecfd5ae9 128 //void compFilter() {icm20602.complementaryFilter(&pitchAngle, &rollAngle);}
sarahbest 3:788eecfd5ae9 129 //void read_imu() {icm20602.read_IMU_data(&acc[3], &gyro[3]);}
BaserK 0:d8d055e8f830 130 /* Tilt compensated compass data */
BaserK 0:d8d055e8f830 131 // Works well for tilt in +/- 50 deg range
sarahbest 3:788eecfd5ae9 132 //void tiltCompensatedAngle()
sarahbest 3:788eecfd5ae9 133 //{
sarahbest 3:788eecfd5ae9 134 // float mag_Data[3], Xh, Yh;
sarahbest 3:788eecfd5ae9 135 // hmc5883l.readMagData(mag_Data);
sarahbest 3:788eecfd5ae9 136 //
sarahbest 3:788eecfd5ae9 137 // Xh = mag_Data[0] * cos(rollAngle*PI/180) - mag_Data[2] * sin(rollAngle*PI/180) ;
sarahbest 3:788eecfd5ae9 138 //
sarahbest 3:788eecfd5ae9 139 // Yh = mag_Data[0] * sin(pitchAngle*PI/180) * sin(rollAngle*PI/180) +
sarahbest 3:788eecfd5ae9 140 // mag_Data[1] * cos(pitchAngle*PI/180) -
sarahbest 3:788eecfd5ae9 141 // mag_Data[2] * sin(pitchAngle*PI/180) * cos(rollAngle*PI/180) ;
sarahbest 3:788eecfd5ae9 142 //
sarahbest 3:788eecfd5ae9 143 // /* Calculate the compensated heading angle */
sarahbest 3:788eecfd5ae9 144 // double heading = atan2(Yh, Xh);
sarahbest 3:788eecfd5ae9 145 //
sarahbest 3:788eecfd5ae9 146 // // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
sarahbest 3:788eecfd5ae9 147 // // declinationAngle can be found here http://www.magnetic-declination.com/
sarahbest 3:788eecfd5ae9 148 // // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
sarahbest 3:788eecfd5ae9 149 // float declinationAngle = 0.096;
sarahbest 3:788eecfd5ae9 150 // heading += declinationAngle;
sarahbest 3:788eecfd5ae9 151 //
sarahbest 3:788eecfd5ae9 152 // // Correct for when signs are reversed.
sarahbest 3:788eecfd5ae9 153 // if(heading < 0)
sarahbest 3:788eecfd5ae9 154 // heading += 2*PI;
sarahbest 3:788eecfd5ae9 155 //
sarahbest 3:788eecfd5ae9 156 // // Check for wrap due to addition of declination.
sarahbest 3:788eecfd5ae9 157 // if(heading > 2*PI)
sarahbest 3:788eecfd5ae9 158 // heading -= 2*PI;
sarahbest 3:788eecfd5ae9 159 //
sarahbest 3:788eecfd5ae9 160 // /* Convert radian to degrees */
sarahbest 3:788eecfd5ae9 161 // heading = heading * 180 / PI;
sarahbest 3:788eecfd5ae9 162 //
sarahbest 3:788eecfd5ae9 163 // yawAngle = heading;
sarahbest 3:788eecfd5ae9 164 //}
sarahbest 3:788eecfd5ae9 165 void read_IMU_data() {
BaserK 0:d8d055e8f830 166
sarahbest 3:788eecfd5ae9 167 acc[0] = icm20602.getAccXvalue() * IMU_ONE_G * aRes;
sarahbest 3:788eecfd5ae9 168 acc[1] = icm20602.getAccYvalue() * IMU_ONE_G * aRes;
sarahbest 3:788eecfd5ae9 169 acc[2] = icm20602.getAccZvalue() * IMU_ONE_G * aRes;
sarahbest 3:788eecfd5ae9 170 gyro[0] = icm20602.getGyrXvalue() * gRes;
sarahbest 3:788eecfd5ae9 171 gyro[1] = icm20602.getGyrYvalue() * gRes;
sarahbest 3:788eecfd5ae9 172 gyro[2] = icm20602.getGyrZvalue() * gRes;
sarahbest 3:788eecfd5ae9 173 IMU_tmp = icm20602.getIMUTemp() / 262.144;//326.8;
sarahbest 3:788eecfd5ae9 174 // pc.printf("acc0:%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 175 }
sarahbest 3:788eecfd5ae9 176 void read_MAG_data() {
BaserK 0:d8d055e8f830 177
sarahbest 3:788eecfd5ae9 178 mag[0] = qmc5883l.getMagXvalue() * mRes;
sarahbest 3:788eecfd5ae9 179 mag[1] = qmc5883l.getMagYvalue() * mRes;
sarahbest 3:788eecfd5ae9 180 mag[2] = qmc5883l.getMagZvalue() * mRes;
sarahbest 3:788eecfd5ae9 181 mag_tmp = qmc5883l.getMagTemp() /262.144;// 100;
sarahbest 3:788eecfd5ae9 182 // pc.printf("mag:%.5f,%.5f,%.5f\r\n",mag[0],mag[1],mag[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 183 }
sarahbest 3:788eecfd5ae9 184
sarahbest 3:788eecfd5ae9 185 void IMU_calibration(){
sarahbest 3:788eecfd5ae9 186
sarahbest 3:788eecfd5ae9 187 int ii,jj,i=1,counter=0,timer=10;
sarahbest 3:788eecfd5ae9 188 float acc_cal[6][3];
sarahbest 3:788eecfd5ae9 189 float gyro_cal[6][3];
sarahbest 3:788eecfd5ae9 190 //float acc_sum[3];
sarahbest 3:788eecfd5ae9 191 // float gyro_sum[3];
sarahbest 3:788eecfd5ae9 192 pc.printf("put the IMU still!\n");
BaserK 0:d8d055e8f830 193
sarahbest 3:788eecfd5ae9 194 for(i=0;i<6;i++){
sarahbest 3:788eecfd5ae9 195
sarahbest 3:788eecfd5ae9 196 if(pc.getc()==coll){
sarahbest 3:788eecfd5ae9 197 pc.printf("%dst side start\n",i+1);
sarahbest 3:788eecfd5ae9 198 float acc_sum[3]={0};
sarahbest 3:788eecfd5ae9 199 float gyro_sum[3]={0};
sarahbest 3:788eecfd5ae9 200 while(counter < timer){
sarahbest 3:788eecfd5ae9 201 read_IMU_data();
sarahbest 3:788eecfd5ae9 202 acc_sum[0] += acc[0];
sarahbest 3:788eecfd5ae9 203 acc_sum[1] += acc[1];
sarahbest 3:788eecfd5ae9 204 acc_sum[2] += acc[2];
sarahbest 3:788eecfd5ae9 205
sarahbest 3:788eecfd5ae9 206 gyro_sum[0] += gyro[0];
sarahbest 3:788eecfd5ae9 207 gyro_sum[1] += gyro[1];
sarahbest 3:788eecfd5ae9 208 gyro_sum[2] += gyro[2];
sarahbest 3:788eecfd5ae9 209
sarahbest 3:788eecfd5ae9 210 counter++;
sarahbest 3:788eecfd5ae9 211 //pc.printf(" acc :%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 212 // pc.printf(" gyro :%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 213 // pc.printf(" acc sum:%.5f,%.5f,%.5f\r\n",acc_sum[0],acc_sum[1],acc_sum[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 214 // pc.printf(" gyro sum:%.5f,%.5f,%.5f\r\n",gyro_sum[0],gyro_sum[1],gyro_sum[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 215 // pc.printf("%d----\n",counter);
sarahbest 3:788eecfd5ae9 216 }
sarahbest 3:788eecfd5ae9 217 //pc.printf(" acc :%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 218 // pc.printf(" gyro :%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 219 acc_cal[i][0] = acc_sum[0]/timer;
sarahbest 3:788eecfd5ae9 220 acc_cal[i][1] = acc_sum[1]/timer;
sarahbest 3:788eecfd5ae9 221 acc_cal[i][2] = acc_sum[2]/timer;
sarahbest 3:788eecfd5ae9 222
sarahbest 3:788eecfd5ae9 223 gyro_cal[i][0] = gyro_sum[0]/timer;
sarahbest 3:788eecfd5ae9 224 gyro_cal[i][1] = gyro_sum[1]/timer;
sarahbest 3:788eecfd5ae9 225 gyro_cal[i][2] = gyro_sum[2]/timer;
sarahbest 3:788eecfd5ae9 226 pc.printf("%d--counter\n",counter);
sarahbest 3:788eecfd5ae9 227 counter = 0;
sarahbest 3:788eecfd5ae9 228 //acc_sum[0] =0.0;acc_sum[1] =0.0;acc_sum[2] =0.0;
sarahbest 3:788eecfd5ae9 229 // gyro_sum[0] = 0.0;gyro_sum[1] = 0.0;gyro_sum[2] = 0.0;
sarahbest 3:788eecfd5ae9 230 //pc.printf(" acc sum:%.5f,%.5f,%.5f\r\n",acc_cal[i][0],acc_cal[i][1],acc_cal[i][2]); // send data to matlab
sarahbest 3:788eecfd5ae9 231 // pc.printf(" gyro sum:%.5f,%.5f,%.5f\r\n",gyro_cal[i][0],gyro_cal[i][1],gyro_cal[i][2]); // send data to matlab
sarahbest 3:788eecfd5ae9 232 orientation = detect_orientation(acc_cal[i],gyro_cal[i]);
sarahbest 3:788eecfd5ae9 233
sarahbest 3:788eecfd5ae9 234 pc.printf("%cst side completed\n",orientation);
sarahbest 3:788eecfd5ae9 235 }
sarahbest 3:788eecfd5ae9 236 }
sarahbest 3:788eecfd5ae9 237 //calculate the offset and scale
sarahbest 3:788eecfd5ae9 238 for(ii = 0;ii<3;ii++)
sarahbest 3:788eecfd5ae9 239 {
sarahbest 3:788eecfd5ae9 240 for(jj=0;jj<6;jj++){
sarahbest 3:788eecfd5ae9 241 acc_off[ii] += acc_cal[jj][ii];
sarahbest 3:788eecfd5ae9 242 gyro_off[ii] += gyro_cal[jj][ii];
sarahbest 3:788eecfd5ae9 243 }
sarahbest 3:788eecfd5ae9 244 // pc.printf(" acc offset sum:%.5f,%.5f,%.5f\r\n",acc_off[0],acc_off[1],acc_off[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 245 // pc.printf(" gyro offset sum:%.5f,%.5f,%.5f\r\n",gyro_off[0],gyro_off[1],gyro_off[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 246 acc_off[ii]/=6;
sarahbest 3:788eecfd5ae9 247 gyro_off[ii]/=6;
sarahbest 3:788eecfd5ae9 248 }
sarahbest 3:788eecfd5ae9 249 //pc.printf(" acc offset:%.5f,%.5f,%.5f\r\n",acc_off[0],acc_off[1],acc_off[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 250 // pc.printf(" gyro offset:%.5f,%.5f,%.5f\r\n",gyro_off[0],gyro_off[1],gyro_off[2]); // send data to matlab
sarahbest 3:788eecfd5ae9 251 return;
sarahbest 3:788eecfd5ae9 252 }
sarahbest 3:788eecfd5ae9 253
sarahbest 3:788eecfd5ae9 254 char detect_orientation(float acc_dete[3], float gyro_dete[3])
sarahbest 3:788eecfd5ae9 255 {
sarahbest 3:788eecfd5ae9 256 if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2] - IMU_ONE_G) < 1.0){
sarahbest 3:788eecfd5ae9 257 return DETECT_ORIENTATION_UPSIDE_DOWN;//[0 0 g]
sarahbest 3:788eecfd5ae9 258 }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2] + IMU_ONE_G) < 1.0){
sarahbest 3:788eecfd5ae9 259 return DETECT_ORIENTATION_RIGHTSIDE_UP;//[0 0 -g]
sarahbest 3:788eecfd5ae9 260 }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1] - IMU_ONE_G) < 1.0 && fabsf(acc_dete[2]) < 1.0){
sarahbest 3:788eecfd5ae9 261 return DETECT_ORIENTATION_LEFT;//[0 g 0]
sarahbest 3:788eecfd5ae9 262 }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1] + IMU_ONE_G) < 1.0 && fabsf(acc_dete[2]) < 1.0){
sarahbest 3:788eecfd5ae9 263 return DETECT_ORIENTATION_RIGHT;//[0 -g 0]
sarahbest 3:788eecfd5ae9 264 }else if(fabsf(acc_dete[0] - IMU_ONE_G) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2]) < 1.0){
sarahbest 3:788eecfd5ae9 265 return DETECT_ORIENTATION_TAIL_DOWN;//[g 0 0]
sarahbest 3:788eecfd5ae9 266 }else if(fabsf(acc_dete[0] + IMU_ONE_G) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2]) < 1.0){
sarahbest 3:788eecfd5ae9 267 return DETECT_ORIENTATION_NOSE_DOWN;//[-g 0 0]
sarahbest 3:788eecfd5ae9 268 }else{
sarahbest 3:788eecfd5ae9 269 return DETECT_ORIENTATION_ERROR;
sarahbest 3:788eecfd5ae9 270 }
sarahbest 3:788eecfd5ae9 271 }
sarahbest 3:788eecfd5ae9 272
sarahbest 3:788eecfd5ae9 273 void IMU_compensate()
sarahbest 3:788eecfd5ae9 274 {
sarahbest 3:788eecfd5ae9 275 int k;
sarahbest 3:788eecfd5ae9 276 for(k=0;k<3;k++)
sarahbest 3:788eecfd5ae9 277 {
sarahbest 3:788eecfd5ae9 278 acc_comp[k] = acc[k] - acc_off[k];
sarahbest 3:788eecfd5ae9 279 gyro_comp[k] = gyro[k] - gyro_off[k];
sarahbest 3:788eecfd5ae9 280 }
BaserK 0:d8d055e8f830 281
sarahbest 3:788eecfd5ae9 282 }