00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef UBLOX_SERIALIZATION_H
00030 #define UBLOX_SERIALIZATION_H
00031
00032 #include <ros/console.h>
00033 #include <stdint.h>
00034 #include <boost/call_traits.hpp>
00035 #include <vector>
00036 #include <algorithm>
00037
00038 #include "checksum.h"
00039
00048
00049
00054 namespace ublox {
00055
00057 static const uint8_t DEFAULT_SYNC_A = 0xB5;
00059 static const uint8_t DEFAULT_SYNC_B = 0x62;
00061 static const uint8_t kHeaderLength = 6;
00063 static const uint8_t kChecksumLength = 2;
00064
00068 template <typename T>
00069 struct Serializer {
00076 static void read(const uint8_t *data, uint32_t count,
00077 typename boost::call_traits<T>::reference message);
00085 static uint32_t serializedLength(
00086 typename boost::call_traits<T>::param_type message);
00087
00094 static void write(uint8_t *data, uint32_t size,
00095 typename boost::call_traits<T>::param_type message);
00096 };
00097
00102 template <typename T>
00103 class Message {
00104 public:
00111 static bool canDecode(uint8_t class_id, uint8_t message_id) {
00112 return std::find(keys_.begin(), keys_.end(),
00113 std::make_pair(class_id, message_id)) != keys_.end();
00114 }
00115
00122 static void addKey(uint8_t class_id, uint8_t message_id) {
00123 keys_.push_back(std::make_pair(class_id, message_id));
00124 }
00125
00126 struct StaticKeyInitializer
00127 {
00128 StaticKeyInitializer(uint8_t class_id, uint8_t message_id) {
00129 Message<T>::addKey(class_id, message_id);
00130 }
00131 };
00132
00133 private:
00134 static std::vector<std::pair<uint8_t,uint8_t> > keys_;
00135 };
00136
00140 struct Options {
00144 Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B),
00145 header_length(kHeaderLength), checksum_length(kChecksumLength) {}
00147 uint8_t sync_a;
00149 uint8_t sync_b;
00151 uint8_t header_length;
00153 uint8_t checksum_length;
00154
00159 int wrapper_length() {
00160 return header_length + checksum_length;
00161 }
00162 };
00163
00167 class Reader {
00168 public:
00175 Reader(const uint8_t *data, uint32_t count,
00176 const Options &options = Options()) :
00177 data_(data), count_(count), found_(false), options_(options) {}
00178
00179 typedef const uint8_t *iterator;
00180
00185 iterator search()
00186 {
00187 if (found_) next();
00188
00189
00190 for( ; count_ > 0; --count_, ++data_) {
00191 if (data_[0] == options_.sync_a &&
00192 (count_ == 1 || data_[1] == options_.sync_b))
00193 break;
00194 }
00195
00196 return data_;
00197 }
00198
00203 bool found()
00204 {
00205 if (found_) return true;
00206
00207 if (count_ < options_.wrapper_length()) return false;
00208
00209 if (data_[0] != options_.sync_a || data_[1] != options_.sync_b)
00210 return false;
00211
00212
00213 if (count_ < length() + options_.wrapper_length()) return false;
00214
00215 found_ = true;
00216 return true;
00217 }
00218
00226 iterator next() {
00227 if (found()) {
00228 uint32_t size = length() + options_.wrapper_length();
00229 data_ += size; count_ -= size;
00230 }
00231 found_ = false;
00232 return data_;
00233 }
00234
00239 iterator pos() {
00240 return data_;
00241 }
00242
00243 iterator end() {
00244 return data_ + count_;
00245 }
00246
00247 uint8_t classId() { return data_[2]; }
00248 uint8_t messageId() { return data_[3]; }
00249
00257 uint32_t length() { return (data_[5] << 8) + data_[4]; }
00258 const uint8_t *data() { return data_ + options_.header_length; }
00259
00265 uint16_t checksum() {
00266 return *reinterpret_cast<const uint16_t *>(data_ + options_.header_length +
00267 length());
00268 }
00269
00275 template <typename T>
00276 bool read(typename boost::call_traits<T>::reference message,
00277 bool search = false) {
00278 if (search) this->search();
00279 if (!found()) return false;
00280 if (!Message<T>::canDecode(classId(), messageId())) return false;
00281
00282 uint16_t chk;
00283 if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) {
00284
00285 ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(),
00286 messageId());
00287 return false;
00288 }
00289
00290 Serializer<T>::read(data_ + options_.header_length, length(), message);
00291 return true;
00292 }
00293
00299 template <typename T>
00300 bool hasType() {
00301 if (!found()) return false;
00302 return Message<T>::canDecode(classId(), messageId());
00303 }
00304
00310 bool isMessage(uint8_t class_id, uint8_t message_id) {
00311 if (!found()) return false;
00312 return (classId() == class_id && messageId() == message_id);
00313 }
00314
00315 private:
00317 const uint8_t *data_;
00319 uint32_t count_;
00321 bool found_;
00323 Options options_;
00324 };
00325
00329 class Writer {
00330 public:
00331 typedef uint8_t *iterator;
00332
00339 Writer(uint8_t *data, uint32_t size, const Options &options = Options()) :
00340 data_(data), size_(size), options_(options) {}
00341
00349 template <typename T> bool write(const T& message,
00350 uint8_t class_id = T::CLASS_ID,
00351 uint8_t message_id = T::MESSAGE_ID) {
00352
00353 uint32_t length = Serializer<T>::serializedLength(message);
00354 if (size_ < length + options_.wrapper_length()) {
00355 ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written",
00356 class_id, message_id);
00357 return false;
00358 }
00359
00360 Serializer<T>::write(data_ + options_.header_length,
00361 size_ - options_.header_length, message);
00362 return write(0, length, class_id, message_id);
00363 }
00364
00374 bool write(const uint8_t* message, uint32_t length, uint8_t class_id,
00375 uint8_t message_id) {
00376 if (size_ < length + options_.wrapper_length()) {
00377 ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written",
00378 class_id, message_id);
00379 return false;
00380 }
00381 iterator start = data_;
00382
00383
00384 *data_++ = options_.sync_a;
00385 *data_++ = options_.sync_b;
00386 *data_++ = class_id;
00387 *data_++ = message_id;
00388 *data_++ = length & 0xFF;
00389 *data_++ = (length >> 8) & 0xFF;
00390 size_ -= options_.header_length;
00391
00392
00393 if (message) std::copy(message, message + length, data_);
00394 data_ += length;
00395 size_ -= length;
00396
00397
00398 uint8_t ck_a, ck_b;
00399 calculateChecksum(start + 2, length + 4, ck_a, ck_b);
00400 *data_++ = ck_a;
00401 *data_++ = ck_b;
00402 size_ -= options_.checksum_length;
00403
00404 return true;
00405 }
00406
00407 iterator end() {
00408 return data_;
00409 }
00410
00411 private:
00413 iterator data_;
00415
00416 uint32_t size_;
00418 Options options_;
00419 };
00420
00421 }
00422
00423
00424 #define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \
00425 template class ublox::Serializer<package::message>; \
00426 template class ublox::Message<package::message>; \
00427 namespace package { namespace { \
00428 static const ublox::Message<message>::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \
00429 } } \
00430
00431
00432
00433
00434 #define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \
00435 namespace package { namespace { \
00436 static const ublox::Message<message>::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \
00437 } } \
00438
00439
00440
00441 #include "serialization_ros.h"
00442
00443 #endif // UBLOX_SERIALIZATION_H