Diff: main.cpp

Revision:

0:c81f86a07902

Child:

1:db73b542b7bc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jun 09 10:43:58 2022 +0000
@@ -0,0 +1,693 @@
+#include "mbed.h"
+#include <math.h>
+
+/*
+TO-DO: i dati arrivano in DigitalIn e DigitalOut, anziché essere della classe Serial.
+Al fine di poter immagazzinare i dati, associo a determinati livelli logici (1/0) dei caratteri,
+riempio il buffer e ritrasmetto con i caratteri riconvertiti
+
+comando per azionamento periferiche: SPARE
+*/
+
+// ************* INFO *******************
+// master-slave: slave MIMA comunica con DORA con collegamento UART (tx, rx, gnd)
+// PC S/C sends packets to DORA -> MIMA (STM32) -> PC
+
+//  ------                  ------------
+// | MIMA | ---- DORA ---- | SPACE/CRAFT |
+//  ------                  ------------
+
+// ************ DEFINE ******************
+// definisce dimensione pacchetto inviato da pc
+// telecommand: 15 bit + 1 di controllo
+#define PACKETSIZE 20           // lunghezza massima della stringa trasmissibile prima di interrupt
+#define PACKET 15               // 15 bit, 1 di parità di controllo
+#define PACKET_CHECKED 16       // COMMAND IDENTIFIER + TAG + PAYLOAD + PARITY CHECK
+#define IDENTIFIER 5
+#define TAG 2
+#define PAYLOAD 8
+#define CMD_NUM 32
+#define OPTION_1 10            // 57 * 16
+#define OPTION_2 90608          // 5663 * 16
+#define tb 7                    // bit sampling period
+#define BufferXOR 175           // 2800/16 - 1
+#define TIMEOUT 15000           // milliseconds
+#define word 16
+
+
+// ************ CN7+USB ******************
+Serial SC(USBTX, USBRX);                // A4 TX, A6 RX - DATA OUT, DATA IN
+
+// ************** CN10 *******************
+Serial MIMA(PC_4, PC_5);              // TERZO - TERZULTIMO -> D0 e D1 non vanno bene! Risorse già occupate
+
+// ************** TIMER *****************
+Timer t;
+
+// ******* L16 Actuatonix - Relay ********
+PwmOut M1 (PA_6);                       // D12
+PwmOut M2 (PA_8);                       // D7
+DigitalOut Relay5V (PA_7);              // D11
+DigitalOut Relay12V (PB_4);             // D5
+DigitalOut Relay24V (PB_5);             // D4
+DigitalOut Relay15_15_5V (PB_10);       // D6 | +15, -15, -5
+float offset = 0.0f;
+volatile char ch;
+
+// DATA_OUT_CLOCK.write(0) -> wait_ms(7) -> DATA_OUT_CLOCK.write(0) -> wait_ms(7)
+
+// *********** VARIABLES *****************
+volatile int nIndex;                    // indice del pacchetto per lo "spacchettamento"
+char getPacket[PACKETSIZE];             // buffer - pacchetto inviato di dimensione max PACKETSIZE
+char PacketChecked[PACKET_CHECKED];     // lato Space/Craft
+char TLM[OPTION_1];                     // lato MIMA per check
+char TLM_SD[OPTION_2];                  // lato MIMA per check
+
+
+volatile int LEN;                       // numero di bit da ricevere per ricezione corretta 
+volatile int commandId;
+volatile int payloadDec; 
+volatile int size;
+volatile unsigned char data;            // in arrivo da MIMA per telemetria
+
+int base = 2;                           // base per l'esponenziale (conversione binary to decimal)
+int CMD[CMD_NUM];                       // CMD in decimale
+
+volatile int option_1 = 0;              // default
+volatile int option_2 = 0;
+
+int tempCheck[word];                    // temp
+char checksumWord[word];                // checksum sul pacchetto elaborato
+char checksumReceived[word];            // checksum ricevuto
+
+// ************** SETUP *******************
+void CMD_REF();                             // crea la lista dei comandi
+void clearBuffer(char *arr, int arrLen);    // ripulisce il buffer di ricezione
+void checksum(char *a, int aSize);          // effettua checksum
+void RxTelecommand();
+void TxTelecommand();
+void RxTelemetry();
+void TxTelemetry();
+void ActuatorControl(int command);           // controllo L16 e Relay
+
+
+// *********************************************************************
+
+
+// ************** MAIN *******************
+int main()
+{
+    // baudrate
+    SC.baud(115200);
+    MIMA.baud(9600);
+    
+    // Setup PWM
+    M1.period_us(1000); 
+    M2.period_us(1000);
+
+    // Setup Relay: default (pull-up)
+    Relay5V = 1;
+    Relay12V = 1;
+    Relay15_15_5V = 1; // +15, -15, -5
+    Relay24V = 1;
+
+    // diagnostic
+    SC.printf("*******Starting... *******\n\r");
+
+    // interrupt
+    SC.attach(&RxTelecommand, Serial::RxIrq);
+    MIMA.attach(&RxTelemetry, Serial::RxIrq);
+
+}
+
+
+// *********************************************************************
+
+
+// ************** FUNCTIONS *******************
+void CMD_REF()
+{
+    for (int i = 0; i < CMD_NUM; i++)
+    {
+        CMD[i] = i;
+    }
+}
+
+// clear
+void clearBuffer(char *arr, int arrLen)
+{
+    // reset pacchetto
+    nIndex = 0;
+    for (nIndex = 0; nIndex < arrLen; nIndex++)
+    {
+        arr[nIndex] = '\0';
+    }
+}
+
+// DORA riceve telecomando da SC
+void RxTelecommand()
+{
+    CMD_REF();  // carica vettore dei comandi
+
+    while (SC.readable())   // finché sulla porta USB arrivano dati, esegui
+    {
+        clearBuffer(getPacket, sizeof(getPacket) / sizeof(getPacket[0]));   // clear the buffer
+        SC.gets(getPacket, sizeof(getPacket));  // leggi la stringa in ingresso
+        LEN = strlen(getPacket) - 2;    // calcola la sua lunghezza ignorando i tag \r\n
+        ch = getPacket[0];
+
+        if (LEN != PACKET)
+        {
+            if (LEN == 1)
+            {
+                SC.printf("> Bit number: %i is not equal to 15  // no tele-command\n\r", LEN);  // diagnostica        
+            }
+        }
+        else
+        {
+            for (int n = 0; n < LEN; n++)
+            {
+                PacketChecked[n] = getPacket[n];    // riempi il nuovo array
+            }
+
+            // ***CONTROLLO DEI BIT - LSB: EVEN/ODD ***
+            int ones = 0;
+            int check = 0;
+            while (check < PACKET)
+            {
+                if (getPacket[check] == '1')
+                {
+                    ones++;
+                }
+
+                check++;
+            }
+
+            char newItem;
+
+            if (ones % 2 == 0)
+            {
+                newItem = '0';  // If the number of ones in[B0÷B14] bits is even (making the total number of ones even)
+            }
+            else
+            {
+                newItem = '1';  // If the number of ones in[B0÷B14] bits is odd (making the total number of ones even)
+            }
+
+            int nPacket = PACKET + 1;
+            // shift elements 
+            for (int i = nPacket - 1; i >= PACKET_CHECKED; i--)
+            {
+                PacketChecked[i] = PacketChecked[i - 1];
+            }
+
+            // insert LSB
+            PacketChecked[PACKET_CHECKED - 1] = newItem;
+
+            SC.printf("> Send: ");
+            for (int z = 0; z < PACKET_CHECKED; z++)
+            {
+                SC.printf("%c", PacketChecked[z]);
+            }
+
+            SC.printf("\n\r");
+
+            // ************DIAGNOSTICA ************
+            // un telecommand è formato da 16 bit. i primi 5 sono identificativi,
+            // poi ho due bit che mi specificano se i dati in arrivo sono una o più parole,
+            // dopodiché ho un payload da 8 bit e per concludere ho LSB (parity bit)
+            // MSB IIII TT PPPPPPPP PC
+
+            // ***COMMAND IDENTIFIER ***
+            int CMDIndex = 0;   // puntatore dell'identificativo
+            commandId = 0;
+
+            int B7 = PacketChecked[7];
+            int B9 = PacketChecked[9];
+            int B11 = PacketChecked[11];
+
+            // trasformo l'identificatore in numero
+            while (CMDIndex < IDENTIFIER)
+            {
+                if (PacketChecked[CMDIndex] == '1')
+                {
+                    int raise = IDENTIFIER - 1 - CMDIndex;
+                    commandId += pow((float) base, (float) raise);
+                }
+                else
+                {
+                    commandId += 0;
+                }
+
+                CMDIndex++;
+            }
+
+            // scorro la lista di comandi disponibli e verifico se il comando trasmesso rientra
+            // nella lista, altrimenti finisco in uno dei 3 SPARE
+            int k = 0;
+            int isElementPresent = 0;
+            while (k < CMD_NUM)
+            {
+                if (commandId == CMD[k])
+                {
+                    isElementPresent = 1;
+                    SC.printf("> Telcommand sent belgons to MIMA command list: CMD %i\n\r", k);
+
+                    if (k == 2 || k == 15 || k == 30) // CMD02, CMD15, CMD30
+                    {
+                        SC.printf("> CMD %i : SPARE\n\r", k);
+                    }
+                    else if (k == 0 && PacketChecked[5] == '0' && PacketChecked[6] == '0') // CMD0 - spare
+                    {
+                        payloadDec = 0; // inizializzazione del payload in formato digitale
+                        int counter = 7;    // parto dal settimo bit
+
+                        while (counter < PACKET)
+                        {
+                            if (PacketChecked[counter] == '1')
+                            {
+                                int raise = PACKET - 1 - counter;
+                                payloadDec += pow((float) base, (float) raise);
+                            }
+                            else
+                            {
+                                payloadDec += 0;
+                            }
+
+                            counter++;
+                        }
+
+                        ActuatorControl(payloadDec);
+                        break;
+                    }
+                    else if (k == 1) // CMD01
+                    {
+                        if (PacketChecked[11] == '0' &&
+                            PacketChecked[12] == '0' &&
+                            PacketChecked[13] == '0' &&
+                            PacketChecked[14] == '1')
+                        {
+                            SC.printf("> Setting MIMA... Sleeping mode\n\r");
+                        }
+                        else if (PacketChecked[11] == '0' &&
+                            PacketChecked[12] == '0' &&
+                            PacketChecked[13] == '1' &&
+                            PacketChecked[14] == '0')
+                        {
+                            SC.printf("> Setting MIMA... Awake mode\n\r");
+                        }
+                        else if (PacketChecked[11] == '0' &&
+                            PacketChecked[12] == '0' &&
+                            PacketChecked[13] == '1' &&
+                            PacketChecked[14] == '1')
+                        {
+                            SC.printf("> Setting MIMA... Calibration mode\n\r");
+                        }
+                        else if (PacketChecked[11] == '0' &&
+                            PacketChecked[12] == '1' &&
+                            PacketChecked[13] == '0' &&
+                            PacketChecked[14] == '0')
+                        {
+                            SC.printf("> Setting MIMA... Observation mode\n\r");
+                        }
+                        else if (PacketChecked[11] == '0' &&
+                            PacketChecked[12] == '1' &&
+                            PacketChecked[13] == '0' &&
+                            PacketChecked[14] == '1')
+                        {
+                            SC.printf("> Setting MIMA... Auto-test mode\n\r");
+                        }
+                        else if (PacketChecked[11] == '0' &&
+                            PacketChecked[12] == '1' &&
+                            PacketChecked[13] == '1' &&
+                            PacketChecked[14] == '0')
+                        {
+                            SC.printf("> Setting MIMA... Full testing mode\n\r");
+                        }
+                    }
+                    else if (k == 7) // CMD07
+                    {
+                        // IR sensor thermal loop
+                        if (B7 == '1')
+                        {
+                            SC.printf("> IR sensor thermal loop...\n\r");
+                            SC.printf("IRT1 telemetry: feedback signal in the thermal control loop (default). IRT2 only for monitoring\n\r");
+                        }
+                        else
+                        {
+                            SC.printf("> IR sensor thermal loop...\n\r");
+                            SC.printf("IRT2 telemetry: feedback signal in the thermal control. IRT1 only for monitoring\n\r");
+                        }
+
+                        // Laser Diode thermal loop
+                        if (B9 == '1')
+                        {
+                            SC.printf("> Laser Diode thermal loop...\n\r");
+                            SC.printf("LDT1 telemetry: feedback signal in the thermal control loop (default). LDT2 only for monitoring\n\r");
+                        }
+                        else
+                        {
+                            SC.printf("> Laser Diode thermal loop...\n\r");
+                            SC.printf("LDT2 telemetry: feedback signal in the thermal control. LDT1 only for monitoring\n\r");
+
+                        }
+
+                        // BlackBody thermal loop
+                        if (B11 == '1')
+                        {
+                            SC.printf("> BlackBody thermal loop...\n\r");
+                            SC.printf("BBT1 telemetry: feedback signal in the thermal control loop (default). BBT2 only for monitoring\n\r");
+                        }
+                        else
+                        {
+                            SC.printf("> BlackBody thermal loop...\n\r");
+                            SC.printf("BBT2 telemetry: feedback signal in the thermal control. BBT1 only for monitoring\n\r");
+                        }
+                    }
+
+                }
+                
+                k++;
+            }
+
+            if (isElementPresent == '0')
+            {
+                SC.printf("Telecommand sent doesn't exist\n\r");
+            }
+
+            // ***TAG ***
+            int B5 = PacketChecked[5];
+            int B6 = PacketChecked[6];
+
+            if (B5 == '0' && B6 == '0')
+            {
+                // Single word telemetry
+                SC.printf("Single word telemetry\n\r");
+            }
+            else if (B5 == '0' && B6 == '1')
+            {
+                // Multiple words telemetry (1st word)
+                SC.printf("Multiple words telemetry (1st word)\n\r");
+            }
+            else if (B5 == '1' && B6 == '0')
+            {
+                // Multiple words telemetry (2nd word)
+                SC.printf("Multiple words telemetry (2nd word)\n\r");
+            }
+            else
+            {
+                // Not used
+                SC.printf("NOT USED\n\r");
+            }
+
+            // ***PAYLOAD ***
+            SC.printf("> Payload: ");
+            for (int IndexPayload = 7; IndexPayload < PACKET; IndexPayload++)
+            {
+                SC.printf("%c", PacketChecked[IndexPayload]);
+            }
+
+            SC.printf("\n\r");  // per leggibilità
+
+            //***********FINE DIAGNOSTICA PACCHETTO***********
+
+        }
+        
+        TxTelecommand();
+    }
+}
+
+/* ***************************************************************** */
+
+// DORA trasmette telecomando a MIMA
+void TxTelecommand()
+{
+    for (int i = 0; i < PACKET_CHECKED + 1; i++)
+    {
+        MIMA.putc(PacketChecked[i]);
+    }
+}
+
+
+/* ***************************************************************** */
+
+// DORA riceve telemetrie/telemetrie+scientific data da MIMA
+void RxTelemetry()
+{
+    // aggiungere timeout, perché non so se raggiungo il numero dei caratteri
+
+    while (MIMA.readable())
+    {
+        t.start();
+        
+        // char prova = MIMA.getc();
+        // SC.printf("> Text: %c\n\r", prova);
+        // check del payload per scelta del buffer
+        // 00000000 -> Option 1: transfer all Housekeeping and Status telemetries (from TLM00 to TLM56)
+        // 11111111 -> Option 2: transfer telemetries and scientific data packets (from TLM00 to TLM5662)
+
+        int j = 7;
+        while (j < PACKET_CHECKED)
+        {
+            if (PacketChecked[j] == '0')
+            {
+                j++;
+            }
+            else
+            {
+                option_1 = 1;
+                break;
+            }
+        }
+
+        int k = 7;
+        while (k < PACKET_CHECKED)
+        {
+            if (PacketChecked[k] == '1')
+            {
+                k++;
+            }
+            else
+            {
+                option_2 = 1;
+                break;
+            }
+        }
+
+        if (commandId == 5) // TLMODE
+        {
+            // STARTCH1 - 2800 scientific points - CHECKSUM - ENDCH1
+            // STARTCH2 - 2800 scientific points - CHECKSUM - ENDCH2
+            // 5606 words
+            if (option_1 == 0)
+            {
+                size = 0;   // reset 
+                clearBuffer(TLM, sizeof(TLM) / sizeof(TLM[0]));
+
+                if (t.read() < TIMEOUT)
+                {
+                    while (size < OPTION_1)
+                    {
+                        data = MIMA.getc(); // prendi carattere in arrivo
+                        TLM[size++] = data;
+                    }
+                }
+                else
+                {
+                    t.stop();
+                    SC.printf("> Time elapsed: %.2f\n\r", t.read());
+                    clearBuffer(TLM, sizeof(TLM) / sizeof(TLM[0]));
+                }
+            }
+            else if (option_2 == 0)
+            {
+                size = 0;
+                clearBuffer(TLM_SD, sizeof(TLM_SD) / sizeof(TLM_SD[0]));
+
+                t.start();
+                if (t.read() < TIMEOUT)
+                {
+                    while (size < OPTION_2)
+                    {
+                        data = MIMA.getc(); // prendi carattere in arrivo
+                        TLM_SD[size++] = data;
+                    }
+                    //checksum(TLM_SD, OPTION_2);
+                }
+                else
+                {
+                    t.stop();
+                    clearBuffer(TLM_SD, sizeof(TLM_SD) / sizeof(TLM_SD[0]));
+                }
+            }
+            
+            TxTelemetry();
+        }
+    }
+}
+
+/* ***************************************************************** */
+
+// integrità dei dati ricevuti da MIMA 
+void checksum(char *a, int aSize) 
+{
+    volatile int temp;
+    volatile int var;
+    volatile int p = 0;
+  
+    for (int i = word; i < 2*word; i++) 
+    {
+        for (int j = 2*i; j < aSize; j+=i)
+        {
+            if (j == 2*i) 
+            {
+                temp = (int)(a[i] ^ a[j]); 
+            } else
+            {
+                temp = temp ^ (int)a[j];   
+            }
+
+        }
+
+        tempCheck[p++] = temp; // vettore del checksum in int
+    }
+    
+    for (int i = 0; i < word; i++) // trasformo in char
+    {
+        if (tempCheck[i] == 0 || tempCheck[i] == 48)
+        {
+            checksumWord[i++] = '0';
+        } 
+        else if (tempCheck[i] == 1 || tempCheck[i] == 49)
+        {
+            checksumWord[i++] = '1';
+        }
+    }
+    
+    
+    volatile int t = 0;
+    // confronto
+    for (int i = 2801*word; i < 2802*16; i++) 
+    {
+        checksumReceived[t++] = TLM_SD[i];
+    }
+    
+    for (int i = 0; i < word; i++) 
+    {
+        if (checksumWord[i] != checksumReceived[i])
+        {
+            SC.printf("\n\r> ERROR detected: BIT n. %i\n\r", i+1);
+            break;
+        }         
+    }
+            
+    
+}
+
+/* ***************************************************************** */
+
+// DORA trasmette dati verso Space/craft
+void TxTelemetry()
+{
+    // The data are read on the falling-edge of the DATA_CLOCK_OUT signal
+    if (option_1 == 0)
+    {
+        SC.printf("\n\r> Receiving 57 words from MIMA... \n\r");
+
+        for (int i = 0; i < OPTION_1; i++)
+        {
+            SC.putc(TLM[i]);
+        }
+        
+        /*SC.printf("\n\r> Receiving 57 words from DORA... \n\r");
+        
+        for (int i = 0; i < OPTION_1; i++) 
+        {
+            ARDUINO.putc(DEVICE.putc(TLM[i]));
+        }
+        */
+    }
+    else if (option_2 == 0)
+    {
+        SC.printf("Receiving 5663 words...\n\r");
+        
+        for (int i = 0; i < OPTION_2; i++)
+        {
+            SC.putc(TLM_SD[i]);
+        }
+        
+        /*
+        SC.printf("\n\r> Receiving 5663 words from DORA... \n\r");
+        
+        for (int i = 0; i < OPTION_2; i++) 
+        {
+            ARDUINO.putc(DEVICE.putc(TLM_SD[i]));
+        }
+        */
+    }
+    
+}
+
+/* ***************************************************************** */
+
+// controllo dell'attuatore e dei relay
+void ActuatorControl(int command) 
+{
+        SC.printf("\n\r *** L16 ACTUATONIX - RELAY CONTROL *** \n\r"); // leggibilità
+        
+        // *** estensione/ritrazione dell'attuatore ***
+        if((command == 128) && (offset < 1.0f)) { 
+            offset += 0.2f; 
+        } else if((command == 64) && (offset > 0.0f)) {
+            offset -= 0.2f; 
+            
+        // *** gestione accensione/spegnimento relay alimentazione ***
+        }    // Relay 5V
+          else if (command == 1) {
+            Relay5V = 0;
+            SC.printf("\r\nRelay 5V ON\r\n");
+        } else if (command == 0) {
+            Relay5V = 1;
+            SC.printf("\r\nRelay 5V OFF\r\n");
+                    
+            // Relay 12V
+        } else if (command == 3) {
+            Relay12V = 0;
+            SC.printf("\r\nRelay 12V ON\r\n");
+               
+        }  else if (command == 2) {
+            Relay12V = 1;
+            SC.printf("\r\nRelay 12V OFF\r\n");
+               
+            // Relay +15V, -15V, -5V
+        } else if (command == 7) {
+            Relay15_15_5V = 0;
+            SC.printf("\r\nRelay +15V, -15V, -5V ON\r\n");
+               
+        } else if (command == 6) {
+            Relay15_15_5V = 1;
+            SC.printf("\r\nRelay +15V, -15V, -5V OFF\r\n");
+        
+            // Relay 24V
+        } else if (command == 15) {
+            Relay24V = 0;
+            SC.printf("\r\nRelay 24V ON\r\n");
+               
+        } else if (command == 8) {
+            Relay24V = 1;
+            SC.printf("\r\nRelay 24V OFF\r\n");   
+            
+            // RESET
+        } else if (command == 255) {
+            // spegni tutto
+            Relay5V = 1;
+            Relay12V = 1;
+            Relay15_15_5V = 1; // +15, -15, -5
+            Relay24V = 1;
+        }
+            
+        M1.write(offset);
+        M2.write(offset); 
+                    
+        SC.printf("> Duty Cycle %.2f / estensione \n \r", offset);   
+}