CID10DOF.cpp

00001 #include "CID10DOF.h"
00002 #include <math.h> 
00003 #include "rtos.h"
00004 #include "Matrix.h"
00005 #include "Servo.h" 
00006 #include "PIDcontroller.h"
00007 
00008 #ifndef min
00009 #define min(a,b) ( (a) < (b) ? (a) : (b) )
00010 #endif
00011 
00012 #ifndef max
00013 #define max(a,b) ( (a) > (b) ? (a) : (b) )
00014 #endif
00015 
00016 PID PID_ROLL(kp, ki, kd, Td);
00017 PID PID_PITCH(kp, ki, kd, Td);
00018 
00019 FIS_TYPE g_fisInput[fis_gcI];
00020 FIS_TYPE g_fisOutput[fis_gcO];
00021 
00022 
00023 CID10DOF::CID10DOF(PinName sda, PinName scl,PinName FL, PinName FR, PinName BL, PinName BR) : mpu9250(sda,scl),pc(USBTX,USBRX),telem(PC_0, PC_1)
00024 {
00025 
00026     pc.baud(115200);
00027     telem.baud(57600); 
00028     
00029     motor[0] = new Servo (FL);  //motors are of class Servo as ESC are used in the similar manner
00030     motor[1] = new Servo (FR);  
00031     motor[2] = new Servo (BL);
00032     motor[3] = new Servo (BR);
00033     
00034     min_calibrate = 0.0;
00035     max_calibrate = 1.0;
00036          
00037     e = 0;
00038     last_e = 0;  
00039     l = 1.0f;
00040 
00041     
00042 }
00043 
00044 /**
00045  * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
00046 */
00047 
00048 void CID10DOF::MPU9250init()
00049 {
00050 
00051  uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00052     pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
00053   
00054     if (whoami == 0x71) // WHO_AM_I should always be 0x68
00055     {  
00056           
00057         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00058         mpu9250.MPU9250SelfTest(SelfTest);
00059         mpu9250.getAres(); // Get accelerometer sensitivity
00060         mpu9250.getGres(); // Get gyro sensitivity
00061         mpu9250.getMres(); // Get magnetometer sensitivity
00062         mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
00063         wait(2);  
00064         
00065         mpu9250.initMPU9250();
00066         pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00067         wait(1);
00068 
00069         mpu9250.initAK8963(magCalibration);
00070         pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
00071         pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00072         pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00073         
00074         if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
00075         if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
00076         if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
00077         if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
00078 
00079         pc.printf("Mag Calibration: Wave device in a figure eight until done!");
00080         wait(4);
00081         
00082         magbias[0] = -41.563381;
00083         magbias[1] = 165.252426;
00084         magbias[2] = 55.095879;
00085         
00086         magScale[0] = 0.866279;
00087         magScale[1] = 1.087591;
00088         magScale[2] = 1.079710;
00089         
00090         //mpu9250.magcalMPU9250(magbias, magScale);
00091         pc.printf("Mag Calibration done!\n\r");
00092         pc.printf("x mag bias = %f\n\r", magbias[0]);
00093         pc.printf("y mag bias = %f\n\r", magbias[1]);
00094         pc.printf("z mag bias = %f\n\r", magbias[2]);
00095         //pc.printf("Mag Calibration done!\n\r");
00096         pc.printf("x mag Scale = %f\n\r", magScale[0]);
00097         pc.printf("y mag Scale = %f\n\r", magScale[1]);
00098         pc.printf("z mag Scale = %f\n\r", magScale[2]);
00099         wait(2);
00100         
00101         
00102     }else{
00103         
00104         while(1) ; // Loop forever if communication doesn't happen
00105     }
00106  
00107         mpu9250.getAres(); // Get accelerometer sensitivity
00108         mpu9250.getGres(); // Get gyro sensitivity
00109         mpu9250.getMres(); // Get magnetometer sensitivity
00110     
00111         t.start();
00112     
00113 }
00114 
00115 void CID10DOF::MPU9250GetValues()
00116 {
00117     mpu9250.readAccelData(accelCount);  
00118         
00119     ax = (float)accelCount[0]*aRes - accelBias[0]; 
00120     ay = (float)accelCount[1]*aRes - accelBias[1];   
00121     az = (float)accelCount[2]*aRes - accelBias[2]; 
00122     
00123     mpu9250.readGyroData(gyroCount);  
00124  
00125     gx = (float)gyroCount[0]*gRes - gyroBias[0];  
00126     gy = (float)gyroCount[1]*gRes - gyroBias[1];  
00127     gz = (float)gyroCount[2]*gRes - gyroBias[2];   
00128   
00129     mpu9250.readMagData(magCount);    
00130     
00131     mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  
00132     my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
00133     mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
00134     mx *= magScale[0]; // poor man's soft iron calibration
00135     my *= magScale[1];
00136     mz *= magScale[2];   
00137         
00138     tempCount = mpu9250.readTempData();  
00139     temperature = ((float) tempCount) / 333.87f + 21.0f;   
00140 }
00141 
00142 void CID10DOF::MPU9250getYawPitchRoll()
00143 {
00144       yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
00145       pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00146       roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
00147      
00148       //float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
00149       //float Yh = my*cos(roll)+mz*sin(roll);
00150  
00151       //yawmag = atan2(Yh,Xh)+PI;
00152         
00153       pitch *= 180.0f / PI;
00154       yaw   *= 180.0f / PI; 
00155       yaw   -= 4.28; // 
00156       if(yaw < 0) yaw   += 360.0f;
00157       roll  *= 180.0f / PI;
00158      
00159 }
00160 
00161 void CID10DOF::MPU9250ReadAxis(double * dat)
00162 {
00163             
00164            if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {    
00165                 MPU9250GetValues();
00166            }
00167             
00168             Now = t.read_us();
00169             deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00170             lastUpdate = Now;
00171         
00172             sum += deltat;
00173             sumCount++;
00174             
00175            
00176             
00177             mpu9250.MahonyQuaternionUpdate( ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00178         
00179             delt_t = t.read_ms() - count;    
00180                
00181             if (delt_t > 50) { 
00182                 MPU9250getYawPitchRoll();
00183                 sum = 0;
00184                 sumCount = 0; 
00185                  dat[0] = yaw;
00186             dat[1] = pitch;
00187             dat[2] = roll;
00188             }
00189             
00190             
00191 }
00192 
00193 //----------------------------Function for ESC calibration---------------------
00194 void CID10DOF::Calibrate_Motors()
00195 {
00196     for (int i = 0; i < 4; i++){    //run motors for some time in min speed
00197         *motor[i] = max_calibrate;
00198     }   
00199     wait(6.0);                      //wait for the response from ESC modules
00200     for (int i = 0; i < 4; i++){
00201         *motor[i] = min_calibrate;  //run motors at maximum speed
00202     }
00203     wait(2.0);  
00204 }
00205 
00206 //--------------------------Function for setting calibration limits-----------
00207 void CID10DOF::setLimits(double min, double max)
00208 {
00209   if (min > max){             //here detect if someone tried making min to be more than max. If that is the case, then flip them together
00210         min_calibrate = max;
00211         max_calibrate = min;   
00212     } else {   
00213         min_calibrate = min;
00214         max_calibrate = max;
00215     }    
00216     if ((min_calibrate > 1.0) || (min_calibrate < 0.0)) //here chech if values are in correct range. If they are not, make them to be in correct range
00217         min_calibrate = 0.0;
00218     if ((max_calibrate > 1.0) || (max_calibrate < 0.0))
00219         max_calibrate = 1.0;
00220 }
00221 
00222 //-------------------------------------Function for Stabilising---------------
00223 void CID10DOF::stabilise(double *speed, double *actSpeed, double *Diff){
00224   
00225     
00226 }
00227 
00228 //-----------------------Function for producing thrust in Z direction --------
00229 void CID10DOF::run (double *speed){
00230     for (int i = 0; i < 4; i++){
00231             *motor[i] = speed[i];
00232     }
00233 }
00234  
00235 void CID10DOF::PID_Config(double *St_point, double *bias)
00236 {
00237    PID_ROLL.setInputLimits(IN_MIN_ROLL,IN_MAX_ROLL);
00238    PID_ROLL.setOutputLimits(OUT_MIN_ROLL, OUT_MAX_ROLL);
00239    PID_ROLL.setBias(0.0);  
00240      
00241    PID_PITCH.setInputLimits(IN_MIN_PITCH,IN_MAX_PITCH);
00242    PID_PITCH.setOutputLimits(OUT_MIN_PITCH, OUT_MAX_PITCH); 
00243    PID_PITCH.setBias(0.0);  
00244    
00245 }
00246 
00247 void  CID10DOF::PID_Run(double *processVariable, double *setPoint,double *pid_diff)
00248 {
00249      
00250      PID_ROLL.setProcessValue(processVariable[2]);
00251      PID_ROLL.setSetPoint(setPoint[1]);
00252      pid_diff[2] = PID_ROLL.compute();
00253      
00254      PID_PITCH.setProcessValue(processVariable[1]);
00255      PID_PITCH.setSetPoint(setPoint[1]);
00256      pid_diff[1] = PID_PITCH.compute();
00257     
00258 }
00259 
00260 float CID10DOF::FLC_roll(float Phi, float dPhi, float iPhi)
00261 {
00262     g_fisInput[0] = Phi;// Read Input: Phi
00263     g_fisInput[1] = dPhi;// Read Input: dPhi
00264     g_fisInput[2] = iPhi;// Read Input: iPhi
00265     g_fisOutput[0] = 0;
00266     fis_evaluate();
00267     
00268     return g_fisOutput[0];
00269 }
00270 
00271 
00272 
00273 //***********************************************************************
00274 // Support functions for Fuzzy Inference System                          
00275 //***********************************************************************
00276 // Trapezoidal Member Function
00277 FIS_TYPE fis_trapmf(FIS_TYPE x, FIS_TYPE* p)
00278 {
00279     FIS_TYPE a = p[0], b = p[1], c = p[2], d = p[3];
00280     FIS_TYPE t1 = ((x <= c) ? 1 : ((d < x) ? 0 : ((c != d) ? ((d - x) / (d - c)) : 0)));
00281     FIS_TYPE t2 = ((b <= x) ? 1 : ((x < a) ? 0 : ((a != b) ? ((x - a) / (b - a)) : 0)));
00282     return (FIS_TYPE) min(t1, t2);
00283 }
00284 
00285 // Triangular Member Function
00286 FIS_TYPE fis_trimf(FIS_TYPE x, FIS_TYPE* p)
00287 {
00288     FIS_TYPE a = p[0], b = p[1], c = p[2];
00289     FIS_TYPE t1 = (x - a) / (b - a);
00290     FIS_TYPE t2 = (c - x) / (c - b);
00291     if ((a == b) && (b == c)) return (FIS_TYPE) (x == a);
00292     if (a == b) return (FIS_TYPE) (t2*(b <= x)*(x <= c));
00293     if (b == c) return (FIS_TYPE) (t1*(a <= x)*(x <= b));
00294     t1 = min(t1, t2);
00295     return (FIS_TYPE) max(t1, 0);
00296 }
00297 
00298 FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
00299 {
00300     return min(a, b);
00301 }
00302 
00303 FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
00304 {
00305     return max(a, b);
00306 }
00307 
00308 FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
00309 {
00310     int i;
00311     FIS_TYPE ret = 0;
00312 
00313     if (size == 0) return ret;
00314     if (size == 1) return array[0];
00315 
00316     ret = array[0];
00317     for (i = 1; i < size; i++)
00318     {
00319         ret = (*pfnOp)(ret, array[i]);
00320     }
00321 
00322     return ret;
00323 }
00324 
00325 
00326 //***********************************************************************
00327 // Data for Fuzzy Inference System                                       
00328 //***********************************************************************
00329 // Pointers to the implementations of member functions
00330 _FIS_MF fis_gMF[] =
00331 {
00332     fis_trapmf, fis_trimf
00333 };
00334 
00335 // Count of member function for each Input
00336 int fis_gIMFCount[] = { 3, 3, 3 };
00337 
00338 // Count of member function for each Output 
00339 int fis_gOMFCount[] = { 5 };
00340 
00341 // Coefficients for the Input Member Functions
00342 FIS_TYPE fis_gMFI0Coeff1[] = { -10, -10, -0.2, 0 };
00343 FIS_TYPE fis_gMFI0Coeff2[] = { -0.1, 0, 0.1 };
00344 FIS_TYPE fis_gMFI0Coeff3[] = { 0, 0.2, 10, 10 };
00345 FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3 };
00346 FIS_TYPE fis_gMFI1Coeff1[] = { -20, -20, -1, -0.25 };
00347 FIS_TYPE fis_gMFI1Coeff2[] = { -1, 0, 1 };
00348 FIS_TYPE fis_gMFI1Coeff3[] = { 0.25, 1, 20, 20 };
00349 FIS_TYPE* fis_gMFI1Coeff[] = { fis_gMFI1Coeff1, fis_gMFI1Coeff2, fis_gMFI1Coeff3 };
00350 FIS_TYPE fis_gMFI2Coeff1[] = { -20, -20, -0.8, -0.25 };
00351 FIS_TYPE fis_gMFI2Coeff2[] = { -0.6, 0, 0.6 };
00352 FIS_TYPE fis_gMFI2Coeff3[] = { 0, 1, 20, 20 };
00353 FIS_TYPE* fis_gMFI2Coeff[] = { fis_gMFI2Coeff1, fis_gMFI2Coeff2, fis_gMFI2Coeff3 };
00354 FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff, fis_gMFI1Coeff, fis_gMFI2Coeff };
00355 
00356 // Coefficients for the Input Member Functions
00357 FIS_TYPE fis_gMFO0Coeff1[] = { -0.1, -0.1, -0.05, -0.045 };
00358 FIS_TYPE fis_gMFO0Coeff2[] = { -0.05, -0.045, -0.01, -0.003 };
00359 FIS_TYPE fis_gMFO0Coeff3[] = { -0.01, 0, 0.01 };
00360 FIS_TYPE fis_gMFO0Coeff4[] = { 0.003, 0.01, 0.045, 0.05 };
00361 FIS_TYPE fis_gMFO0Coeff5[] = { 0.0455, 0.05, 0.1, 0.1 };
00362 FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
00363 FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };
00364 
00365 // Input membership function set
00366 int fis_gMFI0[] = { 0, 1, 0 };
00367 int fis_gMFI1[] = { 0, 1, 0 };
00368 int fis_gMFI2[] = { 0, 1, 0 };
00369 int* fis_gMFI[] = { fis_gMFI0, fis_gMFI1, fis_gMFI2};
00370 
00371 // Output membership function set
00372 int fis_gMFO0[] = { 0, 0, 1, 0, 0 };
00373 int* fis_gMFO[] = { fis_gMFO0};
00374 
00375 // Rule Weights
00376 FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };
00377 
00378 // Rule Type
00379 int fis_gRType[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };
00380 
00381 // Rule Inputs
00382 int fis_gRI0[] = { 1, 1, 1 };
00383 int fis_gRI1[] = { 2, 1, 1 };
00384 int fis_gRI2[] = { 3, 1, 1 };
00385 int fis_gRI3[] = { 1, 1, 2 };
00386 int fis_gRI4[] = { 2, 1, 2 };
00387 int fis_gRI5[] = { 3, 1, 2 };
00388 int fis_gRI6[] = { 1, 1, 3 };
00389 int fis_gRI7[] = { 2, 1, 3 };
00390 int fis_gRI8[] = { 3, 1, 3 };
00391 int fis_gRI9[] = { 1, 2, 1 };
00392 int fis_gRI10[] = { 2, 2, 1 };
00393 int fis_gRI11[] = { 3, 2, 1 };
00394 int fis_gRI12[] = { 1, 2, 2 };
00395 int fis_gRI13[] = { 2, 2, 2 };
00396 int fis_gRI14[] = { 3, 2, 2 };
00397 int fis_gRI15[] = { 1, 2, 3 };
00398 int fis_gRI16[] = { 2, 2, 3 };
00399 int fis_gRI17[] = { 3, 2, 3 };
00400 int fis_gRI18[] = { 1, 3, 1 };
00401 int fis_gRI19[] = { 2, 3, 1 };
00402 int fis_gRI20[] = { 3, 3, 1 };
00403 int fis_gRI21[] = { 1, 3, 2 };
00404 int fis_gRI22[] = { 2, 3, 2 };
00405 int fis_gRI23[] = { 3, 3, 2 };
00406 int fis_gRI24[] = { 1, 3, 3 };
00407 int fis_gRI25[] = { 2, 3, 3 };
00408 int fis_gRI26[] = { 3, 3, 3 };
00409 int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4, fis_gRI5, fis_gRI6, fis_gRI7, fis_gRI8, fis_gRI9, fis_gRI10, fis_gRI11, fis_gRI12, fis_gRI13, fis_gRI14, fis_gRI15, fis_gRI16, fis_gRI17, fis_gRI18, fis_gRI19, fis_gRI20, fis_gRI21, fis_gRI22, fis_gRI23, fis_gRI24, fis_gRI25, fis_gRI26 };
00410 
00411 // Rule Outputs
00412 int fis_gRO0[] = { 5 };
00413 int fis_gRO1[] = { 4 };
00414 int fis_gRO2[] = { 2 };
00415 int fis_gRO3[] = { 5 };
00416 int fis_gRO4[] = { 4 };
00417 int fis_gRO5[] = { 2 };
00418 int fis_gRO6[] = { 5 };
00419 int fis_gRO7[] = { 4 };
00420 int fis_gRO8[] = { 2 };
00421 int fis_gRO9[] = { 4 };
00422 int fis_gRO10[] = { 4 };
00423 int fis_gRO11[] = { 2 };
00424 int fis_gRO12[] = { 4 };
00425 int fis_gRO13[] = { 3 };
00426 int fis_gRO14[] = { 2 };
00427 int fis_gRO15[] = { 4 };
00428 int fis_gRO16[] = { 2 };
00429 int fis_gRO17[] = { 2 };
00430 int fis_gRO18[] = { 4 };
00431 int fis_gRO19[] = { 2 };
00432 int fis_gRO20[] = { 1 };
00433 int fis_gRO21[] = { 4 };
00434 int fis_gRO22[] = { 2 };
00435 int fis_gRO23[] = { 1 };
00436 int fis_gRO24[] = { 4 };
00437 int fis_gRO25[] = { 2 };
00438 int fis_gRO26[] = { 1 };
00439 int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4, fis_gRO5, fis_gRO6, fis_gRO7, fis_gRO8, fis_gRO9, fis_gRO10, fis_gRO11, fis_gRO12, fis_gRO13, fis_gRO14, fis_gRO15, fis_gRO16, fis_gRO17, fis_gRO18, fis_gRO19, fis_gRO20, fis_gRO21, fis_gRO22, fis_gRO23, fis_gRO24, fis_gRO25, fis_gRO26 };
00440 
00441 // Input range Min
00442 FIS_TYPE fis_gIMin[] = { -10, -20, -20 };
00443 
00444 // Input range Max
00445 FIS_TYPE fis_gIMax[] = { 10, 20, 20 };
00446 
00447 // Output range Min
00448 FIS_TYPE fis_gOMin[] = { -0.1 };
00449 
00450 // Output range Max
00451 FIS_TYPE fis_gOMax[] = { 0.1 };
00452 
00453 //***********************************************************************
00454 // Data dependent support functions for Fuzzy Inference System                          
00455 //***********************************************************************
00456 FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)
00457 {
00458     FIS_TYPE mfOut;
00459     int r;
00460 
00461     for (r = 0; r < fis_gcR; ++r)
00462     {
00463         int index = fis_gRO[r][o];
00464         if (index > 0)
00465         {
00466             index = index - 1;
00467             mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
00468         }
00469         else if (index < 0)
00470         {
00471             index = -index - 1;
00472             mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
00473         }
00474         else
00475         {
00476             mfOut = 0;
00477         }
00478 
00479         fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
00480     }
00481     return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
00482 }
00483 
00484 FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
00485 {
00486     FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
00487     FIS_TYPE area = 0;
00488     FIS_TYPE momentum = 0;
00489     FIS_TYPE dist, slice;
00490     int i;
00491 
00492     // calculate the area under the curve formed by the MF outputs
00493     for (i = 0; i < FIS_RESOLUSION; ++i){
00494         dist = fis_gOMin[o] + (step * i);
00495         slice = step * fis_MF_out(fuzzyRuleSet, dist, o);
00496         area += slice;
00497         momentum += slice*dist;
00498     }
00499 
00500     return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
00501 }
00502 
00503 //***********************************************************************
00504 // Fuzzy Inference System                                                
00505 //***********************************************************************
00506 void CID10DOF::fis_evaluate()
00507 {
00508     FIS_TYPE fuzzyInput0[] = { 0, 0, 0 };
00509     FIS_TYPE fuzzyInput1[] = { 0, 0, 0 };
00510     FIS_TYPE fuzzyInput2[] = { 0, 0, 0 };
00511     FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, fuzzyInput1, fuzzyInput2, };
00512     FIS_TYPE fuzzyOutput0[] = { 0, 0, 0, 0, 0 };
00513     FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, };
00514     FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
00515     FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
00516     FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
00517     FIS_TYPE sW = 0;
00518 
00519     // Transforming input to fuzzy Input
00520     int i, j, r, o;
00521     for (i = 0; i < fis_gcI; ++i)
00522     {
00523         for (j = 0; j < fis_gIMFCount[i]; ++j)
00524         {
00525             fuzzyInput[i][j] =
00526                 (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
00527         }
00528     }
00529 
00530     int index = 0;
00531     for (r = 0; r < fis_gcR; ++r)
00532     {
00533         if (fis_gRType[r] == 1)
00534         {
00535             fuzzyFires[r] = FIS_MAX;
00536             for (i = 0; i < fis_gcI; ++i)
00537             {
00538                 index = fis_gRI[r][i];
00539                 if (index > 0)
00540                     fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
00541                 else if (index < 0)
00542                     fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
00543                 else
00544                     fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
00545             }
00546         }
00547         else
00548         {
00549             fuzzyFires[r] = FIS_MIN;
00550             for (i = 0; i < fis_gcI; ++i)
00551             {
00552                 index = fis_gRI[r][i];
00553                 if (index > 0)
00554                     fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);
00555                 else if (index < 0)
00556                     fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
00557                 else
00558                     fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
00559             }
00560         }
00561 
00562         fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
00563         sW += fuzzyFires[r];
00564     }
00565 
00566     if (sW == 0)
00567     {
00568         for (o = 0; o < fis_gcO; ++o)
00569         {
00570             g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
00571         }
00572     }
00573     else
00574     {
00575         for (o = 0; o < fis_gcO; ++o)
00576         {
00577             g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);
00578         }
00579     }
00580 }
00581 
00582 
00583 
00584  
00585 
00586 const float def_sea_press = 1013.25;
00587 
00588 
00589 
00590 float CID10DOF::getBaroTem()
00591 {
00592    
00593 }
00594 
00595 float CID10DOF::getBaroPress()
00596 {
00597     
00598 }
00599 
00600 float CID10DOF::getBaroAlt()
00601 {
00602       
00603 }
00604 
00605 
00606 float invSqrt(float number)
00607 {
00608     volatile long i;
00609     volatile float x, y;
00610     volatile const float f = 1.5F;
00611 
00612     x = number * 0.5F;
00613     y = number;
00614     i = * ( long * ) &y;
00615     i = 0x5f375a86 - ( i >> 1 );
00616     y = * ( float * ) &i;
00617     y = y * ( f - ( x * y * y ) );
00618     return y;
00619 }
00620 
00621 double CID10DOF::map(double x, double in_min, double in_max, double out_min, double out_max){   //simply maps values in the given range
00622     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00623 }
00624 
00625 
00626 
00627 
00628