Go to the documentation of this file.00001
00035 #include "um7/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 "um7/registers.h"
00045
00046 namespace um7
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
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
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
00074 type = header_bytes[3];
00075 address = header_bytes[4];
00076 }
00077 else
00078 {
00079
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
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_INFO("Received packet %02x without data.", address);
00119 }
00120
00121
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
00134
00135 if ((data.length() > 0) && registers)
00136 {
00137 registers->write_raw(address, data);
00138 }
00139
00140
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 }