comms.cpp
Go to the documentation of this file.
1 
35 #include "um6/comms.h"
36 
37 #include <arpa/inet.h>
38 #include <boost/algorithm/string/predicate.hpp>
39 #include <boost/foreach.hpp>
40 #include <string>
41 
42 #include "ros/console.h"
43 #include "serial/serial.h"
44 #include "um6/registers.h"
45 
46 namespace um6
47 {
48 
49 const uint8_t Comms::PACKET_HAS_DATA = 1 << 7;
50 const uint8_t Comms::PACKET_IS_BATCH = 1 << 6;
51 const uint8_t Comms::PACKET_BATCH_LENGTH_MASK = 0x0F;
52 const uint8_t Comms::PACKET_BATCH_LENGTH_OFFSET = 2;
53 
54 int16_t Comms::receive(Registers* registers = NULL)
55 {
56  // Search the serial stream for a start-of-packet sequence.
57  try
58  {
59  size_t available = serial_->available();
60  if (available > 255)
61  {
62  ROS_WARN_STREAM("Serial read buffer is " << available << ", now flushing in an attempt to catch up.");
64  }
65 
66  // Optimistically assume that the next five bytes on the wire are a packet header.
67  uint8_t header_bytes[5];
68  serial_->read(header_bytes, 5);
69 
70  uint8_t type, address;
71  if (memcmp(header_bytes, "snp", 3) == 0)
72  {
73  // Optimism win.
74  type = header_bytes[3];
75  address = header_bytes[4];
76  }
77  else
78  {
79  // Optimism fail. Search the serial stream for a header.
80  std::string snp;
81  serial_->readline(snp, 96, "snp");
82  if (!boost::algorithm::ends_with(snp, "snp")) throw SerialTimeout();
83  if (snp.length() > 3)
84  {
86  "Discarded " << 5 + snp.length() - 3 << " junk byte(s) preceeding packet.");
87  }
88  if (serial_->read(&type, 1) != 1) throw SerialTimeout();
89  if (serial_->read(&address, 1) != 1) throw SerialTimeout();
90  }
91 
92  first_spin_ = false;
93 
94  uint16_t checksum_calculated = 's' + 'n' + 'p' + type + address;
95  std::string data;
96  if (type & PACKET_HAS_DATA)
97  {
98  uint8_t data_length = 1;
99  if (type & PACKET_IS_BATCH)
100  {
101  data_length = (type >> PACKET_BATCH_LENGTH_OFFSET) & PACKET_BATCH_LENGTH_MASK;
102  ROS_DEBUG("Received packet %02x with batched (%d) data.", address, data_length);
103  }
104  else
105  {
106  ROS_DEBUG("Received packet %02x with non-batched data.", address);
107  }
108 
109  // Read data bytes initially into a buffer so that we can compute the checksum.
110  if (serial_->read(data, data_length * 4) != data_length * 4) throw SerialTimeout();
111  BOOST_FOREACH(uint8_t ch, data)
112  {
113  checksum_calculated += ch;
114  }
115  }
116  else
117  {
118  ROS_DEBUG("Received packet %02x without data.", address);
119  }
120 
121  // Compare computed checksum with transmitted value.
122  uint16_t checksum_transmitted;
123  if (serial_->read(reinterpret_cast<uint8_t*>(&checksum_transmitted), 2) != 2)
124  {
125  throw SerialTimeout();
126  }
127  checksum_transmitted = ntohs(checksum_transmitted);
128  if (checksum_transmitted != checksum_calculated)
129  {
130  throw BadChecksum();
131  }
132 
133  // Copy data from checksum buffer into registers, if specified.
134  // Note that byte-order correction (as necessary) happens at access-time.
135  if ((data.length() > 0) && registers)
136  {
137  registers->write_raw(address, data);
138  }
139 
140  // Successful packet read, return address byte.
141  return address;
142  }
143  catch(const SerialTimeout& e)
144  {
145  ROS_WARN("Timed out waiting for packet from device.");
146  }
147  catch(const BadChecksum& e)
148  {
149  ROS_WARN("Discarding packet due to bad checksum.");
150  }
151  return -1;
152 }
153 
154 std::string Comms::checksum(const std::string& s)
155 {
156  uint16_t checksum = 0;
157  BOOST_FOREACH(uint8_t ch, s)
158  {
159  checksum += ch;
160  }
161  checksum = htons(checksum);
162  ROS_DEBUG("Computed checksum on string of length %zd as %04x.", s.length(), checksum);
163  std::string out(2, 0);
164  memcpy(&out[0], &checksum, 2);
165  return out;
166 }
167 
168 std::string Comms::message(uint8_t address, std::string data)
169 {
170  uint8_t type = 0;
171  if (data.length() > 0)
172  {
173  type |= PACKET_HAS_DATA;
174  }
175  if (data.length() > 4)
176  {
177  type |= PACKET_IS_BATCH;
178  type |= (data.length() / 4) << PACKET_BATCH_LENGTH_OFFSET;
179  }
180 
181  std::stringstream ss(std::stringstream::out | std::stringstream::binary);
182  ss << "snp" << type << address << data;
183  std::string output = ss.str();
184  std::string c = checksum(output);
185  ss << c;
186  output = ss.str();
187  ROS_DEBUG("Generated message %02x of overall length %zd.", address, output.length());
188  return output;
189 }
190 
191 void Comms::send(const Accessor_& r) const
192 {
193  uint8_t address = r.index;
194  std::string data(reinterpret_cast<char*>(r.raw()), r.length * 4);
195  serial_->write(message(r.index, data));
196 }
197 
199 {
200  const uint8_t tries = 5;
201  for (uint8_t t = 0; t < tries; t++)
202  {
203  send(r);
204  const uint8_t listens = 20;
205  for (uint8_t i = 0; i < listens; i++)
206  {
207  int16_t received = receive();
208  if (received == r.index)
209  {
210  ROS_DEBUG("Message %02x ack received.", received);
211  return true;
212  }
213  else if (received == -1)
214  {
215  ROS_DEBUG("Serial read timed out waiting for ack. Attempting to retransmit.");
216  break;
217  }
218  }
219  }
220  return false;
221 }
222 } // namespace um6
size_t available()
static std::string checksum(const std::string &s)
Definition: comms.cpp:154
const uint16_t length
Definition: registers.h:116
size_t readline(std::string &buffer, size_t size=65536, std::string eol="\n")
static const uint8_t PACKET_HAS_DATA
Definition: comms.h:75
serial::Serial * serial_
Definition: comms.h:86
Provides the Registers class, which initializes with a suite of accessors suitable for reading and wr...
static const uint8_t PACKET_IS_BATCH
Definition: comms.h:76
const uint8_t index
Definition: registers.h:107
void * raw() const
Definition: registers.cpp:39
Definition: comms.h:46
static std::string message(uint8_t address, std::string data)
Definition: comms.cpp:168
#define ROS_WARN(...)
static const uint8_t PACKET_BATCH_LENGTH_OFFSET
Definition: comms.h:78
int16_t receive(Registers *r)
Definition: comms.cpp:54
#define ROS_WARN_STREAM_COND(cond, args)
#define ROS_WARN_STREAM(args)
static const uint8_t PACKET_BATCH_LENGTH_MASK
Definition: comms.h:77
bool sendWaitAck(const Accessor_ &a)
Definition: comms.cpp:198
size_t read(uint8_t *buffer, size_t size)
Comms class definition. Does not manage the serial connection itself, but takes care of reading and w...
bool first_spin_
Definition: comms.h:85
void send(const Accessor_ &a) const
Definition: comms.cpp:191
size_t write(const uint8_t *data, size_t size)
#define ROS_DEBUG(...)


um6
Author(s): Mike Purvis
autogenerated on Thu Sep 26 2019 03:18:02