comms.cpp
Go to the documentation of this file.
00001 
00035 #include "um6/comms.h"
00036 
00037 #include <arpa/inet.h>
00038 #include <boost/algorithm/string/predicate.hpp>
00039 #include <boost/foreach.hpp>
00040 #include <string>
00041 
00042 #include "ros/console.h"
00043 #include "serial/serial.h"
00044 #include "um6/registers.h"
00045 
00046 namespace um6
00047 {
00048 
00049 const uint8_t Comms::PACKET_HAS_DATA = 1 << 7;
00050 const uint8_t Comms::PACKET_IS_BATCH = 1 << 6;
00051 const uint8_t Comms::PACKET_BATCH_LENGTH_MASK = 0x0F;
00052 const uint8_t Comms::PACKET_BATCH_LENGTH_OFFSET = 2;
00053 
00054 int16_t Comms::receive(Registers* registers = NULL)
00055 {
00056   // Search the serial stream for a start-of-packet sequence.
00057   try
00058   {
00059     size_t available = serial_->available();
00060     if (available > 255)
00061     {
00062       ROS_WARN_STREAM("Serial read buffer is " << available << ", now flushing in an attempt to catch up.");
00063       serial_->flushInput();
00064     }
00065 
00066     // Optimistically assume that the next five bytes on the wire are a packet header.
00067     uint8_t header_bytes[5];
00068     serial_->read(header_bytes, 5);
00069 
00070     uint8_t type, address;
00071     if (memcmp(header_bytes, "snp", 3) == 0)
00072     {
00073       // Optimism win.
00074       type = header_bytes[3];
00075       address = header_bytes[4];
00076     }
00077     else
00078     {
00079       // Optimism fail. Search the serial stream for a header.
00080       std::string snp;
00081       serial_->readline(snp, 96, "snp");
00082       if (!boost::algorithm::ends_with(snp, "snp")) throw SerialTimeout();
00083       if (snp.length() > 3)
00084       {
00085         ROS_WARN_STREAM_COND(!first_spin_,
00086                              "Discarded " << 5 + snp.length() - 3 << " junk byte(s) preceeding packet.");
00087       }
00088       if (serial_->read(&type, 1) != 1) throw SerialTimeout();
00089       if (serial_->read(&address, 1) != 1) throw SerialTimeout();
00090     }
00091 
00092     first_spin_ = false;
00093 
00094     uint16_t checksum_calculated = 's' + 'n' + 'p' + type + address;
00095     std::string data;
00096     if (type & PACKET_HAS_DATA)
00097     {
00098       uint8_t data_length = 1;
00099       if (type & PACKET_IS_BATCH)
00100       {
00101         data_length = (type >> PACKET_BATCH_LENGTH_OFFSET) & PACKET_BATCH_LENGTH_MASK;
00102         ROS_DEBUG("Received packet %02x with batched (%d) data.", address, data_length);
00103       }
00104       else
00105       {
00106         ROS_DEBUG("Received packet %02x with non-batched data.", address);
00107       }
00108 
00109       // Read data bytes initially into a buffer so that we can compute the checksum.
00110       if (serial_->read(data, data_length * 4) != data_length * 4) throw SerialTimeout();
00111       BOOST_FOREACH(uint8_t ch, data)
00112       {
00113         checksum_calculated += ch;
00114       }
00115     }
00116     else
00117     {
00118       ROS_DEBUG("Received packet %02x without data.", address);
00119     }
00120 
00121     // Compare computed checksum with transmitted value.
00122     uint16_t checksum_transmitted;
00123     if (serial_->read(reinterpret_cast<uint8_t*>(&checksum_transmitted), 2) != 2)
00124     {
00125       throw SerialTimeout();
00126     }
00127     checksum_transmitted = ntohs(checksum_transmitted);
00128     if (checksum_transmitted != checksum_calculated)
00129     {
00130       throw BadChecksum();
00131     }
00132 
00133     // Copy data from checksum buffer into registers, if specified.
00134     // Note that byte-order correction (as necessary) happens at access-time.
00135     if ((data.length() > 0) && registers)
00136     {
00137       registers->write_raw(address, data);
00138     }
00139 
00140     // Successful packet read, return address byte.
00141     return address;
00142   }
00143   catch(const SerialTimeout& e)
00144   {
00145     ROS_WARN("Timed out waiting for packet from device.");
00146   }
00147   catch(const BadChecksum& e)
00148   {
00149     ROS_WARN("Discarding packet due to bad checksum.");
00150   }
00151   return -1;
00152 }
00153 
00154 std::string Comms::checksum(const std::string& s)
00155 {
00156   uint16_t checksum = 0;
00157   BOOST_FOREACH(uint8_t ch, s)
00158   {
00159     checksum += ch;
00160   }
00161   checksum = htons(checksum);
00162   ROS_DEBUG("Computed checksum on string of length %zd as %04x.", s.length(), checksum);
00163   std::string out(2, 0);
00164   memcpy(&out[0], &checksum, 2);
00165   return out;
00166 }
00167 
00168 std::string Comms::message(uint8_t address, std::string data)
00169 {
00170   uint8_t type = 0;
00171   if (data.length() > 0)
00172   {
00173     type |= PACKET_HAS_DATA;
00174   }
00175   if (data.length() > 4)
00176   {
00177     type |= PACKET_IS_BATCH;
00178     type |= (data.length() / 4) << PACKET_BATCH_LENGTH_OFFSET;
00179   }
00180 
00181   std::stringstream ss(std::stringstream::out | std::stringstream::binary);
00182   ss << "snp" << type << address << data;
00183   std::string output = ss.str();
00184   std::string c = checksum(output);
00185   ss << c;
00186   output = ss.str();
00187   ROS_DEBUG("Generated message %02x of overall length %zd.", address, output.length());
00188   return output;
00189 }
00190 
00191 void Comms::send(const Accessor_& r) const
00192 {
00193   uint8_t address = r.index;
00194   std::string data(reinterpret_cast<char*>(r.raw()), r.length * 4);
00195   serial_->write(message(r.index, data));
00196 }
00197 
00198 bool Comms::sendWaitAck(const Accessor_& r)
00199 {
00200   const uint8_t tries = 5;
00201   for (uint8_t t = 0; t < tries; t++)
00202   {
00203     send(r);
00204     const uint8_t listens = 20;
00205     for (uint8_t i = 0; i < listens; i++)
00206     {
00207       int16_t received = receive();
00208       if (received == r.index)
00209       {
00210         ROS_DEBUG("Message %02x ack received.", received);
00211         return true;
00212       }
00213       else if (received == -1)
00214       {
00215         ROS_DEBUG("Serial read timed out waiting for ack. Attempting to retransmit.");
00216         break;
00217       }
00218     }
00219   }
00220   return false;
00221 }
00222 }  // namespace um6


um6
Author(s): Mike Purvis
autogenerated on Thu Jun 6 2019 19:02:20