Remi Beges / telemetry Featured

First class data visualization and communication library with embedded devices. Code is maintained at github.com/Overdrivr/Telemetry

Dependents:   telemetry_car_demo telemetry_demo_FRDM-TFC telemetry_example_01 telemetry_indexed_data_demo ... more

telemetry.cpp@0:3a30eba5d8f7, 2016-01-27 (annotated)

Committer:
Overdrivr
Date:
Wed Jan 27 17:39:36 2016 +0000
Revision:
0:3a30eba5d8f7
Port of github.com/Overdrivr/Telemetry to mbed. Current commit corresponds to release tagged 1.0.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Overdrivr 0:3a30eba5d8f7 1 #include "telemetry.hpp"
Overdrivr 0:3a30eba5d8f7 2 #include "framing.hpp"
Overdrivr 0:3a30eba5d8f7 3 #include "crc16.hpp"
Overdrivr 0:3a30eba5d8f7 4 #include "string.h"
Overdrivr 0:3a30eba5d8f7 5
Overdrivr 0:3a30eba5d8f7 6 static TM_state * statePtr;
Overdrivr 0:3a30eba5d8f7 7 static TM_transport * transportPtr;
Overdrivr 0:3a30eba5d8f7 8 static uint8_t incomingBuffer[INCOMING_BUFFER_SIZE];
Overdrivr 0:3a30eba5d8f7 9 static uint8_t outgoingBuffer[OUTGOING_BUFFER_SIZE];
Overdrivr 0:3a30eba5d8f7 10 static char topicBuffer[TOPIC_BUFFER_SIZE];
Overdrivr 0:3a30eba5d8f7 11
Overdrivr 0:3a30eba5d8f7 12 static void (*userCallback)(TM_state * s, TM_msg * m);
Overdrivr 0:3a30eba5d8f7 13
Overdrivr 0:3a30eba5d8f7 14 uint16_t header(TM_type type);
Overdrivr 0:3a30eba5d8f7 15 uint16_t topic(const char * topic, uint16_t crc);
Overdrivr 0:3a30eba5d8f7 16 uint16_t payload(const void * payload, uint32_t size, uint16_t crc);
Overdrivr 0:3a30eba5d8f7 17 void frame(const char * t, TM_type type, const void * data, uint32_t datasize);
Overdrivr 0:3a30eba5d8f7 18 void send(void * buf, uint32_t size);
Overdrivr 0:3a30eba5d8f7 19 void on_incoming_frame(uint8_t * storage, uint32_t size);
Overdrivr 0:3a30eba5d8f7 20 void on_incoming_error(int32_t errCode);
Overdrivr 0:3a30eba5d8f7 21 void emptyCallback(TM_state * s, TM_msg * m);
Overdrivr 0:3a30eba5d8f7 22
Overdrivr 0:3a30eba5d8f7 23 void init_telemetry(TM_state* s, TM_transport * t)
Overdrivr 0:3a30eba5d8f7 24 {
Overdrivr 0:3a30eba5d8f7 25 statePtr = s;
Overdrivr 0:3a30eba5d8f7 26 transportPtr = t;
Overdrivr 0:3a30eba5d8f7 27 userCallback = emptyCallback;
Overdrivr 0:3a30eba5d8f7 28
Overdrivr 0:3a30eba5d8f7 29 // Setup framing
Overdrivr 0:3a30eba5d8f7 30 initialize_framing();
Overdrivr 0:3a30eba5d8f7 31 incoming_storage(incomingBuffer,INCOMING_BUFFER_SIZE);
Overdrivr 0:3a30eba5d8f7 32 outgoing_storage(outgoingBuffer, OUTGOING_BUFFER_SIZE);
Overdrivr 0:3a30eba5d8f7 33 set_on_incoming_frame(on_incoming_frame);
Overdrivr 0:3a30eba5d8f7 34 set_on_incoming_error(on_incoming_error);
Overdrivr 0:3a30eba5d8f7 35 }
Overdrivr 0:3a30eba5d8f7 36
Overdrivr 0:3a30eba5d8f7 37 uint32_t emplace(TM_msg* m, char * buf, size_t bufSize)
Overdrivr 0:3a30eba5d8f7 38 {
Overdrivr 0:3a30eba5d8f7 39 if(m->type != TM_string)
Overdrivr 0:3a30eba5d8f7 40 return 0;
Overdrivr 0:3a30eba5d8f7 41
Overdrivr 0:3a30eba5d8f7 42 uint32_t size = m->size;
Overdrivr 0:3a30eba5d8f7 43
Overdrivr 0:3a30eba5d8f7 44 if(bufSize - 1 < size)
Overdrivr 0:3a30eba5d8f7 45 size = bufSize - 1;
Overdrivr 0:3a30eba5d8f7 46
Overdrivr 0:3a30eba5d8f7 47 strncpy(buf, (char*)(m->buffer), size);
Overdrivr 0:3a30eba5d8f7 48 buf[size] = '\0';
Overdrivr 0:3a30eba5d8f7 49
Overdrivr 0:3a30eba5d8f7 50 return 1;
Overdrivr 0:3a30eba5d8f7 51 }
Overdrivr 0:3a30eba5d8f7 52
Overdrivr 0:3a30eba5d8f7 53 uint32_t emplace_u8(TM_msg* m, uint8_t* dst)
Overdrivr 0:3a30eba5d8f7 54 {
Overdrivr 0:3a30eba5d8f7 55 if(m->type != TM_uint8)
Overdrivr 0:3a30eba5d8f7 56 return 0;
Overdrivr 0:3a30eba5d8f7 57
Overdrivr 0:3a30eba5d8f7 58 memcpy(dst,m->buffer,1);
Overdrivr 0:3a30eba5d8f7 59 return 1;
Overdrivr 0:3a30eba5d8f7 60 }
Overdrivr 0:3a30eba5d8f7 61
Overdrivr 0:3a30eba5d8f7 62 uint32_t emplace_u16(TM_msg* m, uint16_t* dst)
Overdrivr 0:3a30eba5d8f7 63 {
Overdrivr 0:3a30eba5d8f7 64 if(m->type != TM_uint16)
Overdrivr 0:3a30eba5d8f7 65 return 0;
Overdrivr 0:3a30eba5d8f7 66
Overdrivr 0:3a30eba5d8f7 67 memcpy(dst,m->buffer,2);
Overdrivr 0:3a30eba5d8f7 68 return 1;
Overdrivr 0:3a30eba5d8f7 69 }
Overdrivr 0:3a30eba5d8f7 70
Overdrivr 0:3a30eba5d8f7 71 uint32_t emplace_u32(TM_msg* m, uint32_t* dst)
Overdrivr 0:3a30eba5d8f7 72 {
Overdrivr 0:3a30eba5d8f7 73 if(m->type != TM_uint32)
Overdrivr 0:3a30eba5d8f7 74 return 0;
Overdrivr 0:3a30eba5d8f7 75
Overdrivr 0:3a30eba5d8f7 76 memcpy(dst,m->buffer,4);
Overdrivr 0:3a30eba5d8f7 77 return 1;
Overdrivr 0:3a30eba5d8f7 78 }
Overdrivr 0:3a30eba5d8f7 79
Overdrivr 0:3a30eba5d8f7 80 uint32_t emplace_i8(TM_msg* m, int8_t* dst)
Overdrivr 0:3a30eba5d8f7 81 {
Overdrivr 0:3a30eba5d8f7 82 if(m->type != TM_int8)
Overdrivr 0:3a30eba5d8f7 83 return 0;
Overdrivr 0:3a30eba5d8f7 84
Overdrivr 0:3a30eba5d8f7 85 memcpy(dst,m->buffer,1);
Overdrivr 0:3a30eba5d8f7 86 return 1;
Overdrivr 0:3a30eba5d8f7 87 }
Overdrivr 0:3a30eba5d8f7 88
Overdrivr 0:3a30eba5d8f7 89 uint32_t emplace_i16(TM_msg* m, int16_t* dst)
Overdrivr 0:3a30eba5d8f7 90 {
Overdrivr 0:3a30eba5d8f7 91 if(m->type != TM_int16)
Overdrivr 0:3a30eba5d8f7 92 return 0;
Overdrivr 0:3a30eba5d8f7 93
Overdrivr 0:3a30eba5d8f7 94 memcpy(dst,m->buffer,2);
Overdrivr 0:3a30eba5d8f7 95 return 1;
Overdrivr 0:3a30eba5d8f7 96 }
Overdrivr 0:3a30eba5d8f7 97
Overdrivr 0:3a30eba5d8f7 98 uint32_t emplace_i32(TM_msg* m, int32_t* dst)
Overdrivr 0:3a30eba5d8f7 99 {
Overdrivr 0:3a30eba5d8f7 100 if(m->type != TM_int32)
Overdrivr 0:3a30eba5d8f7 101 return 0;
Overdrivr 0:3a30eba5d8f7 102
Overdrivr 0:3a30eba5d8f7 103 memcpy(dst,m->buffer,4);
Overdrivr 0:3a30eba5d8f7 104 return 1;
Overdrivr 0:3a30eba5d8f7 105 }
Overdrivr 0:3a30eba5d8f7 106
Overdrivr 0:3a30eba5d8f7 107 uint32_t emplace_f32(TM_msg* m, float* dst)
Overdrivr 0:3a30eba5d8f7 108 {
Overdrivr 0:3a30eba5d8f7 109 if(m->type != TM_float32)
Overdrivr 0:3a30eba5d8f7 110 return 0;
Overdrivr 0:3a30eba5d8f7 111
Overdrivr 0:3a30eba5d8f7 112 memcpy(dst,m->buffer,4);
Overdrivr 0:3a30eba5d8f7 113 return 1;
Overdrivr 0:3a30eba5d8f7 114 }
Overdrivr 0:3a30eba5d8f7 115
Overdrivr 0:3a30eba5d8f7 116 void publish(const char * t, char * msg)
Overdrivr 0:3a30eba5d8f7 117 {
Overdrivr 0:3a30eba5d8f7 118 frame(t,TM_string,msg,strlen(msg));
Overdrivr 0:3a30eba5d8f7 119 }
Overdrivr 0:3a30eba5d8f7 120
Overdrivr 0:3a30eba5d8f7 121 void publish_u8(const char * t, uint8_t msg)
Overdrivr 0:3a30eba5d8f7 122 {
Overdrivr 0:3a30eba5d8f7 123 void * ptr = (void *)(&msg);
Overdrivr 0:3a30eba5d8f7 124 frame(t,TM_uint8,ptr,1);
Overdrivr 0:3a30eba5d8f7 125 }
Overdrivr 0:3a30eba5d8f7 126
Overdrivr 0:3a30eba5d8f7 127 void publish_u16(const char * t, uint16_t msg)
Overdrivr 0:3a30eba5d8f7 128 {
Overdrivr 0:3a30eba5d8f7 129 void * ptr = (void *)(&msg);
Overdrivr 0:3a30eba5d8f7 130 frame(t,TM_uint16,ptr,2);
Overdrivr 0:3a30eba5d8f7 131 }
Overdrivr 0:3a30eba5d8f7 132
Overdrivr 0:3a30eba5d8f7 133 void publish_u32(const char * t, uint32_t msg)
Overdrivr 0:3a30eba5d8f7 134 {
Overdrivr 0:3a30eba5d8f7 135 void * ptr = (void *)(&msg);
Overdrivr 0:3a30eba5d8f7 136 frame(t,TM_uint32,ptr,4);
Overdrivr 0:3a30eba5d8f7 137 }
Overdrivr 0:3a30eba5d8f7 138
Overdrivr 0:3a30eba5d8f7 139 void publish_i8(const char * t, int8_t msg)
Overdrivr 0:3a30eba5d8f7 140 {
Overdrivr 0:3a30eba5d8f7 141 void * ptr = (void *)(&msg);
Overdrivr 0:3a30eba5d8f7 142 frame(t,TM_int8,ptr,1);
Overdrivr 0:3a30eba5d8f7 143 }
Overdrivr 0:3a30eba5d8f7 144
Overdrivr 0:3a30eba5d8f7 145 void publish_i16(const char * t, int16_t msg)
Overdrivr 0:3a30eba5d8f7 146 {
Overdrivr 0:3a30eba5d8f7 147 void * ptr = (void *)(&msg);
Overdrivr 0:3a30eba5d8f7 148 frame(t,TM_int16,ptr,2);
Overdrivr 0:3a30eba5d8f7 149 }
Overdrivr 0:3a30eba5d8f7 150
Overdrivr 0:3a30eba5d8f7 151 void publish_i32(const char * t, int32_t msg)
Overdrivr 0:3a30eba5d8f7 152 {
Overdrivr 0:3a30eba5d8f7 153 void * ptr = (void *)(&msg);
Overdrivr 0:3a30eba5d8f7 154 frame(t,TM_int32,ptr,4);
Overdrivr 0:3a30eba5d8f7 155 }
Overdrivr 0:3a30eba5d8f7 156
Overdrivr 0:3a30eba5d8f7 157 void publish_f32(const char * t, float msg)
Overdrivr 0:3a30eba5d8f7 158 {
Overdrivr 0:3a30eba5d8f7 159 void * ptr = (void *)(&msg);
Overdrivr 0:3a30eba5d8f7 160 frame(t,TM_float32,ptr,4);
Overdrivr 0:3a30eba5d8f7 161 }
Overdrivr 0:3a30eba5d8f7 162
Overdrivr 0:3a30eba5d8f7 163 void subscribe(void (*callback)(TM_state* s, TM_msg* m))
Overdrivr 0:3a30eba5d8f7 164 {
Overdrivr 0:3a30eba5d8f7 165 userCallback = callback;
Overdrivr 0:3a30eba5d8f7 166 }
Overdrivr 0:3a30eba5d8f7 167
Overdrivr 0:3a30eba5d8f7 168 void update_telemetry(float elapsedTime)
Overdrivr 0:3a30eba5d8f7 169 {
Overdrivr 0:3a30eba5d8f7 170 uint32_t amount = transportPtr->readable();
Overdrivr 0:3a30eba5d8f7 171 for(uint32_t i = 0 ; i < amount ; i++)
Overdrivr 0:3a30eba5d8f7 172 {
Overdrivr 0:3a30eba5d8f7 173 uint8_t c;
Overdrivr 0:3a30eba5d8f7 174 transportPtr->read(&c,1);
Overdrivr 0:3a30eba5d8f7 175 feed(c);
Overdrivr 0:3a30eba5d8f7 176 }
Overdrivr 0:3a30eba5d8f7 177 }
Overdrivr 0:3a30eba5d8f7 178
Overdrivr 0:3a30eba5d8f7 179 uint16_t header(TM_type type)
Overdrivr 0:3a30eba5d8f7 180 {
Overdrivr 0:3a30eba5d8f7 181 // header data
Overdrivr 0:3a30eba5d8f7 182 uint16_t h = type;
Overdrivr 0:3a30eba5d8f7 183 uint8_t * ptr = (uint8_t*)(&h);
Overdrivr 0:3a30eba5d8f7 184
Overdrivr 0:3a30eba5d8f7 185 // add data to frame
Overdrivr 0:3a30eba5d8f7 186 append2(h);
Overdrivr 0:3a30eba5d8f7 187
Overdrivr 0:3a30eba5d8f7 188 // compute crc and return it
Overdrivr 0:3a30eba5d8f7 189 return crc16(ptr, 2);
Overdrivr 0:3a30eba5d8f7 190 }
Overdrivr 0:3a30eba5d8f7 191
Overdrivr 0:3a30eba5d8f7 192 uint16_t topic(const char * t, uint16_t crc)
Overdrivr 0:3a30eba5d8f7 193 {
Overdrivr 0:3a30eba5d8f7 194 const uint8_t * ptr = (uint8_t*)t;
Overdrivr 0:3a30eba5d8f7 195 for(uint32_t i = 0 ; i < strlen(t) ; i++)
Overdrivr 0:3a30eba5d8f7 196 {
Overdrivr 0:3a30eba5d8f7 197 // TODO : Replace with Huffman compression
Overdrivr 0:3a30eba5d8f7 198 append(ptr[i]);
Overdrivr 0:3a30eba5d8f7 199 crc = crc16_recursive(ptr[i], crc);
Overdrivr 0:3a30eba5d8f7 200 }
Overdrivr 0:3a30eba5d8f7 201 // Add NULL character
Overdrivr 0:3a30eba5d8f7 202 append(0);
Overdrivr 0:3a30eba5d8f7 203 return crc16_recursive(0,crc);
Overdrivr 0:3a30eba5d8f7 204 }
Overdrivr 0:3a30eba5d8f7 205
Overdrivr 0:3a30eba5d8f7 206 uint16_t payload(const void * p, uint32_t size, uint16_t crc)
Overdrivr 0:3a30eba5d8f7 207 {
Overdrivr 0:3a30eba5d8f7 208 const uint8_t * ptr = (uint8_t*)p;
Overdrivr 0:3a30eba5d8f7 209 for(uint32_t i = 0 ; i < size ; i++)
Overdrivr 0:3a30eba5d8f7 210 {
Overdrivr 0:3a30eba5d8f7 211 append(ptr[i]);
Overdrivr 0:3a30eba5d8f7 212 crc = crc16_recursive(ptr[i], crc);
Overdrivr 0:3a30eba5d8f7 213 }
Overdrivr 0:3a30eba5d8f7 214 return crc;
Overdrivr 0:3a30eba5d8f7 215 }
Overdrivr 0:3a30eba5d8f7 216
Overdrivr 0:3a30eba5d8f7 217 void frame(const char * t, TM_type type, const void * data, uint32_t datasize)
Overdrivr 0:3a30eba5d8f7 218 {
Overdrivr 0:3a30eba5d8f7 219 // start new frame
Overdrivr 0:3a30eba5d8f7 220 begin();
Overdrivr 0:3a30eba5d8f7 221
Overdrivr 0:3a30eba5d8f7 222 // header
Overdrivr 0:3a30eba5d8f7 223 uint16_t crc = header(type);
Overdrivr 0:3a30eba5d8f7 224
Overdrivr 0:3a30eba5d8f7 225 // topic
Overdrivr 0:3a30eba5d8f7 226 crc = topic(t, crc);
Overdrivr 0:3a30eba5d8f7 227
Overdrivr 0:3a30eba5d8f7 228 // payload
Overdrivr 0:3a30eba5d8f7 229 crc = payload(data, datasize, crc);
Overdrivr 0:3a30eba5d8f7 230
Overdrivr 0:3a30eba5d8f7 231 // crc
Overdrivr 0:3a30eba5d8f7 232 append2(crc);
Overdrivr 0:3a30eba5d8f7 233
Overdrivr 0:3a30eba5d8f7 234 // complete frame
Overdrivr 0:3a30eba5d8f7 235 uint32_t bytesAmount = end();
Overdrivr 0:3a30eba5d8f7 236
Overdrivr 0:3a30eba5d8f7 237 // send data
Overdrivr 0:3a30eba5d8f7 238 send(outgoingBuffer, bytesAmount);
Overdrivr 0:3a30eba5d8f7 239 }
Overdrivr 0:3a30eba5d8f7 240
Overdrivr 0:3a30eba5d8f7 241 void send(void * buf, uint32_t size)
Overdrivr 0:3a30eba5d8f7 242 {
Overdrivr 0:3a30eba5d8f7 243 if(transportPtr->writeable() && size > 0)
Overdrivr 0:3a30eba5d8f7 244 {
Overdrivr 0:3a30eba5d8f7 245 transportPtr->write(outgoingBuffer, size);
Overdrivr 0:3a30eba5d8f7 246 }
Overdrivr 0:3a30eba5d8f7 247 }
Overdrivr 0:3a30eba5d8f7 248
Overdrivr 0:3a30eba5d8f7 249 void on_incoming_frame(uint8_t * storage, uint32_t size)
Overdrivr 0:3a30eba5d8f7 250 {
Overdrivr 0:3a30eba5d8f7 251 if(size < 2)
Overdrivr 0:3a30eba5d8f7 252 return;
Overdrivr 0:3a30eba5d8f7 253 // Read header
Overdrivr 0:3a30eba5d8f7 254 uint16_t head;
Overdrivr 0:3a30eba5d8f7 255 uint8_t * ptr;
Overdrivr 0:3a30eba5d8f7 256 ptr = (uint8_t*)(&head);
Overdrivr 0:3a30eba5d8f7 257 memcpy(ptr,storage,2);
Overdrivr 0:3a30eba5d8f7 258
Overdrivr 0:3a30eba5d8f7 259 // Read topic
Overdrivr 0:3a30eba5d8f7 260 uint32_t cursor = 2;
Overdrivr 0:3a30eba5d8f7 261 uint32_t topicSize = 0;
Overdrivr 0:3a30eba5d8f7 262 while(cursor < size)
Overdrivr 0:3a30eba5d8f7 263 {
Overdrivr 0:3a30eba5d8f7 264 if(storage[cursor] == 0)
Overdrivr 0:3a30eba5d8f7 265 break;
Overdrivr 0:3a30eba5d8f7 266 topicSize++;
Overdrivr 0:3a30eba5d8f7 267 cursor++;
Overdrivr 0:3a30eba5d8f7 268 }
Overdrivr 0:3a30eba5d8f7 269
Overdrivr 0:3a30eba5d8f7 270 if(topicSize == 0)
Overdrivr 0:3a30eba5d8f7 271 return;
Overdrivr 0:3a30eba5d8f7 272
Overdrivr 0:3a30eba5d8f7 273 // payload = total - header - topic - /0 - crc
Overdrivr 0:3a30eba5d8f7 274 int32_t payloadSize = size - 2 - topicSize - 1 - 2;
Overdrivr 0:3a30eba5d8f7 275
Overdrivr 0:3a30eba5d8f7 276 if(payloadSize <= 0)
Overdrivr 0:3a30eba5d8f7 277 return;
Overdrivr 0:3a30eba5d8f7 278
Overdrivr 0:3a30eba5d8f7 279 // Check crc
Overdrivr 0:3a30eba5d8f7 280 uint16_t expected_crc = crc16(storage, size-2);
Overdrivr 0:3a30eba5d8f7 281 uint16_t rcv_crc;
Overdrivr 0:3a30eba5d8f7 282 ptr = (uint8_t*)(&rcv_crc);
Overdrivr 0:3a30eba5d8f7 283 memcpy(ptr,storage+size-2,2);
Overdrivr 0:3a30eba5d8f7 284
Overdrivr 0:3a30eba5d8f7 285 if(expected_crc != rcv_crc)
Overdrivr 0:3a30eba5d8f7 286 return;
Overdrivr 0:3a30eba5d8f7 287
Overdrivr 0:3a30eba5d8f7 288 // Store topic
Overdrivr 0:3a30eba5d8f7 289 char * t = (char*)(storage);
Overdrivr 0:3a30eba5d8f7 290 strcpy(topicBuffer, t + 2);
Overdrivr 0:3a30eba5d8f7 291
Overdrivr 0:3a30eba5d8f7 292 // ptr to beginning of payload
Overdrivr 0:3a30eba5d8f7 293 ptr = (uint8_t*)(storage) + (uint32_t)(2 + topicSize + 1);
Overdrivr 0:3a30eba5d8f7 294
Overdrivr 0:3a30eba5d8f7 295 TM_msg packet;
Overdrivr 0:3a30eba5d8f7 296 packet.topic = topicBuffer;
Overdrivr 0:3a30eba5d8f7 297 packet.type = (TM_type)head;
Overdrivr 0:3a30eba5d8f7 298 packet.buffer = (void *)(ptr);
Overdrivr 0:3a30eba5d8f7 299 packet.size = (uint32_t)payloadSize;
Overdrivr 0:3a30eba5d8f7 300
Overdrivr 0:3a30eba5d8f7 301 // Call callback
Overdrivr 0:3a30eba5d8f7 302 userCallback(statePtr,&packet);
Overdrivr 0:3a30eba5d8f7 303 }
Overdrivr 0:3a30eba5d8f7 304
Overdrivr 0:3a30eba5d8f7 305 void on_incoming_error(int32_t errCode)
Overdrivr 0:3a30eba5d8f7 306 {
Overdrivr 0:3a30eba5d8f7 307 // TODO : Error management
Overdrivr 0:3a30eba5d8f7 308 }
Overdrivr 0:3a30eba5d8f7 309
Overdrivr 0:3a30eba5d8f7 310 void emptyCallback(TM_state * s, TM_msg * m)
Overdrivr 0:3a30eba5d8f7 311 {
Overdrivr 0:3a30eba5d8f7 312 // Called only if the user forgot to subscribe a callback
Overdrivr 0:3a30eba5d8f7 313 }