rmp_io.cc
Go to the documentation of this file.
00001 #include "segwayrmp/segwayrmp.h"
00002 #include "segwayrmp/impl/rmp_io.h"
00003 
00004 inline void printHex(char * data, int length) {
00005   for(int i = 0; i < length; ++i) {
00006     printf("0x%.2X ", (unsigned)(unsigned char)data[i]);
00007   }
00008   printf("\n");
00009 }
00010 
00011 using namespace segwayrmp;
00012 
00013 static unsigned int BUFFER_SIZE = 256;
00014 
00016 // RMPIO
00017 
00018 void RMPIO::getPacket(Packet &packet) {
00019   if(!this->connected)
00020     RMP_THROW_MSG_AND_ID(PacketRetrievalException, "Not connected.", 1);
00021   
00022   unsigned char usb_packet[18];
00023   bool packet_complete = false;
00024   int packet_index = 0;
00025   
00026   while(!packet_complete && !this->canceled) {
00027     // Top the buffer off
00028     size_t prev_size = this->data_buffer.size();
00029     if(prev_size < 18) {
00030       this->fillBuffer();
00031       // Ensure that data was read into the buffer
00032       if(prev_size == this->data_buffer.size()) {
00033         RMP_THROW_MSG_AND_ID(PacketRetrievalException, "No data received "
00034           "from Segway.", 3);
00035       }
00036     }
00037     
00038     // If looking for start of packet and start of packet
00039     if(packet_index == 0 && this->data_buffer[0] == 0xF0) {
00040       // Put the 0xF0 in the packet
00041       usb_packet[packet_index] = this->data_buffer[0];
00042       // Remove the 0xF0 from the buffer
00043       this->data_buffer.erase(this->data_buffer.begin());
00044       // Look for next packet byte
00045       packet_index += 1;
00046     } else if(packet_index == 0) { // If we were looking for the first byte, but didn't find it
00047       // Remove the invalid byte from the buffer
00048       this->data_buffer.erase(this->data_buffer.begin());
00049     }
00050     
00051     // If looking for second byte of packet and second byte of packet
00052     if(packet_index == 1 && this->data_buffer[0] == 0x55) {
00053       // Put the 0x55 in the packet
00054       usb_packet[packet_index] = this->data_buffer[0];
00055       // Remove the 0x55 from the buffer
00056       this->data_buffer.erase(this->data_buffer.begin());
00057       // Look for next packet byte
00058       packet_index += 1;
00059     } else if(packet_index == 1) { // Else were looking for second byte but didn't find it
00060       // Reset the packet index to start search for packet over
00061       packet_index = 0;
00062     }
00063     
00064     // If looking for channel byte and channel A or B
00065     if(packet_index == 2 && (this->data_buffer[0] == 0xAA || this->data_buffer[0] == 0xBB)) {
00066       // Put the channel in the packet
00067       usb_packet[packet_index] = this->data_buffer[0];
00068       // Remove the channel from the buffer
00069       this->data_buffer.erase(this->data_buffer.begin());
00070       // Look for next packet byte
00071       packet_index += 1;
00072     } else if(packet_index == 2) { // Else were looking for channel byte but didn't find it
00073       // Reset the packet index to start search for packet over
00074       packet_index = 0;
00075     }
00076     
00077     // If packet_index >= 3 then we just need to collect the rest of the bytes
00078     // (we assume that if the previous three bytes were recieved then this is a valid packet, 
00079     //  if it isn't the checksum will fail)
00080     if(packet_index >= 3) {
00081       // Put the next btye in the packet
00082       usb_packet[packet_index] = this->data_buffer[0];
00083       // Remove the byte from the buffer
00084       this->data_buffer.erase(this->data_buffer.begin());
00085       // Look for next packet byte
00086       packet_index += 1;
00087     }
00088     
00089     // If packet_index is 18 then we have a full packet
00090     if(packet_index == 18)
00091       packet_complete = true;
00092   }
00093   
00094   // Check the Checksum
00095   if(usb_packet[17] != this->computeChecksum(usb_packet)) {
00096     RMP_THROW_MSG_AND_ID(PacketRetrievalException, "Checksum mismatch.", 2);
00097   }
00098   
00099   // Convert to the packet type
00100   packet.channel = usb_packet[2];
00101   packet.id = ((usb_packet[4] << 3) | ((usb_packet[5] >> 5) & 7)) & 0x0fff;
00102   for (int i = 0; i < 8; i++)  {
00103     packet.data[i] = usb_packet[i + 9];
00104   }
00105   
00106   return;
00107 }
00108 
00109 void RMPIO::sendPacket(Packet &packet) {
00110   unsigned char usb_packet[18] = {0xF0, 0x55, 0x00, 0x00, 0x00, 0x00, 0x04,
00111                                   0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00112                                   0x00, 0x00, 0x00, 0x00};
00113   // Set the id
00114   usb_packet[6] = (packet.id & 0xFF00) >> 8;
00115   usb_packet[7] = packet.id & 0x00FF;
00116   // Set the desitnation channel, 0x01 for 0xAA and 0x02 for 0xBB
00117   usb_packet[2] = packet.channel;
00118   // Copy movement and configuration commands
00119   for(int i = 0; i < 8; ++i) {
00120     usb_packet[9+i] = packet.data[i];
00121   }
00122   // Compute and set the checksum
00123   usb_packet[17] = this->computeChecksum(usb_packet);
00124   
00125   // Write the data
00126   this->write(usb_packet, 18);
00127 }
00128 
00129 void RMPIO::fillBuffer() {
00130   unsigned char buffer[BUFFER_SIZE];
00131   // Read up to BUFFER_SIZE what ever is needed to fill the vector
00132   // to BUFFER_SIZE
00133   int bytes_read = this->read(buffer, BUFFER_SIZE-this->data_buffer.size());
00134   // Append the buffered data to the vector
00135   this->data_buffer.insert(this->data_buffer.end(), buffer, buffer+bytes_read);
00136 }
00137 
00138 unsigned char RMPIO::computeChecksum(unsigned char* usb_packet) {
00139   unsigned short checksum = 0;
00140   unsigned short checksum_hi = 0;
00141   
00142   for(int i = 0; i < 17; i++) {
00143     checksum += (short)usb_packet[i];
00144   }
00145   
00146   checksum_hi = (unsigned short)(checksum >> 8);
00147   checksum &= 0xff;
00148   checksum += checksum_hi;
00149   checksum_hi = (unsigned short)(checksum >> 8);
00150   checksum &= 0xff;
00151   checksum += checksum_hi;
00152   checksum = (~checksum + 1) & 0xff;
00153   return (unsigned char)checksum;
00154 }


libsegwayrmp
Author(s): William Woodall
autogenerated on Mon Oct 6 2014 01:48:09