File content as of revision 23:61526647cc8a:

// LLComms.cpp

#include "LLComms.h"

LLComms::LLComms() : 
    queue(32 * EVENTS_EVENT_SIZE), //32 //8 * EVENTS_EVENT_SIZE
    pinGate6(PE_11),
    spi_0(PC_12, PC_11, PC_10),
    spi_1(PF_9, PF_8, PF_7),
    pinCheck(PE_5),
    // 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),
    pinGate4(PF_3),
    pinGate5(PF_13),
    //pinGate6(PE_11), // See above nonsense
    pinGate7(PE_13),
    pinReset(PD_2)
{ // Constructor

    PinName LLPins[8] = {PD_15, PE_10, PD_11, PD_14, PE_7, PD_12, PF_10, PD_13};
    //PinName LLPins[8] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};
    PinName ADCPins[8] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1};
    for (short int i = 0; i < 8; 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;
    }
    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
    
    // 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));
    pinGate4.rise(callback(this,&LLComms::rise4));
    pinGate5.rise(callback(this,&LLComms::rise5));
    pinGate6.rise(callback(this,&LLComms::rise6));
    pinGate7.rise(callback(this,&LLComms::rise7));
    // 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));
    pinGate4.fall(callback(this,&LLComms::fall4));
    pinGate5.fall(callback(this,&LLComms::fall5));
    pinGate6.fall(callback(this,&LLComms::fall6));
    pinGate7.fall(callback(this,&LLComms::fall7));
}

//LLComms::~LLComms(void) { } // Destructor

void LLComms::SendReceiveData(int channel) {
    //pinTesty = 1;
    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(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_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
    /*if(channel == 0) {
        intGlobalTest = intPosSPI_Rx[channel];
        dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
    }*/
    
    // Sort out received data
    //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
}

// Common rise handler function 
void LLComms::common_rise_handler(int channel) {
    pinCheck = 1;
    if (isDataReady[channel]) { // Check if data is ready for tranmission
        ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
    }
}

// Common fall handler functions
void LLComms::common_fall_handler(int channel) {
    pinCheck = 0;
    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); }
void LLComms::rise4(void) { common_rise_handler(4); }
void LLComms::rise5(void) { common_rise_handler(5); }
void LLComms::rise6(void) { common_rise_handler(6); }
void LLComms::rise7(void) { common_rise_handler(7); }
// 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); }
void LLComms::fall4(void) { common_fall_handler(4); }
void LLComms::fall5(void) { common_fall_handler(5); }
void LLComms::fall6(void) { common_fall_handler(6); }
void LLComms::fall7(void) { common_fall_handler(7); }

// NEEDS CALIBRATING
double LLComms::ReadADCPosition_mtrs(int channel) {
    unsigned int outputA;
    unsigned int outputB;
    int output;
    double dblOutput;

    if( channel < 4 ) {
        spi_0.format(8,0);
        spi_0.frequency(1000000);
        
        *cs_ADC[channel] = 0;
        spi_0.write(PREAMBLE);
        outputA = spi_0.write(CHAN_3);
        outputB = spi_0.write(0xFF);
        *cs_ADC[channel] = 1;
    } else {
        spi_1.format(8,0);
        spi_1.frequency(1000000);
        
        *cs_ADC[channel] = 0;
        spi_1.write(PREAMBLE);
        outputA = spi_1.write(CHAN_3);
        outputB = spi_1.write(0xFF);
        *cs_ADC[channel] = 1;
    }
    
    outputA = outputA & DATA_MASK;
    outputA = outputA<<8;
    output = (outputA | outputB);
    output = 4095- output;
    dblOutput = (double) (output);
    dblOutput = dblOutput*0.0229 - 21.582;
    return dblOutput;
}
    
double LLComms::ReadADCPressure_bar(int channel) {
    unsigned int outputA;
    unsigned int outputB;
    int output;
    double dblOutput;

    if( channel < 4 ) {
        spi_0.format(8,0);
        spi_0.frequency(1000000);
        
        *cs_ADC[channel] = 0;
        spi_0.write(PREAMBLE);
        outputA = spi_0.write(CHAN_1);
        outputB = spi_0.write(0xFF);
        *cs_ADC[channel] = 1;
    } else {
        spi_1.format(8,0);
        spi_1.frequency(1000000);
        
        *cs_ADC[channel] = 0;
        spi_1.write(PREAMBLE);
        outputA = spi_1.write(CHAN_1);
        outputB = spi_1.write(0xFF);
        *cs_ADC[channel] = 1;
    }
    
    outputA = outputA & DATA_MASK;
    outputA = outputA<<8;
    output = (outputA | outputB);
    
    dblOutput = (double)(output);
    dblOutput = dblOutput-502.0;
    dblOutput = dblOutput/4095.0*8.0;
    return dblOutput;
}