Dependencies: ros_lib_kinetic
Diff: LLComms.cpp
- Revision:
- 26:7c59002c9cd7
- Parent:
- 25:88e6cccde856
- Child:
- 27:6853ee8ffefd
--- a/LLComms.cpp Mon Dec 17 15:09:44 2018 +0000
+++ b/LLComms.cpp Mon Jan 28 21:24:48 2019 +0000
@@ -39,6 +39,9 @@
// All chip selects in off state
*cs_LL[i] = 1;
*cs_ADC[i] = 1;
+ // Initialise pressures/positions
+ pressureSensor_bar[i] = -1.0;
+ positionSensor_mm[i] = -1.0;
}
pinReset = 1; // Initialise reset pin to not reset the controllers.
wait(0.25);
@@ -67,8 +70,7 @@
}
//LLComms::~LLComms(void) { } // Destructor
-
-void LLComms::formatMessage(short int type, double dblValue, double dblMaxValue) {
+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
@@ -76,7 +78,7 @@
unsigned int intMsg = intValue;
// Calculate checksum (the decimal sum of the position data)
int intCheckSum = 0, intTempVar = intValue;
- while( intTempVar>0 ) { //591
+ while( intTempVar>0 ) {
intCheckSum += intTempVar%10;
intTempVar = floor(intTempVar/10.0);
}
@@ -85,6 +87,7 @@
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++) {
@@ -94,46 +97,161 @@
bool boolParity = !(bool)(count%2);
intMsg = intMsg<<1;
intMsg = intMsg | (int)boolParity;
+ //printf("%i \t %i \t %i \t %i \t %i \r\n",type,intValue,intCheckSum,boolParity,intMsg);
+ 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;
+ for(short int i=0; i<3; i++) { // Loop 3 times for 3 SPI messages
+ if(i==0) {
+ inboundMsg = spi->write(outboundMsgs[0]);
+ } else if(i==1) {
+ inboundMsg = spi->write(outboundMsgs[1]);
+ } else {
+ inboundMsg = spi->write(dummyMsg);
+ }
+ 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(1,demandPosition[channel],MAX_ACTUATOR_LENGTH);
- unsigned int intVelocityMsg = formatMessage(0,demandSpeed[channel],MAX_SPEED_MMPS);
+ 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 intPositionMsg = formatMessage(0,demandPosition_mm[channel],MAX_ACTUATOR_LIMIT_MM);
+ //unsigned int intPositionMsg = formatMessage(0,20.0,MAX_ACTUATOR_LIMIT_MM);
+ //unsigned int intVelocityMsg = formatMessage(1,4.0,MAX_SPEED_MMPS);
+
+ *cs_LL[channel] = 0; // Select relevant chip
+ unsigned int outboundMsgs[2] = { intPositionMsg , intVelocityMsg };
+ unsigned int inboundMsgsData[2];
+ bool isSPIsuccess = false;
+ if( channel < 4 ) {
+ isSPIsuccess = PerformMasterSPI(&spi_0,outboundMsgs,inboundMsgsData);
+ } else {
+ isSPIsuccess = PerformMasterSPI(&spi_1,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_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);
+ //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);
+ }
+
+ 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 ) {
- intPosSPI_Rx[channel] = spi_0.write(intPositionMsg); // Transmit & receive
- intPresSPI_Rx[channel] = intPresSPI_Rx[channel] | spi_0.write(intVelocityMsg); // Transmit & receive
+ 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 {
- intPosSPI_Rx[channel] = spi_1.write(intPositionMsg); // Transmit & receive
- intPresSPI_Rx[channel] = intPresSPI_Rx[channel] | spi_1.write(intVelocityMsg); // Transmit & receive
+ 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
- isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
+
+ bool isPositionValid = CheckMessage(intPosSPI_Rx[channel],0);
+ bool isPressureValid = CheckMessage(intPresSPI_Rx[channel],1);
- /*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;
- positionSensor_m[channel] = (double)intPosSPI_Rx[channel]/511*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
- //pressureSensor_bar[channel] = ...
+ 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;
- if (isDataReady[channel]) { // Check if data is ready for tranmission
+ //printf("%d %d common_rise_handler\n\r",channel,isDataReady[channel]);
+ if (isDataReady[channel]) { // Check if data is ready for transmission
ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
}
}