Benjamin Hepp / ait_link

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

uwb_link/uwb_link.h@3:bfc4928cd279, 2016-04-06 (annotated)

Committer:
bhepp
Date:
Wed Apr 06 08:27:25 2016 +0000
Revision:
3:bfc4928cd279
Parent:
2:502d1a5f79a0 
Updated license

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bhepp 3:bfc4928cd279 1 //
bhepp 3:bfc4928cd279 2 // Simple message protocol for UWB.
bhepp 3:bfc4928cd279 3 //
bhepp 3:bfc4928cd279 4 // Created by Benjamin Hepp on 02.04.16.
bhepp 3:bfc4928cd279 5 // Copyright (c) 2016 Benjamin Hepp. All rights reserved.
bhepp 3:bfc4928cd279 6 //
bhepp 3:bfc4928cd279 7
bhepp 2:502d1a5f79a0 8 #pragma once
bhepp 2:502d1a5f79a0 9
bhepp 2:502d1a5f79a0 10 #include <string.h>
bhepp 2:502d1a5f79a0 11 #include <vector>
bhepp 2:502d1a5f79a0 12
bhepp 2:502d1a5f79a0 13 #include <ait_link/ait_link.h>
bhepp 2:502d1a5f79a0 14
bhepp 2:502d1a5f79a0 15 namespace ait {
bhepp 2:502d1a5f79a0 16
bhepp 2:502d1a5f79a0 17 class UWBMessageBody {
bhepp 2:502d1a5f79a0 18 public:
bhepp 2:502d1a5f79a0 19 virtual int getSize() const = 0;
bhepp 2:502d1a5f79a0 20 virtual void buildMessage(uint8_t* buffer) const = 0;
bhepp 2:502d1a5f79a0 21 virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) = 0;
bhepp 2:502d1a5f79a0 22 };
bhepp 2:502d1a5f79a0 23
bhepp 2:502d1a5f79a0 24 class UWBMessageString : public UWBMessageBody {
bhepp 2:502d1a5f79a0 25 public:
bhepp 2:502d1a5f79a0 26 UWBMessageString()
bhepp 2:502d1a5f79a0 27 : str_length_(-1), str_(NULL) {
bhepp 2:502d1a5f79a0 28 }
bhepp 2:502d1a5f79a0 29
bhepp 2:502d1a5f79a0 30 UWBMessageString(const char* str)
bhepp 2:502d1a5f79a0 31 : str_(str) {
bhepp 2:502d1a5f79a0 32 str_length_ = strlen(str);
bhepp 2:502d1a5f79a0 33 }
bhepp 2:502d1a5f79a0 34
bhepp 2:502d1a5f79a0 35 virtual int getSize() const {
bhepp 2:502d1a5f79a0 36 return str_length_ + 1;
bhepp 2:502d1a5f79a0 37 }
bhepp 2:502d1a5f79a0 38
bhepp 2:502d1a5f79a0 39 virtual void buildMessage(uint8_t* buffer) const {
bhepp 2:502d1a5f79a0 40 memcpy(buffer, str_, getSize());
bhepp 2:502d1a5f79a0 41 }
bhepp 2:502d1a5f79a0 42
bhepp 2:502d1a5f79a0 43 virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) {
bhepp 2:502d1a5f79a0 44 #ifdef __MBED__
bhepp 2:502d1a5f79a0 45 // TODO: This can lead to a buffer overrun (something like strnlen should be used)
bhepp 2:502d1a5f79a0 46 str_length_ = strlen(reinterpret_cast<const char*>(buffer));
bhepp 2:502d1a5f79a0 47 #else
bhepp 2:502d1a5f79a0 48 str_length_ = strnlen(reinterpret_cast<const char*>(buffer), buffer_size);
bhepp 2:502d1a5f79a0 49 if (str_length_ >= buffer_size) {
bhepp 2:502d1a5f79a0 50 return false;
bhepp 2:502d1a5f79a0 51 }
bhepp 2:502d1a5f79a0 52 #endif
bhepp 2:502d1a5f79a0 53 str_ = reinterpret_cast<const char*>(buffer);
bhepp 2:502d1a5f79a0 54 return true;
bhepp 2:502d1a5f79a0 55 }
bhepp 2:502d1a5f79a0 56
bhepp 2:502d1a5f79a0 57 int getStringLength() const {
bhepp 2:502d1a5f79a0 58 return str_length_;
bhepp 2:502d1a5f79a0 59 }
bhepp 2:502d1a5f79a0 60
bhepp 2:502d1a5f79a0 61 const char* getString() const {
bhepp 2:502d1a5f79a0 62 return str_;
bhepp 2:502d1a5f79a0 63 }
bhepp 2:502d1a5f79a0 64
bhepp 2:502d1a5f79a0 65 private:
bhepp 2:502d1a5f79a0 66 int str_length_;
bhepp 2:502d1a5f79a0 67 const char* str_;
bhepp 2:502d1a5f79a0 68 };
bhepp 2:502d1a5f79a0 69
bhepp 2:502d1a5f79a0 70 struct UWBMessageMultiRange : public UWBMessageBody {
bhepp 2:502d1a5f79a0 71 UWBMessageMultiRange()
bhepp 2:502d1a5f79a0 72 : address(0), remote_address(0) {
bhepp 2:502d1a5f79a0 73 }
bhepp 2:502d1a5f79a0 74
bhepp 2:502d1a5f79a0 75 UWBMessageMultiRange(uint8_t address, uint8_t remote_address)
bhepp 2:502d1a5f79a0 76 : address(address), remote_address(remote_address) {
bhepp 2:502d1a5f79a0 77 }
bhepp 2:502d1a5f79a0 78
bhepp 2:502d1a5f79a0 79 void clearMeasurements() {
bhepp 2:502d1a5f79a0 80 timestamp_master_request_1.clear();
bhepp 2:502d1a5f79a0 81 timestamp_slave_reply.clear();
bhepp 2:502d1a5f79a0 82 timestamp_master_request_2.clear();
bhepp 2:502d1a5f79a0 83 }
bhepp 2:502d1a5f79a0 84
bhepp 2:502d1a5f79a0 85 void addModuleMeasurement(uint64_t timestamp_master_request_1, uint64_t timestamp_slave_reply, uint64_t timestamp_master_request_2) {
bhepp 2:502d1a5f79a0 86 this->timestamp_master_request_1.push_back(timestamp_master_request_1);
bhepp 2:502d1a5f79a0 87 this->timestamp_slave_reply.push_back(timestamp_slave_reply);
bhepp 2:502d1a5f79a0 88 this->timestamp_master_request_2.push_back(timestamp_master_request_2);
bhepp 2:502d1a5f79a0 89 }
bhepp 2:502d1a5f79a0 90
bhepp 2:502d1a5f79a0 91 void setSlaveMeasurement(uint64_t timestamp_master_request_1_recv, uint64_t timestamp_slave_reply_send, uint64_t timestamp_master_request_2_recv) {
bhepp 2:502d1a5f79a0 92 this->timestamp_master_request_1_recv = timestamp_master_request_1_recv;
bhepp 2:502d1a5f79a0 93 this->timestamp_slave_reply_send = timestamp_slave_reply_send;
bhepp 2:502d1a5f79a0 94 this->timestamp_master_request_2_recv = timestamp_master_request_2_recv;
bhepp 2:502d1a5f79a0 95 }
bhepp 2:502d1a5f79a0 96
bhepp 2:502d1a5f79a0 97 int getNumOfModules() const {
bhepp 2:502d1a5f79a0 98 return timestamp_master_request_1.size();
bhepp 2:502d1a5f79a0 99 }
bhepp 2:502d1a5f79a0 100
bhepp 2:502d1a5f79a0 101 virtual int getSize() const {
bhepp 2:502d1a5f79a0 102 return sizeof(uint8_t) + sizeof(address) + sizeof(remote_address) + (getNumOfModules() + 1) * 3 * sizeof(uint64_t);
bhepp 2:502d1a5f79a0 103 }
bhepp 2:502d1a5f79a0 104
bhepp 2:502d1a5f79a0 105 virtual void buildMessage(uint8_t* buffer) const {
bhepp 2:502d1a5f79a0 106 // Number of modules and addresses
bhepp 2:502d1a5f79a0 107 uint8_t num_of_modules = static_cast<uint8_t>(getNumOfModules());
bhepp 2:502d1a5f79a0 108 *buffer = num_of_modules;
bhepp 2:502d1a5f79a0 109 ++buffer;
bhepp 2:502d1a5f79a0 110 *buffer = address;
bhepp 2:502d1a5f79a0 111 ++buffer;
bhepp 2:502d1a5f79a0 112 *buffer = remote_address;
bhepp 2:502d1a5f79a0 113 ++buffer;
bhepp 2:502d1a5f79a0 114 // Slave timestamps
bhepp 2:502d1a5f79a0 115 uint64_t* buffer_64 = reinterpret_cast<uint64_t*>(buffer);
bhepp 2:502d1a5f79a0 116 *buffer_64 = timestamp_master_request_1_recv;
bhepp 2:502d1a5f79a0 117 ++buffer_64;
bhepp 2:502d1a5f79a0 118 *buffer_64 = timestamp_slave_reply_send;
bhepp 2:502d1a5f79a0 119 ++buffer_64;
bhepp 2:502d1a5f79a0 120 *buffer_64 = timestamp_master_request_2_recv;
bhepp 2:502d1a5f79a0 121 ++buffer_64;
bhepp 2:502d1a5f79a0 122 // Master and listener timestamps
bhepp 2:502d1a5f79a0 123 for (int i = 0; i < num_of_modules; ++i) {
bhepp 2:502d1a5f79a0 124 *buffer_64 = timestamp_master_request_1[i];
bhepp 2:502d1a5f79a0 125 ++buffer_64;
bhepp 2:502d1a5f79a0 126 *buffer_64 = timestamp_slave_reply[i];
bhepp 2:502d1a5f79a0 127 ++buffer_64;
bhepp 2:502d1a5f79a0 128 *buffer_64 = timestamp_master_request_2[i];
bhepp 2:502d1a5f79a0 129 ++buffer_64;
bhepp 2:502d1a5f79a0 130 }
bhepp 2:502d1a5f79a0 131 }
bhepp 2:502d1a5f79a0 132
bhepp 2:502d1a5f79a0 133 virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) {
bhepp 2:502d1a5f79a0 134 if (buffer_size < sizeof(uint8_t) + sizeof(address) + sizeof(remote_address)) {
bhepp 2:502d1a5f79a0 135 return false;
bhepp 2:502d1a5f79a0 136 }
bhepp 2:502d1a5f79a0 137
bhepp 2:502d1a5f79a0 138 clearMeasurements();
bhepp 2:502d1a5f79a0 139
bhepp 2:502d1a5f79a0 140 // Number of modules and addresses
bhepp 2:502d1a5f79a0 141 int num_of_modules = *buffer;
bhepp 2:502d1a5f79a0 142 ++buffer;
bhepp 2:502d1a5f79a0 143 address = *buffer;
bhepp 2:502d1a5f79a0 144 ++buffer;
bhepp 2:502d1a5f79a0 145 remote_address = *buffer;
bhepp 2:502d1a5f79a0 146 ++buffer;
bhepp 2:502d1a5f79a0 147 if (buffer_size < getSize()) {
bhepp 2:502d1a5f79a0 148 return false;
bhepp 2:502d1a5f79a0 149 }
bhepp 2:502d1a5f79a0 150 // Slave timestamps
bhepp 2:502d1a5f79a0 151 const uint64_t* buffer_64 = reinterpret_cast<const uint64_t*>(buffer);
bhepp 2:502d1a5f79a0 152 timestamp_master_request_1_recv = *buffer_64;
bhepp 2:502d1a5f79a0 153 ++buffer_64;
bhepp 2:502d1a5f79a0 154 timestamp_slave_reply_send = *buffer_64;
bhepp 2:502d1a5f79a0 155 ++buffer_64;
bhepp 2:502d1a5f79a0 156 timestamp_master_request_2_recv = *buffer_64;
bhepp 2:502d1a5f79a0 157 ++buffer_64;
bhepp 2:502d1a5f79a0 158 // Master and listener timestamps
bhepp 2:502d1a5f79a0 159 for (int i = 0; i < num_of_modules; ++i) {
bhepp 2:502d1a5f79a0 160 timestamp_master_request_1.push_back(*buffer_64);
bhepp 2:502d1a5f79a0 161 ++buffer_64;
bhepp 2:502d1a5f79a0 162 timestamp_slave_reply.push_back(*buffer_64);
bhepp 2:502d1a5f79a0 163 ++buffer_64;
bhepp 2:502d1a5f79a0 164 timestamp_master_request_2.push_back(*buffer_64);
bhepp 2:502d1a5f79a0 165 ++buffer_64;
bhepp 2:502d1a5f79a0 166 }
bhepp 2:502d1a5f79a0 167 return true;
bhepp 2:502d1a5f79a0 168 }
bhepp 2:502d1a5f79a0 169
bhepp 2:502d1a5f79a0 170 uint8_t address;
bhepp 2:502d1a5f79a0 171 uint8_t remote_address;
bhepp 1:9f8c0dbb0fc7 172
bhepp 2:502d1a5f79a0 173 uint64_t timestamp_master_request_1_recv;
bhepp 2:502d1a5f79a0 174 uint64_t timestamp_slave_reply_send;
bhepp 2:502d1a5f79a0 175 uint64_t timestamp_master_request_2_recv;
bhepp 2:502d1a5f79a0 176 std::vector<uint64_t> timestamp_master_request_1;
bhepp 2:502d1a5f79a0 177 std::vector<uint64_t> timestamp_slave_reply;
bhepp 2:502d1a5f79a0 178 std::vector<uint64_t> timestamp_master_request_2;
bhepp 2:502d1a5f79a0 179 };
bhepp 2:502d1a5f79a0 180
bhepp 2:502d1a5f79a0 181 class UWBMessage {
bhepp 2:502d1a5f79a0 182 public:
bhepp 2:502d1a5f79a0 183 const static uint8_t UWB_MESSAGE_TYPE_NOP = 0x00;
bhepp 2:502d1a5f79a0 184 const static uint8_t UWB_MESSAGE_TYPE_STATUS = 0x01;
bhepp 2:502d1a5f79a0 185 const static uint8_t UWB_MESSAGE_TYPE_MULTI_RANGE = 0x02;
bhepp 2:502d1a5f79a0 186
bhepp 2:502d1a5f79a0 187 UWBMessage()
bhepp 2:502d1a5f79a0 188 : type_(UWB_MESSAGE_TYPE_NOP), body_(NULL), part_allocated_(false) {
bhepp 2:502d1a5f79a0 189 }
bhepp 2:502d1a5f79a0 190
bhepp 2:502d1a5f79a0 191 UWBMessage(uint8_t type)
bhepp 2:502d1a5f79a0 192 : type_(type), body_(NULL), part_allocated_(false) {
bhepp 2:502d1a5f79a0 193 }
bhepp 2:502d1a5f79a0 194
bhepp 2:502d1a5f79a0 195 UWBMessage(uint8_t type, const UWBMessageBody* body)
bhepp 2:502d1a5f79a0 196 : type_(type), body_(body), part_allocated_(false) {
bhepp 2:502d1a5f79a0 197 }
bhepp 2:502d1a5f79a0 198
bhepp 2:502d1a5f79a0 199 ~UWBMessage() {
bhepp 2:502d1a5f79a0 200 clearMessageBody();
bhepp 2:502d1a5f79a0 201 }
bhepp 2:502d1a5f79a0 202
bhepp 2:502d1a5f79a0 203 uint8_t getType() const {
bhepp 3:bfc4928cd279 204 return type_;
bhepp 2:502d1a5f79a0 205 }
bhepp 2:502d1a5f79a0 206
bhepp 2:502d1a5f79a0 207 const UWBMessageBody* getMessageBody() const {
bhepp 3:bfc4928cd279 208 return body_;
bhepp 2:502d1a5f79a0 209 }
bhepp 2:502d1a5f79a0 210
bhepp 2:502d1a5f79a0 211 void setMessageBody(const UWBMessageBody* body) {
bhepp 3:bfc4928cd279 212 clearMessageBody();
bhepp 3:bfc4928cd279 213 body_ = body;
bhepp 2:502d1a5f79a0 214 }
bhepp 2:502d1a5f79a0 215
bhepp 2:502d1a5f79a0 216 int getSize() const {
bhepp 3:bfc4928cd279 217 int size = sizeof(type_);
bhepp 3:bfc4928cd279 218 if (body_ != NULL) {
bhepp 3:bfc4928cd279 219 size += body_->getSize();
bhepp 3:bfc4928cd279 220 }
bhepp 3:bfc4928cd279 221 return size;
bhepp 2:502d1a5f79a0 222 }
bhepp 2:502d1a5f79a0 223
bhepp 2:502d1a5f79a0 224 void buildMessage(uint8_t* buffer) const {
bhepp 2:502d1a5f79a0 225 buffer[0] = type_;
bhepp 2:502d1a5f79a0 226 if (body_ != NULL) {
bhepp 2:502d1a5f79a0 227 buffer += sizeof(type_);
bhepp 2:502d1a5f79a0 228 body_->buildMessage(buffer);
bhepp 2:502d1a5f79a0 229 }
bhepp 2:502d1a5f79a0 230 }
bhepp 2:502d1a5f79a0 231
bhepp 2:502d1a5f79a0 232 bool decodeMessage(const uint8_t* buffer, size_t buffer_size) {
bhepp 2:502d1a5f79a0 233 clearMessageBody();
bhepp 2:502d1a5f79a0 234 part_allocated_ = true;
bhepp 2:502d1a5f79a0 235 if (buffer_size < sizeof(type_)) {
bhepp 2:502d1a5f79a0 236 return false;
bhepp 2:502d1a5f79a0 237 }
bhepp 2:502d1a5f79a0 238 type_ = buffer[0];
bhepp 2:502d1a5f79a0 239 buffer += sizeof(type_);
bhepp 2:502d1a5f79a0 240 buffer_size -= sizeof(type_);
bhepp 2:502d1a5f79a0 241 switch (type_) {
bhepp 2:502d1a5f79a0 242 case UWB_MESSAGE_TYPE_NOP: {
bhepp 2:502d1a5f79a0 243 break;
bhepp 2:502d1a5f79a0 244 }
bhepp 2:502d1a5f79a0 245 case UWB_MESSAGE_TYPE_STATUS: {
bhepp 2:502d1a5f79a0 246 UWBMessageString *msg_string = new UWBMessageString();
bhepp 2:502d1a5f79a0 247 if (msg_string->decodeMessage(buffer, buffer_size)) {
bhepp 2:502d1a5f79a0 248 body_ = msg_string;
bhepp 2:502d1a5f79a0 249 } else {
bhepp 2:502d1a5f79a0 250 delete msg_string;
bhepp 2:502d1a5f79a0 251 return false;
bhepp 2:502d1a5f79a0 252 }
bhepp 2:502d1a5f79a0 253 break;
bhepp 2:502d1a5f79a0 254 }
bhepp 2:502d1a5f79a0 255 case UWB_MESSAGE_TYPE_MULTI_RANGE: {
bhepp 2:502d1a5f79a0 256 UWBMessageMultiRange *msg_multi_range = new UWBMessageMultiRange();
bhepp 2:502d1a5f79a0 257 if (msg_multi_range->decodeMessage(buffer, buffer_size)) {
bhepp 2:502d1a5f79a0 258 body_ = msg_multi_range;
bhepp 2:502d1a5f79a0 259 } else {
bhepp 2:502d1a5f79a0 260 delete msg_multi_range;
bhepp 2:502d1a5f79a0 261 return false;
bhepp 2:502d1a5f79a0 262 }
bhepp 2:502d1a5f79a0 263 break;
bhepp 2:502d1a5f79a0 264 }
bhepp 2:502d1a5f79a0 265 default:
bhepp 2:502d1a5f79a0 266 return false;
bhepp 2:502d1a5f79a0 267 }
bhepp 2:502d1a5f79a0 268 return true;
bhepp 2:502d1a5f79a0 269 }
bhepp 2:502d1a5f79a0 270
bhepp 2:502d1a5f79a0 271 private:
bhepp 2:502d1a5f79a0 272 void clearMessageBody() {
bhepp 2:502d1a5f79a0 273 if (part_allocated_) {
bhepp 2:502d1a5f79a0 274 delete body_;
bhepp 2:502d1a5f79a0 275 part_allocated_ = false;
bhepp 2:502d1a5f79a0 276 }
bhepp 2:502d1a5f79a0 277 body_ = NULL;
bhepp 2:502d1a5f79a0 278 }
bhepp 2:502d1a5f79a0 279
bhepp 2:502d1a5f79a0 280 uint8_t type_;
bhepp 2:502d1a5f79a0 281 const UWBMessageBody* body_;
bhepp 2:502d1a5f79a0 282 bool part_allocated_;
bhepp 2:502d1a5f79a0 283 };
bhepp 2:502d1a5f79a0 284
bhepp 2:502d1a5f79a0 285 class UWBLink {
bhepp 2:502d1a5f79a0 286 public:
bhepp 2:502d1a5f79a0 287 UWBLink(AITLink* ait_link, int buffer_size = 1024)
bhepp 2:502d1a5f79a0 288 : handle_message_callback_(NULL), callback_user_data_(NULL),
bhepp 2:502d1a5f79a0 289 ait_link_(ait_link), buffer_size_(buffer_size) {
bhepp 2:502d1a5f79a0 290 buffer_ = new uint8_t[buffer_size];
bhepp 2:502d1a5f79a0 291 ait_link_->registerFrameHandler(&UWBLink::handleFrameWrapper, this);
bhepp 2:502d1a5f79a0 292 }
bhepp 2:502d1a5f79a0 293
bhepp 2:502d1a5f79a0 294 virtual ~UWBLink() {
bhepp 2:502d1a5f79a0 295 delete[] buffer_;
bhepp 2:502d1a5f79a0 296 }
bhepp 2:502d1a5f79a0 297
bhepp 2:502d1a5f79a0 298 bool sendMessage(const UWBMessage& msg) {
bhepp 2:502d1a5f79a0 299 int size = msg.getSize();
bhepp 2:502d1a5f79a0 300 if (size > buffer_size_) {
bhepp 2:502d1a5f79a0 301 return false;
bhepp 2:502d1a5f79a0 302 }
bhepp 2:502d1a5f79a0 303 msg.buildMessage(buffer_);
bhepp 2:502d1a5f79a0 304 ait_link_->sendFrame(buffer_, size);
bhepp 2:502d1a5f79a0 305 return true;
bhepp 2:502d1a5f79a0 306 }
bhepp 2:502d1a5f79a0 307
bhepp 2:502d1a5f79a0 308 void registerMessageHandler(void (*callback)(void* user_data, const UWBMessage& msg), void* user_data) {
bhepp 2:502d1a5f79a0 309 handle_message_callback_ = callback;
bhepp 2:502d1a5f79a0 310 callback_user_data_ = user_data;
bhepp 2:502d1a5f79a0 311 }
bhepp 2:502d1a5f79a0 312
bhepp 2:502d1a5f79a0 313 void inputReceivedChar(uint8_t data) {
bhepp 2:502d1a5f79a0 314 ait_link_->inputReceivedChar(data);
bhepp 2:502d1a5f79a0 315 }
bhepp 2:502d1a5f79a0 316
bhepp 2:502d1a5f79a0 317 protected:
bhepp 2:502d1a5f79a0 318 virtual void handleMessage(const UWBMessage& msg) {
bhepp 2:502d1a5f79a0 319 if (handle_message_callback_ != NULL) {
bhepp 2:502d1a5f79a0 320 (*handle_message_callback_)(callback_user_data_, msg);
bhepp 2:502d1a5f79a0 321 }
bhepp 2:502d1a5f79a0 322 }
bhepp 2:502d1a5f79a0 323
bhepp 2:502d1a5f79a0 324 void (*handle_message_callback_)(void* user_data, const UWBMessage& msg);
bhepp 2:502d1a5f79a0 325 void* callback_user_data_;
bhepp 2:502d1a5f79a0 326
bhepp 2:502d1a5f79a0 327 private:
bhepp 2:502d1a5f79a0 328 void handleFrame(const uint8_t* frame_buffer, size_t frame_length) {
bhepp 2:502d1a5f79a0 329 UWBMessage msg;
bhepp 2:502d1a5f79a0 330 if (msg.decodeMessage(frame_buffer, frame_length)) {
bhepp 2:502d1a5f79a0 331 handleMessage(msg);
bhepp 2:502d1a5f79a0 332 } else {
bhepp 2:502d1a5f79a0 333 fprintf(stderr, "Failed to decode UWB message");
bhepp 2:502d1a5f79a0 334 }
bhepp 2:502d1a5f79a0 335 }
bhepp 2:502d1a5f79a0 336
bhepp 2:502d1a5f79a0 337 static void handleFrameWrapper(void* user_data, const uint8_t* frame_buffer, size_t frame_length) {
bhepp 2:502d1a5f79a0 338 UWBLink* uwb_link = reinterpret_cast<UWBLink*>(user_data);
bhepp 2:502d1a5f79a0 339 uwb_link->handleFrame(frame_buffer, frame_length);
bhepp 2:502d1a5f79a0 340 }
bhepp 2:502d1a5f79a0 341
bhepp 2:502d1a5f79a0 342 AITLink* ait_link_;
bhepp 2:502d1a5f79a0 343 uint8_t* buffer_;
bhepp 2:502d1a5f79a0 344 int buffer_size_;
bhepp 2:502d1a5f79a0 345 };
bhepp 2:502d1a5f79a0 346
bhepp 2:502d1a5f79a0 347 }