serialization.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef UBLOX_SERIALIZATION_H
00030 #define UBLOX_SERIALIZATION_H
00031 
00032 #include <stdint.h>
00033 #include <boost/call_traits.hpp>
00034 #include <vector>
00035 #include <algorithm>
00036 
00037 #include "checksum.h"
00038 
00039 namespace ublox {
00040 
00041 static const uint8_t DEFAULT_SYNC_A = 0xB5;
00042 static const uint8_t DEFAULT_SYNC_B = 0x62;
00043 
00044 template <typename T>
00045 struct Serializer {
00046   static void read(const uint8_t *data, uint32_t count, typename boost::call_traits<T>::reference message);
00047   static uint32_t serializedLength(typename boost::call_traits<T>::param_type message);
00048   static void write(uint8_t *data, uint32_t size, typename boost::call_traits<T>::param_type message);
00049 };
00050 
00051 template <typename T>
00052 class Message {
00053 public:
00054   static bool canDecode(uint8_t class_id, uint8_t message_id) {
00055     return std::find(keys_.begin(), keys_.end(), std::make_pair(class_id, message_id)) != keys_.end();
00056   }
00057 
00058   static void addKey(uint8_t class_id, uint8_t message_id) {
00059     keys_.push_back(std::make_pair(class_id, message_id));
00060   }
00061 
00062   struct StaticKeyInitializer
00063   {
00064     StaticKeyInitializer(uint8_t class_id, uint8_t message_id) { Message<T>::addKey(class_id, message_id); }
00065   };
00066 
00067 private:
00068   static std::vector<std::pair<uint8_t,uint8_t> > keys_;
00069 };
00070 
00071 struct Options
00072 {
00073   Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B) {}
00074   uint8_t sync_a, sync_b;
00075 };
00076 
00077 class Reader {
00078 public:
00079   Reader(const uint8_t *data, uint32_t count, const Options &options = Options()) : data_(data), count_(count), found_(false), options_(options) {}
00080 
00081   typedef const uint8_t *iterator;
00082 
00083   iterator search()
00084   {
00085     if (found_) next();
00086 
00087     for( ; count_ > 0; --count_, ++data_) {
00088       if (data_[0] == options_.sync_a && (count_ == 1 || data_[1] == options_.sync_b)) break;
00089     }
00090 
00091     return data_;
00092   }
00093 
00094   bool found()
00095   {
00096     if (found_) return true;
00097     if (count_ < 6) return false;
00098     if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) return false;
00099     if (count_ < length() + 8) return false;
00100 
00101     found_ = true;
00102     return true;
00103   }
00104 
00105   iterator next() {
00106     if (found()) {
00107       uint32_t size = length() + 8;
00108       data_ += size; count_ -= size;
00109     }
00110     found_ = false;
00111     return data_;
00112   }
00113 
00114   iterator pos() {
00115     return data_;
00116   }
00117 
00118   iterator end() {
00119     return data_ + count_;
00120   }
00121 
00122   uint8_t classId() { return data_[2]; }
00123   uint8_t messageId() { return data_[3]; }
00124   uint32_t length() { return (data_[5] << 8) + data_[4]; }
00125   const uint8_t *data() { return data_ + 6; }
00126   uint16_t checksum() { return *reinterpret_cast<const uint16_t *>(data_ + 6 + length()); }
00127 
00128   template <typename T>
00129   bool read(typename boost::call_traits<T>::reference message, bool search = false)
00130   {
00131     if (search) this->search();
00132     if (!found()) return false;
00133     if (!Message<T>::canDecode(classId(), messageId())) return false;
00134 
00135     uint16_t chk;
00136     if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) {
00137       // checksum error
00138       return false;
00139     }
00140 
00141     Serializer<T>::read(data_ + 6, length(), message);
00142     return true;
00143   }
00144 
00145   template <typename T> bool hasType()
00146   {
00147     if (!found()) return false;
00148     return Message<T>::canDecode(classId(), messageId());
00149   }
00150 
00151   bool isMessage(uint8_t class_id, uint8_t message_id)
00152   {
00153     if (!found()) return false;
00154     return (data_[2] == class_id && data_[3] == message_id);
00155   }
00156 
00157 private:
00158   const uint8_t *data_;
00159   uint32_t count_;
00160   bool found_;
00161   Options options_;
00162 };
00163 
00164 class Writer
00165 {
00166 public:
00167   Writer(uint8_t *data, uint32_t size, const Options &options = Options()) : data_(data), size_(size), options_(options) {}
00168 
00169   typedef uint8_t *iterator;
00170 
00171   template <typename T> bool write(const T& message, uint8_t class_id = T::CLASS_ID, uint8_t message_id = T::MESSAGE_ID) {
00172     uint32_t length = Serializer<T>::serializedLength(message);
00173     if (size_ < length + 8) return false;
00174     Serializer<T>::write(data_ + 6, size_ - 6, message);
00175     return write(0, length, class_id, message_id);
00176   }
00177 
00178   bool write(const uint8_t* message, uint32_t length, uint8_t class_id, uint8_t message_id) {
00179     if (size_ < length + 8) return false;
00180     uint8_t *start = data_;
00181 
00182     // write header
00183     *data_++ = options_.sync_a;
00184     *data_++ = options_.sync_b;
00185     *data_++ = class_id;
00186     *data_++ = message_id;
00187     *data_++ = length & 0xFF;
00188     *data_++ = (length >> 8) & 0xFF;
00189     size_ -= 6;
00190 
00191     // write message
00192     if (message) std::copy(message, message + length, data_);
00193     data_ += length;
00194     size_ -= length;
00195 
00196     // write checksum
00197     uint8_t ck_a, ck_b;
00198     calculateChecksum(start + 2, length + 4, ck_a, ck_b);
00199     *data_++ = ck_a;
00200     *data_++ = ck_b;
00201     size_ -= 2;
00202 
00203     return true;
00204   }
00205 
00206   iterator end() {
00207     return data_;
00208   }
00209 
00210 private:
00211   uint8_t *data_;
00212   uint32_t size_;
00213   Options options_;
00214 };
00215 
00216 } // namespace ublox
00217 
00218 #define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \
00219   template class ublox::Serializer<package::message>; \
00220   template class ublox::Message<package::message>; \
00221   namespace package { namespace { \
00222     static const ublox::Message<message>::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \
00223   } } \
00224 
00225 // use implementation of class Serializer in "serialization_ros.h"
00226 #include "serialization_ros.h"
00227 
00228 #endif // UBLOX_SERIALIZATION_H


ublox_serialization
Author(s): Johannes Meyer
autogenerated on Wed Aug 26 2015 16:35:56