uwb_link.h

00001 //
00002 // Simple message protocol for UWB.
00003 //
00004 // Created by Benjamin Hepp on 02.04.16.
00005 // Copyright (c) 2016 Benjamin Hepp. All rights reserved.
00006 //
00007 
00008 #pragma once
00009 
00010 #include <string.h>
00011 #include <vector>
00012 
00013 #include <ait_link/ait_link.h>
00014 
00015 namespace ait {
00016 
00017   class UWBMessageBody {
00018   public:
00019     virtual int getSize() const = 0;
00020     virtual void buildMessage(uint8_t* buffer) const = 0;
00021     virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) = 0;
00022   };
00023 
00024   class UWBMessageString : public UWBMessageBody {
00025   public:
00026     UWBMessageString()
00027             : str_length_(-1), str_(NULL) {
00028     }
00029 
00030     UWBMessageString(const char* str)
00031             : str_(str) {
00032       str_length_ = strlen(str);
00033     }
00034 
00035     virtual int getSize() const {
00036       return str_length_ + 1;
00037     }
00038 
00039     virtual void buildMessage(uint8_t* buffer) const {
00040       memcpy(buffer, str_, getSize());
00041     }
00042 
00043     virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) {
00044 #ifdef __MBED__
00045       // TODO: This can lead to a buffer overrun (something like strnlen should be used)
00046       str_length_ = strlen(reinterpret_cast<const char*>(buffer));
00047 #else
00048       str_length_ = strnlen(reinterpret_cast<const char*>(buffer), buffer_size);
00049       if (str_length_ >= buffer_size) {
00050         return false;
00051       }
00052 #endif
00053       str_ = reinterpret_cast<const char*>(buffer);
00054       return true;
00055     }
00056 
00057     int getStringLength() const {
00058       return str_length_;
00059     }
00060 
00061     const char* getString() const {
00062       return str_;
00063     }
00064 
00065   private:
00066     int str_length_;
00067     const char* str_;
00068   };
00069 
00070   struct UWBMessageMultiRange : public UWBMessageBody {
00071     UWBMessageMultiRange()
00072             : address(0), remote_address(0) {
00073     }
00074 
00075     UWBMessageMultiRange(uint8_t address, uint8_t remote_address)
00076             : address(address), remote_address(remote_address) {
00077     }
00078 
00079     void clearMeasurements() {
00080       timestamp_master_request_1.clear();
00081       timestamp_slave_reply.clear();
00082       timestamp_master_request_2.clear();
00083     }
00084 
00085     void addModuleMeasurement(uint64_t timestamp_master_request_1, uint64_t timestamp_slave_reply, uint64_t timestamp_master_request_2) {
00086       this->timestamp_master_request_1.push_back(timestamp_master_request_1);
00087       this->timestamp_slave_reply.push_back(timestamp_slave_reply);
00088       this->timestamp_master_request_2.push_back(timestamp_master_request_2);
00089     }
00090 
00091     void setSlaveMeasurement(uint64_t timestamp_master_request_1_recv, uint64_t timestamp_slave_reply_send, uint64_t timestamp_master_request_2_recv) {
00092       this->timestamp_master_request_1_recv = timestamp_master_request_1_recv;
00093       this->timestamp_slave_reply_send = timestamp_slave_reply_send;
00094       this->timestamp_master_request_2_recv = timestamp_master_request_2_recv;
00095     }
00096 
00097     int getNumOfModules() const {
00098       return timestamp_master_request_1.size();
00099     }
00100 
00101     virtual int getSize() const {
00102       return sizeof(uint8_t) + sizeof(address) + sizeof(remote_address) + (getNumOfModules() + 1) * 3 * sizeof(uint64_t);
00103     }
00104 
00105     virtual void buildMessage(uint8_t* buffer) const {
00106       // Number of modules and addresses
00107       uint8_t num_of_modules = static_cast<uint8_t>(getNumOfModules());
00108       *buffer = num_of_modules;
00109       ++buffer;
00110       *buffer = address;
00111       ++buffer;
00112       *buffer = remote_address;
00113       ++buffer;
00114       // Slave timestamps
00115       uint64_t* buffer_64 = reinterpret_cast<uint64_t*>(buffer);
00116       *buffer_64 = timestamp_master_request_1_recv;
00117       ++buffer_64;
00118       *buffer_64 = timestamp_slave_reply_send;
00119       ++buffer_64;
00120       *buffer_64 = timestamp_master_request_2_recv;
00121       ++buffer_64;
00122       // Master and listener timestamps
00123       for (int i = 0; i < num_of_modules; ++i) {
00124         *buffer_64 = timestamp_master_request_1[i];
00125         ++buffer_64;
00126         *buffer_64 = timestamp_slave_reply[i];
00127         ++buffer_64;
00128         *buffer_64 = timestamp_master_request_2[i];
00129         ++buffer_64;
00130       }
00131     }
00132 
00133     virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) {
00134       if (buffer_size < sizeof(uint8_t) + sizeof(address) + sizeof(remote_address)) {
00135         return false;
00136       }
00137 
00138       clearMeasurements();
00139 
00140       // Number of modules and addresses
00141       int num_of_modules = *buffer;
00142       ++buffer;
00143       address = *buffer;
00144       ++buffer;
00145       remote_address = *buffer;
00146       ++buffer;
00147       if (buffer_size < getSize()) {
00148         return false;
00149       }
00150       // Slave timestamps
00151       const uint64_t* buffer_64 = reinterpret_cast<const uint64_t*>(buffer);
00152       timestamp_master_request_1_recv = *buffer_64;
00153       ++buffer_64;
00154       timestamp_slave_reply_send = *buffer_64;
00155       ++buffer_64;
00156       timestamp_master_request_2_recv = *buffer_64;
00157       ++buffer_64;
00158       // Master and listener timestamps
00159       for (int i = 0; i < num_of_modules; ++i) {
00160         timestamp_master_request_1.push_back(*buffer_64);
00161         ++buffer_64;
00162         timestamp_slave_reply.push_back(*buffer_64);
00163         ++buffer_64;
00164         timestamp_master_request_2.push_back(*buffer_64);
00165         ++buffer_64;
00166       }
00167       return true;
00168     }
00169 
00170     uint8_t address;
00171     uint8_t remote_address;
00172 
00173     uint64_t timestamp_master_request_1_recv;
00174     uint64_t timestamp_slave_reply_send;
00175     uint64_t timestamp_master_request_2_recv;
00176     std::vector<uint64_t> timestamp_master_request_1;
00177     std::vector<uint64_t> timestamp_slave_reply;
00178     std::vector<uint64_t> timestamp_master_request_2;
00179   };
00180 
00181   class UWBMessage {
00182   public:
00183     const static uint8_t UWB_MESSAGE_TYPE_NOP = 0x00;
00184     const static uint8_t UWB_MESSAGE_TYPE_STATUS = 0x01;
00185     const static uint8_t UWB_MESSAGE_TYPE_MULTI_RANGE = 0x02;
00186 
00187     UWBMessage()
00188             : type_(UWB_MESSAGE_TYPE_NOP), body_(NULL), part_allocated_(false) {
00189     }
00190 
00191     UWBMessage(uint8_t type)
00192             : type_(type), body_(NULL), part_allocated_(false) {
00193     }
00194 
00195     UWBMessage(uint8_t type, const UWBMessageBody* body)
00196             : type_(type), body_(body), part_allocated_(false) {
00197     }
00198 
00199     ~UWBMessage() {
00200       clearMessageBody();
00201     }
00202 
00203     uint8_t getType() const {
00204       return type_;
00205     }
00206 
00207     const UWBMessageBody* getMessageBody() const {
00208       return body_;
00209     }
00210 
00211     void setMessageBody(const UWBMessageBody* body) {
00212       clearMessageBody();
00213       body_ = body;
00214     }
00215 
00216     int getSize() const {
00217       int size = sizeof(type_);
00218       if (body_ != NULL) {
00219         size += body_->getSize();
00220       }
00221       return size;
00222     }
00223 
00224     void buildMessage(uint8_t* buffer) const {
00225       buffer[0] = type_;
00226       if (body_ != NULL) {
00227         buffer += sizeof(type_);
00228         body_->buildMessage(buffer);
00229       }
00230     }
00231 
00232     bool decodeMessage(const uint8_t* buffer, size_t buffer_size) {
00233       clearMessageBody();
00234       part_allocated_ = true;
00235       if (buffer_size < sizeof(type_)) {
00236         return false;
00237       }
00238       type_ = buffer[0];
00239       buffer += sizeof(type_);
00240       buffer_size -= sizeof(type_);
00241       switch (type_) {
00242         case UWB_MESSAGE_TYPE_NOP: {
00243           break;
00244         }
00245         case UWB_MESSAGE_TYPE_STATUS: {
00246           UWBMessageString *msg_string = new UWBMessageString();
00247           if (msg_string->decodeMessage(buffer, buffer_size)) {
00248             body_ = msg_string;
00249           } else {
00250             delete msg_string;
00251             return false;
00252           }
00253           break;
00254         }
00255         case UWB_MESSAGE_TYPE_MULTI_RANGE: {
00256           UWBMessageMultiRange *msg_multi_range = new UWBMessageMultiRange();
00257           if (msg_multi_range->decodeMessage(buffer, buffer_size)) {
00258             body_ = msg_multi_range;
00259           } else {
00260             delete msg_multi_range;
00261             return false;
00262           }
00263           break;
00264         }
00265         default:
00266           return false;
00267       }
00268       return true;
00269     }
00270 
00271   private:
00272     void clearMessageBody() {
00273       if (part_allocated_) {
00274         delete body_;
00275         part_allocated_ = false;
00276       }
00277       body_ = NULL;
00278     }
00279 
00280     uint8_t type_;
00281     const UWBMessageBody* body_;
00282     bool part_allocated_;
00283   };
00284 
00285   class UWBLink {
00286   public:
00287     UWBLink(AITLink* ait_link, int buffer_size = 1024)
00288             : handle_message_callback_(NULL), callback_user_data_(NULL),
00289               ait_link_(ait_link), buffer_size_(buffer_size) {
00290       buffer_ = new uint8_t[buffer_size];
00291       ait_link_->registerFrameHandler(&UWBLink::handleFrameWrapper, this);
00292     }
00293 
00294     virtual ~UWBLink() {
00295       delete[] buffer_;
00296     }
00297 
00298     bool sendMessage(const UWBMessage& msg) {
00299       int size = msg.getSize();
00300       if (size > buffer_size_) {
00301         return false;
00302       }
00303       msg.buildMessage(buffer_);
00304       ait_link_->sendFrame(buffer_, size);
00305       return true;
00306     }
00307 
00308     void registerMessageHandler(void (*callback)(void* user_data, const UWBMessage& msg), void* user_data) {
00309       handle_message_callback_ = callback;
00310       callback_user_data_ = user_data;
00311     }
00312 
00313     void inputReceivedChar(uint8_t data) {
00314       ait_link_->inputReceivedChar(data);
00315     }
00316 
00317   protected:
00318     virtual void handleMessage(const UWBMessage& msg) {
00319       if (handle_message_callback_ != NULL) {
00320         (*handle_message_callback_)(callback_user_data_, msg);
00321       }
00322     }
00323 
00324     void (*handle_message_callback_)(void* user_data, const UWBMessage& msg);
00325     void* callback_user_data_;
00326 
00327   private:
00328     void handleFrame(const uint8_t* frame_buffer, size_t frame_length) {
00329       UWBMessage msg;
00330       if (msg.decodeMessage(frame_buffer, frame_length)) {
00331         handleMessage(msg);
00332       } else {
00333         fprintf(stderr, "Failed to decode UWB message");
00334       }
00335     }
00336 
00337     static void handleFrameWrapper(void* user_data, const uint8_t* frame_buffer, size_t frame_length) {
00338       UWBLink* uwb_link = reinterpret_cast<UWBLink*>(user_data);
00339       uwb_link->handleFrame(frame_buffer, frame_length);
00340     }
00341 
00342     AITLink* ait_link_;
00343     uint8_t* buffer_;
00344     int buffer_size_;
00345   };
00346 
00347 }