robotics/common/comm.c

Go to the documentation of this file.

#include        "comm_config.h"

static uint8 au8_inPacket[IN_PACKET_SIZE];

static uint8 au8_outPacket[OUT_PACKET_SIZE];

union {
  uint8 u8_commError;
  struct {
unsigned int errH1:
    1;
unsigned int errDupH1:
    1;
unsigned int errH2:
    1;
unsigned int errT1:
    1;
unsigned int errT2:
    1;
unsigned int badState:
    1;
unsigned int unused:
    2;
  } commErrorBits;
} commError;

static uint8 u8_inIndex = IN_PACKET_HEADER_SIZE;
static uint8 u8_outIndex = OUT_PACKET_HEADER_SIZE;

#define COMM_GET_IN_DATA_GENERIC(datatype, func_name)                           \
datatype func_name(void) {                                                                                      \
        datatype temp;                                                                                                  \
        ASSERT((u8_inIndex + sizeof(datatype)) <=                       \
               (IN_PACKET_SIZE - IN_PACKET_HEADER_SIZE));                   \
        memcpy(&temp, (au8_inPacket + u8_inIndex), sizeof(datatype));   \
        u8_inIndex += sizeof(datatype);                                                                 \
        return temp;                                                                                                    \
}

#define COMM_PUT_OUT_DATA_GENERIC(datatype, func_name)                          \
void func_name(datatype temp) {                                                                         \
        ASSERT((u8_outIndex + sizeof(datatype)) <=                      \
               (OUT_PACKET_SIZE - OUT_PACKET_HEADER_SIZE));                 \
        memcpy((au8_outPacket + u8_outIndex), &temp, sizeof(datatype)); \
        u8_outIndex += sizeof(datatype);                                                                \
}

COMM_GET_IN_DATA_GENERIC(uint8, CommGetInDataUInt8)
COMM_GET_IN_DATA_GENERIC(int8, CommGetInDataInt8)
COMM_GET_IN_DATA_GENERIC(uint16, CommGetInDataUInt16)
COMM_GET_IN_DATA_GENERIC(int16, CommGetInDataInt16)
COMM_GET_IN_DATA_GENERIC(uint32, CommGetInDataUInt32)
COMM_GET_IN_DATA_GENERIC(int32, CommGetInDataInt32)

COMM_PUT_OUT_DATA_GENERIC(uint8, CommPutOutDataUInt8)
COMM_PUT_OUT_DATA_GENERIC(int8, CommPutOutDataInt8)
COMM_PUT_OUT_DATA_GENERIC(uint16, CommPutOutDataUInt16)
COMM_PUT_OUT_DATA_GENERIC(int16, CommPutOutDataInt16)
COMM_PUT_OUT_DATA_GENERIC(uint32, CommPutOutDataUInt32)
COMM_PUT_OUT_DATA_GENERIC(int32, CommPutOutDataInt32)

/* Documented in comm.h */
uint8 CommGetInDataUInt8Error(void) {
  return commError.u8_commError;
}

/*
* Any error < 8 means a packet was potentially dropped, but the
* current packet is safe to distribute.  Any packet with an
* error > 8 should not be distributed.
*/
uint8 CommGetInDataIsValid(void) {
  if (commError.u8_commError < 8)
    return 1;
  else
    return 0;
}

/* Documented in comm.h */
void CommGetInDataClearError(void) {
  commError.u8_commError = 0;
}

/* Documented in comm.h */
ESOS_USER_TASK(comm) {
  static uint8 u8_Char;
  static uint8 u8_state = 0;

  ESOS_TASK_BEGIN();
  while (TRUE) {
    // parse incoming communication //
    while (TRUE) {
      ESOS_TASK_WAIT_ON_AVAILABLE_IN_COMM();
      if (u8_state < 2)
        ESOS_TASK_WAIT_ON_GET_UINT8(u8_Char);
      else
        ESOS_TASK_WAIT_ON_GET_U8BUFFER(au8_inPacket + 2, sizeof(au8_inPacket) - 2);  //exclude the header, already gotten
      ESOS_TASK_SIGNAL_AVAILABLE_IN_COMM();
      if (u8_state == 0) {
        if (u8_Char == IN_PACKET_HEADER_1) { //wait for the specified character
          u8_state++;
          au8_inPacket[0] = u8_Char;
        } else { //set error if it's not received
          commError.commErrorBits.errH1 = 1;
        }
      } else if (u8_state == 1) {
        if (u8_Char == IN_PACKET_HEADER_2) {
          u8_state++;
          au8_inPacket[1] = u8_Char;
        } else if (u8_Char != IN_PACKET_HEADER_1) { //restart parsing
          commError.commErrorBits.errH2 = 1;
          u8_state = 0;
        } else { //a repeat character of IN_PACKET_HEADER_1
          commError.commErrorBits.errDupH1 = 1;
        }
      } else if (u8_state == 2) {
        // double check the trailer //
        if (au8_inPacket[IN_PACKET_SIZE - 2] != IN_PACKET_TRAILER_1) {
          commError.commErrorBits.errT1 = 1;
        }
        if (au8_inPacket[IN_PACKET_SIZE - 1] != IN_PACKET_TRAILER_2) {
          commError.commErrorBits.errT2 = 1;
        }
        u8_state = 0;
        break;
      } else {
        commError.commErrorBits.badState = 1;
        u8_state = 0;  //reset state
      }
    }
    u8_inIndex = 2;
    u8_outIndex = 2;
    divvy_packet();
    CommGetInDataClearError();

    // outgoing communication //
    ESOS_TASK_WAIT_ON_AVAILABLE_OUT_COMM();
    ESOS_TASK_WAIT_ON_SEND_U8BUFFER(au8_outPacket, sizeof(au8_outPacket));
    ESOS_TASK_SIGNAL_AVAILABLE_OUT_COMM();
  } //end while(TRUE)
  ESOS_TASK_END();
}

/* Documented in comm.h */
void initialize_comm(void) {
  au8_outPacket[0] = OUT_PACKET_HEADER_1;
  au8_outPacket[1] = OUT_PACKET_HEADER_2;
  au8_outPacket[OUT_PACKET_SIZE - 2] = OUT_PACKET_TRAILER_1;
  au8_outPacket[OUT_PACKET_SIZE - 1] = OUT_PACKET_TRAILER_2;
  CommGetInDataClearError();

  esos_RegisterTask(comm);
}

---