Alan Huchin Herrera / Mbed 2 deprecated Nucleo_MPU_9250_

Quadrirotor

Dependencies:   CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed

Fork of Nucleo_MPU_9250 by Alan Huchin Herrera

CID10DOF/CID10DOF.cpp@0:89cf0851969b, 2018-06-26 (annotated)

Committer:
AlanHuchin
Date:
Tue Jun 26 18:24:45 2018 +0000
Revision:
0:89cf0851969b
hello

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AlanHuchin 0:89cf0851969b 1 #include "CID10DOF.h"
AlanHuchin 0:89cf0851969b 2 #include <math.h>
AlanHuchin 0:89cf0851969b 3 #include "rtos.h"
AlanHuchin 0:89cf0851969b 4 #include "Matrix.h"
AlanHuchin 0:89cf0851969b 5 #include "Servo.h"
AlanHuchin 0:89cf0851969b 6 #include "PIDcontroller.h"
AlanHuchin 0:89cf0851969b 7
AlanHuchin 0:89cf0851969b 8 #ifndef min
AlanHuchin 0:89cf0851969b 9 #define min(a,b) ( (a) < (b) ? (a) : (b) )
AlanHuchin 0:89cf0851969b 10 #endif
AlanHuchin 0:89cf0851969b 11
AlanHuchin 0:89cf0851969b 12 #ifndef max
AlanHuchin 0:89cf0851969b 13 #define max(a,b) ( (a) > (b) ? (a) : (b) )
AlanHuchin 0:89cf0851969b 14 #endif
AlanHuchin 0:89cf0851969b 15
AlanHuchin 0:89cf0851969b 16 PID PID_ROLL(kp, ki, kd, Td);
AlanHuchin 0:89cf0851969b 17 PID PID_PITCH(kp, ki, kd, Td);
AlanHuchin 0:89cf0851969b 18
AlanHuchin 0:89cf0851969b 19 FIS_TYPE g_fisInput[fis_gcI];
AlanHuchin 0:89cf0851969b 20 FIS_TYPE g_fisOutput[fis_gcO];
AlanHuchin 0:89cf0851969b 21
AlanHuchin 0:89cf0851969b 22
AlanHuchin 0:89cf0851969b 23 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)
AlanHuchin 0:89cf0851969b 24 {
AlanHuchin 0:89cf0851969b 25
AlanHuchin 0:89cf0851969b 26 pc.baud(115200);
AlanHuchin 0:89cf0851969b 27 telem.baud(57600);
AlanHuchin 0:89cf0851969b 28
AlanHuchin 0:89cf0851969b 29 motor[0] = new Servo (FL); //motors are of class Servo as ESC are used in the similar manner
AlanHuchin 0:89cf0851969b 30 motor[1] = new Servo (FR);
AlanHuchin 0:89cf0851969b 31 motor[2] = new Servo (BL);
AlanHuchin 0:89cf0851969b 32 motor[3] = new Servo (BR);
AlanHuchin 0:89cf0851969b 33
AlanHuchin 0:89cf0851969b 34 min_calibrate = 0.0;
AlanHuchin 0:89cf0851969b 35 max_calibrate = 1.0;
AlanHuchin 0:89cf0851969b 36
AlanHuchin 0:89cf0851969b 37 e = 0;
AlanHuchin 0:89cf0851969b 38 last_e = 0;
AlanHuchin 0:89cf0851969b 39 l = 1.0f;
AlanHuchin 0:89cf0851969b 40
AlanHuchin 0:89cf0851969b 41
AlanHuchin 0:89cf0851969b 42 }
AlanHuchin 0:89cf0851969b 43
AlanHuchin 0:89cf0851969b 44 /**
AlanHuchin 0:89cf0851969b 45 * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
AlanHuchin 0:89cf0851969b 46 */
AlanHuchin 0:89cf0851969b 47
AlanHuchin 0:89cf0851969b 48 void CID10DOF::MPU9250init()
AlanHuchin 0:89cf0851969b 49 {
AlanHuchin 0:89cf0851969b 50
AlanHuchin 0:89cf0851969b 51 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
AlanHuchin 0:89cf0851969b 52 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
AlanHuchin 0:89cf0851969b 53
AlanHuchin 0:89cf0851969b 54 if (whoami == 0x71) // WHO_AM_I should always be 0x68
AlanHuchin 0:89cf0851969b 55 {
AlanHuchin 0:89cf0851969b 56
AlanHuchin 0:89cf0851969b 57 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
AlanHuchin 0:89cf0851969b 58 mpu9250.MPU9250SelfTest(SelfTest);
AlanHuchin 0:89cf0851969b 59 mpu9250.getAres(); // Get accelerometer sensitivity
AlanHuchin 0:89cf0851969b 60 mpu9250.getGres(); // Get gyro sensitivity
AlanHuchin 0:89cf0851969b 61 mpu9250.getMres(); // Get magnetometer sensitivity
AlanHuchin 0:89cf0851969b 62 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
AlanHuchin 0:89cf0851969b 63 wait(2);
AlanHuchin 0:89cf0851969b 64
AlanHuchin 0:89cf0851969b 65 mpu9250.initMPU9250();
AlanHuchin 0:89cf0851969b 66 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
AlanHuchin 0:89cf0851969b 67 wait(1);
AlanHuchin 0:89cf0851969b 68
AlanHuchin 0:89cf0851969b 69 mpu9250.initAK8963(magCalibration);
AlanHuchin 0:89cf0851969b 70 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
AlanHuchin 0:89cf0851969b 71 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
AlanHuchin 0:89cf0851969b 72 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
AlanHuchin 0:89cf0851969b 73
AlanHuchin 0:89cf0851969b 74 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
AlanHuchin 0:89cf0851969b 75 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
AlanHuchin 0:89cf0851969b 76 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
AlanHuchin 0:89cf0851969b 77 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
AlanHuchin 0:89cf0851969b 78
AlanHuchin 0:89cf0851969b 79 pc.printf("Mag Calibration: Wave device in a figure eight until done!");
AlanHuchin 0:89cf0851969b 80 wait(4);
AlanHuchin 0:89cf0851969b 81
AlanHuchin 0:89cf0851969b 82 magbias[0] = -41.563381;
AlanHuchin 0:89cf0851969b 83 magbias[1] = 165.252426;
AlanHuchin 0:89cf0851969b 84 magbias[2] = 55.095879;
AlanHuchin 0:89cf0851969b 85
AlanHuchin 0:89cf0851969b 86 magScale[0] = 0.866279;
AlanHuchin 0:89cf0851969b 87 magScale[1] = 1.087591;
AlanHuchin 0:89cf0851969b 88 magScale[2] = 1.079710;
AlanHuchin 0:89cf0851969b 89
AlanHuchin 0:89cf0851969b 90 //mpu9250.magcalMPU9250(magbias, magScale);
AlanHuchin 0:89cf0851969b 91 pc.printf("Mag Calibration done!\n\r");
AlanHuchin 0:89cf0851969b 92 pc.printf("x mag bias = %f\n\r", magbias[0]);
AlanHuchin 0:89cf0851969b 93 pc.printf("y mag bias = %f\n\r", magbias[1]);
AlanHuchin 0:89cf0851969b 94 pc.printf("z mag bias = %f\n\r", magbias[2]);
AlanHuchin 0:89cf0851969b 95 //pc.printf("Mag Calibration done!\n\r");
AlanHuchin 0:89cf0851969b 96 pc.printf("x mag Scale = %f\n\r", magScale[0]);
AlanHuchin 0:89cf0851969b 97 pc.printf("y mag Scale = %f\n\r", magScale[1]);
AlanHuchin 0:89cf0851969b 98 pc.printf("z mag Scale = %f\n\r", magScale[2]);
AlanHuchin 0:89cf0851969b 99 wait(2);
AlanHuchin 0:89cf0851969b 100
AlanHuchin 0:89cf0851969b 101
AlanHuchin 0:89cf0851969b 102 }else{
AlanHuchin 0:89cf0851969b 103
AlanHuchin 0:89cf0851969b 104 while(1) ; // Loop forever if communication doesn't happen
AlanHuchin 0:89cf0851969b 105 }
AlanHuchin 0:89cf0851969b 106
AlanHuchin 0:89cf0851969b 107 mpu9250.getAres(); // Get accelerometer sensitivity
AlanHuchin 0:89cf0851969b 108 mpu9250.getGres(); // Get gyro sensitivity
AlanHuchin 0:89cf0851969b 109 mpu9250.getMres(); // Get magnetometer sensitivity
AlanHuchin 0:89cf0851969b 110
AlanHuchin 0:89cf0851969b 111 t.start();
AlanHuchin 0:89cf0851969b 112
AlanHuchin 0:89cf0851969b 113 }
AlanHuchin 0:89cf0851969b 114
AlanHuchin 0:89cf0851969b 115 void CID10DOF::MPU9250GetValues()
AlanHuchin 0:89cf0851969b 116 {
AlanHuchin 0:89cf0851969b 117 mpu9250.readAccelData(accelCount);
AlanHuchin 0:89cf0851969b 118
AlanHuchin 0:89cf0851969b 119 ax = (float)accelCount[0]*aRes - accelBias[0];
AlanHuchin 0:89cf0851969b 120 ay = (float)accelCount[1]*aRes - accelBias[1];
AlanHuchin 0:89cf0851969b 121 az = (float)accelCount[2]*aRes - accelBias[2];
AlanHuchin 0:89cf0851969b 122
AlanHuchin 0:89cf0851969b 123 mpu9250.readGyroData(gyroCount);
AlanHuchin 0:89cf0851969b 124
AlanHuchin 0:89cf0851969b 125 gx = (float)gyroCount[0]*gRes - gyroBias[0];
AlanHuchin 0:89cf0851969b 126 gy = (float)gyroCount[1]*gRes - gyroBias[1];
AlanHuchin 0:89cf0851969b 127 gz = (float)gyroCount[2]*gRes - gyroBias[2];
AlanHuchin 0:89cf0851969b 128
AlanHuchin 0:89cf0851969b 129 mpu9250.readMagData(magCount);
AlanHuchin 0:89cf0851969b 130
AlanHuchin 0:89cf0851969b 131 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];
AlanHuchin 0:89cf0851969b 132 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
AlanHuchin 0:89cf0851969b 133 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
AlanHuchin 0:89cf0851969b 134 mx *= magScale[0]; // poor man's soft iron calibration
AlanHuchin 0:89cf0851969b 135 my *= magScale[1];
AlanHuchin 0:89cf0851969b 136 mz *= magScale[2];
AlanHuchin 0:89cf0851969b 137
AlanHuchin 0:89cf0851969b 138 tempCount = mpu9250.readTempData();
AlanHuchin 0:89cf0851969b 139 temperature = ((float) tempCount) / 333.87f + 21.0f;
AlanHuchin 0:89cf0851969b 140 }
AlanHuchin 0:89cf0851969b 141
AlanHuchin 0:89cf0851969b 142 void CID10DOF::MPU9250getYawPitchRoll()
AlanHuchin 0:89cf0851969b 143 {
AlanHuchin 0:89cf0851969b 144 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]);
AlanHuchin 0:89cf0851969b 145 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
AlanHuchin 0:89cf0851969b 146 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]);
AlanHuchin 0:89cf0851969b 147
AlanHuchin 0:89cf0851969b 148 //float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
AlanHuchin 0:89cf0851969b 149 //float Yh = my*cos(roll)+mz*sin(roll);
AlanHuchin 0:89cf0851969b 150
AlanHuchin 0:89cf0851969b 151 //yawmag = atan2(Yh,Xh)+PI;
AlanHuchin 0:89cf0851969b 152
AlanHuchin 0:89cf0851969b 153 pitch *= 180.0f / PI;
AlanHuchin 0:89cf0851969b 154 yaw *= 180.0f / PI;
AlanHuchin 0:89cf0851969b 155 yaw -= 4.28; //
AlanHuchin 0:89cf0851969b 156 if(yaw < 0) yaw += 360.0f;
AlanHuchin 0:89cf0851969b 157 roll *= 180.0f / PI;
AlanHuchin 0:89cf0851969b 158
AlanHuchin 0:89cf0851969b 159 }
AlanHuchin 0:89cf0851969b 160
AlanHuchin 0:89cf0851969b 161 void CID10DOF::MPU9250ReadAxis(double * dat)
AlanHuchin 0:89cf0851969b 162 {
AlanHuchin 0:89cf0851969b 163
AlanHuchin 0:89cf0851969b 164 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
AlanHuchin 0:89cf0851969b 165 MPU9250GetValues();
AlanHuchin 0:89cf0851969b 166 }
AlanHuchin 0:89cf0851969b 167
AlanHuchin 0:89cf0851969b 168 Now = t.read_us();
AlanHuchin 0:89cf0851969b 169 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
AlanHuchin 0:89cf0851969b 170 lastUpdate = Now;
AlanHuchin 0:89cf0851969b 171
AlanHuchin 0:89cf0851969b 172 sum += deltat;
AlanHuchin 0:89cf0851969b 173 sumCount++;
AlanHuchin 0:89cf0851969b 174
AlanHuchin 0:89cf0851969b 175
AlanHuchin 0:89cf0851969b 176
AlanHuchin 0:89cf0851969b 177 mpu9250.MahonyQuaternionUpdate( ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
AlanHuchin 0:89cf0851969b 178
AlanHuchin 0:89cf0851969b 179 delt_t = t.read_ms() - count;
AlanHuchin 0:89cf0851969b 180
AlanHuchin 0:89cf0851969b 181 if (delt_t > 50) {
AlanHuchin 0:89cf0851969b 182 MPU9250getYawPitchRoll();
AlanHuchin 0:89cf0851969b 183 sum = 0;
AlanHuchin 0:89cf0851969b 184 sumCount = 0;
AlanHuchin 0:89cf0851969b 185 dat[0] = yaw;
AlanHuchin 0:89cf0851969b 186 dat[1] = pitch;
AlanHuchin 0:89cf0851969b 187 dat[2] = roll;
AlanHuchin 0:89cf0851969b 188 }
AlanHuchin 0:89cf0851969b 189
AlanHuchin 0:89cf0851969b 190
AlanHuchin 0:89cf0851969b 191 }
AlanHuchin 0:89cf0851969b 192
AlanHuchin 0:89cf0851969b 193 //----------------------------Function for ESC calibration---------------------
AlanHuchin 0:89cf0851969b 194 void CID10DOF::Calibrate_Motors()
AlanHuchin 0:89cf0851969b 195 {
AlanHuchin 0:89cf0851969b 196 for (int i = 0; i < 4; i++){ //run motors for some time in min speed
AlanHuchin 0:89cf0851969b 197 *motor[i] = max_calibrate;
AlanHuchin 0:89cf0851969b 198 }
AlanHuchin 0:89cf0851969b 199 wait(6.0); //wait for the response from ESC modules
AlanHuchin 0:89cf0851969b 200 for (int i = 0; i < 4; i++){
AlanHuchin 0:89cf0851969b 201 *motor[i] = min_calibrate; //run motors at maximum speed
AlanHuchin 0:89cf0851969b 202 }
AlanHuchin 0:89cf0851969b 203 wait(2.0);
AlanHuchin 0:89cf0851969b 204 }
AlanHuchin 0:89cf0851969b 205
AlanHuchin 0:89cf0851969b 206 //--------------------------Function for setting calibration limits-----------
AlanHuchin 0:89cf0851969b 207 void CID10DOF::setLimits(double min, double max)
AlanHuchin 0:89cf0851969b 208 {
AlanHuchin 0:89cf0851969b 209 if (min > max){ //here detect if someone tried making min to be more than max. If that is the case, then flip them together
AlanHuchin 0:89cf0851969b 210 min_calibrate = max;
AlanHuchin 0:89cf0851969b 211 max_calibrate = min;
AlanHuchin 0:89cf0851969b 212 } else {
AlanHuchin 0:89cf0851969b 213 min_calibrate = min;
AlanHuchin 0:89cf0851969b 214 max_calibrate = max;
AlanHuchin 0:89cf0851969b 215 }
AlanHuchin 0:89cf0851969b 216 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
AlanHuchin 0:89cf0851969b 217 min_calibrate = 0.0;
AlanHuchin 0:89cf0851969b 218 if ((max_calibrate > 1.0) || (max_calibrate < 0.0))
AlanHuchin 0:89cf0851969b 219 max_calibrate = 1.0;
AlanHuchin 0:89cf0851969b 220 }
AlanHuchin 0:89cf0851969b 221
AlanHuchin 0:89cf0851969b 222 //-------------------------------------Function for Stabilising---------------
AlanHuchin 0:89cf0851969b 223 void CID10DOF::stabilise(double *speed, double *actSpeed, double *Diff){
AlanHuchin 0:89cf0851969b 224
AlanHuchin 0:89cf0851969b 225
AlanHuchin 0:89cf0851969b 226 }
AlanHuchin 0:89cf0851969b 227
AlanHuchin 0:89cf0851969b 228 //-----------------------Function for producing thrust in Z direction --------
AlanHuchin 0:89cf0851969b 229 void CID10DOF::run (double *speed){
AlanHuchin 0:89cf0851969b 230 for (int i = 0; i < 4; i++){
AlanHuchin 0:89cf0851969b 231 *motor[i] = speed[i];
AlanHuchin 0:89cf0851969b 232 }
AlanHuchin 0:89cf0851969b 233 }
AlanHuchin 0:89cf0851969b 234
AlanHuchin 0:89cf0851969b 235 void CID10DOF::PID_Config(double *St_point, double *bias)
AlanHuchin 0:89cf0851969b 236 {
AlanHuchin 0:89cf0851969b 237 PID_ROLL.setInputLimits(IN_MIN_ROLL,IN_MAX_ROLL);
AlanHuchin 0:89cf0851969b 238 PID_ROLL.setOutputLimits(OUT_MIN_ROLL, OUT_MAX_ROLL);
AlanHuchin 0:89cf0851969b 239 PID_ROLL.setBias(0.0);
AlanHuchin 0:89cf0851969b 240
AlanHuchin 0:89cf0851969b 241 PID_PITCH.setInputLimits(IN_MIN_PITCH,IN_MAX_PITCH);
AlanHuchin 0:89cf0851969b 242 PID_PITCH.setOutputLimits(OUT_MIN_PITCH, OUT_MAX_PITCH);
AlanHuchin 0:89cf0851969b 243 PID_PITCH.setBias(0.0);
AlanHuchin 0:89cf0851969b 244
AlanHuchin 0:89cf0851969b 245 }
AlanHuchin 0:89cf0851969b 246
AlanHuchin 0:89cf0851969b 247 void CID10DOF::PID_Run(double *processVariable, double *setPoint,double *pid_diff)
AlanHuchin 0:89cf0851969b 248 {
AlanHuchin 0:89cf0851969b 249
AlanHuchin 0:89cf0851969b 250 PID_ROLL.setProcessValue(processVariable[2]);
AlanHuchin 0:89cf0851969b 251 PID_ROLL.setSetPoint(setPoint[1]);
AlanHuchin 0:89cf0851969b 252 pid_diff[2] = PID_ROLL.compute();
AlanHuchin 0:89cf0851969b 253
AlanHuchin 0:89cf0851969b 254 PID_PITCH.setProcessValue(processVariable[1]);
AlanHuchin 0:89cf0851969b 255 PID_PITCH.setSetPoint(setPoint[1]);
AlanHuchin 0:89cf0851969b 256 pid_diff[1] = PID_PITCH.compute();
AlanHuchin 0:89cf0851969b 257
AlanHuchin 0:89cf0851969b 258 }
AlanHuchin 0:89cf0851969b 259
AlanHuchin 0:89cf0851969b 260 float CID10DOF::FLC_roll(float Phi, float dPhi, float iPhi)
AlanHuchin 0:89cf0851969b 261 {
AlanHuchin 0:89cf0851969b 262 g_fisInput[0] = Phi;// Read Input: Phi
AlanHuchin 0:89cf0851969b 263 g_fisInput[1] = dPhi;// Read Input: dPhi
AlanHuchin 0:89cf0851969b 264 g_fisInput[2] = iPhi;// Read Input: iPhi
AlanHuchin 0:89cf0851969b 265 g_fisOutput[0] = 0;
AlanHuchin 0:89cf0851969b 266 fis_evaluate();
AlanHuchin 0:89cf0851969b 267
AlanHuchin 0:89cf0851969b 268 return g_fisOutput[0];
AlanHuchin 0:89cf0851969b 269 }
AlanHuchin 0:89cf0851969b 270
AlanHuchin 0:89cf0851969b 271
AlanHuchin 0:89cf0851969b 272
AlanHuchin 0:89cf0851969b 273 //***********************************************************************
AlanHuchin 0:89cf0851969b 274 // Support functions for Fuzzy Inference System
AlanHuchin 0:89cf0851969b 275 //***********************************************************************
AlanHuchin 0:89cf0851969b 276 // Trapezoidal Member Function
AlanHuchin 0:89cf0851969b 277 FIS_TYPE fis_trapmf(FIS_TYPE x, FIS_TYPE* p)
AlanHuchin 0:89cf0851969b 278 {
AlanHuchin 0:89cf0851969b 279 FIS_TYPE a = p[0], b = p[1], c = p[2], d = p[3];
AlanHuchin 0:89cf0851969b 280 FIS_TYPE t1 = ((x <= c) ? 1 : ((d < x) ? 0 : ((c != d) ? ((d - x) / (d - c)) : 0)));
AlanHuchin 0:89cf0851969b 281 FIS_TYPE t2 = ((b <= x) ? 1 : ((x < a) ? 0 : ((a != b) ? ((x - a) / (b - a)) : 0)));
AlanHuchin 0:89cf0851969b 282 return (FIS_TYPE) min(t1, t2);
AlanHuchin 0:89cf0851969b 283 }
AlanHuchin 0:89cf0851969b 284
AlanHuchin 0:89cf0851969b 285 // Triangular Member Function
AlanHuchin 0:89cf0851969b 286 FIS_TYPE fis_trimf(FIS_TYPE x, FIS_TYPE* p)
AlanHuchin 0:89cf0851969b 287 {
AlanHuchin 0:89cf0851969b 288 FIS_TYPE a = p[0], b = p[1], c = p[2];
AlanHuchin 0:89cf0851969b 289 FIS_TYPE t1 = (x - a) / (b - a);
AlanHuchin 0:89cf0851969b 290 FIS_TYPE t2 = (c - x) / (c - b);
AlanHuchin 0:89cf0851969b 291 if ((a == b) && (b == c)) return (FIS_TYPE) (x == a);
AlanHuchin 0:89cf0851969b 292 if (a == b) return (FIS_TYPE) (t2*(b <= x)*(x <= c));
AlanHuchin 0:89cf0851969b 293 if (b == c) return (FIS_TYPE) (t1*(a <= x)*(x <= b));
AlanHuchin 0:89cf0851969b 294 t1 = min(t1, t2);
AlanHuchin 0:89cf0851969b 295 return (FIS_TYPE) max(t1, 0);
AlanHuchin 0:89cf0851969b 296 }
AlanHuchin 0:89cf0851969b 297
AlanHuchin 0:89cf0851969b 298 FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
AlanHuchin 0:89cf0851969b 299 {
AlanHuchin 0:89cf0851969b 300 return min(a, b);
AlanHuchin 0:89cf0851969b 301 }
AlanHuchin 0:89cf0851969b 302
AlanHuchin 0:89cf0851969b 303 FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
AlanHuchin 0:89cf0851969b 304 {
AlanHuchin 0:89cf0851969b 305 return max(a, b);
AlanHuchin 0:89cf0851969b 306 }
AlanHuchin 0:89cf0851969b 307
AlanHuchin 0:89cf0851969b 308 FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
AlanHuchin 0:89cf0851969b 309 {
AlanHuchin 0:89cf0851969b 310 int i;
AlanHuchin 0:89cf0851969b 311 FIS_TYPE ret = 0;
AlanHuchin 0:89cf0851969b 312
AlanHuchin 0:89cf0851969b 313 if (size == 0) return ret;
AlanHuchin 0:89cf0851969b 314 if (size == 1) return array[0];
AlanHuchin 0:89cf0851969b 315
AlanHuchin 0:89cf0851969b 316 ret = array[0];
AlanHuchin 0:89cf0851969b 317 for (i = 1; i < size; i++)
AlanHuchin 0:89cf0851969b 318 {
AlanHuchin 0:89cf0851969b 319 ret = (*pfnOp)(ret, array[i]);
AlanHuchin 0:89cf0851969b 320 }
AlanHuchin 0:89cf0851969b 321
AlanHuchin 0:89cf0851969b 322 return ret;
AlanHuchin 0:89cf0851969b 323 }
AlanHuchin 0:89cf0851969b 324
AlanHuchin 0:89cf0851969b 325
AlanHuchin 0:89cf0851969b 326 //***********************************************************************
AlanHuchin 0:89cf0851969b 327 // Data for Fuzzy Inference System
AlanHuchin 0:89cf0851969b 328 //***********************************************************************
AlanHuchin 0:89cf0851969b 329 // Pointers to the implementations of member functions
AlanHuchin 0:89cf0851969b 330 _FIS_MF fis_gMF[] =
AlanHuchin 0:89cf0851969b 331 {
AlanHuchin 0:89cf0851969b 332 fis_trapmf, fis_trimf
AlanHuchin 0:89cf0851969b 333 };
AlanHuchin 0:89cf0851969b 334
AlanHuchin 0:89cf0851969b 335 // Count of member function for each Input
AlanHuchin 0:89cf0851969b 336 int fis_gIMFCount[] = { 3, 3, 3 };
AlanHuchin 0:89cf0851969b 337
AlanHuchin 0:89cf0851969b 338 // Count of member function for each Output
AlanHuchin 0:89cf0851969b 339 int fis_gOMFCount[] = { 5 };
AlanHuchin 0:89cf0851969b 340
AlanHuchin 0:89cf0851969b 341 // Coefficients for the Input Member Functions
AlanHuchin 0:89cf0851969b 342 FIS_TYPE fis_gMFI0Coeff1[] = { -10, -10, -0.2, 0 };
AlanHuchin 0:89cf0851969b 343 FIS_TYPE fis_gMFI0Coeff2[] = { -0.1, 0, 0.1 };
AlanHuchin 0:89cf0851969b 344 FIS_TYPE fis_gMFI0Coeff3[] = { 0, 0.2, 10, 10 };
AlanHuchin 0:89cf0851969b 345 FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3 };
AlanHuchin 0:89cf0851969b 346 FIS_TYPE fis_gMFI1Coeff1[] = { -20, -20, -1, -0.25 };
AlanHuchin 0:89cf0851969b 347 FIS_TYPE fis_gMFI1Coeff2[] = { -1, 0, 1 };
AlanHuchin 0:89cf0851969b 348 FIS_TYPE fis_gMFI1Coeff3[] = { 0.25, 1, 20, 20 };
AlanHuchin 0:89cf0851969b 349 FIS_TYPE* fis_gMFI1Coeff[] = { fis_gMFI1Coeff1, fis_gMFI1Coeff2, fis_gMFI1Coeff3 };
AlanHuchin 0:89cf0851969b 350 FIS_TYPE fis_gMFI2Coeff1[] = { -20, -20, -0.8, -0.25 };
AlanHuchin 0:89cf0851969b 351 FIS_TYPE fis_gMFI2Coeff2[] = { -0.6, 0, 0.6 };
AlanHuchin 0:89cf0851969b 352 FIS_TYPE fis_gMFI2Coeff3[] = { 0, 1, 20, 20 };
AlanHuchin 0:89cf0851969b 353 FIS_TYPE* fis_gMFI2Coeff[] = { fis_gMFI2Coeff1, fis_gMFI2Coeff2, fis_gMFI2Coeff3 };
AlanHuchin 0:89cf0851969b 354 FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff, fis_gMFI1Coeff, fis_gMFI2Coeff };
AlanHuchin 0:89cf0851969b 355
AlanHuchin 0:89cf0851969b 356 // Coefficients for the Input Member Functions
AlanHuchin 0:89cf0851969b 357 FIS_TYPE fis_gMFO0Coeff1[] = { -0.1, -0.1, -0.05, -0.045 };
AlanHuchin 0:89cf0851969b 358 FIS_TYPE fis_gMFO0Coeff2[] = { -0.05, -0.045, -0.01, -0.003 };
AlanHuchin 0:89cf0851969b 359 FIS_TYPE fis_gMFO0Coeff3[] = { -0.01, 0, 0.01 };
AlanHuchin 0:89cf0851969b 360 FIS_TYPE fis_gMFO0Coeff4[] = { 0.003, 0.01, 0.045, 0.05 };
AlanHuchin 0:89cf0851969b 361 FIS_TYPE fis_gMFO0Coeff5[] = { 0.0455, 0.05, 0.1, 0.1 };
AlanHuchin 0:89cf0851969b 362 FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
AlanHuchin 0:89cf0851969b 363 FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };
AlanHuchin 0:89cf0851969b 364
AlanHuchin 0:89cf0851969b 365 // Input membership function set
AlanHuchin 0:89cf0851969b 366 int fis_gMFI0[] = { 0, 1, 0 };
AlanHuchin 0:89cf0851969b 367 int fis_gMFI1[] = { 0, 1, 0 };
AlanHuchin 0:89cf0851969b 368 int fis_gMFI2[] = { 0, 1, 0 };
AlanHuchin 0:89cf0851969b 369 int* fis_gMFI[] = { fis_gMFI0, fis_gMFI1, fis_gMFI2};
AlanHuchin 0:89cf0851969b 370
AlanHuchin 0:89cf0851969b 371 // Output membership function set
AlanHuchin 0:89cf0851969b 372 int fis_gMFO0[] = { 0, 0, 1, 0, 0 };
AlanHuchin 0:89cf0851969b 373 int* fis_gMFO[] = { fis_gMFO0};
AlanHuchin 0:89cf0851969b 374
AlanHuchin 0:89cf0851969b 375 // Rule Weights
AlanHuchin 0:89cf0851969b 376 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 };
AlanHuchin 0:89cf0851969b 377
AlanHuchin 0:89cf0851969b 378 // Rule Type
AlanHuchin 0:89cf0851969b 379 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 };
AlanHuchin 0:89cf0851969b 380
AlanHuchin 0:89cf0851969b 381 // Rule Inputs
AlanHuchin 0:89cf0851969b 382 int fis_gRI0[] = { 1, 1, 1 };
AlanHuchin 0:89cf0851969b 383 int fis_gRI1[] = { 2, 1, 1 };
AlanHuchin 0:89cf0851969b 384 int fis_gRI2[] = { 3, 1, 1 };
AlanHuchin 0:89cf0851969b 385 int fis_gRI3[] = { 1, 1, 2 };
AlanHuchin 0:89cf0851969b 386 int fis_gRI4[] = { 2, 1, 2 };
AlanHuchin 0:89cf0851969b 387 int fis_gRI5[] = { 3, 1, 2 };
AlanHuchin 0:89cf0851969b 388 int fis_gRI6[] = { 1, 1, 3 };
AlanHuchin 0:89cf0851969b 389 int fis_gRI7[] = { 2, 1, 3 };
AlanHuchin 0:89cf0851969b 390 int fis_gRI8[] = { 3, 1, 3 };
AlanHuchin 0:89cf0851969b 391 int fis_gRI9[] = { 1, 2, 1 };
AlanHuchin 0:89cf0851969b 392 int fis_gRI10[] = { 2, 2, 1 };
AlanHuchin 0:89cf0851969b 393 int fis_gRI11[] = { 3, 2, 1 };
AlanHuchin 0:89cf0851969b 394 int fis_gRI12[] = { 1, 2, 2 };
AlanHuchin 0:89cf0851969b 395 int fis_gRI13[] = { 2, 2, 2 };
AlanHuchin 0:89cf0851969b 396 int fis_gRI14[] = { 3, 2, 2 };
AlanHuchin 0:89cf0851969b 397 int fis_gRI15[] = { 1, 2, 3 };
AlanHuchin 0:89cf0851969b 398 int fis_gRI16[] = { 2, 2, 3 };
AlanHuchin 0:89cf0851969b 399 int fis_gRI17[] = { 3, 2, 3 };
AlanHuchin 0:89cf0851969b 400 int fis_gRI18[] = { 1, 3, 1 };
AlanHuchin 0:89cf0851969b 401 int fis_gRI19[] = { 2, 3, 1 };
AlanHuchin 0:89cf0851969b 402 int fis_gRI20[] = { 3, 3, 1 };
AlanHuchin 0:89cf0851969b 403 int fis_gRI21[] = { 1, 3, 2 };
AlanHuchin 0:89cf0851969b 404 int fis_gRI22[] = { 2, 3, 2 };
AlanHuchin 0:89cf0851969b 405 int fis_gRI23[] = { 3, 3, 2 };
AlanHuchin 0:89cf0851969b 406 int fis_gRI24[] = { 1, 3, 3 };
AlanHuchin 0:89cf0851969b 407 int fis_gRI25[] = { 2, 3, 3 };
AlanHuchin 0:89cf0851969b 408 int fis_gRI26[] = { 3, 3, 3 };
AlanHuchin 0:89cf0851969b 409 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 };
AlanHuchin 0:89cf0851969b 410
AlanHuchin 0:89cf0851969b 411 // Rule Outputs
AlanHuchin 0:89cf0851969b 412 int fis_gRO0[] = { 5 };
AlanHuchin 0:89cf0851969b 413 int fis_gRO1[] = { 4 };
AlanHuchin 0:89cf0851969b 414 int fis_gRO2[] = { 2 };
AlanHuchin 0:89cf0851969b 415 int fis_gRO3[] = { 5 };
AlanHuchin 0:89cf0851969b 416 int fis_gRO4[] = { 4 };
AlanHuchin 0:89cf0851969b 417 int fis_gRO5[] = { 2 };
AlanHuchin 0:89cf0851969b 418 int fis_gRO6[] = { 5 };
AlanHuchin 0:89cf0851969b 419 int fis_gRO7[] = { 4 };
AlanHuchin 0:89cf0851969b 420 int fis_gRO8[] = { 2 };
AlanHuchin 0:89cf0851969b 421 int fis_gRO9[] = { 4 };
AlanHuchin 0:89cf0851969b 422 int fis_gRO10[] = { 4 };
AlanHuchin 0:89cf0851969b 423 int fis_gRO11[] = { 2 };
AlanHuchin 0:89cf0851969b 424 int fis_gRO12[] = { 4 };
AlanHuchin 0:89cf0851969b 425 int fis_gRO13[] = { 3 };
AlanHuchin 0:89cf0851969b 426 int fis_gRO14[] = { 2 };
AlanHuchin 0:89cf0851969b 427 int fis_gRO15[] = { 4 };
AlanHuchin 0:89cf0851969b 428 int fis_gRO16[] = { 2 };
AlanHuchin 0:89cf0851969b 429 int fis_gRO17[] = { 2 };
AlanHuchin 0:89cf0851969b 430 int fis_gRO18[] = { 4 };
AlanHuchin 0:89cf0851969b 431 int fis_gRO19[] = { 2 };
AlanHuchin 0:89cf0851969b 432 int fis_gRO20[] = { 1 };
AlanHuchin 0:89cf0851969b 433 int fis_gRO21[] = { 4 };
AlanHuchin 0:89cf0851969b 434 int fis_gRO22[] = { 2 };
AlanHuchin 0:89cf0851969b 435 int fis_gRO23[] = { 1 };
AlanHuchin 0:89cf0851969b 436 int fis_gRO24[] = { 4 };
AlanHuchin 0:89cf0851969b 437 int fis_gRO25[] = { 2 };
AlanHuchin 0:89cf0851969b 438 int fis_gRO26[] = { 1 };
AlanHuchin 0:89cf0851969b 439 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 };
AlanHuchin 0:89cf0851969b 440
AlanHuchin 0:89cf0851969b 441 // Input range Min
AlanHuchin 0:89cf0851969b 442 FIS_TYPE fis_gIMin[] = { -10, -20, -20 };
AlanHuchin 0:89cf0851969b 443
AlanHuchin 0:89cf0851969b 444 // Input range Max
AlanHuchin 0:89cf0851969b 445 FIS_TYPE fis_gIMax[] = { 10, 20, 20 };
AlanHuchin 0:89cf0851969b 446
AlanHuchin 0:89cf0851969b 447 // Output range Min
AlanHuchin 0:89cf0851969b 448 FIS_TYPE fis_gOMin[] = { -0.1 };
AlanHuchin 0:89cf0851969b 449
AlanHuchin 0:89cf0851969b 450 // Output range Max
AlanHuchin 0:89cf0851969b 451 FIS_TYPE fis_gOMax[] = { 0.1 };
AlanHuchin 0:89cf0851969b 452
AlanHuchin 0:89cf0851969b 453 //***********************************************************************
AlanHuchin 0:89cf0851969b 454 // Data dependent support functions for Fuzzy Inference System
AlanHuchin 0:89cf0851969b 455 //***********************************************************************
AlanHuchin 0:89cf0851969b 456 FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)
AlanHuchin 0:89cf0851969b 457 {
AlanHuchin 0:89cf0851969b 458 FIS_TYPE mfOut;
AlanHuchin 0:89cf0851969b 459 int r;
AlanHuchin 0:89cf0851969b 460
AlanHuchin 0:89cf0851969b 461 for (r = 0; r < fis_gcR; ++r)
AlanHuchin 0:89cf0851969b 462 {
AlanHuchin 0:89cf0851969b 463 int index = fis_gRO[r][o];
AlanHuchin 0:89cf0851969b 464 if (index > 0)
AlanHuchin 0:89cf0851969b 465 {
AlanHuchin 0:89cf0851969b 466 index = index - 1;
AlanHuchin 0:89cf0851969b 467 mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
AlanHuchin 0:89cf0851969b 468 }
AlanHuchin 0:89cf0851969b 469 else if (index < 0)
AlanHuchin 0:89cf0851969b 470 {
AlanHuchin 0:89cf0851969b 471 index = -index - 1;
AlanHuchin 0:89cf0851969b 472 mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
AlanHuchin 0:89cf0851969b 473 }
AlanHuchin 0:89cf0851969b 474 else
AlanHuchin 0:89cf0851969b 475 {
AlanHuchin 0:89cf0851969b 476 mfOut = 0;
AlanHuchin 0:89cf0851969b 477 }
AlanHuchin 0:89cf0851969b 478
AlanHuchin 0:89cf0851969b 479 fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
AlanHuchin 0:89cf0851969b 480 }
AlanHuchin 0:89cf0851969b 481 return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
AlanHuchin 0:89cf0851969b 482 }
AlanHuchin 0:89cf0851969b 483
AlanHuchin 0:89cf0851969b 484 FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
AlanHuchin 0:89cf0851969b 485 {
AlanHuchin 0:89cf0851969b 486 FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
AlanHuchin 0:89cf0851969b 487 FIS_TYPE area = 0;
AlanHuchin 0:89cf0851969b 488 FIS_TYPE momentum = 0;
AlanHuchin 0:89cf0851969b 489 FIS_TYPE dist, slice;
AlanHuchin 0:89cf0851969b 490 int i;
AlanHuchin 0:89cf0851969b 491
AlanHuchin 0:89cf0851969b 492 // calculate the area under the curve formed by the MF outputs
AlanHuchin 0:89cf0851969b 493 for (i = 0; i < FIS_RESOLUSION; ++i){
AlanHuchin 0:89cf0851969b 494 dist = fis_gOMin[o] + (step * i);
AlanHuchin 0:89cf0851969b 495 slice = step * fis_MF_out(fuzzyRuleSet, dist, o);
AlanHuchin 0:89cf0851969b 496 area += slice;
AlanHuchin 0:89cf0851969b 497 momentum += slice*dist;
AlanHuchin 0:89cf0851969b 498 }
AlanHuchin 0:89cf0851969b 499
AlanHuchin 0:89cf0851969b 500 return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
AlanHuchin 0:89cf0851969b 501 }
AlanHuchin 0:89cf0851969b 502
AlanHuchin 0:89cf0851969b 503 //***********************************************************************
AlanHuchin 0:89cf0851969b 504 // Fuzzy Inference System
AlanHuchin 0:89cf0851969b 505 //***********************************************************************
AlanHuchin 0:89cf0851969b 506 void CID10DOF::fis_evaluate()
AlanHuchin 0:89cf0851969b 507 {
AlanHuchin 0:89cf0851969b 508 FIS_TYPE fuzzyInput0[] = { 0, 0, 0 };
AlanHuchin 0:89cf0851969b 509 FIS_TYPE fuzzyInput1[] = { 0, 0, 0 };
AlanHuchin 0:89cf0851969b 510 FIS_TYPE fuzzyInput2[] = { 0, 0, 0 };
AlanHuchin 0:89cf0851969b 511 FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, fuzzyInput1, fuzzyInput2, };
AlanHuchin 0:89cf0851969b 512 FIS_TYPE fuzzyOutput0[] = { 0, 0, 0, 0, 0 };
AlanHuchin 0:89cf0851969b 513 FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, };
AlanHuchin 0:89cf0851969b 514 FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
AlanHuchin 0:89cf0851969b 515 FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
AlanHuchin 0:89cf0851969b 516 FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
AlanHuchin 0:89cf0851969b 517 FIS_TYPE sW = 0;
AlanHuchin 0:89cf0851969b 518
AlanHuchin 0:89cf0851969b 519 // Transforming input to fuzzy Input
AlanHuchin 0:89cf0851969b 520 int i, j, r, o;
AlanHuchin 0:89cf0851969b 521 for (i = 0; i < fis_gcI; ++i)
AlanHuchin 0:89cf0851969b 522 {
AlanHuchin 0:89cf0851969b 523 for (j = 0; j < fis_gIMFCount[i]; ++j)
AlanHuchin 0:89cf0851969b 524 {
AlanHuchin 0:89cf0851969b 525 fuzzyInput[i][j] =
AlanHuchin 0:89cf0851969b 526 (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
AlanHuchin 0:89cf0851969b 527 }
AlanHuchin 0:89cf0851969b 528 }
AlanHuchin 0:89cf0851969b 529
AlanHuchin 0:89cf0851969b 530 int index = 0;
AlanHuchin 0:89cf0851969b 531 for (r = 0; r < fis_gcR; ++r)
AlanHuchin 0:89cf0851969b 532 {
AlanHuchin 0:89cf0851969b 533 if (fis_gRType[r] == 1)
AlanHuchin 0:89cf0851969b 534 {
AlanHuchin 0:89cf0851969b 535 fuzzyFires[r] = FIS_MAX;
AlanHuchin 0:89cf0851969b 536 for (i = 0; i < fis_gcI; ++i)
AlanHuchin 0:89cf0851969b 537 {
AlanHuchin 0:89cf0851969b 538 index = fis_gRI[r][i];
AlanHuchin 0:89cf0851969b 539 if (index > 0)
AlanHuchin 0:89cf0851969b 540 fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
AlanHuchin 0:89cf0851969b 541 else if (index < 0)
AlanHuchin 0:89cf0851969b 542 fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
AlanHuchin 0:89cf0851969b 543 else
AlanHuchin 0:89cf0851969b 544 fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
AlanHuchin 0:89cf0851969b 545 }
AlanHuchin 0:89cf0851969b 546 }
AlanHuchin 0:89cf0851969b 547 else
AlanHuchin 0:89cf0851969b 548 {
AlanHuchin 0:89cf0851969b 549 fuzzyFires[r] = FIS_MIN;
AlanHuchin 0:89cf0851969b 550 for (i = 0; i < fis_gcI; ++i)
AlanHuchin 0:89cf0851969b 551 {
AlanHuchin 0:89cf0851969b 552 index = fis_gRI[r][i];
AlanHuchin 0:89cf0851969b 553 if (index > 0)
AlanHuchin 0:89cf0851969b 554 fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);
AlanHuchin 0:89cf0851969b 555 else if (index < 0)
AlanHuchin 0:89cf0851969b 556 fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
AlanHuchin 0:89cf0851969b 557 else
AlanHuchin 0:89cf0851969b 558 fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
AlanHuchin 0:89cf0851969b 559 }
AlanHuchin 0:89cf0851969b 560 }
AlanHuchin 0:89cf0851969b 561
AlanHuchin 0:89cf0851969b 562 fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
AlanHuchin 0:89cf0851969b 563 sW += fuzzyFires[r];
AlanHuchin 0:89cf0851969b 564 }
AlanHuchin 0:89cf0851969b 565
AlanHuchin 0:89cf0851969b 566 if (sW == 0)
AlanHuchin 0:89cf0851969b 567 {
AlanHuchin 0:89cf0851969b 568 for (o = 0; o < fis_gcO; ++o)
AlanHuchin 0:89cf0851969b 569 {
AlanHuchin 0:89cf0851969b 570 g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
AlanHuchin 0:89cf0851969b 571 }
AlanHuchin 0:89cf0851969b 572 }
AlanHuchin 0:89cf0851969b 573 else
AlanHuchin 0:89cf0851969b 574 {
AlanHuchin 0:89cf0851969b 575 for (o = 0; o < fis_gcO; ++o)
AlanHuchin 0:89cf0851969b 576 {
AlanHuchin 0:89cf0851969b 577 g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);
AlanHuchin 0:89cf0851969b 578 }
AlanHuchin 0:89cf0851969b 579 }
AlanHuchin 0:89cf0851969b 580 }
AlanHuchin 0:89cf0851969b 581
AlanHuchin 0:89cf0851969b 582
AlanHuchin 0:89cf0851969b 583
AlanHuchin 0:89cf0851969b 584
AlanHuchin 0:89cf0851969b 585
AlanHuchin 0:89cf0851969b 586 const float def_sea_press = 1013.25;
AlanHuchin 0:89cf0851969b 587
AlanHuchin 0:89cf0851969b 588
AlanHuchin 0:89cf0851969b 589
AlanHuchin 0:89cf0851969b 590 float CID10DOF::getBaroTem()
AlanHuchin 0:89cf0851969b 591 {
AlanHuchin 0:89cf0851969b 592
AlanHuchin 0:89cf0851969b 593 }
AlanHuchin 0:89cf0851969b 594
AlanHuchin 0:89cf0851969b 595 float CID10DOF::getBaroPress()
AlanHuchin 0:89cf0851969b 596 {
AlanHuchin 0:89cf0851969b 597
AlanHuchin 0:89cf0851969b 598 }
AlanHuchin 0:89cf0851969b 599
AlanHuchin 0:89cf0851969b 600 float CID10DOF::getBaroAlt()
AlanHuchin 0:89cf0851969b 601 {
AlanHuchin 0:89cf0851969b 602
AlanHuchin 0:89cf0851969b 603 }
AlanHuchin 0:89cf0851969b 604
AlanHuchin 0:89cf0851969b 605
AlanHuchin 0:89cf0851969b 606 float invSqrt(float number)
AlanHuchin 0:89cf0851969b 607 {
AlanHuchin 0:89cf0851969b 608 volatile long i;
AlanHuchin 0:89cf0851969b 609 volatile float x, y;
AlanHuchin 0:89cf0851969b 610 volatile const float f = 1.5F;
AlanHuchin 0:89cf0851969b 611
AlanHuchin 0:89cf0851969b 612 x = number * 0.5F;
AlanHuchin 0:89cf0851969b 613 y = number;
AlanHuchin 0:89cf0851969b 614 i = * ( long * ) &y;
AlanHuchin 0:89cf0851969b 615 i = 0x5f375a86 - ( i >> 1 );
AlanHuchin 0:89cf0851969b 616 y = * ( float * ) &i;
AlanHuchin 0:89cf0851969b 617 y = y * ( f - ( x * y * y ) );
AlanHuchin 0:89cf0851969b 618 return y;
AlanHuchin 0:89cf0851969b 619 }
AlanHuchin 0:89cf0851969b 620
AlanHuchin 0:89cf0851969b 621 double CID10DOF::map(double x, double in_min, double in_max, double out_min, double out_max){ //simply maps values in the given range
AlanHuchin 0:89cf0851969b 622 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
AlanHuchin 0:89cf0851969b 623 }
AlanHuchin 0:89cf0851969b 624
AlanHuchin 0:89cf0851969b 625
AlanHuchin 0:89cf0851969b 626
AlanHuchin 0:89cf0851969b 627
AlanHuchin 0:89cf0851969b 628