main.cpp

00001 #include "mbed.h"
00002 #include <math.h>
00003 
00004 /*
00005 TO-DO: i dati arrivano in DigitalIn e DigitalOut, anziché essere della classe Serial.
00006 Al fine di poter immagazzinare i dati, associo a determinati livelli logici (1/0) dei caratteri,
00007 riempio il buffer e ritrasmetto con i caratteri riconvertiti
00008 
00009 comando per azionamento periferiche: SPARE
00010 
00011 modifica sulla ricezione: devo ricevere solo se un certo comando è stato selezionato
00012 */
00013 
00014 // ************* INFO *******************
00015 // master-slave: slave MIMA comunica con DORA con collegamento UART (tx, rx, gnd)
00016 // PC S/C sends packets to DORA -> MIMA (STM32) -> PC
00017 
00018 //  ------                  ------------
00019 // | MIMA | ---- DORA ---- | SPACE/CRAFT |
00020 //  ------                  ------------
00021 
00022 // ************ DEFINE ******************
00023 // definisce dimensione pacchetto inviato da pc
00024 // telecommand: 15 bit + 1 di controllo
00025 #define PACKETSIZE 20           // lunghezza massima della stringa trasmissibile prima di interrupt
00026 #define PACKET 15               // 15 bit, 1 di parità di controllo
00027 #define PACKET_CHECKED 16       // COMMAND IDENTIFIER + TAG + PAYLOAD + PARITY CHECK
00028 #define IDENTIFIER 5
00029 #define TAG 2
00030 #define PAYLOAD 8
00031 #define CMD_NUM 32
00032 #define OPTION_1 10            // 57 * 16
00033 #define OPTION_2 90608          // 5663 * 16
00034 #define tb 7                    // bit sampling period
00035 #define BufferXOR 175           // 2800/16 - 1
00036 #define word 16
00037 
00038 
00039 // ************ CN7+USB ******************
00040 Serial SC(USBTX, USBRX);                // A4 TX, A6 RX - DATA OUT, DATA IN
00041 
00042 // ************** CN10 *******************
00043 Serial MIMA(PC_4, PC_5);              // TERZO - TERZULTIMO -> D0 e D1 non vanno bene! Risorse già occupate
00044 
00045 // ************** TIMER *****************
00046 Timer t;
00047 float TIMEOUT = 10;
00048 float tTimeStart;
00049 float tTimeStop;
00050 float tTimeRead;
00051 
00052 // ******* L16 Actuatonix - Relay ********
00053 PwmOut M1 (PA_6);                       // D12
00054 PwmOut M2 (PA_8);                       // D7
00055 DigitalOut Relay5V (PA_7);              // D11
00056 DigitalOut Relay12V (PB_4);             // D5
00057 DigitalOut Relay24V (PB_5);             // D4
00058 DigitalOut Relay15_15_5V (PB_10);       // D6 | +15, -15, -5
00059 float offset = 0.0f;
00060 volatile char ch;
00061 
00062 // DATA_OUT_CLOCK.write(0) -> wait_ms(7) -> DATA_OUT_CLOCK.write(0) -> wait_ms(7)
00063 
00064 // *********** VARIABLES *****************
00065 volatile int nIndex;                    // indice del pacchetto per lo "spacchettamento"
00066 char getPacket[PACKETSIZE];             // buffer - pacchetto inviato di dimensione max PACKETSIZE
00067 char PacketChecked[PACKET_CHECKED];     // lato Space/Craft
00068 char TLM[OPTION_1];                     // lato MIMA per check
00069 char TLM_SD[OPTION_2];                  // lato MIMA per check
00070 
00071 
00072 volatile int LEN;                       // numero di bit da ricevere per ricezione corretta 
00073 volatile int commandId;
00074 volatile int payloadDec; 
00075 volatile int sizeBuffer;
00076 volatile unsigned char data;            // in arrivo da MIMA per telemetria
00077 
00078 int base = 2;                           // base per l'esponenziale (conversione binary to decimal)
00079 int CMD[CMD_NUM];                       // CMD in decimale
00080 
00081 volatile int option_1 = 0;              // default
00082 volatile int option_2 = 0;
00083 
00084 int tempCheck[word];                    // temp
00085 char checksumWord[word];                // checksum sul pacchetto elaborato
00086 char checksumReceived[word];            // checksum ricevuto
00087 
00088 // ************** SETUP *******************
00089 void CMD_REF();                             // crea la lista dei comandi
00090 void clearBuffer(char *arr, int arrLen);    // ripulisce il buffer di ricezione
00091 void checksum(char *a, int aSize);          // effettua checksum
00092 void RxTelecommand();
00093 void TxTelecommand();
00094 void RxTelemetry();
00095 void TxTelemetry();
00096 void ActuatorControl(int command);           // controllo L16 e Relay
00097 
00098 
00099 // *********************************************************************
00100 
00101 
00102 // ************** MAIN *******************
00103 int main()
00104 {
00105     // baudrate
00106     SC.baud(115200);
00107     MIMA.baud(9600);
00108     
00109     // Setup PWM
00110     M1.period_us(1000); 
00111     M2.period_us(1000);
00112 
00113     // Setup Relay: default (pull-up)
00114     Relay5V = 1;
00115     Relay12V = 1;
00116     Relay15_15_5V = 1; // +15, -15, -5
00117     Relay24V = 1;
00118     
00119     // diagnostic
00120     SC.printf("*******Starting... *******\n\r");
00121 
00122     // interrupt
00123     SC.attach(&RxTelecommand, Serial::RxIrq);
00124     MIMA.attach(&RxTelemetry, Serial::RxIrq);
00125 
00126 }
00127 
00128 
00129 // *********************************************************************
00130 
00131 
00132 // ************** FUNCTIONS *******************
00133 void CMD_REF()
00134 {
00135     for (int i = 0; i < CMD_NUM; i++)
00136     {
00137         CMD[i] = i;
00138     }
00139 }
00140 
00141 // clear
00142 void clearBuffer(char *arr, int arrLen)
00143 {
00144     for (nIndex = 0; nIndex < arrLen; nIndex++)
00145     {
00146         arr[nIndex] = '\0';
00147     }
00148 }
00149 
00150 // DORA riceve telecomando da SC
00151 void RxTelecommand()
00152 {
00153     CMD_REF();  // carica vettore dei comandi
00154 
00155     while (SC.readable())   // finché sulla porta USB arrivano dati, esegui
00156     {
00157         clearBuffer(getPacket, sizeof(getPacket) / sizeof(getPacket[0]));   // clear the buffer
00158         SC.gets(getPacket, sizeof(getPacket));  // leggi la stringa in ingresso
00159         LEN = strlen(getPacket) - 2;    // calcola la sua lunghezza ignorando i tag \r\n
00160         ch = getPacket[0];
00161 
00162         if (LEN != PACKET)
00163         {
00164             if (LEN == 1)
00165             {
00166                 SC.printf("> Bit number: %i is not equal to 15  // no tele-command\n\r", LEN);  // diagnostica        
00167             }
00168         }
00169         else
00170         {
00171             for (int n = 0; n < LEN; n++)
00172             {
00173                 PacketChecked[n] = getPacket[n];    // riempi il nuovo array
00174             }
00175 
00176             // ***CONTROLLO DEI BIT - LSB: EVEN/ODD ***
00177             int ones = 0;
00178             int check = 0;
00179             while (check < PACKET)
00180             {
00181                 if (getPacket[check] == '1')
00182                 {
00183                     ones++;
00184                 }
00185 
00186                 check++;
00187             }
00188 
00189             char newItem;
00190 
00191             if (ones % 2 == 0)
00192             {
00193                 newItem = '0';  // If the number of ones in[B0÷B14] bits is even (making the total number of ones even)
00194             }
00195             else
00196             {
00197                 newItem = '1';  // If the number of ones in[B0÷B14] bits is odd (making the total number of ones even)
00198             }
00199 
00200             int nPacket = PACKET + 1;
00201             // shift elements 
00202             for (int i = nPacket - 1; i >= PACKET_CHECKED; i--)
00203             {
00204                 PacketChecked[i] = PacketChecked[i - 1];
00205             }
00206 
00207             // insert LSB
00208             PacketChecked[PACKET_CHECKED - 1] = newItem;
00209 
00210             SC.printf("> Send: ");
00211             for (int z = 0; z < PACKET_CHECKED; z++)
00212             {
00213                 SC.printf("%c", PacketChecked[z]);
00214             }
00215 
00216             SC.printf("\n\r");
00217 
00218             // ************DIAGNOSTICA ************
00219             // un telecommand è formato da 16 bit. i primi 5 sono identificativi,
00220             // poi ho due bit che mi specificano se i dati in arrivo sono una o più parole,
00221             // dopodiché ho un payload da 8 bit e per concludere ho LSB (parity bit)
00222             // MSB IIII TT PPPPPPPP PC
00223 
00224             // ***COMMAND IDENTIFIER ***
00225             int CMDIndex = 0;   // puntatore dell'identificativo
00226             commandId = 0;
00227 
00228             int B7 = PacketChecked[7];
00229             int B9 = PacketChecked[9];
00230             int B11 = PacketChecked[11];
00231 
00232             // trasformo l'identificatore in numero
00233             while (CMDIndex < IDENTIFIER)
00234             {
00235                 if (PacketChecked[CMDIndex] == '1')
00236                 {
00237                     int raise = IDENTIFIER - 1 - CMDIndex;
00238                     commandId += pow((float) base, (float) raise);
00239                 }
00240                 else
00241                 {
00242                     commandId += 0;
00243                 }
00244 
00245                 CMDIndex++;
00246             }
00247 
00248             // scorro la lista di comandi disponibli e verifico se il comando trasmesso rientra
00249             // nella lista, altrimenti finisco in uno dei 3 SPARE
00250             int k = 0;
00251             int isElementPresent = 0;
00252             while (k < CMD_NUM)
00253             {
00254                 if (commandId == CMD[k])
00255                 {
00256                     isElementPresent = 1;
00257                     SC.printf("> Telcommand sent belgons to MIMA command list: CMD %i\n\r", k);
00258 
00259                     if (k == 2 || k == 15 || k == 30) // CMD02, CMD15, CMD30
00260                     {
00261                         SC.printf("> CMD %i : SPARE\n\r", k);
00262                     }
00263                     else if (k == 0 && PacketChecked[5] == '0' && PacketChecked[6] == '0') // CMD0 - spare
00264                     {
00265                         payloadDec = 0; // inizializzazione del payload in formato digitale
00266                         int counter = 7;    // parto dal settimo bit
00267 
00268                         while (counter < PACKET)
00269                         {
00270                             if (PacketChecked[counter] == '1')
00271                             {
00272                                 int raise = PACKET - 1 - counter;
00273                                 payloadDec += pow((float) base, (float) raise);
00274                             }
00275                             else
00276                             {
00277                                 payloadDec += 0;
00278                             }
00279 
00280                             counter++;
00281                         }
00282 
00283                         ActuatorControl(payloadDec);
00284                         break;
00285                     }
00286                     else if (k == 1) // CMD01
00287                     {
00288                         if (PacketChecked[11] == '0' &&
00289                             PacketChecked[12] == '0' &&
00290                             PacketChecked[13] == '0' &&
00291                             PacketChecked[14] == '1')
00292                         {
00293                             SC.printf("> Setting MIMA... Sleeping mode\n\r");
00294                         }
00295                         else if (PacketChecked[11] == '0' &&
00296                             PacketChecked[12] == '0' &&
00297                             PacketChecked[13] == '1' &&
00298                             PacketChecked[14] == '0')
00299                         {
00300                             SC.printf("> Setting MIMA... Awake mode\n\r");
00301                         }
00302                         else if (PacketChecked[11] == '0' &&
00303                             PacketChecked[12] == '0' &&
00304                             PacketChecked[13] == '1' &&
00305                             PacketChecked[14] == '1')
00306                         {
00307                             SC.printf("> Setting MIMA... Calibration mode\n\r");
00308                         }
00309                         else if (PacketChecked[11] == '0' &&
00310                             PacketChecked[12] == '1' &&
00311                             PacketChecked[13] == '0' &&
00312                             PacketChecked[14] == '0')
00313                         {
00314                             SC.printf("> Setting MIMA... Observation mode\n\r");
00315                         }
00316                         else if (PacketChecked[11] == '0' &&
00317                             PacketChecked[12] == '1' &&
00318                             PacketChecked[13] == '0' &&
00319                             PacketChecked[14] == '1')
00320                         {
00321                             SC.printf("> Setting MIMA... Auto-test mode\n\r");
00322                         }
00323                         else if (PacketChecked[11] == '0' &&
00324                             PacketChecked[12] == '1' &&
00325                             PacketChecked[13] == '1' &&
00326                             PacketChecked[14] == '0')
00327                         {
00328                             SC.printf("> Setting MIMA... Full testing mode\n\r");
00329                         }
00330                     }
00331                     else if (k == 7) // CMD07
00332                     {
00333                         // IR sensor thermal loop
00334                         if (B7 == '1')
00335                         {
00336                             SC.printf("> IR sensor thermal loop...\n\r");
00337                             SC.printf("IRT1 telemetry: feedback signal in the thermal control loop (default). IRT2 only for monitoring\n\r");
00338                         }
00339                         else
00340                         {
00341                             SC.printf("> IR sensor thermal loop...\n\r");
00342                             SC.printf("IRT2 telemetry: feedback signal in the thermal control. IRT1 only for monitoring\n\r");
00343                         }
00344 
00345                         // Laser Diode thermal loop
00346                         if (B9 == '1')
00347                         {
00348                             SC.printf("> Laser Diode thermal loop...\n\r");
00349                             SC.printf("LDT1 telemetry: feedback signal in the thermal control loop (default). LDT2 only for monitoring\n\r");
00350                         }
00351                         else
00352                         {
00353                             SC.printf("> Laser Diode thermal loop...\n\r");
00354                             SC.printf("LDT2 telemetry: feedback signal in the thermal control. LDT1 only for monitoring\n\r");
00355 
00356                         }
00357 
00358                         // BlackBody thermal loop
00359                         if (B11 == '1')
00360                         {
00361                             SC.printf("> BlackBody thermal loop...\n\r");
00362                             SC.printf("BBT1 telemetry: feedback signal in the thermal control loop (default). BBT2 only for monitoring\n\r");
00363                         }
00364                         else
00365                         {
00366                             SC.printf("> BlackBody thermal loop...\n\r");
00367                             SC.printf("BBT2 telemetry: feedback signal in the thermal control. BBT1 only for monitoring\n\r");
00368                         }
00369                     }
00370 
00371                 }
00372                 
00373                 k++;
00374             }
00375 
00376             if (isElementPresent == '0')
00377             {
00378                 SC.printf("Telecommand sent doesn't exist\n\r");
00379             }
00380 
00381             // ***TAG ***
00382             int B5 = PacketChecked[5];
00383             int B6 = PacketChecked[6];
00384 
00385             if (B5 == '0' && B6 == '0')
00386             {
00387                 // Single word telemetry
00388                 SC.printf("Single word telemetry\n\r");
00389             }
00390             else if (B5 == '0' && B6 == '1')
00391             {
00392                 // Multiple words telemetry (1st word)
00393                 SC.printf("Multiple words telemetry (1st word)\n\r");
00394             }
00395             else if (B5 == '1' && B6 == '0')
00396             {
00397                 // Multiple words telemetry (2nd word)
00398                 SC.printf("Multiple words telemetry (2nd word)\n\r");
00399             }
00400             else
00401             {
00402                 // Not used
00403                 SC.printf("NOT USED\n\r");
00404             }
00405 
00406             // ***PAYLOAD ***
00407             SC.printf("> Payload: ");
00408             for (int IndexPayload = 7; IndexPayload < PACKET; IndexPayload++)
00409             {
00410                 SC.printf("%c", PacketChecked[IndexPayload]);
00411             }
00412 
00413             SC.printf("\n\r");  // per leggibilità
00414 
00415             //***********FINE DIAGNOSTICA PACCHETTO***********
00416 
00417         }
00418         
00419         TxTelecommand();
00420     }
00421 }
00422 
00423 /* ***************************************************************** */
00424 
00425 // DORA trasmette telecomando a MIMA
00426 void TxTelecommand()
00427 {
00428     for (int i = 0; i < PACKET_CHECKED + 1; i++)
00429     {
00430         MIMA.putc(PacketChecked[i]);
00431     }
00432 }
00433 
00434 
00435 /* ***************************************************************** */
00436 
00437 // DORA riceve telemetrie/telemetrie+scientific data da MIMA
00438 void RxTelemetry()
00439 {
00440     // aggiungere timeout, perché non so se raggiungo il numero dei caratteri
00441 
00442     while (MIMA.readable())
00443     {        
00444         // char prova = MIMA.getc();
00445         // SC.printf("> Text: %c\n\r", prova);
00446         // check del payload per scelta del buffer
00447         // 00000000 -> Option 1: transfer all Housekeeping and Status telemetries (from TLM00 to TLM56)
00448         // 11111111 -> Option 2: transfer telemetries and scientific data packets (from TLM00 to TLM5662)
00449 
00450         int j = 7;
00451         while (j < PACKET_CHECKED)
00452         {
00453             if (PacketChecked[j] == '0')
00454             {
00455                 j++;
00456             }
00457             else
00458             {
00459                 option_1 = 1;
00460                 break;
00461             }
00462         }
00463 
00464         int k = 7;
00465         while (k < PACKET_CHECKED)
00466         {
00467             if (PacketChecked[k] == '1')
00468             {
00469                 k++;
00470             }
00471             else
00472             {
00473                 option_2 = 1;
00474                 break;
00475             }
00476         }
00477 
00478         if (commandId == 5) // TLMODE
00479         {
00480             // STARTCH1 - 2800 scientific points - CHECKSUM - ENDCH1
00481             // STARTCH2 - 2800 scientific points - CHECKSUM - ENDCH2
00482             // 5606 words
00483             if (option_1 == 0)
00484             {
00485                 sizeBuffer = 0;   // reset 
00486                 clearBuffer(TLM, sizeof(TLM)/sizeof(TLM[0]));
00487                 
00488                 t.start();
00489                 tTimeStart = t.read(); // in secondi 
00490                 tTimeRead = tTimeStart;
00491                 
00492                 while (sizeBuffer < OPTION_1 && ((tTimeRead - tTimeStart) < TIMEOUT))
00493                 {
00494                     data = MIMA.getc(); // prendi carattere in arrivo
00495                     TLM[sizeBuffer++] = data;
00496                     tTimeRead = t.read();          
00497                 }
00498                 
00499                 tTimeRead = t.read();
00500                 
00501                 if ((tTimeRead - tTimeStart) >= TIMEOUT)
00502                 {
00503                     sizeBuffer = 0;
00504                     clearBuffer(TLM, OPTION_1);
00505                     SC.printf("> Time elapsed: %f \n", tTimeRead);
00506                 } 
00507                 
00508             }
00509             else if (option_2 == 0)
00510             {
00511                 sizeBuffer = 0;
00512                 clearBuffer(TLM_SD, sizeof(TLM_SD)/sizeof(TLM_SD[0]));
00513 
00514                 while (sizeBuffer < OPTION_2)
00515                 {
00516                     data = MIMA.getc(); // prendi carattere in arrivo
00517                     TLM_SD[sizeBuffer++] = data;
00518                 }
00519             }
00520             
00521         }
00522         
00523         TxTelemetry();
00524     }
00525 }
00526 
00527 /* ***************************************************************** */
00528 
00529 // integrità dei dati ricevuti da MIMA 
00530 void checksum(char *a, int aSize) 
00531 {
00532     volatile int temp;
00533     volatile int var;
00534     volatile int p = 0;
00535   
00536     for (int i = word; i < 2*word; i++) 
00537     {
00538         for (int j = 2*i; j < aSize; j+=i)
00539         {
00540             if (j == 2*i) 
00541             {
00542                 temp = (int)(a[i] ^ a[j]); 
00543             } else
00544             {
00545                 temp = temp ^ (int)a[j];   
00546             }
00547 
00548         }
00549 
00550         tempCheck[p++] = temp; // vettore del checksum in int
00551     }
00552     
00553     for (int i = 0; i < word; i++) // trasformo in char
00554     {
00555         if (tempCheck[i] == 0 || tempCheck[i] == 48)
00556         {
00557             checksumWord[i++] = '0';
00558         } 
00559         else if (tempCheck[i] == 1 || tempCheck[i] == 49)
00560         {
00561             checksumWord[i++] = '1';
00562         }
00563     }
00564     
00565     
00566     volatile int t = 0;
00567     // confronto
00568     for (int i = 2801*word; i < 2802*16; i++) 
00569     {
00570         checksumReceived[t++] = TLM_SD[i];
00571     }
00572     
00573     for (int i = 0; i < word; i++) 
00574     {
00575         if (checksumWord[i] != checksumReceived[i])
00576         {
00577             SC.printf("\n\r> ERROR detected: BIT n. %i\n\r", i+1);
00578             break;
00579         }         
00580     }
00581             
00582     
00583 }
00584 
00585 /* ***************************************************************** */
00586 
00587 // DORA trasmette dati verso Space/craft
00588 void TxTelemetry()
00589 {
00590     // The data are read on the falling-edge of the DATA_CLOCK_OUT signal
00591     if (option_1 == 0)
00592     {
00593         SC.printf("\n\r> Receiving 57 words from MIMA... \n\r");
00594         
00595         for (int i = 0; i < OPTION_1; i++)
00596         {
00597             SC.putc(TLM[i]);
00598         }
00599         
00600         SC.printf("\n\r");
00601     }
00602     else if (option_2 == 0)
00603     {
00604         SC.printf("Receiving 5663 words...\n\r");
00605         
00606         for (int i = 0; i < OPTION_2; i++)
00607         {
00608             if (i % word == 0)
00609             {
00610                 SC.printf("\n\r");
00611             }
00612             else 
00613             {
00614                 SC.putc(TLM_SD[i]);
00615             }
00616         }
00617         
00618         SC.printf("\n\r");
00619     }
00620     
00621 }
00622 
00623 /* ***************************************************************** */
00624 
00625 // controllo dell'attuatore e dei relay
00626 void ActuatorControl(int command) 
00627 {
00628         SC.printf("\n\r *** L16 ACTUATONIX - RELAY CONTROL *** \n\r"); // leggibilità
00629         
00630         // *** estensione/ritrazione dell'attuatore ***
00631         if((command == 128) && (offset < 1.0f)) { 
00632             offset += 0.2f; 
00633         } else if((command == 64) && (offset > 0.0f)) {
00634             offset -= 0.2f; 
00635             
00636         // *** gestione accensione/spegnimento relay alimentazione ***
00637         }    // Relay 5V
00638           else if (command == 1) {
00639             Relay5V = 0;
00640             SC.printf("\r\nRelay 5V ON\r\n");
00641         } else if (command == 0) {
00642             Relay5V = 1;
00643             SC.printf("\r\nRelay 5V OFF\r\n");
00644                     
00645             // Relay 12V
00646         } else if (command == 3) {
00647             Relay12V = 0;
00648             SC.printf("\r\nRelay 12V ON\r\n");
00649                
00650         }  else if (command == 2) {
00651             Relay12V = 1;
00652             SC.printf("\r\nRelay 12V OFF\r\n");
00653                
00654             // Relay +15V, -15V, -5V
00655         } else if (command == 7) {
00656             Relay15_15_5V = 0;
00657             SC.printf("\r\nRelay +15V, -15V, -5V ON\r\n");
00658                
00659         } else if (command == 6) {
00660             Relay15_15_5V = 1;
00661             SC.printf("\r\nRelay +15V, -15V, -5V OFF\r\n");
00662         
00663             // Relay 24V
00664         } else if (command == 15) {
00665             Relay24V = 0;
00666             SC.printf("\r\nRelay 24V ON\r\n");
00667                
00668         } else if (command == 8) {
00669             Relay24V = 1;
00670             SC.printf("\r\nRelay 24V OFF\r\n");   
00671             
00672             // RESET
00673         } else if (command == 255) {
00674             // spegni tutto
00675             Relay5V = 1;
00676             Relay12V = 1;
00677             Relay15_15_5V = 1; // +15, -15, -5
00678             Relay24V = 1;
00679         }
00680             
00681         M1.write(offset);
00682         M2.write(offset); 
00683                     
00684         SC.printf("> Duty Cycle %.2f / estensione \n \r", offset);   
00685 }