main.cpp

00001 /**
00002  * Rapid-prototyping protection schemes with IEC 61850
00003  *
00004  * Copyright (c) 2011 Steven Blair
00005  * 
00006  * This program is free software; you can redistribute it and/or
00007  * modify it under the terms of the GNU General Public License
00008  * as published by the Free Software Foundation; either version 2
00009  * of the License, or (at your option) any later version.
00010 
00011  * This program is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  * GNU General Public License for more details.
00015 
00016  * You should have received a copy of the GNU General Public License
00017  * along with this program; if not, write to the Free Software
00018  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00019  */
00020 
00021 #include "mbed.h"
00022 #include "iec61850.h"
00023 
00024 #define TWO_PI              6.283185307179586476925286766559
00025 #define TWO_PI_OVER_THREE   2.0943951023931954923084289221863
00026 #define MAGNITUDE           100
00027 #define INCREMENT           0.39269908169872415480783042290994  // 2 * pi / 16
00028 #define ERROR               (1.0 + (rand() % 5) / 100.0)
00029 
00030 unsigned char bufOut[2048] = {0};
00031 unsigned char bufIn[2048] = {0};
00032 int waitTime = 0;
00033 int lenIn = 0;
00034 int lenOut = 0;
00035 float phase = 0.0;
00036 float MAGNITUDE_NEG_SEQ = 0.0;
00037 float MAGNITUDE_ZERO_SEQ = 0.0;
00038 float offset = 0.0;
00039 
00040 void setFault() {
00041     MAGNITUDE_NEG_SEQ = 50.0;
00042     MAGNITUDE_ZERO_SEQ = 25.0;
00043 }
00044 
00045 void setNormal() {
00046     MAGNITUDE_NEG_SEQ = 0.0;
00047     MAGNITUDE_ZERO_SEQ = 0.0;
00048     offset = 0.0;
00049 }
00050 
00051 DigitalOut watchdogLED(LED1);
00052 DigitalOut inputLED(LED4);
00053 Ethernet eth;
00054 DigitalOut ethLink(p29);
00055 DigitalOut ethAct(p30);
00056 Ticker sv;
00057 
00058 void svSnapshot() {
00059     E1Q1SB1.S1.C1.RMXU_1.AmpLocPhsA.instMag.f = MAGNITUDE * sin(phase + offset) * ERROR + MAGNITUDE_NEG_SEQ * sin(phase + offset) + MAGNITUDE_ZERO_SEQ * sin(phase + offset);
00060     E1Q1SB1.S1.C1.RMXU_1.AmpLocPhsB.instMag.f = MAGNITUDE * sin(phase + offset - TWO_PI_OVER_THREE) * ERROR + MAGNITUDE_NEG_SEQ * sin(phase + offset + TWO_PI_OVER_THREE) + MAGNITUDE_ZERO_SEQ * sin(phase + offset);
00061     E1Q1SB1.S1.C1.RMXU_1.AmpLocPhsC.instMag.f = MAGNITUDE * sin(phase + offset + TWO_PI_OVER_THREE) * ERROR + MAGNITUDE_NEG_SEQ * sin(phase + offset - TWO_PI_OVER_THREE) + MAGNITUDE_ZERO_SEQ * sin(phase + offset);
00062 
00063     lenOut = sv_update_rmxuCB_rmxu(bufOut);
00064     
00065     if (lenOut > 0) {
00066         ethAct = 1;
00067         eth.write((const char *) bufOut, lenOut);
00068         eth.send();
00069         ethAct = 0;
00070     }
00071     
00072     phase += INCREMENT;
00073     if (phase > TWO_PI) {
00074         phase = phase - TWO_PI;
00075     }
00076 }
00077 
00078 int main() {
00079     initialise_iec61850();
00080     
00081     eth.set_link(eth.FullDuplex100);
00082     while (!eth.link() && waitTime++ < 60) {
00083         wait(1);
00084     }
00085     ethLink = 1;
00086     
00087     wait(1);
00088     
00089     sv.attach_us(&svSnapshot, 1250);
00090 
00091     wait(5);
00092     
00093     while(1) {
00094         wait_us(100);
00095         
00096         lenIn = eth.receive();
00097         if (lenIn == 272) {
00098             eth.read((char *) bufIn, lenIn);
00099             gseDecode(bufIn, lenIn);
00100             
00101             //printf("%f\n", D1Q1SB4.S1.C1.RSYN_1.gse_inputs.instMag_1.f);  // monitor values from serial port
00102             
00103             if (D1Q1SB4.S1.C1.RSYN_1.gse_inputs.instMag_1.f > 0) {
00104                 setFault();
00105             }
00106             else {
00107                 setNormal();
00108             }
00109         }
00110     }
00111 }