static void sendCurrentOutBuffer()
{
byte i;
if (flags1.xmitBufferSent) {
printf("Sending already sent packet!\n");
fflush(stdout);
return;
}
if (!flags1.xmitBufferReady) {
printf("Transmit buffer not ready!!\n");
fflush(stdout);
return;
}
flags1.xmitBufferSent = 1;
uartTransmit(RAP_SYNC);
crc = 0;
uartTransmit(computeCRC(out_hdb1));
uartTransmit(computeCRC(address));
uartTransmit(computeCRC(out_dest));
uartTransmit(computeCRC(out_hdb2));
uartTransmit(crc);
if (out_length > 0) {
for(i = 0; i < out_length; i++)
uartTransmit(computeCRC(out_buffer[i]));
uartTransmit(crc);
}
}
bool CDiscoveryApp::fillDiscoveryResponse( void )
{
IP_array ip_array;
memset(&m_stDiscoveryResponse, 0, sizeof(DiscoveryResponse_Msg));
m_stDiscoveryResponse.m_uchMessageVersion=1;
m_stDiscoveryResponse.m_uchMessageType=2;
m_stDiscoveryResponse.m_unAnId=getANId();
memset(&ip_array, 0, sizeof(ip_array));
if (GetNetworkConfiguration(ip_array) < 0)
{
return false;
}
MatchIP_dev(ip_array);
get_default_gateway( NULL, NULL, 0 );
m_stDiscoveryResponse.m_ushCRC=computeCRC((uint8_t *)&m_stDiscoveryResponse,
(uint16_t)(sizeof(DiscoveryResponse_Msg)-sizeof(m_stDiscoveryResponse.m_ushCRC)));
print_DiscoveryResponse( );
return true;
}
static void endMessageIntern()
{
byte i;
if (serialStatus & inSendQueueMsgBit) {
//test if inSendQueueMsgBit is set, otherwise it has been resetted by serialError
// Send the message
uartTransmit(SNAP_SYNC);
crc = 0;
uartTransmit(computeCRC(BIN(01010001))); // Request ACK
uartTransmit(computeCRC(BIN(00110000) | sendPacketLength));
uartTransmit(computeCRC(sendPacketDestination));
uartTransmit(computeCRC(deviceAddress));
for(i = 0; i < sendPacketLength; i++)
uartTransmit(computeCRC(sendPacket[i]));
uartTransmit(crc); /// @todo crc here
serialStatus &= ~inSendQueueMsgBit; //clear
}
}
//! Encode object to text form.
//! @return command in text form.
std::string
encode(void) const
{
std::ostringstream ss;
ss << c_ssc_prefix << ","
<< m_version << ","
<< DUNE::Utils::String::str("%02X", m_flags) << ","
<< m_ttl << ","
<< m_priority << ","
<< m_src << ","
<< m_dsts.size() << ",";
// Destinations.
if (m_dsts.begin() == m_dsts.end())
throw InvalidFormat("no destinations");
for (std::set<unsigned>::const_iterator itr = m_dsts.begin(); itr != m_dsts.end(); ++itr)
ss << *itr << ",";
// Command name.
ss << m_name << ",";
// Command arguments.
std::vector<std::string> args;
size_t rv = encodeArgs(args);
if (rv > 0)
ss << DUNE::Utils::String::join(args.begin(), args.end(), ",") << ",";
// CRC.
std::string cmd(ss.str());
char crc[5] = {0};
std::sprintf(crc, "%04X", computeCRC(cmd, cmd.size()));
cmd.append(crc);
// Termination character.
cmd.append(c_ssc_term);
return cmd;
}
void uartNotifyReceive()
{
byte c;
c = RCREG;
// If error occurred then reset by clearing CREN, but
// attempt to continue processing anyway.
/// @todo Should we do something else in this situation?
if (OERR) {
CREN = 0;
//don't set the error: serialStatus |= serialErrorBit
//because c and the next RCREG will be ok and maybe
//we got a correct message
}
CREN = 1;
if (serialStatus & serialErrorBit) {
uartReceiveError();
return;
}
switch(uartState) {
// ----------------------------------------------------------------------- //
case SNAP_idle:
// In the idle state, we wait for a sync byte. If none is
// received, we remain in this state.
if (c == SNAP_SYNC) {
uartState = SNAP_haveSync;
serialStatus &= ~msgAbortedBit; //clear
}
break;
// ----------------------------------------------------------------------- //
case SNAP_haveSync:
// In this state we are waiting for header definition bytes. First
// HDB2. We currently insist that all packets meet our expected
// format which is 1 byte destination address, 1 byte source
// address, and no protocol specific bytes. The ACK/NAK bits may
// be anything.
in_hdb2 = c;
if ((c & BIN(11111100)) != BIN(01010000)) {
// Unsupported header. Drop it an reset
serialStatus |= serialErrorBit; //set serialError
serialStatus |= wrongByteErrorBit;
uartReceiveError();
} else {
// All is well
if ((c & BIN(00000011)) == BIN(00000001))
serialStatus |= ackRequestedBit; //set ackRequested-Bit
else
serialStatus &= ~ackRequestedBit; //clear
crc = 0;
computeCRC(c);
uartState = SNAP_haveHDB2;
}
break;
// ----------------------------------------------------------------------- //
case SNAP_haveHDB2:
// For HDB1, we insist on high bits are 0011 and low bits are the length
// of the payload.
in_hdb1 = c;
if ((c & BIN(11110000)) != BIN(00110000)) {
serialStatus |= serialErrorBit; //set serialError
serialStatus |= wrongByteErrorBit;
uartReceiveError();
} else {
packetLength = c & 0x0f;
if (packetLength > MAX_PAYLOAD)
packetLength = MAX_PAYLOAD;
computeCRC(c);
uartState = SNAP_haveHDB1;
}
break;
// ----------------------------------------------------------------------- //
case SNAP_haveHDB1:
// We should be reading the destination address now
if (c != deviceAddress) {
uartTransmit(SNAP_SYNC);
uartTransmit(in_hdb2);
uartTransmit(in_hdb1);
uartTransmit(c);
uartState = SNAP_haveDABPass;
serialStatus &= ~ackRequestedBit; //clear
serialStatus |= inTransmitMsgBit;
} else {
computeCRC(c);
uartState = SNAP_haveDAB;
}
break;
// ----------------------------------------------------------------------- //
case SNAP_haveDAB:
// We should be reading the source address now
if (c == deviceAddress) {
// If we receive a packet from ourselves, that means it went
// around the ring and was never picked up, ie the device we
// sent to is off-line or unavailable.
/// @todo Deal with this situation
}
if (serialStatus & processingLockBit) {
//we have not finished the last order, reject
uartTransmit(SNAP_SYNC);
crc = 0;
uartTransmit(computeCRC(BIN(01010011))); //HDB2
// HDB1: 0 bytes, with 8 bit CRC
uartTransmit(computeCRC(BIN(00110000))); //HDB1
uartTransmit(computeCRC(sourceAddress)); // Return to sender
uartTransmit(computeCRC(deviceAddress)); // From us
//TODO: remove
/*for debugging add serialStatus
uartTransmit(computeCRC(BIN(00110001))); //HDB1
uartTransmit(computeCRC(sourceAddress)); // Return to sender
uartTransmit(computeCRC(deviceAddress)); // From us
uartTransmit(computeCRC(serialStatus)); // Return to sender
*/
uartTransmit(crc); // CRC
serialStatus &= ~ackRequestedBit; //clear
serialStatus |= msgAbortedBit; //set
uartState = SNAP_idle;
} else {
sourceAddress = c;
bufferIndex = 0;
computeCRC(c);
uartState = SNAP_readingData;
}
break;
// ----------------------------------------------------------------------- //
case SNAP_readingData:
buffer[bufferIndex] = c;
bufferIndex++;
computeCRC(c);
if (bufferIndex == packetLength)
uartState = SNAP_dataComplete;
break;
// ----------------------------------------------------------------------- //
case SNAP_dataComplete:
// We should be receiving a CRC after data, and it
// should match what we have already computed
{
byte hdb2 = BIN(01010000); // 1 byte addresses
if (c == crc) {
// All is good, so process the command. Rather than calling the
// appropriate function directly, we just set a flag to say
// something is ready for processing. Then in the main loop we
// detect this and process the command. This allows further
// comms processing (such as passing other tokens around the
// ring) while we're actioning the command.
hdb2 |= BIN(10);
serialStatus |= processingLockBit; //set processingLockBit
receivedSourceAddress = sourceAddress;
} else {
// CRC mismatch, so we will NAK the packet
hdb2 |= BIN(11);
}
if (serialStatus & ackRequestedBit) {
// Send ACK or NAK back to source
uartTransmit(SNAP_SYNC);
crc = 0;
uartTransmit(computeCRC(hdb2));
// HDB1: 0 bytes, with 8 bit CRC
uartTransmit(computeCRC(BIN(00110000)));
uartTransmit(computeCRC(sourceAddress)); // Return to sender
uartTransmit(computeCRC(deviceAddress)); // From us
uartTransmit(crc); // CRC
serialStatus &= ~ackRequestedBit; //clear
}
}
uartState = SNAP_idle;
break;
// ----------------------------------------------------------------------- //
case SNAP_haveHDB2Pass:
uartTransmit(c); // We will be reading HDB1; pass it on
packetLength = c & 0x0f;
if (packetLength > MAX_PAYLOAD)
packetLength = MAX_PAYLOAD;
uartState = SNAP_haveHDB1Pass;
break;
// ----------------------------------------------------------------------- //
case SNAP_haveHDB1Pass:
uartTransmit(c); // We will be reading dest addr; pass it on
uartState = SNAP_haveDABPass;
break;
// ----------------------------------------------------------------------- //
case SNAP_haveDABPass:
uartTransmit(c); // We will be reading source addr; pass it on
// Increment data length by 1 so that we just copy the CRC
// at the end as well.
packetLength++;
uartState = SNAP_readingDataPass;
break;
// ----------------------------------------------------------------------- //
case SNAP_readingDataPass:
uartTransmit(c); // This is a data byte; pass it on
if (packetLength > 1)
packetLength--;
else {
uartState = SNAP_idle;
serialStatus &= ~inTransmitMsgBit; //clear
}
break;
default:
serialStatus |= serialErrorBit; //set serialError
serialStatus |= wrongStateErrorBit;
uartReceiveError();
}
}
//retcode: true if DISCOVERY_REPLY must be sent back
//retcode false if no reply is necessary either because the incomming
//message is wrong or because it must be ignored
bool CDiscoveryApp::processData(unsigned char *p, const struct sockaddr_in& p_stClientAddr)
{
if (p==NULL)
{
LOG("ERROR processData: parameter NULL");
return false;
}
if (*(unsigned char *)p!=1)
{
LOG("ERROR processData: incorrect MessageVersion %d", *(unsigned char *)p);
return false;
}
unsigned char *uchMesssageType=p+sizeof(unsigned char);
switch (*uchMesssageType)
{
case DISCOVERY_QRY:
{
LOG("DISCOVERY_QRY");
return fillDiscoveryResponse(); // instruct caller to send the reply back
}
case DISCOVERY_SET:
{
IPSet_Msg * pIPSet = (IPSet_Msg*)p;
LOG("DISCOVERY_SET version %u ANID %u (x%X)", pIPSet->m_uchMessageVersion, ntohl(pIPSet->m_unAnId), ntohl(pIPSet->m_unAnId) );
unsigned short crc=computeCRC((uint8_t *)pIPSet, (uint16_t)(sizeof(IPSet_Msg)-sizeof(pIPSet->m_ushCRC)));
if (crc!=pIPSet->m_ushCRC)
{
LOG("ERROR processData: Wrong CRC in ipSet");
return false;
}
if( ( pIPSet->m_unIP_ethInterf==0x0 )
|| ( pIPSet->m_unDefaultGateway==0x0)
|| ((pIPSet->m_unMask_ethInterf & pIPSet->m_unDefaultGateway) != ( pIPSet->m_unMask_ethInterf & pIPSet->m_unIP_ethInterf)) )
{
LOG("ERROR processData: invalid DISCOVERY_SET (IP/GW are zero or IP&mask != GW&mask)");
log2flash( "ERR DISCOVERY: Invalid SET %s [IP %s/Mask %s/GW %s] from %s", m_szEthInterf, pIPSet->m_unIP_ethInterf, pIPSet->m_unMask_ethInterf, pIPSet->m_unDefaultGateway, inet_ntoa( p_stClientAddr.sin_addr ));
return false;
}
IP_array ip_array;
//read ip/mask/gw to match with packet
if (GetNetworkConfiguration(ip_array) < 0)
{
return false;
}
MatchIP_dev( ip_array );
//both ANID/MAC should either match or mismatch.
if( matchMAC( pIPSet ) && (!matchANID( pIPSet )) )
{
LOG("ERROR processData: MAC match while ANID mismatch (in %u own %u)", ntohl(pIPSet->m_unAnId), ntohl(getANId()) );
}
if( (!matchMAC( pIPSet )) && matchANID( pIPSet ) )
{
LOG_HEX( "ERROR processData: MAC mismatch while ANID match. In: ", pIPSet->m_uchMAC_Address, sizeof(pIPSet->m_uchMAC_Address) );
LOG_HEX( "ERROR processData: MAC mismatch while ANID match. Own:", m_ucEthInterfMAC, sizeof(m_ucEthInterfMAC) );
}
// Compare own MAC and ANID with the ones from the incoming packet
if( !( matchMAC( pIPSet ) && matchANID( pIPSet ) ) )
{
char szTmp[128];
snprintf(szTmp, sizeof(szTmp), "Ignore DISCOVERY_SET for AN %u MAC", ntohl(pIPSet->m_unAnId) );
szTmp[ sizeof(szTmp) - 1 ] = 0;
LOG_HEX( szTmp, pIPSet->m_uchMAC_Address, sizeof(pIPSet->m_uchMAC_Address) );
return false;
}
ConfigureNetwork(*pIPSet);
update_rcnetinfo(RC_NET_INFO, pIPSet->m_unIP_ethInterf, pIPSet->m_unMask_ethInterf, pIPSet->m_unDefaultGateway, p_stClientAddr);
//memset(p, 0, BUFLEN);
fillDiscoveryResponse();
return true; // instruct caller to send the reply back
}
case DISCOVERY_REPLY:
{
LOG("Ignore DISCOVERY_REPLY");
return false;
}
default:
{
LOG("ERROR processData() unknown MessageType %u", *uchMesssageType);
return false;
}
}
}
void uartNotifyReceive()
{
byte c = RCREG;
printTime();
if (address) printf(" ");
printf(" <-- %d rx %02x in state %d\n", address, c, uartState);
fflush(stdout);
switch(uartState) {
// ----------------------------------------------------------------------- //
case RAP_idle:
// In the idle state, we wait for a sync byte. If none is
// received, we remain in this state.
if (c == RAP_SYNC) {
uartState = RAP_expectHDB1;
in_packetTimer = PKT_RECEIVE_TIMEOUT;
}
break;
// ----------------------------------------------------------------------- //
case RAP_expectHDB1:
if (c == 0xff) {
// We have received a token, so if we have any data to send we
// can send it now.
in_tokenTimer = 0;
if (flags1.xmitBufferReady && !flags1.xmitBufferSent) {
sendCurrentOutBuffer();
uartState = RAP_idle;
in_packetTimer = -1;
flags1.out_bufferBusy = 0;
} else if (flags1.out_timedout) {
// If our last send timed out, send it again
out_resendTimer = PKT_SEND_TIMEOUT;
flags1.out_timedout = 0;
flags1.xmitBufferSent = 0;
sendCurrentOutBuffer();
uartState = RAP_idle;
in_packetTimer = -1;
flags1.in_bufferBusy = 0;
} else {
// Nothing to send, so pass the token on
sendTokenImmediate();
}
} else {
// Header of a normal packet
if (flags1.in_bufferBusy) {
// Inward buffer still busy, so have to drop packet.
printf("%d Buffer busy, drop %02x\n", address, c);
fflush(stdout);
uartState = RAP_expectSRCDrop;
flags1.in_dropAction = Drop_None;
} else {
flags1.in_bufferBusy = 1;
in_hdb1 = c;
crc = 0;
computeCRC(c);
uartState = RAP_expectSRC;
}
}
break;
// ----------------------------------------------------------------------- //
case RAP_expectSRC:
in_src = c;
computeCRC(c);
uartState = RAP_expectDST;
break;
// ----------------------------------------------------------------------- //
case RAP_expectDST:
in_dest = c;
computeCRC(c);
uartState = RAP_expectHDB2;
break;
// ----------------------------------------------------------------------- //
case RAP_expectHDB2:
in_hdb2 = c;
computeCRC(c);
uartState = RAP_expectHCRC;
break;
// ----------------------------------------------------------------------- //
case RAP_expectHCRC:
in_bufferIndex = 0;
if (c == crc) {
byte tseq;
byte ok = 1;
flags1.in_dropAction = Drop_None;
printf("Valid header CRC\n");
fflush(stdout);
// Restart timer
in_packetTimer = PKT_RECEIVE_TIMEOUT;
// We now have a valid header, so we can process it
if (in_src == address) {
printf("Packet has cycled, so drop it\n");
fflush(stdout);
uartState = RAP_readingDataDrop;
flags1.in_dropAction = Drop_None;
ok = 0;
} else if (in_dest != address) {
printf("Not for me (%d), forward it on\n", in_dest);
fflush(stdout);
uartTransmit(RAP_SYNC);
uartTransmit(in_hdb1);
uartTransmit(in_src);
uartTransmit(in_dest);
uartTransmit(in_hdb2);
uartTransmit(c);
flags1.in_bufferBusy = 0;
uartState = RAP_readingDataPass;
ok = 0;
}
if (in_hdb1 & RAP_HDB1_RST)
lastReceive = lastTransmit = -1;
if (ok) {
// Deal with incoming transmit sequence (incoming data)
byte lastrs, nextrs;
lastrs = (lastReceive & 3);
nextrs = (lastrs + 1) & 3;
printf("tseq is %d, lastrec is %d\n", tseq, lastReceive);
fflush(stdout);
tseq = RAP_TSEQ(in_hdb2);
if (tseq == lastrs && ((in_hdb2 & RAP_HDB2_LENMASK) == 0)) {
// They are sending a bare ACK
printf("Bare ACK received\n");
fflush(stdout);
} else if (tseq == lastrs) {
// They are re-sending data, so our previous packet must have been
// lost. Retransmit it.
out_resendTimer = -1; // Also kill the resend timer if we had one
printf("Lost previous packet, retransmit\n");
fflush(stdout);
uartState = RAP_readingDataDrop;
flags1.in_dropAction = Drop_ResendLast;
ok = 0;
} else if (tseq == nextrs) {
printf("Valid sequence\n");
fflush(stdout);
} else {
printf("Invalid tsequence %d (lastReceive=%d), drop and reset\n",
tseq, lastReceive);
fflush(stdout);
flags1.in_dropAction = Drop_Reset;
uartState = RAP_readingDataDrop;
ok = 0;
}
}
if (ok && (in_hdb1 & RAP_HDB1_NAK)) {
// TODO: check sequence of NAK
printf("Received NAK, resend last packet\n");
fflush(stdout);
flags1.xmitBufferSent = 0;
sendCurrentOutBuffer();
uartState = RAP_idle;
in_packetTimer = -1;
flags1.in_bufferBusy = 0;
out_resendTimer = PKT_SEND_TIMEOUT;
flags1.out_timedout = 0;
break;
}
// Deal with incoming receive sequence (responded to outgoing data)
if (ok && (in_hdb1 & RAP_HDB1_ACK)) {
byte rseq;
rseq = RAP_RSEQ(in_hdb2);
if (rseq == (lastTransmit & 3)) {
printf("Previous data acknowledged\n");
flags1.xmitBufferReady = 0;
fflush(stdout);
out_resendTimer = 255;
flags1.out_timedout = 0;
} else {
printf("Invalid rsequence, dropping packet\n");
fflush(stdout);
}
}
if (ok) {
if ((in_hdb2 & RAP_HDB2_LENMASK) == 0) {
// We don't bother with the second CRC if there is
// no data payload.
sendTokenImmediate();
} else
uartState = RAP_readingData;
}
} else {
printf("Header CRC failure (got %02x, expected %02x), drop packet\n",
c, crc);
fflush(stdout);
flags1.in_dropAction = Drop_None;
uartState = RAP_readingDataDrop;
}
break;
// ----------------------------------------------------------------------- //
case RAP_readingData:
in_buffer[in_bufferIndex] = c;
in_bufferIndex++;
// Restart timer
in_packetTimer = PKT_RECEIVE_TIMEOUT;
computeCRC(c);
if (in_bufferIndex == (in_hdb2 & RAP_HDB2_LENMASK))
uartState = RAP_expectDCRC;
break;
// ----------------------------------------------------------------------- //
case RAP_expectDCRC:
if (c == crc) {
if ((in_hdb2 & RAP_HDB2_LENMASK) > 0)
lastReceive = (lastReceive + 1) & 3;
packetNotifyReceive((byte *)in_buffer, (in_hdb2 & RAP_HDB2_LENMASK));
sendTokenImmediate();
} else {
// Failure, so NAK the packet. Possible optimisation: rather
// than putting a token out, just NAK immediately.
printf("Data CRC failure (got %02x, expected %02x), NAK packet\n",
c, crc);
fflush(stdout);
if (flags1.out_bufferBusy) {
// Already waiting to send a packet, so just drop it.
printf("Outward buffer already busy, drop\n");
fflush(stdout);
} else {
flags1.out_bufferBusy = 1;
out_hdb1 = RAP_HDB1_NAK;
out_hdb2 = 0;
out_length = 0;
out_dest = in_src;
flags1.xmitBufferReady = 1;
flags1.xmitBufferSent = 0;
flags1.in_bufferBusy = 0;
// Put out a new token
sendTokenImmediate();
}
}
break;
// ----------------------------------------------------------------------- //
case RAP_expectHDB2Pass:
uartTransmit(c);
in_bufferIndex = 0;
uartState = RAP_readingDataPass;
break;
// ----------------------------------------------------------------------- //
case RAP_readingDataPass:
uartTransmit(c);
in_bufferIndex++;
if (in_bufferIndex == (in_hdb2 & RAP_HDB2_LENMASK))
uartState = RAP_expectDCRCPass;
break;
// ----------------------------------------------------------------------- //
case RAP_expectDCRCPass:
uartTransmit(c);
uartState = RAP_idle;
in_packetTimer = -1;
break;
// ----------------------------------------------------------------------- //
case RAP_readingDataDrop:
in_bufferIndex++;
if (in_bufferIndex == (in_hdb2 & RAP_HDB2_LENMASK))
uartState = RAP_expectDCRCDrop;
break;
// ----------------------------------------------------------------------- //
case RAP_expectDCRCDrop:
flags1.in_bufferBusy = 0;
switch(flags1.in_dropAction) {
case Drop_None:
// Drop and do nothing. Except of course, we still
// need to put a token back into circulation
sendTokenImmediate();
break;
case Drop_ResendLast:
flags1.xmitBufferSent = 0;
sendCurrentOutBuffer();
uartState = RAP_idle;
in_packetTimer = -1;
flags1.in_bufferBusy = 0;
out_resendTimer = PKT_SEND_TIMEOUT;
flags1.out_timedout = 0;
break;
case Drop_NAK:
case Drop_Reset:
default:
printf("Unknown drop action %d\n", flags1.in_dropAction);
fflush(stdout);
}
break;
// ----------------------------------------------------------------------- //
default:
printf("%d Unimplemented state %d\n", address, uartState);
fflush(stdout);
break;
}
}
/**
* @brief Manually set a message and send it over USB
* Get the ACK sent back
*/
int main ()
{
/* Open connection */
/* Open connection in blocking mode */
fd = open("/dev/ttyACM0", O_RDWR | O_NOCTTY);
/* Check for error */
if (fd < 0) {
fprintf(stderr, "[TEST FAILED]: Unable to open connection\n");
return EXIT_FAILURE;
}
#ifdef _WIN32
//To make blocking:
unsigned long off = 0;
if (ioctlsocket(fd, FIONBIO, &off) != 0)
{
/* Handle failure. */
}
#else
fcntl(fd, F_SETFL, 0);
#endif
/* Define variables used here */
/* /\* Create a device shape *\/ */
/* DeviceShape ds(9, 9, 9); */
/* /\* Turn on one LED *\/ */
/* ds.on(1, 1, 1); */
/* Create array needed below */
uint8_t myDataMessage[7] = {0};
uint8_t emptyAck[10] = {0};
/* uint8_t *ledBuffer = new uint8_t[92]; */
/* Compute ledBuffer array */
/* ds.toArray(ledBuffer); */
uint16_t crc;
/* First buffer */
/* Manually set header */
myDataMessage[0] = 1;
myDataMessage[1] = 1;
myDataMessage[2] = 0x15;
myDataMessage[3] = 0;
myDataMessage[4] = 0;
/* Copy data into the buffer */
/* memcpy(&myDataMessage[5], ledBuffer, 57); */
/* Compute CRC */
crc = computeCRC(&myDataMessage[0], 5*sizeof(uint8_t));
/* And set CRC */
myDataMessage[5] = crc >> 8;
myDataMessage[6] = crc & 0xFF;
#if DEBUG
printBuffer("BUFFER", &myDataMessage[0], 7);
#endif
/* Send it over USB */
write(fd, &myDataMessage[0], 7);
read(fd, &emptyAck[0], SIZE_ACK);
#if DEBUG
printBuffer("ACK", &emptyAck[0], SIZE_ACK);
#endif
/* /\* Second buffer *\/ */
/* /\* Prepare next buffer to send *\/ */
/* myDataMessage[0] = 0; */
/* myDataMessage[4] = 35; */
/* /\* Copy the rest of the data in the buffer *\/ */
/* memcpy(&myDataMessage[5], ledBuffer + 57, 35); */
/* /\* Compute CRC *\/ */
/* crc = computeCRC(&myDataMessage[0], 62*sizeof(uint8_t)); */
/* /\* And set CRC *\/ */
/* myDataMessage[62] = crc >> 8; */
/* myDataMessage[63] = crc & 0xFF; */
/* #if DEBUG */
/* printBuffer("BUFFER", &myDataMessage[0], 64); */
/* #endif */
/* /\* Send it over USB *\/ */
/* write(fd, &myDataMessage[0], 64); */
/* read(fd, &emptyAck[0], SIZE_ACK); */
/* #if DEBUG */
/* printBuffer("ACK", &emptyAck[0], SIZE_ACK); */
/* #endif */
/* printf("[TEST PASSED]\n"); */
/* End connection */
/* Close file descriptor */
close(fd);
/* /\* Free allocated memory *\/ */
/* delete [] ledBuffer; */
return 0;
}
/**
* @brief Manually set a message and send it over USB
* Get the ACK sent back
*/
int main ()
{
/* Open connection in non blocking mode */
fd = open("/dev/ttyACM0", O_RDWR | O_NOCTTY);
/* Check for error */
if (fd < 0) {
fprintf(stderr, "[TEST FAILED]: Unable to open connection\n");
return EXIT_FAILURE;
}
/* Set blocking mode */
fcntl(fd, F_SETFL, 0);
/* clear the set */
FD_ZERO(&set);
/* add our file descriptor to the set */
FD_SET(fd, &set);
/* Define ACK thread */
std::thread ack_thread(waitForACK);
std::thread ack_handling_thread(handleAck);
/* Create a device shape */
DeviceShape ds(9, 9, 9);
/* Turn on one LED */
ds.on(4, 4, 4);
/* Create a data message */
uint8_t myDataMessage[64] = {0};
uint8_t *ledBuffer = new uint8_t[92];
ds.toArray(ledBuffer);
uint16_t crc;
/* First buffer */
/* Manually set header */
myDataMessage[0] = 1;
myDataMessage[1] = 1;
myDataMessage[2] = 0x42;
myDataMessage[3] = 0;
myDataMessage[4] = 92;
/* Copy data into the buffer */
memcpy(&myDataMessage[5], ledBuffer, 57);
/* Set CRC */
crc = computeCRC(&myDataMessage[0], 62*sizeof(uint8_t));
myDataMessage[62] = crc >> 8;
myDataMessage[63] = crc & 0xFF;
#if DEBUG
printBuffer("Message", myDataMessage, 64);
#endif
message_lock.lock();
message_queue.push(myDataMessage);
message_lock.unlock();
/* Send it over USB */
if (write(fd, &myDataMessage[0], 64) == -1)
printf("Error while sending buffer over USB\n");
/* Second buffer */
/* Prepare next buffer to send */
myDataMessage[0] = 0;
myDataMessage[4] = 35;
/* Copy the rest of the data in the buffer */
memcpy(&myDataMessage[5], ledBuffer + 57, 35);
/* Set the CRC */
crc = computeCRC(&myDataMessage[0], 62*sizeof(uint8_t));
myDataMessage[62] = crc >> 8;
myDataMessage[63] = crc & 0xFF;
#if DEBUG
printBuffer("Message", myDataMessage, 64);
#endif
/* Send the buffer */
write(fd, &myDataMessage[0], 64);
message_lock.lock();
message_queue.push(myDataMessage);
message_lock.unlock();
/* End connection */
printf("[TEST PASSED]\n");
/* Let the thread go */
ack_thread.detach();
/* Close file descriptor */
close(fd);
/* Free allocated memory */
delete [] ledBuffer;
return 0;
}