File content as of revision 42:5a5ad23a4bb1:

// LLComms.cpp

#include "LLComms.h"

LLComms::LLComms() : 
    queue(32 * EVENTS_EVENT_SIZE), //32 //8 * EVENTS_EVENT_SIZE
    spi_0(PC_12, PC_11, PC_10),
//    spi_1(PF_9, PF_8, PF_7),
//    spi_2(PB_15, PC_2, PB_13),
    // These interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
    pinGate0(PF_11),
    pinGate1(PG_14),
    pinGate2(PF_15),
    //pinGate3(PF_12),
    pinReset(PD_2)
{ // Constructor

    spi_0.format(16,2);
    spi_0.frequency(LOW_LEVEL_SPI_FREQUENCY);
//    spi_1.format(16,2);
//    spi_1.frequency(LOW_LEVEL_SPI_FREQUENCY);


    PinName LLPins[N_CHANNELS] = {PD_15, PE_10, PD_14};//, PD_11};//, PE_7, PB_1, PB_12, PD_13, PB_5};
    
    for (short int i=0; i<N_CHANNELS; i++) {
        isDataReady[i] = 0;
        cs_LL[i] = new DigitalOut(LLPins[i]);
        //cs_ADC[i] = new DigitalOut(ADCPins[i]);
    }
    
    // Initialise relevant variables
    for(short int i=0; i<N_CHANNELS; i++) {
        // All chip selects in off state
        *cs_LL[i] = 1;
        //*cs_ADC[i] = 1;
        // Initialise pressures/positions
        pressureSensor_uint[i] = 0.0;
        pressureSensor_bar[i] = -1.0;
        positionSensor_uint[i] = 0.0;
        positionSensor_mm[i] = -1.0;
    }
    pinReset = 1; // Initialise reset pin to not reset the controllers.
    wait(0.25);
    pinReset=0; // Reset controllers to be safe
    wait(0.25);
    pinReset = 1; // Ready to go

//    pinReset.mode(PullUp); // Initialise reset pin to not reset the controllers.
//    wait(0.25);
//    pinReset.write(0); // Reset controllers to be safe
//    wait(0.25);
//    pinReset.mode(PullUp); // Ready to go
//    wait(0.25);
    
    // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
    pinGate0.rise(callback(this,&LLComms::rise0));
    pinGate1.rise(callback(this,&LLComms::rise1));
    pinGate2.rise(callback(this,&LLComms::rise2));
    //pinGate3.rise(callback(this,&LLComms::rise3));

    // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
    pinGate0.fall(callback(this,&LLComms::fall0));
    pinGate1.fall(callback(this,&LLComms::fall1));
    pinGate2.fall(callback(this,&LLComms::fall2));
    //pinGate3.fall(callback(this,&LLComms::fall3));

}

//LLComms::~LLComms(void) { } // Destructor
unsigned int LLComms::formatMessage(short int type, double dblValue, double dblMaxValue) {
    // Convert to a 9-bit number
    int intValue = (int) ((dblValue/dblMaxValue)*511);
    intValue = intValue & 0x1FF; // Ensure number is 9-bit
    // Initialize message with value
    unsigned int intMsg = intValue;
    // Calculate checksum (the decimal sum of the position data)
    int intCheckSum = 0, intTempVar = intValue;
    while( intTempVar>0 ) {
        intCheckSum += intTempVar%10;
        intTempVar = floor(intTempVar/10.0);
    }
    // Add checksum to message
    intMsg = intMsg<<5;
    intMsg = intMsg | intCheckSum;
    // Add type bit (0 == position, 1 == velocity)
    intMsg = intMsg<<1;
    intMsg = intMsg | (int)type; // CAST AS BOOL
    // Calculate decimal parity check for the whole message
    unsigned int count = 0, b = 1;  
    for(short int i=0; i<32; i++) {
        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;
    return intMsg;
}

bool LLComms::CheckMessage(int msg) {
    // 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 and CheckSum match
    bool isParityCorrect = (intParity == (msg&0x1));
    bool isChkSumCorrect = (intChkSum == ((msg>>2)&0x1F));
    bool isCheckPassed = (isParityCorrect && isChkSumCorrect);
    return isCheckPassed;
}

bool LLComms::PerformMasterSPI(SPI *spi, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) {
    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) {
            inboundMsg = spi->write(outboundMsgs[1]);
        } else {
            inboundMsg = spi->write(dummyMsg);
        }
        //rawMsg[i] = inboundMsg;
        if((unsigned int)inboundMsg != dummyMsg) { // Message is not dummy which is only used for reply
            typeBit = inboundMsg>>1 & 0x1;
            inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF;
            if( !CheckMessage(inboundMsg) ) {
                isSuccess = false;
            }
        }
    }
    return isSuccess;
}

