void stkEvaluateRxMessage(void) /* not static to prevent inlining */
{
_u8 i, cmd;
utilWord_t len = {2}; /* defaults to cmd + error code */
void *param;
//
LED_RX_ON();
/*
NOTICE: Please do NOT forget to set the following values
txBuffer[STK_TXMSG_START] : cmd
&txBuffer[STK_TXMSG_START+1] : payload data
len.word : sizeof(cmd) + sizeof(payload data)
len.byte[0] : len.word & 0xFF
*/
cmd = rxBuffer[STK_TXMSG_START];
param = &rxBuffer[STK_TXMSG_START + 1];
// The default return value
txBuffer[STK_TXMSG_START] = cmd;
txBuffer[STK_TXMSG_START + 1] = STK_STATUS_CMD_OK;
// reduce the code size by removing the switch-case jump table
if (cmd == RPINFRA_STK_CMD_ECHO){
// simple echo back
}
else if (cmd == RPINFRA_STK_CMD_GET_ADC){
*(_u16 *)&txBuffer[STK_TXMSG_START+1] = analogRead(A_ADC0);
len.bytes[0] = sizeof(unsigned long) + 1;
}else if (cmd == RPINFRA_STK_CMD_SET_PWM){
PWM_ENABLE( IO_PWM0 );
PWM_SET( IO_PWM0, rxBuffer[STK_TXMSG_START + 1] );
}else if (cmd == SCANNER_CMD_SET_HEADING) {
// set heading
onSetHeading(*(_u16 *)&rxBuffer[STK_TXMSG_START + 1]);
}else if (cmd == SCANNER_CMD_ENABLE_LASER) {
onEnableLaser();
}else if (cmd == SCANNER_CMD_DISABLE_LASER) {
onDisableLaser();
}
LED_RX_OFF();
//
stkSetTxMessage(len.word);
}
void ST_RadioReceiveIsrCallback(u8 *packet,
boolean ackFramePendingSet,
u32 time,
u16 errors,
s8 rssi)
{
LED_RX_ON();
receiving_packet = 0;
/* Copy packet into the buffer. It is better to do this here. */
if(add_to_rxbuf(packet)){
process_poll(&stm32w_radio_process);
last_rssi = rssi;
}
LED_RX_OFF();
}
/*---------------------------------------------------------------------------*/
void
ST_RadioReceiveIsrCallback(uint8_t *packet,
boolean ackFramePendingSet,
uint32_t time, uint16_t errors, int8_t rssi)
{
LED_RX_ON();
PRINTF("stm32w: incomming packet received\n");
receiving_packet = 0;
/* Copy packet into the buffer. It is better to do this here. */
if(add_to_rxbuf(packet)) {
process_poll(&stm32w_radio_process);
last_rssi = rssi;
}
LED_RX_OFF();
GET_LOCK();
is_transmit_ack = 1;
/* Wait for sending ACK */
BUSYWAIT_UNTIL(!is_transmit_ack, RTIMER_SECOND / 1500);
RELEASE_LOCK();
}
/******************************************************************************
* Function: void ProcessIO
*
* PreCondition: None
*
* Input: None
*
* Output: None
*
* Side Effects: None
*
* Overview: This function is a place holder for other user routines.
* It is a mixture of both USB and non-USB tasks.
*
* Note: None
*****************************************************************************/
void ProcessIO(void) {
unsigned char n, b, c, y;
int a;
// User Application USB tasks
if (!(isUsbPowered)) //Only generate traffic if NOT connected to USB
{
CheckLoadButton();
CANLoadTX();
return;
}
if ((usb_device_state < CONFIGURED_STATE) || (UCONbits.SUSPND == 1)) return;
//----- Read USB buffer if it has data from the host -----
if (HIDRxReport(inbuffer, 64)) // USB receive buffer has data
{
LED_RX_ON(); //Turn LED on
T0CONbits.TMR0ON = 1; //Start timer for TX LED on time
gTimeout = 0; //Reset timout
//---- CANmsgs: Check if host has requested CAN messages to be transmited
switch (inbuffer[u_CANmsgs]) // interpret command
{
case 0x00: // No messages are available
break;
case 0x01: // Message 1 is available
GetUSBData(m1_SIDH, 13, DataArray);
n = SPILoadTX(CAN_LOAD_TX, 13, DataArray);
if (n == CAN_LD_TXB0_ID) //if TX0 is loaded
{
RTS0_2515_LOW();
RTS0_2515_HIGH();
} else if (n == CAN_LD_TXB1_ID) //if TX1 is loaded
{
RTS1_2515_LOW();
RTS1_2515_HIGH();
} else if (n == CAN_LD_TXB2_ID) //if TX2 is loaded
{
RTS2_2515_LOW();
RTS2_2515_HIGH();
}
break;
case 0x02: // Message 1 and 2 are available
//Message 1
GetUSBData(m1_SIDH, m1_DLC + 5, DataArray);
n = SPILoadTX(CAN_LOAD_TX, m1_DLC + 5, DataArray);
if (n == CAN_LD_TXB0_ID) //if TX0 is loaded
{
RTS0_2515_LOW();
RTS0_2515_HIGH();
} else if (n == CAN_LD_TXB1_ID) //if TX1 is loaded
{
RTS1_2515_LOW();
RTS1_2515_HIGH();
} else if (n == CAN_LD_TXB2_ID) //if TX2 is loaded
{
RTS2_2515_LOW();
RTS2_2515_HIGH();
}
//Message 2
GetUSBData(m2_SIDH, m2_DLC + 5, DataArray);
n = SPILoadTX(CAN_LOAD_TX, m2_DLC + 5, DataArray);
if (n == CAN_LD_TXB0_ID) //if TX0 is loaded
{
RTS0_2515_LOW();
RTS0_2515_HIGH();
} else if (n == CAN_LD_TXB1_ID) //if TX1 is loaded
{
RTS1_2515_LOW();
RTS1_2515_HIGH();
} else if (n == CAN_LD_TXB2_ID) //if TX2 is loaded
{
RTS2_2515_LOW();
RTS2_2515_HIGH();
}
break;
case 0x03: // Message 1, 2, and 3 are available
//Message 1
GetUSBData(m1_SIDH, m1_DLC + 5, DataArray);
n = SPILoadTX(CAN_LOAD_TX, m1_DLC + 5, DataArray);
if (n == CAN_LD_TXB0_ID) //if TX0 is loaded
{
RTS0_2515_LOW();
RTS0_2515_HIGH();
} else if (n == CAN_LD_TXB1_ID) //if TX1 is loaded
{
RTS1_2515_LOW();
RTS1_2515_HIGH();
} else if (n == CAN_LD_TXB2_ID) //if TX2 is loaded
{
RTS2_2515_LOW();
RTS2_2515_HIGH();
}
//Message 2
GetUSBData(m2_SIDH, m2_DLC + 5, DataArray);
n = SPILoadTX(CAN_LOAD_TX, m2_DLC + 5, DataArray);
if (n == CAN_LD_TXB0_ID) //if TX0 is loaded
{
RTS0_2515_LOW();
RTS0_2515_HIGH();
} else if (n == CAN_LD_TXB1_ID) //if TX1 is loaded
{
RTS1_2515_LOW();
RTS1_2515_HIGH();
} else if (n == CAN_LD_TXB2_ID) //if TX2 is loaded
{
RTS2_2515_LOW();
RTS2_2515_HIGH();
}
//Message 3
GetUSBData(m3_SIDH, m3_DLC + 5, DataArray);
n = SPILoadTX(CAN_LOAD_TX, m3_DLC + 5, DataArray);
if (n == CAN_LD_TXB0_ID) //if TX0 is loaded
{
RTS0_2515_LOW();
RTS0_2515_HIGH();
} else if (n == CAN_LD_TXB1_ID) //if TX1 is loaded
{
RTS1_2515_LOW();
RTS1_2515_HIGH();
} else if (n == CAN_LD_TXB2_ID) //if TX2 is loaded
{
RTS2_2515_LOW();
RTS2_2515_HIGH();
}
break;
case 0x04: //--FUTURE-- Message 1, 2, 3, and 4 are available
break;
default: // unrecognized or null command
;
}// END switch: inbuffer[u_CANmsgs]
//---- CANCTRL: Write to the CANCTRL register if changed
if (inbuffer[u_CANCTRL] != old_CANCTRL) //If host sent new CANCTRL value
{
SPIByteWrite(CANCTRL, inbuffer[u_CANCTRL]); //Write to CANCTRL
EEPROMBYTEWrite(CANCTRL, inbuffer[u_CANCTRL]);
EEPROMCRCWrite(0, 128);
old_CANCTRL = inbuffer[u_CANCTRL]; //
outbuffer[u_CANSTAT] = SPIByteRead(CANSTAT);
while ((outbuffer[u_CANSTAT] & 0xE0) != (inbuffer[u_CANCTRL] & 0xE0))//if didn't change modes yet
{
outbuffer[u_CANSTAT] = SPIByteRead(CANSTAT);
}
UserFlag.USBsend = 1; //Set flag so will send USB message
}
//---- SPI: SPI command from host
if (inbuffer[u_SPI]) //If host sent SPI command (non-zero)
{
switch (inbuffer[u_SPI]) {
case CAN_RESET: //
SPIReset();
CANInit();
break;
case CAN_READ: //
if (!UserFlag.USBQueue) // If previous message is queued
{
outbuffer[u_SPI] = CAN_READ; //Send back to host
outbuffer[u_REG] = inbuffer[u_REG]; //Send back to host
outbuffer[u_DATA] = SPIByteRead(inbuffer[u_REG]); //Send back to host
}
UserFlag.USBsend = 1; //Set flag so will send USB message
UserFlag.USBQueue = 1; //Indicates msg is queued, but not sent
break;
case CAN_WRITE: //
//outbuffer[u_SPI] = 0; //Send back to host //JM
SPIByteWrite(inbuffer[u_REG], inbuffer[u_DATA]);
EEPROMBYTEWrite(inbuffer[u_REG], inbuffer[u_DATA]);
EEPROMCRCWrite(0, 128);
break;
case CAN_RTS: //
SPI_RTS(inbuffer[u_DATA]);
break;
case CAN_RD_STATUS: //
outbuffer[u_DATA] = SPIReadStatus();
UserFlag.USBsend = 1; //Set flag so will send USB message
break;
case FIRMWARE_VER_RD:
memmove(&outbuffer[u_STATUS], firmware_version, sizeof (firmware_version));
outbuffer[u_STATUS + sizeof (firmware_version)] = 0;
UserFlag.USBsend = 1; //Set flag so will send USB message
break;
default: // unrecognized or null command
;
}// END switch: inbuffer[u_SPI]
}
}//END if (HIDRxReport(inbuffer, 1)
//---- Check RXnBF pins and service messages as needed ---
switch (CheckCANRX()) // Check if CAN message received
{
case 0x01: // Message in RXB0 (Msgs in this buffer are Standard)
SPIReadRX(CAN_RD_START_RXB0SIDH, 13, ReadArray);
LoadUSBString(ReadArray);
break;
case 0x02: // Message in RXB1 (Msgs in this buffer are Extended)
SPIReadRX(CAN_RD_START_RXB1SIDH, 13, ReadArray);
LoadUSBString(ReadArray);
break;
case 0x03: // Message in RXB0 and RXB1
SPIReadRX(CAN_RD_START_RXB0SIDH, 13, ReadArray);
LoadUSBString(ReadArray);
SPIReadRX(CAN_RD_START_RXB1SIDH, 13, ReadArray);
LoadUSBString(ReadArray);
break;
default: // unrecognized or null command
;
}// END switch: CheckCANRX()
//---- The following turns off the TX and RX USB indicator LEDs after some time
//Inst. cycle = 200 ns; TMR0IF sets every 51 us
if (INTCONbits.TMR0IF) {
TimerCounter++;
if (!TimerCounter) //if rolled over, set flag. User code will handle the rest.
{
LED_TX_OFF(); //Turn LED off
LED_RX_OFF(); //Turn LED off
T0CONbits.TMR0ON = 0; //Start timer for TX LED on time
TimerCounter = 0xFE;
gTimeout = 1; //Reset timout
}
INTCONbits.TMR0IF = 0;
}
//------ Load USB Data to be transmitted to the host --------
if (UserFlag.MCP_RXBn | UserFlag.USBsend) {
if (!mHIDTxIsBusy()) {
HIDTxReport(outbuffer, 64);
outbuffer[0] = 0x00; //PKR$$$ Need this??
UserFlag.MCP_RXBn = 0; //clear flag
UserFlag.USBsend = 0; //clear flag
UserFlag.USBQueue = 0; //Clear message queue
LED_TX_ON(); //Turn LED on
T0CONbits.TMR0ON = 1; //Start timer for TX LED on time
gTimeout = 0; //Reset timout
outbuffer[u_SPI] = 0x00; //clear back to 00h so host doesn't detect "SPI response"
USB_ptr = 0xFF; //Point to location 0xFF
outbuffer[u_CANmsgs] = 0x00; //Clear message status
}
}
}//end ProcessIO
void llp_poll(LLPCtx *ctx) {
int c;
#if DISABLE_INTERLEAVE
while ((c = fgetc(ctx->ch)) != EOF) {
if (!ctx->escape && c == HDLC_FLAG) {
if (ctx->frame_len >= LLP_MIN_FRAME_LENGTH) {
if (PASSALL || ctx->crc_in == LLP_CRC_CORRECT) {
#if OPEN_SQUELCH == true
LED_RX_ON();
#endif
llp_decode(ctx);
}
}
ctx->sync = true;
ctx->crc_in = CRC_CCIT_INIT_VAL;
ctx->frame_len = 0;
continue;
}
if (!ctx->escape && c == HDLC_RESET) {
ctx->sync = false;
continue;
}
if (!ctx->escape && c == LLP_ESC) {
ctx->escape = true;
continue;
}
if (ctx->sync) {
if (ctx->frame_len < LLP_MAX_FRAME_LENGTH) {
ctx->buf[ctx->frame_len++] = c;
ctx->crc_in = update_crc_ccit(c, ctx->crc_in);
} else {
ctx->sync = false;
}
}
ctx->escape = false;
}
#else
while ((c = fgetc(ctx->ch)) != EOF) {
/////////////////////////////////////////////
// Start of forward error correction block //
/////////////////////////////////////////////
if ((ctx->sync && (c != LLP_ESC )) || (ctx->sync && (ctx->escape && (c == LLP_ESC || c == HDLC_FLAG || c == HDLC_RESET)))) {
// We have a byte, increment our read counter
ctx->readLength++;
// Check if we have read 12 bytes. If we
// have, we should now have a block of two
// data bytes and a parity byte. This block
if (ctx->readLength % LLP_INTERLEAVE_SIZE == 0) {
// If the last character in the block
// looks like a control character, we
// need to set the escape indicator to
// false, since the next byte will be
// read immediately after the FEC
// routine, and thus, the normal reading
// code will not reset the indicator.
if (c == LLP_ESC || c == HDLC_FLAG || c == HDLC_RESET) ctx->escape = false;
// The block is interleaved, so we will
// first put the received bytes in the
// deinterleaving buffer
for (int i = 1; i < LLP_INTERLEAVE_SIZE; i++) {
ctx->interleaveIn[i-1] = ctx->buf[ctx->frame_len-(LLP_INTERLEAVE_SIZE-i)];
}
ctx->interleaveIn[LLP_INTERLEAVE_SIZE-1] = c;
// We then deinterleave the block
llpDeinterleave(ctx);
// Adjust the packet length, since we will get
// parity bytes in the data buffer with block
// sizes larger than 3
ctx->frame_len -= LLP_INTERLEAVE_SIZE/3 - 1;
// For each 3-byte block in the deinterleaved
// bytes, we apply forward error correction
for (int i = 0; i < LLP_INTERLEAVE_SIZE; i+=3) {
// We now calculate a parity byte on the
// received data.
// Deinterleaved data bytes
uint8_t a = ctx->interleaveIn[i];
uint8_t b = ctx->interleaveIn[i+1];
// Deinterleaved parity byte
uint8_t p = ctx->interleaveIn[i+2];
ctx->calculatedParity = llpParityBlock(a, b);
// By XORing the calculated parity byte
// with the received parity byte, we get
// what is called the "syndrome". This
// number will tell us if we had any
// errors during transmission, and if so
// where they are. Using Hamming code, we
// can only detect single bit errors in a
// byte though, which is why we interleave
// the data, since most errors will usually
// occur in bursts of more than one bit.
// With 2 data byte interleaving we can
// correct 2 consecutive bit errors.
uint8_t syndrome = ctx->calculatedParity ^ p;
if (syndrome == 0x00) {
// If the syndrome equals 0, we either
// don't have any errors, or the error
// is unrecoverable, so we don't do
// anything
} else {
// If the syndrome is not equal to 0,
// there is a problem, and we will try
// to correct it. We first need to split
// the syndrome byte up into the two
// actual syndrome numbers, one for
// each data byte.
uint8_t syndromes[2];
syndromes[0] = syndrome & 0x0f;
syndromes[1] = (syndrome & 0xf0) >> 4;
// Then we look at each syndrome number
// to determine what bit in the data
// bytes to correct.
for (int i = 0; i < 2; i++) {
uint8_t s = syndromes[i];
uint8_t correction = 0x00;
if (s == 1 || s == 2 || s == 4 || s == 8) {
// This signifies an error in the
// parity block, so we actually
// don't need any correction
continue;
}
// The following determines what
// bit to correct according to
// the syndrome value.
if (s == 3) correction = 0x01;
if (s == 5) correction = 0x02;
if (s == 6) correction = 0x04;
if (s == 7) correction = 0x08;
if (s == 9) correction = 0x10;
if (s == 10) correction = 0x20;
if (s == 11) correction = 0x40;
if (s == 12) correction = 0x80;
// And finally we apply the correction
if (i == 1) a ^= correction;
if (i == 0) b ^= correction;
// This is just for testing purposes.
// Nice to know when corrections were
// actually made.
if (s != 0) ctx->correctionsMade += 1;
}
}
// We now update the checksum of the packet
// with the deinterleaved and possibly
// corrected bytes.
ctx->crc_in = update_crc_ccit(a, ctx->crc_in);
ctx->crc_in = update_crc_ccit(b, ctx->crc_in);
ctx->buf[ctx->frame_len-(LLP_DATA_BLOCK_SIZE)+((i/3)*2)] = a;
ctx->buf[ctx->frame_len-(LLP_DATA_BLOCK_SIZE-1)+((i/3)*2)] = b;
}
continue;
}
}
/////////////////////////////////////////////
// End of forward error correction block //
/////////////////////////////////////////////
if (!ctx->escape && c == HDLC_FLAG) {
if (ctx->frame_len >= LLP_MIN_FRAME_LENGTH) {
if (PASSALL || ctx->crc_in == LLP_CRC_CORRECT) {
#if OPEN_SQUELCH == true
LED_RX_ON();
#endif
llp_decode(ctx);
}
}
ctx->sync = true;
ctx->crc_in = CRC_CCIT_INIT_VAL;
ctx->frame_len = 0;
ctx->readLength = 0;
ctx->correctionsMade = 0;
continue;
}
if (!ctx->escape && c == HDLC_RESET) {
ctx->sync = false;
continue;
}
if (!ctx->escape && c == LLP_ESC) {
ctx->escape = true;
continue;
}
if (ctx->sync) {
if (ctx->frame_len < LLP_MAX_FRAME_LENGTH) {
ctx->buf[ctx->frame_len++] = c;
} else {
ctx->sync = false;
}
}
ctx->escape = false;
}
