unsigned long __RSPacketGetPacketSize(__RSPacket* packet)
{
if (packet && isValidPacket(packet))
{
return getPayloadSize(packet);
}
return 0;
}
unsigned long __RSPacketGetDataLength(__RSPacket* packet)
{
if (packet && isValidPacket(packet))
{
return getDataLength(packet);
}
return 0;
}
bool Adafruit_Controller::process()
{
if (comm.available())
{
char in = comm.read();
if (in == '!') { // start of packet
packet_idx = 2;
memset(packet_buffer.buffer, 0, 21);
}
packet_buffer.buffer[packet_idx++] = in;
if (isValidPacket())
{
switch (packet_buffer.pkt.action) {
case 'A':
if (accelHandler)
accelHandler(packet_buffer.pkt.type.accel.x, packet_buffer.pkt.type.accel.y, packet_buffer.pkt.type.accel.z);
break;
case 'G':
if (gyroHandler)
gyroHandler(packet_buffer.pkt.type.gyro.x, packet_buffer.pkt.type.gyro.y, packet_buffer.pkt.type.gyro.z);
break;
case 'M':
if (magHandler)
magHandler(packet_buffer.pkt.type.mag.x, packet_buffer.pkt.type.mag.y, packet_buffer.pkt.type.mag.z);
break;
case 'Q':
if (quatHandler)
quatHandler(packet_buffer.pkt.type.quat.x, packet_buffer.pkt.type.quat.y, packet_buffer.pkt.type.quat.z, packet_buffer.pkt.type.quat.w);
break;
case 'B':
if (buttonHandler) {
byte btn = packet_buffer.pkt.type.button.button - '0';
bool state = packet_buffer.pkt.type.button.state == '1' ? true : false;
buttonHandler(btn, state);
}
break;
case 'C':
if (colorHandler)
colorHandler(packet_buffer.pkt.type.color.red, packet_buffer.pkt.type.color.green, packet_buffer.pkt.type.color.blue);
break;
case 'L':
if (locationHandler)
locationHandler(packet_buffer.pkt.type.location.lat, packet_buffer.pkt.type.location.lon, packet_buffer.pkt.type.location.alt);
break;
default: // unknown type
return false;
break;
}
return true;
}
}
return false;
}
void UDPReceiver::tryToReceivePacket() {
// A blocking method that tries to receive a UDP packet. Returns if successful, keeps listening on socket if not
int receivedPacketLength=0;
while(true) {
receivedPacketLength = recvfrom(udpSocketID,packetBuffer,packetSize,0,(struct sockaddr *) &socketAddressOther, &socketLength);
if (receivedPacketLength==-1) {
printf("%s%s\n","Unable to receive UDP packet: ", strerror(errno));
exit(-1);
}
if (isValidPacket(packetBuffer)) {return;}
}
}
unsigned long __RSPacketGetData(__RSPacket* packet, unsigned char* databuf, unsigned long bufferSize)
{
if (packet && isValidPacket(packet) && databuf && bufferSize)
{
databuf[bufferSize - 1] = 0; // test the data buffer size is ture or NO.
bufferSize = min(bufferSize, getDataLength(packet));
if (bufferSize)
{
memcpy(databuf, packet->payload.data, bufferSize);
}
return bufferSize;
}
return 0;
}
void readPackets (CANMsg *msg, int usbfd)
{
char buf[MAX_MSG_LEN];
buf[0] = '\0';
int i = 0;
while (i < MAX_MSG_LEN && buf[i] != '\r'){
int bytes = read(usbfd, buf, 1);
if (bytes == -1){
/* Fail */
return;
}
/* If it was well formed, parse and reset. */
if (isValidPacket(buf[0])){
parsePacket (msg, buf);
sendPacket (sockfd, msg);
buf[0] = '\0';
i = 0;
} /* Only parse it if it was well formed. */
}
}
void eatCharacter(unsigned char c)
{
#define MINGOOD 6
int oldHasMatched;
static int goodCount;
unsigned short vals[MAXCHANNELS];
int ns;
int didMatch = 0;
oldHasMatched = hasMatchedProtocol;
if (bufCount == PROTOWINDOW - 1)
memmove(buf, buf+1, PROTOWINDOW - 1);
buf[bufCount] = c;
if (bufCount < PROTOWINDOW - 1)
bufCount += 1;
if (hasMatchedProtocol) {
if (isP2)
didMatch = doesMatchP2(c, vals, &ns);
if (isP3)
didMatch = doesMatchP3(c, vals, &ns);
} else {
if ( (didMatch = doesMatchP2(c, vals, &ns)) ) {
if (goodCount > MINGOOD) {
hasMatchedProtocol = 1;
isP2 = 1;
}
} else {
if ( (didMatch = doesMatchP3(c, vals, &ns)) ) {
if (goodCount > MINGOOD) {
hasMatchedProtocol = 1;
isP3 = 1;
}
}
}
}
if (didMatch) {
char *pstr = "xx";
if (isValidPacket(ns, vals)) {
goodCount += 1;
}
else {
rprintf("Warning: invalid serial packet ahead!\n");
}
if (isP2)
pstr = "P2";
if (isP3)
pstr = "P3";
failCount = 0;
// printf("Got %s packet with %d values: ", pstr, ns);
// for (i = 0; i < ns; ++i)
// printf("%d ", vals[i]);
// printf("\n");
}
else {
failCount += 1;
if (failCount % PROTOWINDOW == 0) {
rprintf("Serial packet sync error -- missed window.\n");
goodCount = 0;
}
}
}
BOOL __RSPacketIsValid(__RSPacket* packet)
{
if (packet)
return isValidPacket(packet);
return NO;
}