void LLComms::SendReceiveData(int channel) {
    mutChannel[channel].lock(); // Lock mutex for specific Channel
    
    // Construct messages
    unsigned int intPositionMsg = formatMessage(0,demandPosition_mm[channel],MAX_ACTUATOR_LIMIT_MM);
    unsigned int intVelocityMsg = formatMessage(1,demandSpeed_mmps[channel],MAX_SPEED_MMPS);
    //unsigned int rawMsg[3];
    *cs_LL[channel] = 0; // Select relevant chip
    unsigned int outboundMsgs[2] = { intPositionMsg , intVelocityMsg };
    unsigned int inboundMsgsData[2] = { 0 };
    bool isSPIsuccess = false;
    if( channel < 3 ) {
        isSPIsuccess = PerformMasterSPI(&spi_0,outboundMsgs,inboundMsgsData);
    } //else {
//        isSPIsuccess = PerformMasterSPI(&spi_1,outboundMsgs,inboundMsgsData,rawMsg);
//    }
//    else if( channel < 7 ) {
//        isSPIsuccess = PerformMasterSPI(&spi_1,outboundMsgs,inboundMsgsData);
//    } else {
//        isSPIsuccess = PerformMasterSPI(&spi_2,outboundMsgs,inboundMsgsData);
//    }
    *cs_LL[channel] = 1; // Deselect chip
    if( isSPIsuccess ) {
        isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
        positionSensor_uint[channel] = inboundMsgsData[0];
        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_uint[channel] = inboundMsgsData[1];
        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 );
    } else { // Data is STILL ready and will be resent at the next pin interrupt
        printf("SPI failed: %d. Resending.\n\r",channel);
    }
    
    mutChannel[channel].unlock();//unlock mutex for specific channel
    
 //   if(channel ==3){
//        printf("%x\t%x\t%x\r\n",rawMsg[0], rawMsg[1], rawMsg[2] );
//    }

//    printf("%d, ", channel);
//    if(channel == 3){
//        printf("\r\n");
//    }
}

// Common rise handler function 
void LLComms::common_rise_handler(int channel) {
    //printf("%d %d common_rise_handler\n\r",channel,isDataReady[channel]);
    //countervar++;
    if (isDataReady[channel]) { // Check if data is ready for transmission
        ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
    }
}

// Common fall handler functions
void LLComms::common_fall_handler(int channel) {
    queue.cancel(ThreadID[channel]); // Cancel relevant queued event
}

// Stub rise functions
void LLComms::rise0(void) { common_rise_handler(0); }
void LLComms::rise1(void) { common_rise_handler(1); }
void LLComms::rise2(void) { common_rise_handler(2); }
//void LLComms::rise3(void) { common_rise_handler(3); }

// Stub fall functions
void LLComms::fall0(void) { common_fall_handler(0); }
void LLComms::fall1(void) { common_fall_handler(1); }
void LLComms::fall2(void) { common_fall_handler(2); }
//void LLComms::fall3(void) { common_fall_handler(3); }