long epicsShareAPI dbtpf(const char *pname, const char *pvalue)
{
/* declare buffer long just to ensure correct alignment */
long buffer[100];
long *pbuffer = buffer;
DBADDR addr;
long status = 0;
long options, no_elements;
char *pend;
long val_long;
int validLong;
unsigned long val_ulong;
int validULong;
int valid = 1;
int put_type;
epicsFloat32 fvalue;
epicsFloat64 dvalue;
static TAB_BUFFER msg_Buff;
TAB_BUFFER *pMsgBuff = &msg_Buff;
char *pmsg = pMsgBuff->message;
int tab_size = 10;
if (!pname || !*pname || !pvalue) {
printf("Usage: dbtpf \"pv name\", \"value\"\n");
return 1;
}
if (nameToAddr(pname, &addr))
return -1;
val_long = strtol(pvalue, &pend, 10);
validLong = (*pend == 0);
val_ulong = strtoul(pvalue, &pend, 10);
validULong = (*pend == 0);
for (put_type = DBR_STRING; put_type <= DBF_ENUM; put_type++) {
switch (put_type) {
case DBR_STRING:
status = dbPutField(&addr, put_type, pvalue, 1L);
break;
case DBR_CHAR:
if ((valid = validLong)) {
epicsInt8 val_i8 = (epicsInt8)val_long;
status = dbPutField(&addr, put_type, &val_i8, 1L);
}
break;
case DBR_UCHAR:
if ((valid = validULong)) {
epicsUInt8 val_u8 = (epicsUInt8)val_ulong;
status = dbPutField(&addr, put_type, &val_u8, 1L);
}
break;
case DBR_SHORT:
if ((valid = validLong)) {
epicsInt16 val_i16 = val_long;
status = dbPutField(&addr, put_type, &val_i16,1L);
}
break;
case DBR_USHORT:
if ((valid = validULong)) {
epicsUInt16 val_u16 = val_ulong;
status = dbPutField(&addr, put_type, &val_u16, 1L);
}
break;
case DBR_LONG:
if ((valid = validLong)) {
epicsInt32 val_i32 = val_long;
status = dbPutField(&addr, put_type,&val_i32,1L);
}
break;
case DBR_ULONG:
if ((valid = validULong)) {
epicsUInt32 val_u32 = val_ulong;
status = dbPutField(&addr, put_type, &val_u32, 1L);
}
break;
case DBR_FLOAT:
if ((valid = epicsScanFloat(pvalue, &fvalue) == 1))
status = dbPutField(&addr, put_type, &fvalue, 1L);
break;
case DBR_DOUBLE:
if ((valid = epicsScanDouble(pvalue, &dvalue) == 1))
status = dbPutField(&addr, put_type, &dvalue, 1L);
break;
case DBR_ENUM:
if ((valid = validULong)) {
epicsEnum16 val_e16 = val_ulong;
status = dbPutField(&addr, put_type, &val_e16, 1L);
}
break;
}
if (valid) {
if (status) {
printf("Put as DBR_%s Failed.\n", dbr[put_type]);
} else {
printf("Put as DBR_%-6s Ok, result as ", dbr[put_type]);
no_elements = MIN(addr.no_elements,
((sizeof(buffer))/addr.field_size));
options = 0;
status = dbGetField(&addr, addr.dbr_field_type, pbuffer,
&options, &no_elements, NULL);
printBuffer(status, addr.dbr_field_type, pbuffer, 0L, 0L,
no_elements, pMsgBuff, tab_size);
}
}
}
pmsg[0] = '\0';
dbpr_msgOut(pMsgBuff, tab_size);
return(0);
}
HI_S32 sample_cipher_efuse()
{
HI_S32 s32Ret = HI_SUCCESS;
HI_U32 u32TestDataLen = 16;
HI_U32 u32InputAddrPhy = 0;
HI_U32 u32OutPutAddrPhy = 0;
HI_U32 u32Testcached = 0;
HI_U8 *pInputAddrVir = HI_NULL;
HI_U8 *pOutputAddrVir = HI_NULL;
HI_HANDLE hTestchnid = 0;
HI_UNF_CIPHER_KEY_SRC_E enKeySrc;
HI_U8 u8Key[32];
s32Ret = HI_UNF_CIPHER_Init();
if(HI_SUCCESS != s32Ret)
{
return HI_FAILURE;
}
#ifdef CIPHER_KLAD_SUPPORT
enKeySrc = HI_UNF_CIPHER_KEY_SRC_KLAD_1;
s32Ret = HI_UNF_CIPHER_KladEncryptKey(HI_UNF_CIPHER_KEY_SRC_EFUSE_1, HI_UNF_CIPHER_KLAD_TARGET_AES, aes_128_cbc_key, u8Key, 16);
if(HI_SUCCESS != s32Ret)
{
HI_ERR_CIPHER("Error: Klad Encrypt Key failed!\n");
return HI_FAILURE;
}
if ( 0 != memcmp(u8Key, aes_128_enc_key, 16) )
{
HI_ERR_CIPHER("Memcmp failed!\n");
printBuffer("clean key", aes_128_cbc_key, 16);
printBuffer("encrypt key", u8Key, 16);
return HI_FAILURE;
}
#else
enKeySrc = HI_UNF_CIPHER_KEY_SRC_EFUSE_1;
#endif
s32Ret = HI_UNF_CIPHER_CreateHandle(&hTestchnid);
if(HI_SUCCESS != s32Ret)
{
HI_UNF_CIPHER_DeInit();
return HI_FAILURE;
}
u32InputAddrPhy = (HI_U32)HI_MMZ_New(u32TestDataLen, 0, NULL, "CIPHER_BufIn");
if (0 == u32InputAddrPhy)
{
HI_ERR_CIPHER("Error: Get phyaddr for input failed!\n");
goto __CIPHER_EXIT__;
}
pInputAddrVir = HI_MMZ_Map(u32InputAddrPhy, u32Testcached);
u32OutPutAddrPhy = (HI_U32)HI_MMZ_New(u32TestDataLen, 0, NULL, "CIPHER_BufOut");
if (0 == u32OutPutAddrPhy)
{
HI_ERR_CIPHER("Error: Get phyaddr for outPut failed!\n");
goto __CIPHER_EXIT__;
}
pOutputAddrVir = HI_MMZ_Map(u32OutPutAddrPhy, u32Testcached);
/* For encrypt */
s32Ret = Setconfiginfo(hTestchnid,
enKeySrc,
HI_UNF_CIPHER_ALG_AES,
HI_UNF_CIPHER_WORK_MODE_CBC,
HI_UNF_CIPHER_KEY_AES_128BIT,
u8Key,
aes_128_cbc_IV);
if(HI_SUCCESS != s32Ret)
{
HI_ERR_CIPHER("Set config info failed.\n");
goto __CIPHER_EXIT__;
}
memset(pInputAddrVir, 0x0, u32TestDataLen);
memcpy(pInputAddrVir, aes_128_src_buf, u32TestDataLen);
printBuffer("clear text:", aes_128_src_buf, sizeof(aes_128_src_buf));
memset(pOutputAddrVir, 0x0, u32TestDataLen);
s32Ret = HI_UNF_CIPHER_Encrypt(hTestchnid, u32InputAddrPhy, u32OutPutAddrPhy, u32TestDataLen);
if(HI_SUCCESS != s32Ret)
{
HI_ERR_CIPHER("Cipher encrypt failed.\n");
s32Ret = HI_FAILURE;
goto __CIPHER_EXIT__;
}
printBuffer("encrypted text:", pOutputAddrVir, sizeof(aes_128_dst_buf));
/* compare */
if ( 0 != memcmp(pOutputAddrVir, aes_128_dst_buf, u32TestDataLen) )
{
HI_ERR_CIPHER("Memcmp failed!\n");
s32Ret = HI_FAILURE;
goto __CIPHER_EXIT__;
}
/* For decrypt */
memcpy(pInputAddrVir, aes_128_dst_buf, u32TestDataLen);
memset(pOutputAddrVir, 0x0, u32TestDataLen);
s32Ret = Setconfiginfo(hTestchnid,
enKeySrc,
HI_UNF_CIPHER_ALG_AES,
HI_UNF_CIPHER_WORK_MODE_CBC,
HI_UNF_CIPHER_KEY_AES_128BIT,
u8Key,
aes_128_cbc_IV);
if(HI_SUCCESS != s32Ret)
{
HI_ERR_CIPHER("Set config info failed.\n");
goto __CIPHER_EXIT__;
}
printBuffer("before decrypt:", aes_128_dst_buf, sizeof(aes_128_dst_buf));
s32Ret = HI_UNF_CIPHER_Decrypt(hTestchnid, u32InputAddrPhy, u32OutPutAddrPhy, u32TestDataLen);
if(HI_SUCCESS != s32Ret)
{
HI_ERR_CIPHER("Cipher decrypt failed.\n");
s32Ret = HI_FAILURE;
goto __CIPHER_EXIT__;
}
printBuffer("decrypted text:", pOutputAddrVir, u32TestDataLen);
/* compare */
if ( 0 != memcmp(pOutputAddrVir, aes_128_src_buf, u32TestDataLen) )
{
HI_ERR_CIPHER("Memcmp failed!\n");
s32Ret = HI_FAILURE;
goto __CIPHER_EXIT__;
}
__CIPHER_EXIT__:
if (u32InputAddrPhy> 0)
{
HI_MMZ_Unmap(u32InputAddrPhy);
HI_MMZ_Delete(u32InputAddrPhy);
}
if (u32OutPutAddrPhy > 0)
{
HI_MMZ_Unmap(u32OutPutAddrPhy);
HI_MMZ_Delete(u32OutPutAddrPhy);
}
HI_UNF_CIPHER_DestroyHandle(hTestchnid);
HI_UNF_CIPHER_DeInit();
return s32Ret;
}
void addX (void) {
circularBuffer[nextBufferIndex] = 'X';
nextBufferIndex = (nextBufferIndex + 1) % BUFFER_SIZE;
printBuffer(circularBuffer);
}
// QT5 Family (ie. QT-504)
// the QT-504 is a bit different, it sends 3 packets for login.
int ConnectQT504(int sockFd, int channel)
{
char suppLoginBuf[88] = {0};
struct QSee504Login loginBuf;
int retval;
char recvBuf[532];
static bool beenHere = false;
memset(&loginBuf, 0, sizeof(loginBuf));
loginBuf.vala[0] = 0x31;
loginBuf.vala[1] = 0x31;
loginBuf.vala[2] = 0x31;
loginBuf.vala[3] = 0x31;
loginBuf.vala[4] = 0x88;
loginBuf.vala[8] = 0x01;
loginBuf.vala[9] = 0x01;
loginBuf.vala[12] = 0xff;
loginBuf.vala[13] = 0xff;
loginBuf.vala[14] = 0xff;
loginBuf.vala[15] = 0xff;
loginBuf.vala[16] = 0x04;
loginBuf.vala[20] = 0x78;
loginBuf.vala[24] = 0x03;
strcpy(loginBuf.user, globalArgs.username);
strcpy(loginBuf.pass, globalArgs.password);
gethostname(loginBuf.host, sizeof(loginBuf.host));
loginBuf.filler[22] = 0x50;
loginBuf.filler[23] = 0x56;
loginBuf.filler[24] = 0xc0;
loginBuf.filler[25] = 0x08;
loginBuf.filler[28] = 0x04;
if( globalArgs.verbose && beenHere == false )
{
printBuffer((char*)&loginBuf, sizeof(loginBuf));
}
// Send the login packet (1 of 4)
retval = send(sockFd, (char*)(&loginBuf), sizeof(loginBuf), 0);
if( globalArgs.verbose )
{
printMessage(true, "Ch %i: Send 1 result: %i\n", channel+1, retval);
}
// do reading
// Get header length (4 bytes)
retval = recv(sockFd, &recvBuf, 532, 0);
// Verify send was successful
if( retval != 532 )
{
printMessage(true, "Ch %i: Receive 1 failed: %i\n", channel+1, retval);
// return 1;
}
/* Inbetween the 2 and last packets there is another packet.
* It seems to be optional.
*/
memset(suppLoginBuf, 0, 88);
suppLoginBuf[0] = 0x31;
suppLoginBuf[1] = 0x31;
suppLoginBuf[2] = 0x31;
suppLoginBuf[3] = 0x31;
suppLoginBuf[4] = 0x50;
suppLoginBuf[8] = 0x03;
suppLoginBuf[9] = 0x04;
suppLoginBuf[12] = 0xf0;
suppLoginBuf[13] = 0xb7;
suppLoginBuf[14] = 0x3d;
suppLoginBuf[15] = 0x08;
suppLoginBuf[16] = 0x03;
suppLoginBuf[20] = 0x40;
suppLoginBuf[25] = 0xf8;
suppLoginBuf[32] = 0x01;
suppLoginBuf[33] = 0xf8;
suppLoginBuf[40] = 0x02;
suppLoginBuf[41] = 0xf8;
suppLoginBuf[48] = 0x03;
suppLoginBuf[49] = 0xf8;
suppLoginBuf[56] = 0x40;
suppLoginBuf[57] = 0xf8;
suppLoginBuf[60] = 0x97;
suppLoginBuf[61] = 0xf0;
suppLoginBuf[64] = 0x41;
suppLoginBuf[65] = 0xf8;
// Send the next packet (2 of 4)
retval = send(sockFd, suppLoginBuf, 88, 0);
if( globalArgs.verbose )
{
printMessage(true, "Ch %i: Send 2 result: %i\n", channel+1, retval);
}
retval = 0;
{
int ret = 0;
while( (ret = recv(sockFd, recvBuf, sizeof(recvBuf), 0)) > 0 )
retval += ret;
}
suppLoginBuf[0] = 0x31;
suppLoginBuf[1] = 0x31;
suppLoginBuf[2] = 0x31;
suppLoginBuf[3] = 0x31;
suppLoginBuf[4] = 0x34;
suppLoginBuf[8] = 0x01;
suppLoginBuf[9] = 0x02;
suppLoginBuf[20] = 0x24;
*(short*)&suppLoginBuf[36] = htons(1 << channel);
*(short*)&suppLoginBuf[52] = htons(1 << channel);
if( globalArgs.verbose && beenHere == false )
{
printBuffer((char*)&suppLoginBuf, 60);
beenHere = true;
}
// Send the next packet (3 of 4)
retval = send(sockFd, suppLoginBuf, 60, 0);
if( globalArgs.verbose )
{
printMessage(true, "Ch %i: Send 3 result: %i\n", channel+1, retval);
}
// do reading
retval = recv(sockFd, recvBuf, 124, 0);
// Verify send was successful
if( retval <= 0 )
{
printMessage(true, "Ch %i: Receive 3 failed.\n", channel+1);
return 1;
}
else
printMessage(true, "Ch %i: Receive 3 result: %i bytes.\n", channel+1, retval);
// Reuse the old buffer, last three bytes should still be 0
//suppLoginBuf[5] = 0;
// Send the last packet (4 of 4)
//retval = send(sockFd, suppLoginBuf, 8, 0);
//if( globalArgs.verbose )
//{
// printMessage(true, "Ch %i: Send 4 result: %i\n", channel+1, retval);
//}
// If we got here, the stream will be waiting for us to recv.
return 0;
}
// Queue a CAN frame for sending; spins until it can queue
void OpenLcb_can_queue_xmt_wait(OpenLcbCanBuffer* b) {
printf("queue_xmt_wait: ");
printBuffer(b);
printf("\n");
}
int main()
{
int handle, status, nevents, nwords;
int buffer[2048], i, maxEvBlk = 2;
int *ip, *pBuf, *dict, dictLen, bufLen, bufLenBytes;
uint32_t bank1[3], bank2[3], bank3[3];
char eventBuffer[4 * ((4*8) + (5*3))];
bufLen = (4*8) + (5*3);
bufLenBytes = 4 * bufLen;
memset(eventBuffer, 0, bufLenBytes);
// create 3 little banks
// length of 2 ints
bank1[0] = bank2[0] = bank3[0] = 2;
// tag = 1,2,3, banks contains ints, num = 1,2,3
bank1[1] = 1 << 16 | 0x1 << 8 | 1;
bank2[1] = 2 << 16 | 0x1 << 8 | 2;
bank3[1] = 3 << 16 | 0x1 << 8 | 3;
// data = 1,2,3
bank1[2] = 1;
bank2[2] = 2;
bank3[2] = 3;
printf("\nEvent I/O tests to BUFFER ...\n");
status = evOpenBuffer(eventBuffer, bufLen, "w", &handle);
printf (" Opened buffer, status = %#x\n", status);
// status = evWriteDictionary(handle, dictionary);
// printf (" Write dictionary to buffer, status = %#x\n\n", status);
status = evIoctl(handle, "N", (void *) (&maxEvBlk));
printf (" Changed max events/block to %d, status = %#x\n", maxEvBlk, status);
status = evWrite(handle, bank1);
printf (" Wrote event #1 to buffer, status = %#x\n",status);
if (status != S_SUCCESS) {
printf("Error writing, %s, quit\n", evPerror(status));
exit(1);
}
status = evGetBufferLength(handle, &bufLenBytes);
printf (" Wrote %d bytes to buffer\n", bufLenBytes);
printBuffer((uint32_t *) eventBuffer, bufLenBytes/4);
status = evWrite(handle, bank2);
printf (" Wrote event #2 to buffer, status = %#x\n",status);
if (status != S_SUCCESS) {
printf("Error writing, %s, quit\n", evPerror(status));
exit(1);
}
status = evGetBufferLength(handle, &bufLenBytes);
printf (" Wrote %d bytes to buffer\n", bufLenBytes);
printBuffer((uint32_t *) eventBuffer, bufLenBytes/4);
status = evWrite(handle, bank3);
printf (" Wrote event #3 to buffer, status = %#x\n",status);
if (status != S_SUCCESS) {
printf("Error writing, %s, quit\n", evPerror(status));
exit(1);
}
status = evGetBufferLength(handle, &bufLenBytes);
printf (" Wrote %d bytes to buffer\n", bufLenBytes);
printBuffer((uint32_t *) eventBuffer, bufLenBytes/4);
status = evWrite(handle, bank1);
printf (" Wrote event #1 to buffer, status = %#x\n",status);
if (status != S_SUCCESS) {
printf("Error writing, %s, quit\n", evPerror(status));
exit(1);
}
status = evGetBufferLength(handle, &bufLenBytes);
printf (" Wrote %d bytes to buffer\n", bufLenBytes);
printBuffer((uint32_t *) eventBuffer, bufLenBytes/4);
status = evWrite(handle, bank2);
printf (" Wrote event #2 to buffer, status = %#x\n",status);
if (status != S_SUCCESS) {
printf("Error writing, %s, quit\n", evPerror(status));
exit(1);
}
status = evGetBufferLength(handle, &bufLenBytes);
printf (" Wrote %d bytes to buffer\n", bufLenBytes);
status = evClose(handle);
printf (" \"Closed\" buffer, status = %#x\n\n", status);
printBuffer((uint32_t *) eventBuffer, bufLen);
}
int ConnectSwannDVR8(int sockFd, int channel)
{
char channelBuf[32] = {0};
struct SwannDVR8 loginBuf;
int retval;
static bool beenHere = false;
char recvBuf[9686];
memset(&loginBuf, 0, sizeof(loginBuf));
// Setup login buffer
loginBuf.valc[0] = 0xf0;
loginBuf.valc[1] = 0xde;
loginBuf.valc[2] = 0xbc;
loginBuf.valc[3] = 0x0a;
loginBuf.valc[4] = 0x01;
loginBuf.valc[8] = 0x44;
loginBuf.valc[12] = 0xff;
loginBuf.valc[13] = 0xff;
loginBuf.valc[14] = 0xff;
loginBuf.valc[15] = 0xff;
strcpy(loginBuf.user, globalArgs.username);
strcpy(loginBuf.pass, globalArgs.password);
memset(channelBuf, 0, 32);
// select channel to stream
channelBuf[0] = 0xf0;
channelBuf[1] = 0xde;
channelBuf[2] = 0xbc;
channelBuf[3] = 0x0a;
channelBuf[4] = 0x03; //0x03 request video stream
//0x04 logoff
channelBuf[8] = 0x0c;
*(short*)&channelBuf[11] = htons(channel); //channel number
*(short*)&channelBuf[19] = htons(channel); //channel number
*(short*)&channelBuf[23] = htons(channel); //channel number
channelBuf[28] = 0x01; // Streaming Quality
//seems to be 0x01 for Video: h264 (High), yuv420p, 352x240, 8.83 fps, 4 tbr, 1200k tbn, 8 tbc
//and 0x00 for Video: h264 (High), yuv420p, 704x480, 30 fps, 30 tbr, 1200k tbn, 60 tbc
//total length 32 bytes
if( globalArgs.verbose && beenHere == false )
{
printBuffer((char*)&loginBuf, sizeof(loginBuf));
printBuffer((char*)&channelBuf, sizeof(channelBuf));
beenHere = true;
}
retval = send(sockFd, (char*)(&loginBuf), sizeof(loginBuf), 0);
printMessage(true, "Ch %i: Send Login result: %i\n", channel+1, retval);
retval = recv(sockFd, recvBuf, sizeof(recvBuf), 0);
//printBuffer((char*)&recvBuf, 32);
printMessage(true, "Received Login Result: %i\n", retval);
//printMessage(true, "X: %02x\n", (unsigned char)(recvBuf[8])); Value should be 0x50
// Send packet to open channel
retval = send(sockFd, channelBuf, sizeof(channelBuf), 0);
// Verify send was successful
if( retval != 32 )
{
printMessage(true, "Ch %i: Could not open channel, Streaming failed.\n", channel+1);
return 1;
}
else
printMessage(true, "Ch %i: Send Channel result: %i bytes.\n", channel+1, retval);
// If we got here, the stream will be waiting for us to recv.
return 0;
}
/**
* \brief Test function
*
* Sets a message and send it over USB
* Get the ACK sent back
*/
int main()
{
/* Opens the connection in non blocking mode */
#ifdef _WIN32
fd = open("COM7", O_RDWR | O_NOCTTY);
#else
fd = open("/dev/ttyACM0", O_RDWR | O_NOCTTY);
#endif
/* Check for error */
if (fd < 0) {
fprintf(stderr, "[TEST FAILED]: Unable to open connection\n");
return EXIT_FAILURE;
}
/* Sets blocking mode */
#ifdef _WIN32
//To make blocking:
unsigned long off = 0;
if (ioctlsocket(fd, FIONBIO, &off) != 0)
{
/* Handle failure. */
}
#else
fcntl(fd, F_SETFL, 0);
#endif
/* clear the set */
FD_ZERO(&set);
/* add our file descriptor to the set */
FD_SET(fd, &set);
/* Defines ACK thread */
std::thread ack_thread(waitForACK);
/* Creates a device shape */
DeviceShape ds(9, 9, 9);
ds.on(4, 4, 4);
/* Turns on one LED */
for (int i = 0; i < 9 ; i++) {
ds.on(i,i,8-i);
ds.on(i,i,i);
ds.on(i,8-i,i);
ds.on(8-i, i, i);
}
/* Creates a Request */
Request myRequest(1, 92, SET_LEDSTATS);
/* Encodes data into the buffers created by the Request */
uint8_t *ledBuffer = new uint8_t [ds.getSizeInBytes()];
ds.toArray(ledBuffer);
myRequest.encode(ledBuffer);
/* Prints the message */
#if DEBUG
std::cout << "My Request : " << myRequest.toStringDebug() << "\n";
#endif
// uint8_t* reqLinear = new uint8_t[SIZE_REQUEST];
// reqLinear={0};
// /* Resets the connection */
// RequestMessage resetConnection(1, RESET);
// resetConnection.encodeCrc();
// resetConnection.getListBuffer()[0].toArray(reqLinear);
// #if DEBUG
// std::cout << "Reset Connection : " << resetConnection.toStringDebug() << "\n";
// #endif
/* Sents it over USB */
// write(fd, reqLinear, SIZE_REQUEST);
// message_lock.lock();
// message_queue.push(buffLinear);
// message_lock.unlock();
uint8_t* buffLinear = new uint8_t[SIZE_BUFFER]();
/* Sends the Request */
for (int i = 0; i<myRequest.NbBuffers(); i++) {
/* Prints */
#if DEBUG
std::cout << "My Request buffer " << i << " : "
<< myRequest.getListBuffer()[i].toStringDebug(i) << "\n";
#endif
/* Sends it over USB */
myRequest.getListBuffer()[i].toArray(buffLinear);
#if DEBUG
printBuffer("BUFFER ", buffLinear, SIZE_BUFFER);
#endif
/* Send it over USB */
if (write(fd, buffLinear, SIZE_BUFFER) == -1)
std::cout << "error while sending buffer number " << i << " over USB \n";
message_lock.lock();
message_queue.push(buffLinear);
message_lock.unlock();
}
std::cout << "[TEST PASSED]\n";
ack_thread.detach();
close(fd);
delete [] buffLinear;
// delete [] reqLinear;
delete [] ledBuffer;
return 0;
}
byte Dynamixel::txRxPacket(byte bID, byte bInst, byte bTxParaLen){
//#define TRY_NUM 2//;;2
mDXLtxrxStatus = 0;
byte bTxLen, bRxLenEx, bTryCount;
mBusUsed = 1;
mRxLength = bRxLenEx = 0;
for(bTryCount = 0; bTryCount < gbDXLNumberTxRxAttempts; bTryCount++)//for(bTryCount = 0; bTryCount < TRY_NUM; bTryCount++)
{
//gbDXLReadPointer = gbDXLWritePointer;
mDxlDevice->read_pointer = mDxlDevice->write_pointer;//[ROBOTIS]BufferClear050728
bTxLen = this->txPacket(bID, bInst, bTxParaLen);
if (bTxLen == (bTxParaLen+4+2)){
mDXLtxrxStatus = (1<<COMM_TXSUCCESS);
}
// else{
// return 0;
// }
if(bInst == INST_PING){
if(bID == BROADCAST_ID){
mRxLength = bRxLenEx = 255; //
}
else{
mRxLength = bRxLenEx = 6; // basic response packet length
}
}
else if(bInst == INST_READ){
//mRxLength = bRxLenEx = 6+mParamBuffer[1]; // basic response packet length + requested data length in read instruction
if (gbDXLStatusReturnLevel>0)
mRxLength = bRxLenEx = 6+mParamBuffer[1];
else
mRxLength = bRxLenEx = 0;
}
else if( bID == BROADCAST_ID ){
mRxLength = bRxLenEx = 0; // no response packet in case broadcast id
break;
}
else{
//mRxLength = bRxLenEx = 6; //basic response packet length
if (gbDXLStatusReturnLevel>1)
mRxLength = bRxLenEx = 6+mParamBuffer[1];
else
mRxLength = bRxLenEx = 0;
}
if(bRxLenEx){//(gbpValidServo[bID] > 0x81 || bInst != INST_WRITE)) //ValidServo = 0x80|RETURN_LEVEL
mRxLength = this->rxPacket(bRxLenEx);
//TxDStringC("gbRxLength = ");TxDHex8C(mRxLength);TxDStringC("\r\n");
//TxDStringC("bRxLenEx = ");TxDHex8C(bRxLenEx);TxDStringC("\r\n");
// if(gbRxLength != bRxLenEx) //&& bRxLenEx != 255) before Ver 1.11e
if((mRxLength != bRxLenEx) && (bRxLenEx != 255)) // after Ver 1.11f
{
//TxDStringC(" Length mismatch!!\r\n");
unsigned long ulCounter;
word wTimeoutCount;
ulCounter = 0;
wTimeoutCount = 0;
//TxDStringC("\r\n TEST POINT 0");
while(ulCounter++ < RX_TIMEOUT_COUNT2)
{
//if(gbDXLReadPointer != gbDXLWritePointer)
if(this->available()) //data is available in dxl bus
{
mDxlDevice->read_pointer = mDxlDevice->write_pointer;// gbDXLWritePointer; //BufferClear
ulCounter = 0;
if(wTimeoutCount++ > 100 )
{
//uDelay(0);// porting ydh added
break; //porting ydh 100->245 //;;;;;; min max µÚ¹Ù²ñ found at Ver 1.11e
}
nDelay(NANO_TIME_DELAY);// porting ydh added 20120210.
}
//uDelay(0);// porting ydh added
nDelay(NANO_TIME_DELAY);// porting ydh added
}
//TxDStringC("\r\n TEST POINT 111");
mDxlDevice->read_pointer = mDxlDevice->write_pointer; //BufferClear
}
else{
//TxDStringC("\r\n TEST POINT 6");
break;
}
}//bRxLenEx is exist
}
//TxDStringC("\r\n TEST POINT 2");//TxDString("\r\n Err ID:0x");
mBusUsed = 0;
if((mRxLength != bRxLenEx) && (mTxBuffer[2] != BROADCAST_ID))
{
//TxDByteC('3');//
//TxDStringC("Rx Error\r\n");//TxDString("\r\n Err ID:0x");
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
//TxDString("\r\n Err ID:0x");
//TxDHex8(bID);
TxDStringC("\r\n ->[DXL]Err: ");
PrintBuffer(mTxBuffer,bTxLen);
TxDStringC("\r\n <-[DXL]Err: ");
PrintBuffer(mRxBuffer,mRxLength);
#endif
#ifdef PRINT_OUT_TRACE_ERROR_PRINT_TO_USART2
//TxDString("\r\n {[ERROR:");TxD16Hex(0x8100);TxDByte(':');TxD16Hex(bID);TxDByte(':');TxD8Hex(bInst);TxDByte(']');TxDByte('}');
//TxDByte(bID);TxDByte(' ');
//TxDByte(bInst);TxDByte(' ');
//TxDByte(gbpParameter[0]);TxDByte(' ');
//TxDByte(gbpParameter[1]);TxDByte(' ');
#endif
return 0;
}else if((mRxLength == 0) && (mTxBuffer[4] == INST_PING)){ //[ROBOTIS] 2013-11-22 correct response for ping instruction
return 0;
}
//TxDString("\r\n TEST POINT 4");//TxDString("\r\n Err ID:0x");
#ifdef PRINT_OUT_PACKET_TO_USART2
TxDStringC("\r\n ->[TX Buffer]: ");
printBuffer(mTxBuffer,bTxLen);
TxDStringC("\r\n <-[RX Buffer]: ");
printBuffer(mRxBuffer,mRxLength);
#endif
mDXLtxrxStatus = (1<<COMM_RXSUCCESS);
//gbLengthForPacketMaking =0;
return 1;
}
/**
* @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;
}
void checkSDC(int pipe_num) {
int r_index;
int bytes_avail = bytesReady(replicas, rep_count, pipe_num);
if (bytes_avail == 0) {
return;
}
switch (rep_type) {
case SMR:
// Only one rep, so pretty much have to trust it
sendPipe(pipe_num, 0);
return;
case DMR:
// Can detect, and check what to do
if (compareBuffs(&(replicas[0].vot_pipes[pipe_num]), &(replicas[1].vot_pipes[pipe_num]), bytes_avail) != 0) {
printf("Voting disagreement: caught SDC in DMR but can't do anything about it.\n");
}
sendPipe(pipe_num, 0);
return;
case TMR:
// Send the solution that at least two agree on
// TODO: What if buff_count is off?
for (r_index = 0; r_index < rep_count; r_index++) {
if (compareBuffs(&(replicas[r_index].vot_pipes[pipe_num]), &(replicas[(r_index + 1) % rep_count].vot_pipes[pipe_num]), bytes_avail) == 0) {
// If the third doesn't agree, it should be restarted.
if (compareBuffs(&(replicas[r_index].vot_pipes[pipe_num]), &(replicas[(r_index + 2) % rep_count].vot_pipes[pipe_num]), bytes_avail) != 0) {
int restartee = (r_index + 2) % rep_count;
debug_print("Caught SDC: %s : %d\n", controller_name, replicas[restartee].pid);
if (DEBUG_PRINT) {
// print all three or just two?
// Create typed pipes for meta data
struct typed_pipe print_pipesA[pipe_count];
struct typed_pipe print_pipesB[pipe_count];
convertVoteToTyped(replicas[r_index].vot_pipes, pipe_count, print_pipesA);
convertVoteToTyped(replicas[(r_index + 2) % rep_count].vot_pipes, pipe_count, print_pipesB);
// Copy the buffer over
char *buffer_A = (char *)malloc(sizeof(char) * MAX_VOTE_PIPE_BUFF);
char *buffer_B = (char *)malloc(sizeof(char) * MAX_VOTE_PIPE_BUFF);
copyBuffer(&(replicas[r_index].vot_pipes[pipe_num]), buffer_A, bytes_avail);
copyBuffer(&(replicas[(r_index + 2) % rep_count].vot_pipes[pipe_num]), buffer_B, bytes_avail);
// print them out.
printBuffer(&(print_pipesA[pipe_num]), buffer_A, bytes_avail);
printBuffer(&(print_pipesB[pipe_num]), buffer_B, bytes_avail);
free(buffer_A);
free(buffer_B);
}
restart_prep(restartee, r_index);
} else {
// If all agree, send and be happy. Otherwise the send is done as part of the restart process
sendPipe(pipe_num, r_index);
}
return;
}
}
printf("VoterD: TMR no two replicas agreed.\n");
}
}
void TraceBuffer::finishBuffer() {
printBuffer();
fetchVirtualAddresses();
z_comp->finish();
}
int TraceBuffer::ensureBufferSpace(long size) {
if (BUFFERSIZE - buffer_used <= size) {
return printBuffer();
}
return 0;
}
void removeX (void) {
nextBufferIndex = nextBufferIndex - 1;
nextBufferIndex = (nextBufferIndex < 0 )? BUFFER_SIZE - 1 : nextBufferIndex;
circularBuffer[nextBufferIndex] = 'O';
printBuffer(circularBuffer);
}
uint32_t parseBuffer(Client* client, uint32_t len){
char* buffer = client->outputnetworkBuf->networkBuf;
uint32_t offset = 0;
printBuffer(buffer,len);
while (offset < len){
SerialData* temp = (SerialData*)(buffer + offset);
if (len - offset >= sizeof(uint32_t)*2 && temp->_size <= len - offset){
//cerr<<"parse single "<<endl;
switch(temp->_type)
{
case SerialType::SerialTime:{
SerialTime* st = (SerialTime*)(buffer+offset);
char message[sizeof(SerialTime)];
memset(message,0,sizeof(SerialTime));
SerialTime* data = (SerialTime*)(message);
data->_type = SerialType::SerialTime;
data->_size = sizeof(SerialTime);
data->time = SDL_GetTicks();
data->local = st->local;
cerr<<"send Time "<<data->time<<endl;
send(client->getSocket(),message,sizeof(SerialTime),0);
break;
}
case SerialType::SerialReqJoin:{
SerialReqJoin* st = (SerialReqJoin*)(buffer+offset);
client->setPlayerId(st->_unitId);
client->initTransfere();
break;
}
case SerialType::SerialPCShipTargetPosUpdate:{
SerialPCShipTargetPosUpdate* st = (SerialPCShipTargetPosUpdate*)(buffer+offset);
Processor* processor = networkControl->getProcessor(st->_Id);
if(!processor){
cerr<<"ERROR SFUNCTION SerialType::SerialPCShipTargetPosUpdate processor not found"<<endl;
break;
}
processor->addCommand(new CommandTargetPosUpdate(0,st,client->getId()));
break;
}
case SerialType::SerialReqSetOrdreUnit:{
SerialReqSetOrdreUnit* st = (SerialReqSetOrdreUnit*)(buffer+offset);
SObjI ob = world->getObjs().find(st->_Id);
SObjI toob = world->getObjs().find(st->_ToId);
if(ob != world->getObjs().end() && toob != world->getObjs().end()){
if (ob->second->isShip() && ob->second->getTeam() == client->getTeamId()){
ob->second->isShip()->getOrdreObj()[st->_oindex] = toob->second;
}
}
break;
}
case SerialType::SerialReqSetOrdrePos:{
SerialReqSetOrdrePos* st = (SerialReqSetOrdrePos*)(buffer+offset);
SObjI ob = world->getObjs().find(st->_Id);
if(ob != world->getObjs().end()){
if (ob->second->isShip() && ob->second->getTeam() == client->getTeamId()){
if(ob->second->isShip()->getOrdrePos().find(st->_oindex) == ob->second->isShip()->getOrdrePos().end()){
SPos* tpos = new SPos(st->_Pos_x,st->_Pos_y,st->_Pos_d);
ob->second->isShip()->getOrdrePos()[st->_oindex] = tpos;
}else{
ob->second->isShip()->getOrdrePos()[st->_oindex]->x = st->_Pos_x;
ob->second->isShip()->getOrdrePos()[st->_oindex]->y = st->_Pos_y;
ob->second->isShip()->getOrdrePos()[st->_oindex]->d = st->_Pos_d;
}
}
}
break;
}
case SerialType::SerialReqTransfereCargo:{
SerialReqTransfereCargo* st = (SerialReqTransfereCargo*)(buffer+offset);
CommandCargoTransfere* t = new CommandCargoTransfere(st->_FromId, st->_ToId, st->_itemid, st->_quantity);
if(networkControl->addCommandToProcesable(t,st->_FromId))
delete t;
/*
SObjI fromit = world->getObjs().find(st->_FromId);
SObjI toit = world->getObjs().find(st->_ToId);
map<uint32_t,SItemType*>::iterator it3 = itemlist.find(st->_itemid);
if (it3 == itemlist.end())
break;
if(fromit == world->getObjs().end()|| toit == world->getObjs().end())
break;
if(fromit->second->getTeam() != client->getTeamId())
break;
if(toit->second->getTeam() != client->getTeamId())
break;
if (!fromit->second->getsubable() || !toit->second->getsubable() )
break;
if(!fromit->second->getsubable()->getCargoBay() || !toit->second->getsubable()->getCargoBay())
break;
fromit->second->getsubable()->getCargoBay()->TransfereCargo(toit->second->getsubable()->getCargoBay(),it3->second,st->_quantity);
*/
break;
}
case SerialType::SerialReqChangeSubTG:{
SerialReqChangeSubTG* st = (SerialReqChangeSubTG*)(buffer+offset);
Processor* processor = networkControl->getProcessor(st->_ShipId);
if(!processor){
cerr<<"ERROR SFUNCTION SerialType::SerialReqChangeSubTG processor not found"<<endl;
break;
}
processor->addCommand(new CommandIChangeSubTG(st->_ShipId, st->_SubId, (TargetGroup::Enum)st->_TargetGroup, client->getId()));
break;
}
case SerialType::SerialReqChangeOrdres:{
SerialReqChangeOrdres* st = (SerialReqChangeOrdres*)(buffer+offset);
Processor* processor = networkControl->getProcessor(st->_ShipId);
if(!processor){
cerr<<"ERROR SFUNCTION SerialType::SerialReqChangeOrdres processor not found"<<endl;
break;
}
processor->addCommand(new CommandIChangeOrders(st->_ShipId, st->_OrdreId, client->getId()));
break;
}
case SerialType::SerialReqChangePrio:{
SerialReqChangePrio* st = (SerialReqChangePrio*)(buffer+offset);
//Processor* processor = networkControl->getProcessor(st->_ShipId);
// if(!processor){
// cerr<<"ERROR SFUNCTION SerialType::SerialPCShipTargetPosUpdate processor not found"<<endl;
// break;
//}
//processor->addCommand(new CommandISubStatusField(st->_ShipId,st->_SubId,st->_StatusField, client->getId()));
break;
}
case SerialType::SerialReqChangeSubStatus:{
SerialReqChangeSubStatus* st = (SerialReqChangeSubStatus*)(buffer+offset);
Processor* processor = networkControl->getProcessor(st->_ShipId);
if(!processor){
cerr<<"ERROR SFUNCTION SerialType::SerialPCShipTargetPosUpdate processor not found"<<endl;
break;
}
processor->addCommand(new CommandISubStatusField(st->_ShipId,st->_SubId,st->_StatusField, client->getId()));
break;
}
case SerialType::SerialReqCreateLoadOut:{
SerialReqCreateLoadOut* st = (SerialReqCreateLoadOut*)(buffer+offset);
uint32_t nextFreeId = 0;
for(map<uint32_t, SLoadout*>::iterator it = globalLoadout[client->getPlayerId()].begin(); it !=globalLoadout[client->getPlayerId()].end();it++){
if(it->first >= nextFreeId)
nextFreeId = it->first + 1;
if(it->second->getName() == string(st->_name) && it->second->getShipType()->getId() == st->_shiptype){
nextFreeId = it->first;
if(it->second){
delete it->second;
}
globalLoadout[client->getPlayerId()].erase(it);
break;
}
}
if(unitTypes.find(st->_shiptype) == unitTypes.end())
break;
if(!(unitTypes[st->_shiptype]->isShipType()))
break;
SLoadout* tload = new SLoadout(nextFreeId,string(st->_name),*unitTypes[st->_shiptype]->isShipType());
for(uint32_t i = 0; i< st->_xloaditems;i++){
SerialReqCreateLoadItems* st2 = (SerialReqCreateLoadItems*)(buffer+offset+sizeof(SerialReqCreateLoadOut)+(sizeof(SerialReqCreateLoadItems)*i));
if(itemlist.find(st2->_itemid) != itemlist.end())
tload->addItem(st2->_slotid,itemlist[st2->_itemid],st2->_xitem,(TargetGroup::Enum)st2->_targetgroup);
}
globalLoadout[client->getPlayerId()][nextFreeId] = tload;
client->sendLoadout(nextFreeId);
break;
}
case SerialType::SerialReqFitLoadout:{
SerialReqFitLoadout* st = (SerialReqFitLoadout*)(buffer+offset);
SObjI fromit = world->getObjs().find(st->_FromId);
SObjI toit = world->getObjs().find(st->_ToId);
if(globalLoadout[client->getPlayerId()].find(st->_loadoutId) == globalLoadout[client->getPlayerId()].end())
break;
if(fromit == world->getObjs().end()|| toit == world->getObjs().end())
break;
if (!toit->second->getsubable())
break;
if(fromit->second->getTeam() != client->getTeamId())
break;
if(toit->second->getTeam() != client->getTeamId())
break;
if (!fromit->second->getsubable() || !toit->second->getsubable() )
break;
if(!fromit->second->getsubable()->getCargoBay())
break;
SLoadout* loadout = globalLoadout[client->getPlayerId()][st->_loadoutId];
SSubAble* toobj = toit->second->getsubable();
SCargoBay* frombay = fromit->second->getsubable()->getCargoBay();
loadout->fitTo(toobj,frombay);
break;
}
case SerialType::SerialReqFit:{
SerialReqFit* st = (SerialReqFit*)(buffer+offset);
SObjI fromit = world->getObjs().find(st->_FromId);
SObjI toit = world->getObjs().find(st->_ToId);
map<uint32_t, SSlotNode*>::iterator slotnode;
if(fromit == world->getObjs().end()|| toit == world->getObjs().end())
break;
if (!toit->second->getsubable())
break;
if(fromit->second->getTeam() != client->getTeamId())
break;
if(toit->second->getTeam() != client->getTeamId())
break;
slotnode = toit->second->getsubable()->getSlots().find(st->_subid);
if (slotnode == toit->second->getsubable()->getSlots().end())
break;
map<uint32_t,SItemType*>::iterator it3 = itemlist.find(st->_itemid);
if (it3 == itemlist.end())
break;
if (!fromit->second->getsubable() || !toit->second->getsubable() )
break;
if(!fromit->second->getsubable()->getCargoBay())
break;
SSubAble* toobj = toit->second->getsubable();
SCargoBay* frombay = fromit->second->getsubable()->getCargoBay();
toobj->FitAddSub(it3->second,slotnode->second->getId(),st->_quantity,frombay);
break;
}
case SerialType::SerialReqUnfit:{
SerialReqUnfit* st = (SerialReqUnfit*)(buffer+offset);
SObjI fromit = world->getObjs().find(st->_FromId);
SObjI toit = world->getObjs().find(st->_ToId);
map<uint32_t, SSlotNode*>::iterator slotnode;
if(fromit == world->getObjs().end()|| toit == world->getObjs().end())
break;
if(fromit->second->getTeam() != client->getTeamId())
break;
if(toit->second->getTeam() != client->getTeamId())
break;
if (!toit->second->getsubable())
break;
if (!fromit->second->getsubable() || !toit->second->getsubable() )
break;
slotnode = fromit->second->getsubable()->getSlots().find(st->_subid);
if (slotnode == fromit->second->getsubable()->getSlots().end())
break;
if(!slotnode->second->getSS())
break;
if(!toit->second->getsubable()->getCargoBay())
break;
SSubAble* toobj = toit->second->getsubable();
SSubAble* fromobj = fromit->second->getsubable();
SCargoBay* tobay = toobj->getCargoBay();
fromobj->FitRemoveSub(st->_subid,st->_quantity,tobay);
break;
}
case SerialType::SerialReqInitBuild:{
SerialReqInitBuild* st = (SerialReqInitBuild*)(buffer+offset);
SObjI it = world->getObjs().find(st->_ShipId);
if(it != world->getObjs().end()){
if(it->second->getTeam() != client->getTeamId())
break;
if ( it->second->getsubable()->getSlots().find(st->_SubId) !=it->second->getsubable()->getSlots().end()){
map<uint32_t, SSlotNode*>::iterator it2 =it->second->getsubable()->getSlots().find(st->_SubId);
if (it2->second->getSS()){
if (it2->second->getSS()->getItemType()->getSubType()->isFac()){
SSubSystemFac* fac = (SSubSystemFac*)it2->second->getSS();
map<uint32_t,SItemType*>::iterator it3 = itemlist.find(st->_BuildItem);
if (it3 != itemlist.end()){
if (it3->second){
fac->buildItem(it3->second,st->_Quantity);
}
}
}
}
}
}
break;
}
case SerialType::SerialSubscribeObj:{
SerialSubscribeObj* st = (SerialSubscribeObj*)(buffer+offset);
Processor* processor = networkControl->getProcessor(st->_Id);
if(!processor){
cerr<<"ERROR SFUNCTION SerialType::SerialPCShipTargetPosUpdate processor not found"<<endl;
break;
}
processor->addCommand(new CommandClientSubscription(0,client->getId(),st->_Id,SubscriptionLevel::details));
//TODO fix
/*
SerialSubscribeObj* st = (SerialSubscribeObj*)(buffer+offset);
SObjI it = world->getObjs().find(st->_Id);
if(it != world->getObjs().end()){
it->second->getSubscribers()[SubscriptionLevel::details].push_back(client);
if (it->second->isUnit()){
it->second->isUnit()->sendFull(client);
}
}
*/
break;
}
case SerialType::SerialUnSubscribeObj:{
SerialUnSubscribeObj* st = (SerialUnSubscribeObj*)(buffer+offset);
Processor* processor = networkControl->getProcessor(st->_Id);
if(!processor){
cerr<<"ERROR SFUNCTION SerialType::SerialPCShipTargetPosUpdate processor not found"<<endl;
break;
}
processor->addCommand(new CommandClientSubscription(0,client->getId(),st->_Id,SubscriptionLevel::lowFreq));
//TODO fix
/*
SerialUnSubscribeObj* st = (SerialUnSubscribeObj*)(buffer+offset);
SObjI it = world->getObjs().find(st->_Id);
if(it != world->getObjs().end()){
it->second->getSubscribers()[SubscriptionLevel::details].remove(client);
}
*/
break;
}
default:{
cerr<<"error no packet parse function defined"<<endl;
offset = len;
break;
}
}
offset += temp->_size;
}else{
cerr<<"servermissing data"<<endl;
break;
}
}
//cerr<<"end parse "<<endl;
return offset;
}
/**
* @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;
}
static size_t readPacket(int fd, int timeout, void *buffer, size_t length) {
fd_set nfd;
FD_ZERO(&nfd);
FD_SET(fd, &nfd);
struct timeval tv = {
.tv_sec = timeout,
.tv_usec = timeout ? 0 : 10000
};
size_t hlen = 0;
//LOG(LOGLEVEL_INFO, "length=%lu hlen=%lu\n", length, hlen);
while (length - hlen > 0 && select(fd + 1, &nfd, 0, 0, &tv) > 0) {
hlen += read(fd, buffer + hlen, length - hlen);
//LOG(LOGLEVEL_INFO, "hlen=%lu\n", hlen);
//LOGDO(LOGLEVEL_DEBUG, printBuffer(buffer, hlen));
}
LOG(LOGLEVEL_TRACE, "Read packet:\n");
LOGDO(LOGLEVEL_TRACE, printBuffer(buffer, hlen));
return hlen;
}
int openBaseband(const char *device) {
LOG(LOGLEVEL_INFO, "Opening %s...\n", device);
int fd = open(device, O_NOCTTY | O_RDWR);
ioctl(fd, TIOCNXCL);
fcntl(fd, F_SETFL, 0);
struct termios options;
tcgetattr(fd, &options);
int tcxonxoff = 0;
ioctl(fd, 0x8004540A, &tcxonxoff);//_IOW('T', 10, int) = TCXONC?
cfsetspeed(&options, DEFAULT_SPEED);
cfmakeraw(&options);
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 5;
options.c_iflag = (options.c_iflag | (IGNBRK | IGNPAR)) & ~(ICRNL | IXON | IXOFF | IXANY | INPCK | ISTRIP | BRKINT);
options.c_oflag = options.c_oflag & ~OPOST;
options.c_cflag = (options.c_cflag | (CREAD | CS8 | CLOCAL | HUPCL | CRTSCTS)) & ~PARENB;
options.c_lflag = options.c_lflag & ~(ECHO | ISIG | IEXTEN | ICANON);
tcsetattr(fd, TCSANOW, &options);
ioctl(fd, TIOCSDTR);
ioctl(fd, TIOCCDTR);
int lineBits = TIOCM_DTR | TIOCM_RTS | TIOCM_CTS | TIOCM_DSR;
ioctl(fd, TIOCMSET, &lineBits);
LOG(LOGLEVEL_DEBUG, "Baseband opened.\n");
char buffer[128] = { '`', '\r' };
LOG(LOGLEVEL_INFO, "Waiting for initialization...\n");
size_t length;
do {
if (write(fd, buffer, 2) == -1) {
LOG(LOGLEVEL_ERROR, "Can't write to baseband\n");
close(fd);
return -1;
}
length = readPacket(fd, 0, buffer, sizeof(buffer));
} while (length == 0 || buffer[0] != 0xb);
return fd;
}
byte Dynamixel::txRxPacket(byte bID, byte bInst, int bTxParaLen) {
mDXLtxrxStatus = 0;
word bTxLen, bRxLenEx, bTryCount;
mBusUsed = 1;
mRxLength = bRxLenEx = bTxLen = 0;
for(bTryCount = 0; bTryCount < gbDXLNumberTxRxAttempts; bTryCount++)//for(bTryCount = 0; bTryCount < TRY_NUM; bTryCount++)
{
//gbDXLReadPointer = gbDXLWritePointer;
mDxlDevice->read_pointer = mDxlDevice->write_pointer;//[ROBOTIS]BufferClear050728
/************************************** Transfer packet ***************************************************/
bTxLen = this->txPacket(bID, bInst, bTxParaLen);
if(mPacketType == DXL_PACKET_TYPE1) { //Dxl 1.0 Tx success ?
if (bTxLen == (bTxParaLen+4+2)) mDXLtxrxStatus = (1<<COMM_TXSUCCESS);
} else { //Dxl 2.0 Tx success?
if (bTxLen == (bTxParaLen+3+7)) mDXLtxrxStatus = (1<<COMM_TXSUCCESS);
}
//TxDStringC("bTxLen = ");TxDHex8C(bTxLen);TxDStringC("\r\n");
if(bInst == INST_PING) {
if(mPacketType == DXL_PACKET_TYPE1) { //Dxl 1.0
if(bID == BROADCAST_ID) mRxLength = bRxLenEx = 0xff;
else mRxLength = bRxLenEx = 6; // basic response packet length
} else { //Dxl 2.0
if(bID == BROADCAST_ID) mRxLength = bRxLenEx = 0xffff;
else mRxLength = bRxLenEx = 14;
}
}
else if(bInst == INST_READ) {
if (gbDXLStatusReturnLevel > 0) {
if(mPacketType == DXL_PACKET_TYPE1) mRxLength = bRxLenEx = 6+mParamBuffer[1];
else mRxLength = bRxLenEx = 11+DXL_MAKEWORD(mParamBuffer[2], mParamBuffer[3]);
}
else {
mRxLength = bRxLenEx = 0;
}
}
else if( bID == BROADCAST_ID ) {
if(bInst == INST_SYNC_READ || bInst == INST_BULK_READ) mRxLength = bRxLenEx = 0xffff; //only 2.0 case
else mRxLength = bRxLenEx = 0; // no response packet
}
else {
if (gbDXLStatusReturnLevel>1) {
if(mPacketType == DXL_PACKET_TYPE1) mRxLength = bRxLenEx = 6;//+mParamBuffer[1];
else mRxLength = bRxLenEx = 11;
}
else {
mRxLength = bRxLenEx = 0;
}
}
if(bRxLenEx) {
if(SmartDelayFlag == 1)
delay(150);
/************************************** Receive packet ***************************************************/
mRxLength = this->rxPacket(bRxLenEx);
}//bRxLenEx is exist
} //for() gbDXLNumberTxRxAttempts
//TxDStringC("\r\n TEST POINT 2");//TxDString("\r\n Err ID:0x");
mBusUsed = 0;
if((mRxLength != bRxLenEx) && (mTxBuffer[mPktIdIndex] != BROADCAST_ID))
{
//TxDByteC('3');//
//TxDStringC("Rx Error\r\n");//TxDString("\r\n Err ID:0x");
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
//TxDString("\r\n Err ID:0x");
//TxDHex8(bID);
TxDStringC("\r\n ->[DXL]Err: ");
printBuffer(mTxBuffer,bTxLen);
TxDStringC("\r\n <-[DXL]Err: ");
printBuffer(mRxBuffer,mRxLength);
#endif
#ifdef PRINT_OUT_TRACE_ERROR_PRINT_TO_USART2
//TxDString("\r\n {[ERROR:");TxD16Hex(0x8100);TxDByte(':');TxD16Hex(bID);TxDByte(':');TxD8Hex(bInst);TxDByte(']');TxDByte('}');
//TxDByte(bID);TxDByte(' ');
//TxDByte(bInst);TxDByte(' ');
//TxDByte(gbpParameter[0]);TxDByte(' ');
//TxDByte(gbpParameter[1]);TxDByte(' ');
#endif
return 0;
} else if((mRxLength == 0) && (mTxBuffer[mPktInstIndex] == INST_PING)) { //[ROBOTIS] 2013-11-22 correct response for ping instruction
//return 0;
}
//TxDString("\r\n TEST POINT 4");//TxDString("\r\n Err ID:0x");
#ifdef PRINT_OUT_PACKET_TO_USART2
TxDStringC("\r\n ->[TX Buffer]: ");
printBuffer(mTxBuffer,bTxLen);
TxDStringC("\r\n <-[RX Buffer]: ");
printBuffer(mRxBuffer,mRxLength);
#endif
mDXLtxrxStatus = (1<<COMM_RXSUCCESS);
//gbLengthForPacketMaking =0;
return 1;
}
uint32_t parseBuffer(Client* client, uint32_t len){
char* buffer = client->outputnetworkBuf->networkBuf;
uint32_t offset = 0;
if (SPrintBuff)
printBuffer(buffer,len);
while (offset < len){
SerialData* temp = (SerialData*)(buffer + offset);
if (len - offset >= sizeof(uint32_t)*2 && temp->_size <= len - offset){
switch(temp->_type)
{
case SerialType::SerialTime:{
SerialTime* st = (SerialTime*)(buffer+offset);
char message[sizeof(SerialTime)];
memset(message,0,sizeof(SerialTime));
SerialTime* data = (SerialTime*)(message);
data->_type = SerialType::SerialTime;
data->_size = sizeof(SerialTime);
data->_sendtime = SDL_GetTicks();
data->_local = st->_local;
cerr<<"send Time "<<data->_sendtime<<endl;
send(client->getSocket(),message,sizeof(SerialTime),0);
break;
}
case SerialType::SerialReqJoin:{
SerialReqJoin* st = (SerialReqJoin*)(buffer+offset);
client->setPlayerId(st->_unitId);
client->initTransfere();
break;
}
case SerialType::SerialReqMove:{
SerialReqMove* st = (SerialReqMove*)(buffer+offset);
map<uint32_t,SObj*>::iterator unit =world->getObjs().find(st->_unitId);
if (unit == world->getObjs().end())
break;
SPos pos;
pos.x = st->_pos.x;
pos.y = st->_pos.y;
pos.z = st->_pos.z;
pos.d = st->_pos.d;
//unit->second->reqMove(pos, st->_btime);
unit->second->addCommand(new SC_MoveObj(SDL_GetTicks(), unit->second, pos, st->_btime, st->_time));
break;
}
case SerialType::SerialReqActivatePowerT:{
SerialReqActivatePowerT* st = (SerialReqActivatePowerT*)(buffer+offset);
map<uint32_t,SObj*>::iterator unit =world->getObjs().find(st->_unitId);
map<uint32_t,SObj*>::iterator target =world->getObjs().find(st->_targetId);
if (unit == world->getObjs().end() || target == world->getObjs().end())
break;
if(!unit->second->getCreature())
break;
SPower* power = unit->second->getCreature()->getPower(st->_powerId);
if(!power)
break;
SC_CastSTarget* cmd = new SC_CastSTarget(SDL_GetTicks(),unit->second,target->second,power);
unit->second->addCommand(cmd);
break;
}
default:{
cerr<<"error no packet parse function defined"<<endl;
//offset = len;
break;
}
}
offset += temp->_size;
}else{
cerr<<"servermissing data"<<endl;
break;
}
}
//cerr<<"end parse "<<endl;
return offset;
}
// Queue a CAN frame for sending; spins until it can queue
void OpenLcb_can_queue_xmt_wait(OpenLcbCanBuffer* b) {
printBuffer(b);
}
int ConnectMEYE(int sockFd, int channel)
{
//00000000 00 00 00 48 00 00 00 00 28 00 04 00 05 00 00 00 ...H.... (.......
//00000010 29 00 38 00 61 64 6d 69 6e 00 00 00 00 00 00 00 ).8.admi n.......
//00000020 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 ........ ........
//00000030 00 00 00 00 6e 69 76 6c 32 30 30 35 00 00 00 00 ....nivl 2005....
//00000040 00 00 00 00 00 00 00 00 00 05 00 00 ........ ....
//76 bytes total
struct mEye loginBuf;
int retval;
static bool beenHere = false;
memset(&loginBuf, 0, sizeof(loginBuf));
char initBuf[43] = {0};
char configBuf[18] = {0};
char channelBuf[26] = {0};
char recvBuf[1024];
// Setup the init buffer
strcpy(initBuf, "GET /bubble/live?ch=0&stream=0 HTTP/1.1");
initBuf[39] = 0x0d;
initBuf[40] = 0x0a;
initBuf[41] = 0x0d;
initBuf[42] = 0x0a;
//total length 43 bytes
// Setup login buffer
loginBuf.valc[0] = 0xaa;
loginBuf.valc[4] = 0x35;
loginBuf.valc[13] = 0x2c;
strcpy(loginBuf.user, globalArgs.username);
strcpy(loginBuf.pass, globalArgs.password);
//*(short*)&loginBuf.channel[0] = htons(channel); //channel number
// Now setup configBuf
memset(configBuf, 0, 18);
// setup streaming
configBuf[0] = 0xaa;
configBuf[4] = 0x0d;
configBuf[13] = 0x04;
configBuf[14] = 0x01;
//total length 18 bytes
// Now setup channelBuf
memset(channelBuf, 0, 26);
// select channel to stream
channelBuf[0] = 0xaa;
channelBuf[4] = 0x15;
channelBuf[5] = 0x0a;
*(short*)&channelBuf[9] = htons(channel); //channel number
channelBuf[14] = 0x01; // Quality? 0=High, 1=Low
channelBuf[18] = 0x01;
//total length 26 bytes
if( globalArgs.verbose && beenHere == false )
{
printBuffer((char*)&initBuf, sizeof(initBuf));
printBuffer((char*)&loginBuf, sizeof(loginBuf));
printBuffer((char*)&configBuf, sizeof(configBuf));
printBuffer((char*)&channelBuf, sizeof(channelBuf));
beenHere = true;
}
// send init packet
retval = send(sockFd, (char*)(&initBuf), sizeof(initBuf), 0);
printMessage(true, "Ch %i: Send Init result: %i\n", channel+1, retval);
retval = recv(sockFd, recvBuf, sizeof(recvBuf), 0); // expect 1024 byte response after login request
//printBuffer((char*)&recvBuf, 32);
printMessage(true, "Received Init Result(expect 1024): %i\n", retval);
// send login packet
retval = send(sockFd, (char*)(&loginBuf), sizeof(loginBuf), 0);
printMessage(true, "Ch %i: Send Login result: %i\n", channel+1, retval);
retval = recv(sockFd, recvBuf, 54, 0); // expect 54 byte response after login request
//printBuffer((char*)&recvBuf, 32);
printMessage(true, "Received Login Result(expect 54): %i\n", retval);
// send configure packet
retval = send(sockFd, (char*)(&configBuf), sizeof(configBuf), 0);
printMessage(true, "Ch %i: Send config result: %i\n", channel+1, retval);
retval = recv(sockFd, recvBuf, 22, 0); // expect 22 byte response after config request
//printBuffer((char*)&recvBuf, 32);
printMessage(true, "Received Login Result(expect 22): %i\n", retval);
// Send packet to open channel
retval = send(sockFd, channelBuf, sizeof(channelBuf), 0);
// Verify send was successful, all 26 bytes
if( retval != 26 )
{
printMessage(true, "Ch %i: Could not open channel, Streaming failed.\n", channel+1);
return 1;
}
else
printMessage(true, "Ch %i: Send Channel result: %i bytes.\n", channel+1, retval);
// If we got here, the stream will be waiting for us to recv.
return 0;
}
// Send a CAN frame, waiting until it has been sent
void OpenLcb_can_send_xmt(OpenLcbCanBuffer* b) {
printBuffer(b);
}
// Queue a CAN frame for sending, if possible
// Returns true if queued, false if not currently possible
bool OpenLcb_can_queue_xmt_immediate(OpenLcbCanBuffer* b) {
printf("queue_xmt_immediate: ");
printBuffer(b);
printf("\n");
return true;
}
void playback() {
int err;
unsigned int i;
snd_pcm_t *handle;
// for (i = 0; i < sizeof(buffer); i++)
// buffer[i] = random() & 0xff;
if ((err = snd_pcm_open(&handle, "default", SND_PCM_STREAM_PLAYBACK, 0)) < 0) {
printf("Playback open error: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
initialize(handle);
printBuffer(buffer, 0, 10);
// for (i = 0; i < sizeof(buffer); i++)
// buffer[i] = random() & 0xff;
for (i = 0; i < NUM_FRAMES; i++) {
if ((err = snd_pcm_writei (handle, buffer+FRAME_SIZE*i*mult, FRAME_SIZE)) != FRAME_SIZE) {
fprintf (stderr, "write from audio interface failed (%s)\n",
err, snd_strerror (err));
exit (1);
}
fprintf(stdout, "write %d done\n", i);
printBuffer(buffer, FRAME_SIZE*i*mult, 10);
}
snd_pcm_close(handle);
// int err;
// unsigned int i;
// snd_pcm_t *handle;
// snd_pcm_sframes_t frames;
// for (i = 0; i < sizeof(buffer); i++)
// buffer[i] = random() & 0xff;
// if ((err = snd_pcm_open(&handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0) {
// printf("Playback open error: %s\n", snd_strerror(err));
// exit(EXIT_FAILURE);
// }
// if ((err = snd_pcm_set_params(handle,
// SND_PCM_FORMAT_U8,
// SND_PCM_ACCESS_RW_INTERLEAVED,
// 1,
// 48000,
// 1,
// 500000)) < 0) { /* 0.5sec */
// printf("Playback open error: %s\n", snd_strerror(err));
// exit(EXIT_FAILURE);
// }
// for (i = 0; i < 16; i++) {
// frames = snd_pcm_writei(handle, buffer, sizeof(buffer));
// if (frames < 0)
// frames = snd_pcm_recover(handle, frames, 0);
// if (frames < 0) {
// printf("snd_pcm_writei failed: %s\n", snd_strerror(frames));
// break;
// }
// if (frames > 0 && frames < (long)sizeof(buffer))
// printf("Short write (expected %li, wrote %li)\n", (long)sizeof(buffer), frames);
// }
// snd_pcm_close(handle);
}
// Send a CAN frame, waiting until it has been sent
void OpenLcb_can_send_xmt(OpenLcbCanBuffer* b) {
printf("can_send_xmt: ");
printBuffer(b);
printf("\n");
}
long epicsShareAPI dbtgf(const char *pname)
{
/* declare buffer long just to ensure correct alignment */
long buffer[400];
long *pbuffer = &buffer[0];
DBADDR addr;
long status;
long req_options, ret_options, no_elements;
short dbr_type;
static TAB_BUFFER msg_Buff;
TAB_BUFFER *pMsgBuff = &msg_Buff;
char *pmsg = pMsgBuff->message;
int tab_size = 10;
if (pname==0 || *pname==0) {
printf("Usage: dbtgf \"pv name\"\n");
return 1;
}
if (nameToAddr(pname, &addr))
return -1;
/* try all options first */
req_options = 0xffffffff;
ret_options = req_options;
no_elements = 0;
status = dbGetField(&addr, addr.dbr_field_type, pbuffer,
&ret_options, &no_elements, NULL);
printBuffer(status, addr.dbr_field_type, pbuffer,
req_options, ret_options, no_elements, pMsgBuff, tab_size);
/* Now try all request types */
ret_options=0;
dbr_type = DBR_STRING;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/MAX_STRING_SIZE));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_CHAR;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsInt8)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_UCHAR;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsUInt8)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_SHORT;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsInt16)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_USHORT;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsUInt16)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_LONG;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsInt32)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_ULONG;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsUInt32)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_FLOAT;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsFloat32)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_DOUBLE;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsFloat64)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
dbr_type = DBR_ENUM;
no_elements = MIN(addr.no_elements,((sizeof(buffer))/sizeof(epicsEnum16)));
status = dbGetField(&addr,dbr_type,pbuffer,&ret_options,&no_elements,NULL);
printBuffer(status,dbr_type,pbuffer,0L,0L,no_elements,pMsgBuff,tab_size);
pmsg[0] = '\0';
dbpr_msgOut(pMsgBuff, tab_size);
return(0);
}
void TestOutput::print(const char* str)
{
printBuffer(str);
}
void WaspBT_Pro::parseNames(){
int namesFound =0;
char dummy[4];
int dummies = 0;
for (i = 0; i < 40; i++) theCommand[i] = ' '; // Clear variable
#ifdef DEBUG_MODE
USB.println("Scanning names...");
#endif
if(!(SD.appendln(INQFILE,"Friendly names: "))){
#ifdef DEBUG_MODE
USB.print(ERRORSD1);
#endif
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
long timeout = 60000; // Timeout to wait for name responses.
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
long previous=millis();
while( (millis()-previous<timeout) && (namesFound<numberOfDevices)) {
dummies=0;
for (i = 0; i < COMMAND_SIZE; i++) theCommand[i] = ' '; // Clear variable
if (serialAvailable(1)) {
dummy[0]=serialRead(1);
if (dummy[0]=='A'){
while(serialAvailable(1)<4);
dummy[1]=serialRead(1);
if (dummy[1]=='M'){
dummy[2]=serialRead(1);
if (dummy[2]=='E'){
dummy[0]=serialRead(1); //read space and miss it.
//if here, name sentence found. Now distinguish between ERROR and mac
theCommand[0]=serialRead(1);
if (theCommand[0]=='E'){
//It is an error
// read 11 dummy bytes
while(serialAvailable(1)<11);
for(uint8_t z=0;z<11;z++) dummy[0]=serialRead(1);
// read and save corresponding mac
while(serialAvailable(1)<BLOCK_MAC_SIZE);
for(uint8_t x=0;x<BLOCK_MAC_SIZE;x++) theCommand[x]= serialRead(1);
theCommand[BLOCK_MAC_SIZE+2]='N';
theCommand[BLOCK_MAC_SIZE+3]='O';
theCommand[BLOCK_MAC_SIZE+4]='N';
theCommand[BLOCK_MAC_SIZE+5]='A';
theCommand[BLOCK_MAC_SIZE+6]='M';
theCommand[BLOCK_MAC_SIZE+7]='E';
//save on SD
if(!(SD.appendln(INQFILE,theCommand))){
#ifdef DEBUG_MODE
USB.print(ERRORSD1);
#endif
}
namesFound++;
#ifdef DEBUG_MODE
printBuffer();
#endif
}
else {
// It is a mac. Read mac (16bytes + one already read) + name
while(serialAvailable(1)<20); //!!
for(uint8_t x=1;x<COMMAND_SIZE;x++) {
if(serialAvailable(1)){
theCommand[x]= serialRead(1);
if (theCommand[x]=='"') dummies++;
if (dummies>=2) x = COMMAND_SIZE;
}
}
// NAME read. Now save into SD
if(!(SD.appendln(INQFILE,theCommand))){
#ifdef DEBUG_MODE
USB.print(ERRORSD1);
#endif
}
namesFound++;
#ifdef DEBUG_MODE
printBuffer();
#endif
}
}
}
}
}
// Condition to avoid an overflow (DO NOT REMOVE)
if( millis()-previous < 0 ) previous=millis();
}
// When lefting loop, If timeout is over
if (millis()-previous<timeout) {
//scanNetworkCancel(); //NO HACE NADA, hay que hacer un reset.
reset();
#ifdef DEBUG_MODE
USB.println("Timeout");
#endif
}
#ifdef DEBUG_MODE
printBuffer2();
USB.println("");
#endif
}
