Diff: LLComms.cpp

Revision:

27:6853ee8ffefd

Parent:

26:7c59002c9cd7

Child:

29:10a5cf37a875

--- a/LLComms.cpp	Mon Jan 28 21:24:48 2019 +0000
+++ b/LLComms.cpp	Tue Jan 29 14:58:45 2019 +0000
@@ -94,6 +94,7 @@
         if( intMsg & (b << i) ) count++;
     }
     // Add parity bit to message (0 == Odd, 1 == Even)
+    // Parity selected in this way to prevent 0x0000 from passing checks
     bool boolParity = !(bool)(count%2);
     intMsg = intMsg<<1;
     intMsg = intMsg | (int)boolParity;
@@ -126,7 +127,10 @@
     unsigned int dummyMsg = 0x5555;
     bool isSuccess = true;
     unsigned int inboundMsg, typeBit;
+    AnalogIn ain(PA_4); // Random analogue pin
     for(short int i=0; i<3; i++) { // Loop 3 times for 3 SPI messages
+        ain.read_u16(); // Read analogue pin to cause a delay
+        ain.read_u16();
         if(i==0) {
             inboundMsg = spi->write(outboundMsgs[0]);
         } else if(i==1) {
@@ -157,7 +161,7 @@
     
     *cs_LL[channel] = 0; // Select relevant chip
     unsigned int outboundMsgs[2] = { intPositionMsg , intVelocityMsg };
-    unsigned int inboundMsgsData[2];
+    unsigned int inboundMsgsData[2] = { 0 };
     bool isSPIsuccess = false;
     if( channel < 4 ) {
         isSPIsuccess = PerformMasterSPI(&spi_0,outboundMsgs,inboundMsgsData);
@@ -167,10 +171,10 @@
     *cs_LL[channel] = 1; // Deselect chip
     if( isSPIsuccess ) {
         isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
-        positionSensor_mm[channel] = ((double)inboundMsgsData[0]/511)*(double)MAX_ACTUATOR_LIMIT_MM;
-        positionSensor_mm[channel] = min(max(positionSensor_mm[channel],0.0),(double)MAX_ACTUATOR_LIMIT_MM);
-        pressureSensor_bar[channel] = ((double)inboundMsgsData[1]/511)*(double)MAX_PRESSURE_LIMIT;
-        pressureSensor_bar[channel] = min(max(pressureSensor_bar[channel],0.0),(double)MAX_PRESSURE_LIMIT);
+        positionSensor_mm[channel] = ((double)inboundMsgsData[0]/511) * (double)MAX_ACTUATOR_LENGTH_MM;
+        positionSensor_mm[channel] = min( max(positionSensor_mm[channel],0.0) , (double)MAX_ACTUATOR_LENGTH_MM );
+        pressureSensor_bar[channel] = ((double)inboundMsgsData[1]/511) * (double)MAX_PRESSURE_LIMIT;
+        pressureSensor_bar[channel] = min( max(pressureSensor_bar[channel],0.0) , (double)MAX_PRESSURE_LIMIT );
         //printf("%.10f\t%.10f\n\r",positionSensor_mm[channel],pressureSensor_bar[channel]);
     } else { // Data is STILL ready and will be resent at the next pin interrupt
         //printf("SPI failed: %d%d. Resending.\n\r",isPositionValid,isPressureValid);
@@ -179,74 +183,6 @@
     mutChannel[channel].unlock();//unlock mutex for specific channel
 }
 
-/*bool LLComms::CheckMessage(int msg, short int trueType) {
-    // Find message parity
-    short int count = 0;
-    for(short int i=0; i<32; i++) {
-        if( msg>>1 & (1<<i) ) count++;
-    }
-    int intParity = !(count%2);
-    // Find message CheckSum    
-    int intChkSum = 0;
-    int intTempVar = msg>>7;
-    while(intTempVar > 0) {
-        intChkSum += intTempVar%10;
-        intTempVar = int(intTempVar/10);
-    }
-    // Check if parity, CheckSum and mesage type match
-    bool isParityCorrect = (intParity == (msg&0x1));
-    bool isChkSumCorrect = (intChkSum == ((msg>>2)&0x1F));
-    bool isTypeCorrect = ((msg>>1 & 0x1) == trueType);
-    bool isCheckPassed = (isParityCorrect && isChkSumCorrect && isTypeCorrect);
-    return isCheckPassed;
-}
-
-void LLComms::SendReceiveData(int channel) {
-    mutChannel[channel].lock(); // Lock mutex for specific Channel
-    
-    // Construct messages
-    //unsigned int intPositionMsg = formatMessage(0,demandPosition[channel],MAX_ACTUATOR_LENGTH_MM);
-    //unsigned int intVelocityMsg = formatMessage(1,demandSpeed[channel],MAX_SPEED_MMPS);
-    unsigned int intPositionMsg = formatMessage(0,15.2,30.4);
-    unsigned int intVelocityMsg = formatMessage(1,5.0,MAX_SPEED_MMPS);
-    
-    // Get data from controller
-    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 
-    *cs_LL[channel] = 0; // Select relevant chip
-    if( channel < 4 ) {
-        spi_0.write(intPositionMsg); // Transmit target position & receive bullshit
-        intPosSPI_Rx[channel] = spi_0.write(intVelocityMsg); // Transmit target speed & receive current position
-        intPresSPI_Rx[channel] = spi_0.write(0xFFFF); // Transmit bullshit & receive current pressure
-    } else {
-        spi_1.write(intPositionMsg); // Transmit target position & receive bullshit
-        intPosSPI_Rx[channel] = spi_1.write(intVelocityMsg); // Transmit target speed & receive current position
-        intPresSPI_Rx[channel] = spi_1.write(0xFFFF); // Transmit bullshit & receive current pressure
-    }
-    //printf("%d\n\r",intPresSPI_Rx[channel]);
-    *cs_LL[channel] = 1; // Deselect chip
-    
-    bool isPositionValid = CheckMessage(intPosSPI_Rx[channel],0);
-    bool isPressureValid = CheckMessage(intPresSPI_Rx[channel],1);
-    
-    if( isPositionValid && isPressureValid ) {
-        isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
-        // Sort out received data
-        intPosSPI_Rx[channel] = intPosSPI_Rx[channel]>>7 & 0x1FF;
-        positionSensor_m[channel] = ((double)intPosSPI_Rx[channel]/511)*((double)MAX_ACTUATOR_LENGTH_MM/1000);
-        //positionSensor_m[channel] = fmin( fmax(positionSensor_m[channel],0.0) , ((double)MAX_ACTUATOR_LENGTH_MM)/1000) );
-        intPresSPI_Rx[channel] = intPresSPI_Rx[channel]>>7 & 0x1FF;
-        pressureSensor_bar[channel] = ((double)intPresSPI_Rx[channel]/511)*(double)MAX_PRESSURE_LIMIT;
-        //pressureSensor_bar[channel] = min( max(pressureSensor_bar[channel],0.0) , (double)MAX_PRESSURE_LIMIT) );
-        //printf("%.10f\t%.10f\n\r",positionSensor_m[channel],pressureSensor_bar[channel]);
-    } else {
-        // Data is STILL ready and will be resent at the next pin interrupt
-        //printf("SPI failed: %d%d. Resending.\n\r",isPositionValid,isPressureValid);
-    }
-    
-    mutChannel[channel].unlock();//unlock mutex for specific channel
-}*/
-
 // Common rise handler function 
 void LLComms::common_rise_handler(int channel) {
     pinCheck = 1;