Alan Huchin Herrera
Quadrirotor
Dependencies: CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed
Fork of
Nucleo_MPU_9250
by 
Alan Huchin Herrera
Diff: CID10DOF/CID10DOF.cpp
- Revision:
- 0:89cf0851969b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CID10DOF/CID10DOF.cpp Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,628 @@
+#include "CID10DOF.h"
+#include <math.h>
+#include "rtos.h"
+#include "Matrix.h"
+#include "Servo.h"
+#include "PIDcontroller.h"
+
+#ifndef min
+#define min(a,b) ( (a) < (b) ? (a) : (b) )
+#endif
+
+#ifndef max
+#define max(a,b) ( (a) > (b) ? (a) : (b) )
+#endif
+
+PID PID_ROLL(kp, ki, kd, Td);
+PID PID_PITCH(kp, ki, kd, Td);
+
+FIS_TYPE g_fisInput[fis_gcI];
+FIS_TYPE g_fisOutput[fis_gcO];
+
+
+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)
+{
+
+ pc.baud(115200);
+ telem.baud(57600);
+
+ motor[0] = new Servo (FL); //motors are of class Servo as ESC are used in the similar manner
+ motor[1] = new Servo (FR);
+ motor[2] = new Servo (BL);
+ motor[3] = new Servo (BR);
+
+ min_calibrate = 0.0;
+ max_calibrate = 1.0;
+
+ e = 0;
+ last_e = 0;
+ l = 1.0f;
+
+
+}
+
+/**
+ * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
+*/
+
+void CID10DOF::MPU9250init()
+{
+
+ uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
+ pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
+
+ if (whoami == 0x71) // WHO_AM_I should always be 0x68
+ {
+
+ mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+ mpu9250.MPU9250SelfTest(SelfTest);
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+ mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+ wait(2);
+
+ mpu9250.initMPU9250();
+ pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ wait(1);
+
+ mpu9250.initAK8963(magCalibration);
+ pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+ pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
+ pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
+
+ if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
+ if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
+ if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
+ if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+
+ pc.printf("Mag Calibration: Wave device in a figure eight until done!");
+ wait(4);
+
+ magbias[0] = -41.563381;
+ magbias[1] = 165.252426;
+ magbias[2] = 55.095879;
+
+ magScale[0] = 0.866279;
+ magScale[1] = 1.087591;
+ magScale[2] = 1.079710;
+
+ //mpu9250.magcalMPU9250(magbias, magScale);
+ pc.printf("Mag Calibration done!\n\r");
+ pc.printf("x mag bias = %f\n\r", magbias[0]);
+ pc.printf("y mag bias = %f\n\r", magbias[1]);
+ pc.printf("z mag bias = %f\n\r", magbias[2]);
+ //pc.printf("Mag Calibration done!\n\r");
+ pc.printf("x mag Scale = %f\n\r", magScale[0]);
+ pc.printf("y mag Scale = %f\n\r", magScale[1]);
+ pc.printf("z mag Scale = %f\n\r", magScale[2]);
+ wait(2);
+
+
+ }else{
+
+ while(1) ; // Loop forever if communication doesn't happen
+ }
+
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+
+ t.start();
+
+}
+
+void CID10DOF::MPU9250GetValues()
+{
+ mpu9250.readAccelData(accelCount);
+
+ ax = (float)accelCount[0]*aRes - accelBias[0];
+ ay = (float)accelCount[1]*aRes - accelBias[1];
+ az = (float)accelCount[2]*aRes - accelBias[2];
+
+ mpu9250.readGyroData(gyroCount);
+
+ gx = (float)gyroCount[0]*gRes - gyroBias[0];
+ gy = (float)gyroCount[1]*gRes - gyroBias[1];
+ gz = (float)gyroCount[2]*gRes - gyroBias[2];
+
+ mpu9250.readMagData(magCount);
+
+ mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];
+ my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+ mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+ mx *= magScale[0]; // poor man's soft iron calibration
+ my *= magScale[1];
+ mz *= magScale[2];
+
+ tempCount = mpu9250.readTempData();
+ temperature = ((float) tempCount) / 333.87f + 21.0f;
+}
+
+void CID10DOF::MPU9250getYawPitchRoll()
+{
+ 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]);
+ pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ 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]);
+
+ //float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
+ //float Yh = my*cos(roll)+mz*sin(roll);
+
+ //yawmag = atan2(Yh,Xh)+PI;
+
+ pitch *= 180.0f / PI;
+ yaw *= 180.0f / PI;
+ yaw -= 4.28; //
+ if(yaw < 0) yaw += 360.0f;
+ roll *= 180.0f / PI;
+
+}
+
+void CID10DOF::MPU9250ReadAxis(double * dat)
+{
+
+ if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
+ MPU9250GetValues();
+ }
+
+ Now = t.read_us();
+ deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ lastUpdate = Now;
+
+ sum += deltat;
+ sumCount++;
+
+
+
+ mpu9250.MahonyQuaternionUpdate( ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+
+ delt_t = t.read_ms() - count;
+
+ if (delt_t > 50) {
+ MPU9250getYawPitchRoll();
+ sum = 0;
+ sumCount = 0;
+ dat[0] = yaw;
+ dat[1] = pitch;
+ dat[2] = roll;
+ }
+
+
+}
+
+//----------------------------Function for ESC calibration---------------------
+void CID10DOF::Calibrate_Motors()
+{
+ for (int i = 0; i < 4; i++){ //run motors for some time in min speed
+ *motor[i] = max_calibrate;
+ }
+ wait(6.0); //wait for the response from ESC modules
+ for (int i = 0; i < 4; i++){
+ *motor[i] = min_calibrate; //run motors at maximum speed
+ }
+ wait(2.0);
+}
+
+//--------------------------Function for setting calibration limits-----------
+void CID10DOF::setLimits(double min, double max)
+{
+ if (min > max){ //here detect if someone tried making min to be more than max. If that is the case, then flip them together
+ min_calibrate = max;
+ max_calibrate = min;
+ } else {
+ min_calibrate = min;
+ max_calibrate = max;
+ }
+ 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
+ min_calibrate = 0.0;
+ if ((max_calibrate > 1.0) || (max_calibrate < 0.0))
+ max_calibrate = 1.0;
+}
+
+//-------------------------------------Function for Stabilising---------------
+void CID10DOF::stabilise(double *speed, double *actSpeed, double *Diff){
+
+
+}
+
+//-----------------------Function for producing thrust in Z direction --------
+void CID10DOF::run (double *speed){
+ for (int i = 0; i < 4; i++){
+ *motor[i] = speed[i];
+ }
+}
+
+void CID10DOF::PID_Config(double *St_point, double *bias)
+{
+ PID_ROLL.setInputLimits(IN_MIN_ROLL,IN_MAX_ROLL);
+ PID_ROLL.setOutputLimits(OUT_MIN_ROLL, OUT_MAX_ROLL);
+ PID_ROLL.setBias(0.0);
+
+ PID_PITCH.setInputLimits(IN_MIN_PITCH,IN_MAX_PITCH);
+ PID_PITCH.setOutputLimits(OUT_MIN_PITCH, OUT_MAX_PITCH);
+ PID_PITCH.setBias(0.0);
+
+}
+
+void CID10DOF::PID_Run(double *processVariable, double *setPoint,double *pid_diff)
+{
+
+ PID_ROLL.setProcessValue(processVariable[2]);
+ PID_ROLL.setSetPoint(setPoint[1]);
+ pid_diff[2] = PID_ROLL.compute();
+
+ PID_PITCH.setProcessValue(processVariable[1]);
+ PID_PITCH.setSetPoint(setPoint[1]);
+ pid_diff[1] = PID_PITCH.compute();
+
+}
+
+float CID10DOF::FLC_roll(float Phi, float dPhi, float iPhi)
+{
+ g_fisInput[0] = Phi;// Read Input: Phi
+ g_fisInput[1] = dPhi;// Read Input: dPhi
+ g_fisInput[2] = iPhi;// Read Input: iPhi
+ g_fisOutput[0] = 0;
+ fis_evaluate();
+
+ return g_fisOutput[0];
+}
+
+
+
+//***********************************************************************
+// Support functions for Fuzzy Inference System
+//***********************************************************************
+// Trapezoidal Member Function
+FIS_TYPE fis_trapmf(FIS_TYPE x, FIS_TYPE* p)
+{
+ FIS_TYPE a = p[0], b = p[1], c = p[2], d = p[3];
+ FIS_TYPE t1 = ((x <= c) ? 1 : ((d < x) ? 0 : ((c != d) ? ((d - x) / (d - c)) : 0)));
+ FIS_TYPE t2 = ((b <= x) ? 1 : ((x < a) ? 0 : ((a != b) ? ((x - a) / (b - a)) : 0)));
+ return (FIS_TYPE) min(t1, t2);
+}
+
+// Triangular Member Function
+FIS_TYPE fis_trimf(FIS_TYPE x, FIS_TYPE* p)
+{
+ FIS_TYPE a = p[0], b = p[1], c = p[2];
+ FIS_TYPE t1 = (x - a) / (b - a);
+ FIS_TYPE t2 = (c - x) / (c - b);
+ if ((a == b) && (b == c)) return (FIS_TYPE) (x == a);
+ if (a == b) return (FIS_TYPE) (t2*(b <= x)*(x <= c));
+ if (b == c) return (FIS_TYPE) (t1*(a <= x)*(x <= b));
+ t1 = min(t1, t2);
+ return (FIS_TYPE) max(t1, 0);
+}
+
+FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
+{
+ return min(a, b);
+}
+
+FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
+{
+ return max(a, b);
+}
+
+FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
+{
+ int i;
+ FIS_TYPE ret = 0;
+
+ if (size == 0) return ret;
+ if (size == 1) return array[0];
+
+ ret = array[0];
+ for (i = 1; i < size; i++)
+ {
+ ret = (*pfnOp)(ret, array[i]);
+ }
+
+ return ret;
+}
+
+
+//***********************************************************************
+// Data for Fuzzy Inference System
+//***********************************************************************
+// Pointers to the implementations of member functions
+_FIS_MF fis_gMF[] =
+{
+ fis_trapmf, fis_trimf
+};
+
+// Count of member function for each Input
+int fis_gIMFCount[] = { 3, 3, 3 };
+
+// Count of member function for each Output
+int fis_gOMFCount[] = { 5 };
+
+// Coefficients for the Input Member Functions
+FIS_TYPE fis_gMFI0Coeff1[] = { -10, -10, -0.2, 0 };
+FIS_TYPE fis_gMFI0Coeff2[] = { -0.1, 0, 0.1 };
+FIS_TYPE fis_gMFI0Coeff3[] = { 0, 0.2, 10, 10 };
+FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3 };
+FIS_TYPE fis_gMFI1Coeff1[] = { -20, -20, -1, -0.25 };
+FIS_TYPE fis_gMFI1Coeff2[] = { -1, 0, 1 };
+FIS_TYPE fis_gMFI1Coeff3[] = { 0.25, 1, 20, 20 };
+FIS_TYPE* fis_gMFI1Coeff[] = { fis_gMFI1Coeff1, fis_gMFI1Coeff2, fis_gMFI1Coeff3 };
+FIS_TYPE fis_gMFI2Coeff1[] = { -20, -20, -0.8, -0.25 };
+FIS_TYPE fis_gMFI2Coeff2[] = { -0.6, 0, 0.6 };
+FIS_TYPE fis_gMFI2Coeff3[] = { 0, 1, 20, 20 };
+FIS_TYPE* fis_gMFI2Coeff[] = { fis_gMFI2Coeff1, fis_gMFI2Coeff2, fis_gMFI2Coeff3 };
+FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff, fis_gMFI1Coeff, fis_gMFI2Coeff };
+
+// Coefficients for the Input Member Functions
+FIS_TYPE fis_gMFO0Coeff1[] = { -0.1, -0.1, -0.05, -0.045 };
+FIS_TYPE fis_gMFO0Coeff2[] = { -0.05, -0.045, -0.01, -0.003 };
+FIS_TYPE fis_gMFO0Coeff3[] = { -0.01, 0, 0.01 };
+FIS_TYPE fis_gMFO0Coeff4[] = { 0.003, 0.01, 0.045, 0.05 };
+FIS_TYPE fis_gMFO0Coeff5[] = { 0.0455, 0.05, 0.1, 0.1 };
+FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
+FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };
+
+// Input membership function set
+int fis_gMFI0[] = { 0, 1, 0 };
+int fis_gMFI1[] = { 0, 1, 0 };
+int fis_gMFI2[] = { 0, 1, 0 };
+int* fis_gMFI[] = { fis_gMFI0, fis_gMFI1, fis_gMFI2};
+
+// Output membership function set
+int fis_gMFO0[] = { 0, 0, 1, 0, 0 };
+int* fis_gMFO[] = { fis_gMFO0};
+
+// Rule Weights
+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 };
+
+// Rule Type
+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 };
+
+// Rule Inputs
+int fis_gRI0[] = { 1, 1, 1 };
+int fis_gRI1[] = { 2, 1, 1 };
+int fis_gRI2[] = { 3, 1, 1 };
+int fis_gRI3[] = { 1, 1, 2 };
+int fis_gRI4[] = { 2, 1, 2 };
+int fis_gRI5[] = { 3, 1, 2 };
+int fis_gRI6[] = { 1, 1, 3 };
+int fis_gRI7[] = { 2, 1, 3 };
+int fis_gRI8[] = { 3, 1, 3 };
+int fis_gRI9[] = { 1, 2, 1 };
+int fis_gRI10[] = { 2, 2, 1 };
+int fis_gRI11[] = { 3, 2, 1 };
+int fis_gRI12[] = { 1, 2, 2 };
+int fis_gRI13[] = { 2, 2, 2 };
+int fis_gRI14[] = { 3, 2, 2 };
+int fis_gRI15[] = { 1, 2, 3 };
+int fis_gRI16[] = { 2, 2, 3 };
+int fis_gRI17[] = { 3, 2, 3 };
+int fis_gRI18[] = { 1, 3, 1 };
+int fis_gRI19[] = { 2, 3, 1 };
+int fis_gRI20[] = { 3, 3, 1 };
+int fis_gRI21[] = { 1, 3, 2 };
+int fis_gRI22[] = { 2, 3, 2 };
+int fis_gRI23[] = { 3, 3, 2 };
+int fis_gRI24[] = { 1, 3, 3 };
+int fis_gRI25[] = { 2, 3, 3 };
+int fis_gRI26[] = { 3, 3, 3 };
+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 };
+
+// Rule Outputs
+int fis_gRO0[] = { 5 };
+int fis_gRO1[] = { 4 };
+int fis_gRO2[] = { 2 };
+int fis_gRO3[] = { 5 };
+int fis_gRO4[] = { 4 };
+int fis_gRO5[] = { 2 };
+int fis_gRO6[] = { 5 };
+int fis_gRO7[] = { 4 };
+int fis_gRO8[] = { 2 };
+int fis_gRO9[] = { 4 };
+int fis_gRO10[] = { 4 };
+int fis_gRO11[] = { 2 };
+int fis_gRO12[] = { 4 };
+int fis_gRO13[] = { 3 };
+int fis_gRO14[] = { 2 };
+int fis_gRO15[] = { 4 };
+int fis_gRO16[] = { 2 };
+int fis_gRO17[] = { 2 };
+int fis_gRO18[] = { 4 };
+int fis_gRO19[] = { 2 };
+int fis_gRO20[] = { 1 };
+int fis_gRO21[] = { 4 };
+int fis_gRO22[] = { 2 };
+int fis_gRO23[] = { 1 };
+int fis_gRO24[] = { 4 };
+int fis_gRO25[] = { 2 };
+int fis_gRO26[] = { 1 };
+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 };
+
+// Input range Min
+FIS_TYPE fis_gIMin[] = { -10, -20, -20 };
+
+// Input range Max
+FIS_TYPE fis_gIMax[] = { 10, 20, 20 };
+
+// Output range Min
+FIS_TYPE fis_gOMin[] = { -0.1 };
+
+// Output range Max
+FIS_TYPE fis_gOMax[] = { 0.1 };
+
+//***********************************************************************
+// Data dependent support functions for Fuzzy Inference System
+//***********************************************************************
+FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)
+{
+ FIS_TYPE mfOut;
+ int r;
+
+ for (r = 0; r < fis_gcR; ++r)
+ {
+ int index = fis_gRO[r][o];
+ if (index > 0)
+ {
+ index = index - 1;
+ mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
+ }
+ else if (index < 0)
+ {
+ index = -index - 1;
+ mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
+ }
+ else
+ {
+ mfOut = 0;
+ }
+
+ fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
+ }
+ return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
+}
+
+FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
+{
+ FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
+ FIS_TYPE area = 0;
+ FIS_TYPE momentum = 0;
+ FIS_TYPE dist, slice;
+ int i;
+
+ // calculate the area under the curve formed by the MF outputs
+ for (i = 0; i < FIS_RESOLUSION; ++i){
+ dist = fis_gOMin[o] + (step * i);
+ slice = step * fis_MF_out(fuzzyRuleSet, dist, o);
+ area += slice;
+ momentum += slice*dist;
+ }
+
+ return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
+}
+
+//***********************************************************************
+// Fuzzy Inference System
+//***********************************************************************
+void CID10DOF::fis_evaluate()
+{
+ FIS_TYPE fuzzyInput0[] = { 0, 0, 0 };
+ FIS_TYPE fuzzyInput1[] = { 0, 0, 0 };
+ FIS_TYPE fuzzyInput2[] = { 0, 0, 0 };
+ FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, fuzzyInput1, fuzzyInput2, };
+ FIS_TYPE fuzzyOutput0[] = { 0, 0, 0, 0, 0 };
+ FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, };
+ FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
+ FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
+ FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
+ FIS_TYPE sW = 0;
+
+ // Transforming input to fuzzy Input
+ int i, j, r, o;
+ for (i = 0; i < fis_gcI; ++i)
+ {
+ for (j = 0; j < fis_gIMFCount[i]; ++j)
+ {
+ fuzzyInput[i][j] =
+ (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
+ }
+ }
+
+ int index = 0;
+ for (r = 0; r < fis_gcR; ++r)
+ {
+ if (fis_gRType[r] == 1)
+ {
+ fuzzyFires[r] = FIS_MAX;
+ for (i = 0; i < fis_gcI; ++i)
+ {
+ index = fis_gRI[r][i];
+ if (index > 0)
+ fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
+ else if (index < 0)
+ fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
+ else
+ fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
+ }
+ }
+ else
+ {
+ fuzzyFires[r] = FIS_MIN;
+ for (i = 0; i < fis_gcI; ++i)
+ {
+ index = fis_gRI[r][i];
+ if (index > 0)
+ fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);
+ else if (index < 0)
+ fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
+ else
+ fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
+ }
+ }
+
+ fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
+ sW += fuzzyFires[r];
+ }
+
+ if (sW == 0)
+ {
+ for (o = 0; o < fis_gcO; ++o)
+ {
+ g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
+ }
+ }
+ else
+ {
+ for (o = 0; o < fis_gcO; ++o)
+ {
+ g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);
+ }
+ }
+}
+
+
+
+
+
+const float def_sea_press = 1013.25;
+
+
+
+float CID10DOF::getBaroTem()
+{
+
+}
+
+float CID10DOF::getBaroPress()
+{
+
+}
+
+float CID10DOF::getBaroAlt()
+{
+
+}
+
+
+float invSqrt(float number)
+{
+ volatile long i;
+ volatile float x, y;
+ volatile const float f = 1.5F;
+
+ x = number * 0.5F;
+ y = number;
+ i = * ( long * ) &y;
+ i = 0x5f375a86 - ( i >> 1 );
+ y = * ( float * ) &i;
+ y = y * ( f - ( x * y * y ) );
+ return y;
+}
+
+double CID10DOF::map(double x, double in_min, double in_max, double out_min, double out_max){ //simply maps values in the given range
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+
+
+
+