Diff: LLComms.cpp

Revision:

23:61526647cc8a

Parent:

22:82871f00f89d

Child:

24:bc852aa89e7a

--- a/LLComms.cpp	Wed Dec 12 08:41:06 2018 +0000
+++ b/LLComms.cpp	Fri Dec 14 09:58:24 2018 +0000
@@ -65,21 +65,105 @@
 
 void LLComms::SendReceiveData(int channel) {
     //pinTesty = 1;
-    int intPosSPI_Rx[N_CHANNELS]; // 13 bit value received over SPI from the actuator
+    int intPosSPI_Rx[N_CHANNELS]; // 16bit position value received over SPI from the actuator 
+    int intPresSPI_Rx[N_CHANNELS]; // 16bit pressure value received over SPI from the actuator 
+    int intSumPosition=0;
+    int intSumVelocity=0;
+    int intPositionParity, intVelocityParity;
+    unsigned int intParityPosition, intParityVelocity, intPositionMsg, intVelocityMsg;
+
+    unsigned int count = 0, i, b = 1;
+    
+    int intTempVar;
     
     mutChannel[channel].lock(); // Lock mutex for specific Channel
+    //construct Position message
+    intPositionMsg = demandPosition[channel];
+    
+    //calculate the sum of the position data 
+
+    intTempVar = demandPosition[channel];
+    
+    while (intTempVar >0)
+    {
+        intSumPosition += intTempVar%10;
+        intTempVar = int(intTempVar/10);
+    }
+    
+    //add to message
+    intPositionMsg = intPositionMsg<<5;
+    intPositionMsg = intPositionMsg | intSumPosition;
+    
+    //add type bit (0 == position, 1 == velocity)
+    intPositionMsg = intPositionMsg<<1;
+    
+    //calculate decimal parity on whole message
+ 
+    for(i = 0; i < 32; i++){
+        if( intPositionMsg & (b << i) ){count++;}
+    }
+
+    if( (count % 2) ){ intPositionParity = 0;}
+    else {intPositionParity = 1;}
+    
+    //add parity bit to message
+    intPositionMsg = intPositionMsg<<1;
+    intPositionMsg = intPositionMsg | intPositionParity;
+    
+    //
+    //rinse and repeat for velocity message
+    intVelocityMsg = demandSpeed[channel];
+    
+    //calculate the sum of the position data 
+
+    intTempVar = demandSpeed[channel];
+    
+    while (intTempVar >0)
+    {
+        intSumVelocity += intTempVar%10;
+        intTempVar = int(intTempVar/10);
+    }
+    
+    //add to message
+    intVelocityMsg = intVelocityMsg<<5;
+    intVelocityMsg = intVelocityMsg | intSumVelocity;
+    
+    //add type bit (0 == position, 1 == velocity)
+    intVelocityMsg = intVelocityMsg<<1;
+    intVelocityMsg = intVelocityMsg |1;
+    
+    //calculate decimal parity on whole message
+ 
+    for(i = 0; i < 32; i++){
+        if( intVelocityMsg & (b << i) ){count++;}
+    }
+
+    if( (count % 2) ){ intVelocityParity = 0;}
+    else {intVelocityParity = 1;}
+    
+    //add parity bit to message
+    intVelocityMsg = intVelocityMsg<<1;
+    intVelocityMsg = intVelocityMsg | intVelocityParity;
+    
+    //Combine
+    
+    
     // Get data from controller
     if( channel < 4 ) {
         spi_0.format(16,2); // !! Can probably move to constructor 
         spi_0.frequency(LOW_LEVEL_SPI_FREQUENCY);
         *cs_LL[channel] = 0; // Select relevant chip
-        intPosSPI_Rx[channel] = spi_0.write(demandPosition[channel]); // Transmit & receive
+        intPosSPI_Rx[channel] = spi_0.write(intPositionMsg); // Transmit & receive
+        
+        intPresSPI_Rx[channel] = intPosSPI_Rx[channel] | spi_0.write(intVelocityMsg); // Transmit & receive
         *cs_LL[channel] = 1; // Deselect chip
     } else {
         spi_1.format(16,2); // !! Can probably move to constructor 
         spi_1.frequency(LOW_LEVEL_SPI_FREQUENCY);
         *cs_LL[channel] = 0; // Select relevant chip
-        intPosSPI_Rx[channel] = spi_1.write(demandPosition[channel]); // Transmit & receive
+        intPosSPI_Rx[channel] = spi_0.write(intPositionMsg); // Transmit & receive
+        
+        intPresSPI_Rx[channel] = intPosSPI_Rx[channel] | spi_0.write(intVelocityMsg); // Transmit & receive
         *cs_LL[channel] = 1; // Deselect chip
     }
     isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
@@ -89,10 +173,10 @@
     }*/
     
     // Sort out received data
-    chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
-    
-    intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
-    //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
+    //STILL TO DO!!!!!!
+
+    intPosSPI_Rx[channel] = intPosSPI_Rx[channel]>>7 & 0x1FF;
+    dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/511*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
     mutChannel[channel].unlock();//unlock mutex for specific channel
 }