Diff: embedded/MATH1.cpp

Revision:

0:08606a13a816

Child:

1:c6b089f4ff2a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/embedded/MATH1.cpp	Thu Jun 02 05:04:57 2016 +0000
@@ -0,0 +1,479 @@
+#include "MATH1.h"
+#define pi  3.14159265359
+double mean(double *d, int len)
+{
+    double mean = 0.0;
+    double sum = 0.0;
+
+    for(int i = 0 ; i<len ; i++) {
+        sum += d[i];
+    }
+
+    mean = sum / len ;
+    return mean;
+}
+
+double MAXof3(double one, double two, double three)
+{
+    double MAXMAX = one;
+    double yoyoyo[2]= {two, three};
+    for(int i = 0; i<2 ; i++) {
+        if(yoyoyo[i]>MAXMAX) {
+            MAXMAX = yoyoyo[i];
+        }
+    }
+    return MAXMAX;
+}
+
+///square///
+double MATH::sqr(double value)
+{
+    double sqrvalue = value * value;
+    return sqrvalue;
+}
+
+
+///line voltage RMS///
+double MATH::LVRMS(double* d1, double* d2,  uint16_t len)
+{
+    double sum = 0.0;
+    for (int i = 0; i < len; i++) {
+        sum += sqr((d1[i]-d2[i]));
+    }
+    double RMS = sqrt(sum/len);
+    return RMS;
+}
+
+///RMS///
+double MATH::RMS(double* d, uint16_t len)
+{
+    double sum = 0.0;
+    for (int i = 0; i < len; i++) {
+        sum += d[i]*d[i];
+    }
+    double rms = sqrt(sum/len)*1.0;
+    return rms;
+}
+
+
+///unbalance rate///
+double MATH::UR(double d1, double d2, double d3)
+{
+    double abc_avg = (d1+d2+d3) / 3.0;
+    double x[3] = {fabs(d1 - abc_avg), fabs(d2 - abc_avg), fabs(d3 - abc_avg)};
+    double dmax = 0.0;
+    for(int i=0 ; i<3 ; i++) {
+        if(x[i]>dmax) {
+            dmax=x[i];
+        }
+    }
+    double URv = dmax / abc_avg * 100.0;
+    return URv;
+}
+
+
+///angle(rad)///
+double MATH::angle(double d1, double d2)
+{
+
+    double sqrt_2 = sqrt(2.0);
+    if (d1 > 0.0 ) {
+        if(d2 > 0.0) {
+            double rad = fabs(atan(d2 / d1));
+            return (0.0 + rad);
+        } else {
+            double rad = fabs(atan(d2 / d1));
+            return (0.0 - rad);
+        }
+    } else {
+        if(d2 > 0.0) {
+            double rad = fabs(atan(d2 / d1));
+            return (pi - rad);
+        } else {
+            double rad = fabs(atan(d2 / d1));
+            return (pi + rad);
+        }
+    }
+}
+
+///Unbalance Factor///
+double MATH::UF(double rms_a, double rms_b, double rms_c, double angle_a, double angle_b, double angle_c)
+{
+    double a = 120.0 * pi / 180 ;      /// degree ->  rad
+    double ang_v1_1 = angle_b + a*2.0 ;
+    double ang_v1_2 = angle_c + a;
+    double ang_v2_1 = angle_b + a;
+    double ang_v2_2 = angle_c + a*2.0;
+
+    double v1 = sqrt(sqr((rms_a * cos(angle_a)) + (rms_b * cos(ang_v1_1)) + (rms_c * cos(ang_v1_2)))
+                     + sqr((rms_a * sin(angle_a)) + (rms_b * sin(ang_v1_1)) + (rms_c * sin(ang_v1_2))));
+    double v2 = sqrt(sqr((rms_a * cos(angle_a)) + (rms_b * cos(ang_v2_1)) + (rms_c * cos(ang_v2_2)))
+                     + sqr((rms_a * sin(angle_a)) + (rms_b * sin(ang_v2_1)) + (rms_c * sin(ang_v2_2))));
+
+    return (v2/v1)*100.0 ;
+}
+
+///DFT///
+double MATH::doDFT(double *input, double *output, int N)
+{
+    for (int i=0 ; i<N ; i++) {
+        double re = 0;
+        double im = 0;
+        for(int j=0 ; j<N ; j++) {
+            re += input[j]*cos(2*pi*i*j/N);
+            im += input[j]*(-sin(2*pi*i*j/N));
+        }
+        output[i] = sqrt(sqr(re) + sqr(im))/N;
+    }
+    return *output;
+
+}
+
+
+void MATH::integal(double *in, double *out, int len, int sps)
+{
+
+    double t = 1.0 /sps ;
+    double  mean;
+    double sum = 0.0;
+    out[0] = 0 ;
+    double v = 0;
+
+    for(int i = 1 ; i < len ; i++) {
+        v += ((in[i - 1] + in[i]) * t / 2.0);       ///   m/s --> *1000 mm/s --> *1000 *1000 um/s
+        out[i] = v;
+    }
+
+    mean = mean(out, len);
+
+    for(int i = 0 ; i<len ; i++) {
+        out[i] = ( out[i] - mean ) * 1000;
+    }
+
+
+}
+
+void MATH::detrend(double *y, int len, int sps)
+{
+    int i;
+    double t = 1.0 /sps ;
+
+    double sum_k1 = 0.0, sum_k2 = 0.0, xbar = 0.0, ybar = 0.0;
+
+    double *x, *d1, *d2;
+    x = (double*) malloc(sizeof(double) * len);
+    d1 = (double*) malloc(sizeof(double) * len);
+    d2 = (double*) malloc(sizeof(double) * len);
+
+    for(i=0 ; i<len ; i++) {
+        x[i] = (i+1)*t;
+    }
+
+    xbar = mean(x, len);
+    ybar = mean(y, len);
+
+    for(i=0 ; i<len ; i++) {
+        d1[i] = x[i]-xbar;
+        d2[i] = y[i]-ybar;
+        sum_k1 += d1[i]*d2[i];
+        sum_k2 += d1[i]*d1[i];
+    }
+    
+    double A,B;
+    A = sum_k1/sum_k2;
+    B = ybar-A*xbar;
+
+    double * trend;
+    trend = (double*) malloc(sizeof(double) * len);
+
+    for(i=0 ; i<len ; i++) {
+        trend[i]=A*x[i] + B;
+        y[i]=y[i]-trend[i];
+    }
+
+}
+
+
+double MATH::Peak2Peak(double *in, int len)
+{
+    int i;
+    double outpp;
+
+    double MAX = in[0];
+    for(i = 1 ; i < len ; i++) {
+        MAX = (in[i] > MAX ? in[i]:MAX ) ;
+    }
+
+    double min = in[0];
+    for(i = 1 ; i < len ; i++) {
+        min = (in[i] < min ? in[i]:min ) ;
+    }
+
+    outpp = MAX - min;
+
+    return outpp;
+}
+
+
+double MATH::PeakValue(double *x , double *y, double *z, int len)
+{
+    double xmax = fabs(x[0]), ymax = fabs(y[0]), zmax = fabs(z[0]);
+    for(int i =1; i<len ; i++) {
+        if(fabs(x[i]) > xmax) {
+            xmax = fabs(x[i]);
+        }
+        if(fabs(y[i]) > ymax) {
+            ymax = fabs(y[i]);
+        }
+        if(fabs(z[i]) > zmax) {
+            zmax = fabs(z[i]);
+        }
+    }
+    double MAX = ymax;
+    double xzmax[2]= {xmax, zmax};
+    for(int i=0 ; i<2 ; i++) {
+        if(xzmax[i] > MAX) {
+            MAX = xzmax[i];
+        }
+    }
+
+    return MAX ;
+}
+
+
+double MATH::UBValue(double *maxi, double *x , double *y, double *z, int len)
+{
+    double xmin = fabs(x[0]) , ymin = fabs(y[0]) , zmin = fabs(z[0]);
+    for(int i = 1 ; i<len ; i++) {
+        if(fabs(x[i]) < xmin) {
+            xmin = fabs(x[i]);
+        }
+        if(fabs(y[i]) < ymin) {
+            ymin = fabs(y[i]);
+        }
+        if(fabs(z[i]) < zmin) {
+            zmin = fabs(z[i]);
+        }
+    }
+    double MIN = zmin;
+    double yxmin[2]= {ymin, xmin};
+    for(int i=0 ; i<2 ; i++) {
+        if(yxmin[i] < MIN) {
+            MIN = yxmin[i];
+        }
+    }
+
+    double UB = *maxi + 0.5 *( *maxi - MIN ) / ( len - 1 );
+    return UB;
+}
+
+
+double MATH::THD(double *ass_1, double *ass_2, double *ass_3, int pu)
+{
+    double WTF, WTFa, WTFb, WTFc;
+    double you_1, you_2, you_3, fu_1, fu_2, fu_3;
+
+    switch(pu) {
+        case 10000 :
+            you_1  = (sqr(ass_1[120]) + sqr(ass_1[180]) + sqr(ass_1[240]) + sqr(ass_1[300]) + sqr(ass_1[360]) + sqr(ass_1[420]) + sqr(ass_1[480]) + sqr(ass_1[540]) + sqr(ass_1[600]) + sqr(ass_1[660]));
+            fu_1 = sqrt(you_1);
+            WTFa  = (fu_1 / ass_1[60])*100;
+
+            you_2  = (sqr(ass_2[120]) + sqr(ass_2[180]) + sqr(ass_2[240]) + sqr(ass_2[300]) + sqr(ass_2[360]) + sqr(ass_2[420]) + sqr(ass_2[480]) + sqr(ass_2[540]) + sqr(ass_2[600]) + sqr(ass_2[660]));
+            fu_2 = sqrt(you_2);
+            WTFb  = (fu_2 / ass_2[60])*100;
+
+            you_3  = (sqr(ass_3[120]) + sqr(ass_3[180]) + sqr(ass_3[240]) + sqr(ass_3[300]) + sqr(ass_3[360]) + sqr(ass_3[420]) + sqr(ass_3[480]) + sqr(ass_3[540]) + sqr(ass_3[600]) + sqr(ass_3[660]));
+            fu_3 = sqrt(you_3);
+            WTFc  = (fu_3 / ass_3[60])*100;
+
+            WTF = MAXof3(WTFa, WTFb, WTFc);
+            break;
+        case 8192 :
+
+            double opps_2, opps_3, opps_4, opps_5, opps_6, opps_7, opps_8, opps_9, opps_10, opps_11;
+
+            opps_2 = (ass_1[49] > ass_1[50] ? ass_1[49] :ass_1[50]);
+            opps_4 = (ass_1[98] > ass_1[99] ? ass_1[98] :ass_1[99]);
+            opps_6 = (ass_1[147] > ass_1[148] ? ass_1[147] :ass_1[148]);
+            opps_7 = (ass_1[172] > ass_1[173] ? ass_1[172] :ass_1[173]);
+            opps_9 = (ass_1[221] > ass_1[222] ? ass_1[221] :ass_1[222]);
+            opps_11 = (ass_1[270] > ass_1[271] ? ass_1[270] :ass_1[271]);
+            opps_3 = MAXof3(ass_1[73], ass_1[74], ass_1[75]);
+            opps_5 = MAXof3(ass_1[122], ass_1[123], ass_1[124]);
+            opps_8 = MAXof3(ass_1[196], ass_1[197], ass_1[198]);
+            opps_10 = MAXof3(ass_1[245], ass_1[246], ass_1[247]);
+
+            you_1  = (sqr(opps_2) + sqr(opps_3) + sqr(opps_4) + sqr(opps_5) + sqr(opps_6) + sqr(opps_7) + sqr(opps_8) + sqr(opps_9) + sqr(opps_10) + sqr(opps_11));
+            fu_1 = sqrt(you_1);
+            WTFa  = (fu_1 / ass_1[24])*100;
+
+            opps_2 = (ass_2[49] > ass_2[50] ? ass_2[49] :ass_2[50]);
+            opps_4 = (ass_2[98] > ass_2[99] ? ass_2[98] :ass_2[99]);
+            opps_6 = (ass_2[147] > ass_2[148] ? ass_2[147] :ass_2[148]);
+            opps_7 = (ass_2[172] > ass_2[173] ? ass_2[172] :ass_2[173]);
+            opps_9 = (ass_2[221] > ass_2[222] ? ass_2[221] :ass_2[222]);
+            opps_11 = (ass_2[270] > ass_2[271] ? ass_2[270] :ass_2[271]);
+            opps_3 = MAXof3(ass_2[73], ass_2[74], ass_2[75]);
+            opps_5 = MAXof3(ass_2[122], ass_2[123], ass_2[124]);
+            opps_8 = MAXof3(ass_2[196], ass_2[197], ass_2[198]);
+            opps_10 = MAXof3(ass_2[245], ass_2[246], ass_2[247]);
+
+            you_2  = (sqr(opps_2) + sqr(opps_3) + sqr(opps_4) + sqr(opps_5) + sqr(opps_6) + sqr(opps_7) + sqr(opps_8) + sqr(opps_9) + sqr(opps_10) + sqr(opps_11));
+            fu_2 = sqrt(you_2);
+            WTFb  = (fu_2 / ass_2[24])*100;
+
+            opps_2 = (ass_3[49] > ass_3[50] ? ass_3[49] :ass_3[50]);
+            opps_4 = (ass_3[98] > ass_3[99] ? ass_3[98] :ass_3[99]);
+            opps_6 = (ass_3[147] > ass_3[148] ? ass_3[147] :ass_3[148]);
+            opps_7 = (ass_3[172] > ass_3[173] ? ass_3[172] :ass_3[173]);
+            opps_9 = (ass_3[221] > ass_3[222] ? ass_3[221] :ass_3[222]);
+            opps_11 = (ass_3[270] > ass_3[271] ? ass_3[270] :ass_3[271]);
+            opps_3 = MAXof3(ass_3[73], ass_3[74], ass_3[75]);
+            opps_5 = MAXof3(ass_3[122], ass_3[123], ass_3[124]);
+            opps_8 = MAXof3(ass_3[196], ass_3[197], ass_3[198]);
+            opps_10 = MAXof3(ass_3[245], ass_3[246], ass_3[247]);
+
+            you_3  = (sqr(opps_2) + sqr(opps_3) + sqr(opps_4) + sqr(opps_5) + sqr(opps_6) + sqr(opps_7) + sqr(opps_8) + sqr(opps_9) + sqr(opps_10) + sqr(opps_11));
+            fu_3 = sqrt(you_3);
+            WTFc  = (fu_3 / ass_3[24])*100;
+
+            WTF = MAXof3(WTFa, WTFb, WTFc);
+            break;
+    }
+
+    return WTF;
+}
+
+
+double MATH::HD(double *ck_1, double *ck_2, double *ck_3, int pu)
+{
+    double blow, blow1, blow2, blow3;
+    double su_3, su_5, su_7, su_9, su_11;
+
+    switch(pu) {
+
+        case 10000:
+
+            su_3 = (ck_1[180] / ck_1[60])*100;
+            su_5 = (ck_1[300] / ck_1[60])*100;
+            su_7 = (ck_1[420] / ck_1[60])*100;
+            su_9 = (ck_1[540] / ck_1[60])*100;
+            su_11 = (ck_1[660] / ck_1[60])*100;
+
+            blow1 = su_3;
+            double job[4]= {su_5, su_7, su_9, su_11};
+            for(int i = 0 ; i<4 ; i++) {
+                if(job[i] > blow1) {
+                    blow1 = job[i];
+                }
+            }
+
+            su_3 = (ck_2[180] / ck_2[60])*100;
+            su_5 = (ck_2[300] / ck_2[60])*100;
+            su_7 = (ck_2[420] / ck_2[60])*100;
+            su_9 = (ck_2[540] / ck_2[60])*100;
+            su_11 = (ck_2[660] / ck_2[60])*100;
+
+            blow2 = su_3;
+            double job2[4]= {su_5, su_7, su_9, su_11};
+            for(int i = 0 ; i<4 ; i++) {
+                if(job2[i] > blow2) {
+                    blow2 = job2[i];
+                }
+            }
+
+            su_3 = (ck_3[180] / ck_3[60])*100;
+            su_5 = (ck_3[300] / ck_3[60])*100;
+            su_7 = (ck_3[420] / ck_3[60])*100;
+            su_9 = (ck_3[540] / ck_3[60])*100;
+            su_11 = (ck_3[660] / ck_3[60])*100;
+
+            blow3 = su_3;
+            double job3[4]= {su_5, su_7, su_9, su_11};
+            for(int i = 0 ; i<4 ; i++) {
+                if(job3[i] > blow3) {
+                    blow3 = job3[i];
+                }
+            }
+
+            blow = MAXof3(blow1, blow2, blow3);
+
+            break;
+
+        case 8192 :
+
+            double opps_3, opps_5, opps_7, opps_9, opps_11;
+            opps_3 = MAXof3(ck_1[73], ck_1[74], ck_1[75]);
+            opps_5 = MAXof3(ck_1[122], ck_1[123], ck_1[124]);
+            opps_7 = (ck_1[172] > ck_1[173] ? ck_1[172] :ck_1[173]);
+            opps_9 = (ck_1[221] > ck_1[222] ? ck_1[221] :ck_1[222]);
+            opps_11 = (ck_1[270] > ck_1[271] ? ck_1[270] :ck_1[271]);
+
+
+            su_3 = (opps_3 / ck_1[24])*100;
+            su_5 = (opps_5 / ck_1[24])*100;
+            su_7 = (opps_7 / ck_1[24])*100;
+            su_9 = (opps_9 / ck_1[24])*100;
+            su_11 = (opps_11 / ck_1[24])*100;
+
+            blow1 = su_3;
+            double job4[4]= {su_5, su_7, su_9, su_11};
+            for(int i = 0 ; i<4 ; i++) {
+                if(job4[i] > blow1) {
+                    blow1 = job4[i];
+                }
+            }
+
+
+            opps_3 = MAXof3(ck_2[73], ck_2[74], ck_2[75]);
+            opps_5 = MAXof3(ck_2[122], ck_2[123], ck_2[124]);
+            opps_7 = (ck_2[172] > ck_2[173] ? ck_2[172] :ck_2[173]);
+            opps_9 = (ck_2[221] > ck_2[222] ? ck_2[221] :ck_2[222]);
+            opps_11 = (ck_2[270] > ck_2[271] ? ck_2[270] :ck_2[271]);
+
+            su_3 = (opps_3 / ck_2[24])*100;
+            su_5 = (opps_5 / ck_2[24])*100;
+            su_7 = (opps_7 / ck_2[24])*100;
+            su_9 = (opps_9 / ck_2[24])*100;
+            su_11 = (opps_11 / ck_2[24])*100;
+
+            blow2 = su_3;
+            double job5[4]= {su_5, su_7, su_9, su_11};
+
+            for(int i = 0 ; i<4 ; i++) {
+                if(job5[i] > blow2) {
+                    blow2 = job5[i];
+                }
+            }
+
+
+            opps_3 = MAXof3(ck_3[73], ck_3[74], ck_3[75]);
+            opps_5 = MAXof3(ck_3[122], ck_3[123], ck_3[124]);
+            opps_7 = (ck_3[172] > ck_3[173] ? ck_3[172] :ck_3[173]);
+            opps_9 = (ck_3[221] > ck_3[222] ? ck_3[221] :ck_3[222]);
+            opps_11 = (ck_3[270] > ck_3[271] ? ck_3[270] :ck_3[271]);
+
+            su_3 = (opps_3 / ck_3[24])*100;
+            su_5 = (opps_5 / ck_3[24])*100;
+            su_7 = (opps_7 / ck_3[24])*100;
+            su_9 = (opps_9 / ck_3[24])*100;
+            su_11 = (opps_11 / ck_3[24])*100;
+
+            blow3 = su_3;
+            double job6[4]= {su_5, su_7, su_9, su_11};
+
+            for(int i = 0 ; i<4 ; i++) {
+                if(job6[i] > blow3) {
+                    blow3 = job6[i];
+                }
+            }
+
+            blow = MAXof3(blow1, blow2, blow3);
+
+            break;
+    }
+
+    return blow;
+
+}