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
c_api/telemetry_core.cpp@7:d224bddd5405, 2016-04-12 (annotated)
- Committer:
- Overdrivr
- Date:
- Tue Apr 12 07:40:10 2016 +0000
- Revision:
- 7:d224bddd5405
- Parent:
- 6:f5e1b079bffd
Release 2.0.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Overdrivr | 5:cd94bb58e096 | 1 | #include "telemetry_core.h" |
Overdrivr | 5:cd94bb58e096 | 2 | #include "framing.h" |
Overdrivr | 5:cd94bb58e096 | 3 | #include "crc16.h" |
Overdrivr | 7:d224bddd5405 | 4 | #include "dictionnary.h" |
Overdrivr | 5:cd94bb58e096 | 5 | |
Overdrivr | 5:cd94bb58e096 | 6 | static TM_state * statePtr; |
Overdrivr | 5:cd94bb58e096 | 7 | static TM_transport * transportPtr; |
Overdrivr | 5:cd94bb58e096 | 8 | static uint8_t incomingBuffer[INCOMING_BUFFER_SIZE]; |
Overdrivr | 5:cd94bb58e096 | 9 | static uint8_t outgoingBuffer[OUTGOING_BUFFER_SIZE]; |
Overdrivr | 5:cd94bb58e096 | 10 | static char topicBuffer[TOPIC_BUFFER_SIZE]; |
Overdrivr | 5:cd94bb58e096 | 11 | |
Overdrivr | 7:d224bddd5405 | 12 | struct nlist * hashtab[HASHSIZE]; |
Overdrivr | 7:d224bddd5405 | 13 | |
Overdrivr | 7:d224bddd5405 | 14 | |
Overdrivr | 5:cd94bb58e096 | 15 | static void (*userCallback)(TM_state * s, TM_msg * m); |
Overdrivr | 5:cd94bb58e096 | 16 | |
Overdrivr | 5:cd94bb58e096 | 17 | uint16_t header(TM_type type); |
Overdrivr | 5:cd94bb58e096 | 18 | uint16_t topic(const char * topic, uint16_t crc); |
Overdrivr | 5:cd94bb58e096 | 19 | uint16_t payload(const void * payload, uint32_t size, uint16_t crc); |
Overdrivr | 5:cd94bb58e096 | 20 | void frame(const char * t, TM_type type, const void * data, uint32_t datasize); |
Overdrivr | 5:cd94bb58e096 | 21 | void send(void * buf, uint32_t size); |
Overdrivr | 5:cd94bb58e096 | 22 | void on_incoming_frame(uint8_t * storage, uint32_t size); |
Overdrivr | 5:cd94bb58e096 | 23 | void on_incoming_error(int32_t errCode); |
Overdrivr | 5:cd94bb58e096 | 24 | void emptyCallback(TM_state * s, TM_msg * m); |
Overdrivr | 5:cd94bb58e096 | 25 | |
Overdrivr | 5:cd94bb58e096 | 26 | void init_telemetry(TM_transport * t) |
Overdrivr | 5:cd94bb58e096 | 27 | { |
Overdrivr | 5:cd94bb58e096 | 28 | statePtr = NULL; |
Overdrivr | 5:cd94bb58e096 | 29 | transportPtr = t; |
Overdrivr | 5:cd94bb58e096 | 30 | userCallback = emptyCallback; |
Overdrivr | 5:cd94bb58e096 | 31 | |
Overdrivr | 5:cd94bb58e096 | 32 | // Setup framing |
Overdrivr | 5:cd94bb58e096 | 33 | initialize_framing(); |
Overdrivr | 5:cd94bb58e096 | 34 | incoming_storage(incomingBuffer,INCOMING_BUFFER_SIZE); |
Overdrivr | 5:cd94bb58e096 | 35 | outgoing_storage(outgoingBuffer, OUTGOING_BUFFER_SIZE); |
Overdrivr | 5:cd94bb58e096 | 36 | set_on_incoming_frame(on_incoming_frame); |
Overdrivr | 5:cd94bb58e096 | 37 | set_on_incoming_error(on_incoming_error); |
Overdrivr | 7:d224bddd5405 | 38 | |
Overdrivr | 7:d224bddd5405 | 39 | // Setup update dictionnary |
Overdrivr | 7:d224bddd5405 | 40 | init_table(hashtab); |
Overdrivr | 7:d224bddd5405 | 41 | } |
Overdrivr | 7:d224bddd5405 | 42 | |
Overdrivr | 7:d224bddd5405 | 43 | void attach(const char * name, void (*callback)(TM_msg * m)) |
Overdrivr | 7:d224bddd5405 | 44 | { |
Overdrivr | 7:d224bddd5405 | 45 | install(hashtab, name, (void*)(callback), ptr_function); |
Overdrivr | 7:d224bddd5405 | 46 | } |
Overdrivr | 7:d224bddd5405 | 47 | |
Overdrivr | 7:d224bddd5405 | 48 | void attach_f32(const char * name, float * variable) |
Overdrivr | 7:d224bddd5405 | 49 | { |
Overdrivr | 7:d224bddd5405 | 50 | install(hashtab, name, (void*)(variable), ptr_f32); |
Overdrivr | 7:d224bddd5405 | 51 | } |
Overdrivr | 7:d224bddd5405 | 52 | |
Overdrivr | 7:d224bddd5405 | 53 | void attach_u8(const char * name, uint8_t * variable) |
Overdrivr | 7:d224bddd5405 | 54 | { |
Overdrivr | 7:d224bddd5405 | 55 | install(hashtab, name, (void*)(variable), ptr_u8); |
Overdrivr | 7:d224bddd5405 | 56 | } |
Overdrivr | 7:d224bddd5405 | 57 | |
Overdrivr | 7:d224bddd5405 | 58 | void attach_u16(const char * name, uint16_t * variable) |
Overdrivr | 7:d224bddd5405 | 59 | { |
Overdrivr | 7:d224bddd5405 | 60 | install(hashtab, name, (void*)(variable), ptr_u16); |
Overdrivr | 7:d224bddd5405 | 61 | } |
Overdrivr | 7:d224bddd5405 | 62 | |
Overdrivr | 7:d224bddd5405 | 63 | void attach_u32(const char * name, uint32_t * variable) |
Overdrivr | 7:d224bddd5405 | 64 | { |
Overdrivr | 7:d224bddd5405 | 65 | install(hashtab, name, (void*)(variable), ptr_u32); |
Overdrivr | 7:d224bddd5405 | 66 | } |
Overdrivr | 7:d224bddd5405 | 67 | |
Overdrivr | 7:d224bddd5405 | 68 | void attach_i8(const char * name, int8_t * variable) |
Overdrivr | 7:d224bddd5405 | 69 | { |
Overdrivr | 7:d224bddd5405 | 70 | install(hashtab, name, (void*)(variable), ptr_i8); |
Overdrivr | 7:d224bddd5405 | 71 | } |
Overdrivr | 7:d224bddd5405 | 72 | |
Overdrivr | 7:d224bddd5405 | 73 | void attach_i16(const char * name, int16_t * variable) |
Overdrivr | 7:d224bddd5405 | 74 | { |
Overdrivr | 7:d224bddd5405 | 75 | install(hashtab, name, (void*)(variable), ptr_i16); |
Overdrivr | 7:d224bddd5405 | 76 | } |
Overdrivr | 7:d224bddd5405 | 77 | |
Overdrivr | 7:d224bddd5405 | 78 | void attach_i32(const char * name, int32_t * variable) |
Overdrivr | 7:d224bddd5405 | 79 | { |
Overdrivr | 7:d224bddd5405 | 80 | install(hashtab, name, (void*)(variable), ptr_i32); |
Overdrivr | 5:cd94bb58e096 | 81 | } |
Overdrivr | 5:cd94bb58e096 | 82 | |
Overdrivr | 5:cd94bb58e096 | 83 | void publish(const char * t, const char * msg) |
Overdrivr | 5:cd94bb58e096 | 84 | { |
Overdrivr | 5:cd94bb58e096 | 85 | frame(t,TM_string,msg,strlen(msg)); |
Overdrivr | 5:cd94bb58e096 | 86 | } |
Overdrivr | 5:cd94bb58e096 | 87 | |
Overdrivr | 5:cd94bb58e096 | 88 | void publish_u8(const char * t, uint8_t msg) |
Overdrivr | 5:cd94bb58e096 | 89 | { |
Overdrivr | 5:cd94bb58e096 | 90 | void * ptr = (void *)(&msg); |
Overdrivr | 5:cd94bb58e096 | 91 | frame(t,TM_uint8,ptr,1); |
Overdrivr | 5:cd94bb58e096 | 92 | } |
Overdrivr | 5:cd94bb58e096 | 93 | |
Overdrivr | 5:cd94bb58e096 | 94 | void publish_u16(const char * t, uint16_t msg) |
Overdrivr | 5:cd94bb58e096 | 95 | { |
Overdrivr | 5:cd94bb58e096 | 96 | void * ptr = (void *)(&msg); |
Overdrivr | 5:cd94bb58e096 | 97 | frame(t,TM_uint16,ptr,2); |
Overdrivr | 5:cd94bb58e096 | 98 | } |
Overdrivr | 5:cd94bb58e096 | 99 | |
Overdrivr | 5:cd94bb58e096 | 100 | void publish_u32(const char * t, uint32_t msg) |
Overdrivr | 5:cd94bb58e096 | 101 | { |
Overdrivr | 5:cd94bb58e096 | 102 | void * ptr = (void *)(&msg); |
Overdrivr | 5:cd94bb58e096 | 103 | frame(t,TM_uint32,ptr,4); |
Overdrivr | 5:cd94bb58e096 | 104 | } |
Overdrivr | 5:cd94bb58e096 | 105 | |
Overdrivr | 5:cd94bb58e096 | 106 | void publish_i8(const char * t, int8_t msg) |
Overdrivr | 5:cd94bb58e096 | 107 | { |
Overdrivr | 5:cd94bb58e096 | 108 | void * ptr = (void *)(&msg); |
Overdrivr | 5:cd94bb58e096 | 109 | frame(t,TM_int8,ptr,1); |
Overdrivr | 5:cd94bb58e096 | 110 | } |
Overdrivr | 5:cd94bb58e096 | 111 | |
Overdrivr | 5:cd94bb58e096 | 112 | void publish_i16(const char * t, int16_t msg) |
Overdrivr | 5:cd94bb58e096 | 113 | { |
Overdrivr | 5:cd94bb58e096 | 114 | void * ptr = (void *)(&msg); |
Overdrivr | 5:cd94bb58e096 | 115 | frame(t,TM_int16,ptr,2); |
Overdrivr | 5:cd94bb58e096 | 116 | } |
Overdrivr | 5:cd94bb58e096 | 117 | |
Overdrivr | 5:cd94bb58e096 | 118 | void publish_i32(const char * t, int32_t msg) |
Overdrivr | 5:cd94bb58e096 | 119 | { |
Overdrivr | 5:cd94bb58e096 | 120 | void * ptr = (void *)(&msg); |
Overdrivr | 5:cd94bb58e096 | 121 | frame(t,TM_int32,ptr,4); |
Overdrivr | 5:cd94bb58e096 | 122 | } |
Overdrivr | 5:cd94bb58e096 | 123 | |
Overdrivr | 5:cd94bb58e096 | 124 | void publish_f32(const char * t, float msg) |
Overdrivr | 5:cd94bb58e096 | 125 | { |
Overdrivr | 5:cd94bb58e096 | 126 | void * ptr = (void *)(&msg); |
Overdrivr | 5:cd94bb58e096 | 127 | frame(t,TM_float32,ptr,4); |
Overdrivr | 5:cd94bb58e096 | 128 | } |
Overdrivr | 5:cd94bb58e096 | 129 | |
Overdrivr | 5:cd94bb58e096 | 130 | void subscribe(void (*callback)(TM_state* s, TM_msg* m), TM_state * s) |
Overdrivr | 5:cd94bb58e096 | 131 | { |
Overdrivr | 5:cd94bb58e096 | 132 | statePtr = s; |
Overdrivr | 5:cd94bb58e096 | 133 | userCallback = callback; |
Overdrivr | 5:cd94bb58e096 | 134 | } |
Overdrivr | 5:cd94bb58e096 | 135 | |
Overdrivr | 5:cd94bb58e096 | 136 | void update_telemetry(float elapsedTime) |
Overdrivr | 5:cd94bb58e096 | 137 | { |
Overdrivr | 5:cd94bb58e096 | 138 | // If user forgot to define transport by calling init_telemetry, abort |
Overdrivr | 5:cd94bb58e096 | 139 | if(!transportPtr) |
Overdrivr | 5:cd94bb58e096 | 140 | return; |
Overdrivr | 5:cd94bb58e096 | 141 | |
Overdrivr | 5:cd94bb58e096 | 142 | uint32_t amount = transportPtr->readable(); |
Overdrivr | 5:cd94bb58e096 | 143 | uint32_t i = 0 ; |
Overdrivr | 5:cd94bb58e096 | 144 | for(i = 0 ; i < amount ; i++) |
Overdrivr | 5:cd94bb58e096 | 145 | { |
Overdrivr | 5:cd94bb58e096 | 146 | uint8_t c; |
Overdrivr | 5:cd94bb58e096 | 147 | transportPtr->read(&c,1); |
Overdrivr | 5:cd94bb58e096 | 148 | feed(c); |
Overdrivr | 5:cd94bb58e096 | 149 | } |
Overdrivr | 5:cd94bb58e096 | 150 | } |
Overdrivr | 5:cd94bb58e096 | 151 | |
Overdrivr | 5:cd94bb58e096 | 152 | uint16_t header(TM_type type) |
Overdrivr | 5:cd94bb58e096 | 153 | { |
Overdrivr | 5:cd94bb58e096 | 154 | // header data |
Overdrivr | 5:cd94bb58e096 | 155 | uint16_t h = type; |
Overdrivr | 5:cd94bb58e096 | 156 | uint8_t * ptr = (uint8_t*)(&h); |
Overdrivr | 5:cd94bb58e096 | 157 | |
Overdrivr | 5:cd94bb58e096 | 158 | // add data to frame |
Overdrivr | 5:cd94bb58e096 | 159 | append2(h); |
Overdrivr | 5:cd94bb58e096 | 160 | |
Overdrivr | 5:cd94bb58e096 | 161 | // compute crc and return it |
Overdrivr | 5:cd94bb58e096 | 162 | return crc16(ptr, 2); |
Overdrivr | 5:cd94bb58e096 | 163 | } |
Overdrivr | 5:cd94bb58e096 | 164 | |
Overdrivr | 5:cd94bb58e096 | 165 | uint16_t topic(const char * t, uint16_t crc) |
Overdrivr | 5:cd94bb58e096 | 166 | { |
Overdrivr | 5:cd94bb58e096 | 167 | const uint8_t * ptr = (uint8_t*)t; |
Overdrivr | 5:cd94bb58e096 | 168 | uint32_t i = 0 ; |
Overdrivr | 5:cd94bb58e096 | 169 | for(i = 0 ; i < strlen(t) ; i++) |
Overdrivr | 5:cd94bb58e096 | 170 | { |
Overdrivr | 5:cd94bb58e096 | 171 | // TODO : Replace with Huffman compression |
Overdrivr | 5:cd94bb58e096 | 172 | append(ptr[i]); |
Overdrivr | 5:cd94bb58e096 | 173 | crc = crc16_recursive(ptr[i], crc); |
Overdrivr | 5:cd94bb58e096 | 174 | } |
Overdrivr | 5:cd94bb58e096 | 175 | // Add NULL character |
Overdrivr | 5:cd94bb58e096 | 176 | append(0); |
Overdrivr | 5:cd94bb58e096 | 177 | return crc16_recursive(0,crc); |
Overdrivr | 5:cd94bb58e096 | 178 | } |
Overdrivr | 5:cd94bb58e096 | 179 | |
Overdrivr | 5:cd94bb58e096 | 180 | uint16_t payload(const void * p, uint32_t size, uint16_t crc) |
Overdrivr | 5:cd94bb58e096 | 181 | { |
Overdrivr | 5:cd94bb58e096 | 182 | const uint8_t * ptr = (uint8_t*)p; |
Overdrivr | 5:cd94bb58e096 | 183 | uint32_t i = 0 ; |
Overdrivr | 5:cd94bb58e096 | 184 | for(i = 0 ; i < size ; i++) |
Overdrivr | 5:cd94bb58e096 | 185 | { |
Overdrivr | 5:cd94bb58e096 | 186 | append(ptr[i]); |
Overdrivr | 5:cd94bb58e096 | 187 | crc = crc16_recursive(ptr[i], crc); |
Overdrivr | 5:cd94bb58e096 | 188 | } |
Overdrivr | 5:cd94bb58e096 | 189 | return crc; |
Overdrivr | 5:cd94bb58e096 | 190 | } |
Overdrivr | 5:cd94bb58e096 | 191 | |
Overdrivr | 5:cd94bb58e096 | 192 | void frame(const char * t, TM_type type, const void * data, uint32_t datasize) |
Overdrivr | 5:cd94bb58e096 | 193 | { |
Overdrivr | 5:cd94bb58e096 | 194 | // start new frame |
Overdrivr | 5:cd94bb58e096 | 195 | begin(); |
Overdrivr | 5:cd94bb58e096 | 196 | |
Overdrivr | 5:cd94bb58e096 | 197 | // header |
Overdrivr | 5:cd94bb58e096 | 198 | uint16_t crc = header(type); |
Overdrivr | 5:cd94bb58e096 | 199 | |
Overdrivr | 5:cd94bb58e096 | 200 | // topic |
Overdrivr | 5:cd94bb58e096 | 201 | crc = topic(t, crc); |
Overdrivr | 5:cd94bb58e096 | 202 | |
Overdrivr | 5:cd94bb58e096 | 203 | // payload |
Overdrivr | 5:cd94bb58e096 | 204 | crc = payload(data, datasize, crc); |
Overdrivr | 5:cd94bb58e096 | 205 | |
Overdrivr | 5:cd94bb58e096 | 206 | // crc |
Overdrivr | 5:cd94bb58e096 | 207 | append2(crc); |
Overdrivr | 5:cd94bb58e096 | 208 | |
Overdrivr | 5:cd94bb58e096 | 209 | // complete frame |
Overdrivr | 5:cd94bb58e096 | 210 | uint32_t bytesAmount = end(); |
Overdrivr | 5:cd94bb58e096 | 211 | |
Overdrivr | 5:cd94bb58e096 | 212 | // send data |
Overdrivr | 5:cd94bb58e096 | 213 | send(outgoingBuffer, bytesAmount); |
Overdrivr | 5:cd94bb58e096 | 214 | } |
Overdrivr | 5:cd94bb58e096 | 215 | |
Overdrivr | 5:cd94bb58e096 | 216 | void send(void * buf, uint32_t size) |
Overdrivr | 5:cd94bb58e096 | 217 | { |
Overdrivr | 5:cd94bb58e096 | 218 | // If user forgot to define transport by calling init_telemetry, abort |
Overdrivr | 5:cd94bb58e096 | 219 | if(!transportPtr) |
Overdrivr | 5:cd94bb58e096 | 220 | return; |
Overdrivr | 5:cd94bb58e096 | 221 | |
Overdrivr | 5:cd94bb58e096 | 222 | if(transportPtr->writeable() && size > 0) |
Overdrivr | 5:cd94bb58e096 | 223 | { |
Overdrivr | 5:cd94bb58e096 | 224 | transportPtr->write(outgoingBuffer, size); |
Overdrivr | 5:cd94bb58e096 | 225 | } |
Overdrivr | 5:cd94bb58e096 | 226 | } |
Overdrivr | 5:cd94bb58e096 | 227 | |
Overdrivr | 7:d224bddd5405 | 228 | void try_update_hashtable(TM_msg * msg) |
Overdrivr | 7:d224bddd5405 | 229 | { |
Overdrivr | 7:d224bddd5405 | 230 | struct nlist * np = lookup(hashtab, msg->topic); |
Overdrivr | 7:d224bddd5405 | 231 | |
Overdrivr | 7:d224bddd5405 | 232 | // Topic not found |
Overdrivr | 7:d224bddd5405 | 233 | if(np == NULL) |
Overdrivr | 7:d224bddd5405 | 234 | return; |
Overdrivr | 7:d224bddd5405 | 235 | |
Overdrivr | 7:d224bddd5405 | 236 | switch(msg->type) |
Overdrivr | 7:d224bddd5405 | 237 | { |
Overdrivr | 7:d224bddd5405 | 238 | case TM_float32: |
Overdrivr | 7:d224bddd5405 | 239 | // If hashtable has an entry of type float 32 under received topic |
Overdrivr | 7:d224bddd5405 | 240 | if (np->ptr_f32 == NULL) |
Overdrivr | 7:d224bddd5405 | 241 | break; |
Overdrivr | 7:d224bddd5405 | 242 | emplace_f32(msg, np->ptr_f32); |
Overdrivr | 7:d224bddd5405 | 243 | break; |
Overdrivr | 7:d224bddd5405 | 244 | case TM_uint8: |
Overdrivr | 7:d224bddd5405 | 245 | // If hashtable has an entry of type float 32 under received topic |
Overdrivr | 7:d224bddd5405 | 246 | if (np->ptr_u8 == NULL) |
Overdrivr | 7:d224bddd5405 | 247 | break; |
Overdrivr | 7:d224bddd5405 | 248 | emplace_u8(msg, np->ptr_u8); |
Overdrivr | 7:d224bddd5405 | 249 | break; |
Overdrivr | 7:d224bddd5405 | 250 | case TM_uint16: |
Overdrivr | 7:d224bddd5405 | 251 | // If hashtable has an entry of type float 32 under received topic |
Overdrivr | 7:d224bddd5405 | 252 | if (np->ptr_u16 == NULL) |
Overdrivr | 7:d224bddd5405 | 253 | break; |
Overdrivr | 7:d224bddd5405 | 254 | emplace_u16(msg, np->ptr_u16); |
Overdrivr | 7:d224bddd5405 | 255 | break; |
Overdrivr | 7:d224bddd5405 | 256 | case TM_uint32: |
Overdrivr | 7:d224bddd5405 | 257 | // If hashtable has an entry of type float 32 under received topic |
Overdrivr | 7:d224bddd5405 | 258 | if (np->ptr_u32 == NULL) |
Overdrivr | 7:d224bddd5405 | 259 | break; |
Overdrivr | 7:d224bddd5405 | 260 | emplace_u32(msg, np->ptr_u32); |
Overdrivr | 7:d224bddd5405 | 261 | break; |
Overdrivr | 7:d224bddd5405 | 262 | case TM_int8: |
Overdrivr | 7:d224bddd5405 | 263 | // If hashtable has an entry of type float 32 under received topic |
Overdrivr | 7:d224bddd5405 | 264 | if (np->ptr_i8 == NULL) |
Overdrivr | 7:d224bddd5405 | 265 | break; |
Overdrivr | 7:d224bddd5405 | 266 | emplace_i8(msg, np->ptr_i8); |
Overdrivr | 7:d224bddd5405 | 267 | break; |
Overdrivr | 7:d224bddd5405 | 268 | case TM_int16: |
Overdrivr | 7:d224bddd5405 | 269 | // If hashtable has an entry of type float 32 under received topic |
Overdrivr | 7:d224bddd5405 | 270 | if (np->ptr_i16 == NULL) |
Overdrivr | 7:d224bddd5405 | 271 | break; |
Overdrivr | 7:d224bddd5405 | 272 | emplace_i16(msg, np->ptr_i16); |
Overdrivr | 7:d224bddd5405 | 273 | break; |
Overdrivr | 7:d224bddd5405 | 274 | case TM_int32: |
Overdrivr | 7:d224bddd5405 | 275 | // If hashtable has an entry of type float 32 under received topic |
Overdrivr | 7:d224bddd5405 | 276 | if (np->ptr_i32 == NULL) |
Overdrivr | 7:d224bddd5405 | 277 | break; |
Overdrivr | 7:d224bddd5405 | 278 | emplace_i32(msg, np->ptr_i32); |
Overdrivr | 7:d224bddd5405 | 279 | break; |
Overdrivr | 7:d224bddd5405 | 280 | } |
Overdrivr | 7:d224bddd5405 | 281 | } |
Overdrivr | 7:d224bddd5405 | 282 | |
Overdrivr | 5:cd94bb58e096 | 283 | void on_incoming_frame(uint8_t * storage, uint32_t size) |
Overdrivr | 5:cd94bb58e096 | 284 | { |
Overdrivr | 5:cd94bb58e096 | 285 | if(size < 2) |
Overdrivr | 5:cd94bb58e096 | 286 | return; |
Overdrivr | 5:cd94bb58e096 | 287 | // Read header |
Overdrivr | 5:cd94bb58e096 | 288 | uint16_t head; |
Overdrivr | 5:cd94bb58e096 | 289 | uint8_t * ptr; |
Overdrivr | 5:cd94bb58e096 | 290 | ptr = (uint8_t*)(&head); |
Overdrivr | 5:cd94bb58e096 | 291 | memcpy(ptr,storage,2); |
Overdrivr | 5:cd94bb58e096 | 292 | |
Overdrivr | 5:cd94bb58e096 | 293 | // Read topic |
Overdrivr | 5:cd94bb58e096 | 294 | uint32_t cursor = 2; |
Overdrivr | 5:cd94bb58e096 | 295 | uint32_t topicSize = 0; |
Overdrivr | 5:cd94bb58e096 | 296 | while(cursor < size) |
Overdrivr | 5:cd94bb58e096 | 297 | { |
Overdrivr | 5:cd94bb58e096 | 298 | if(storage[cursor] == 0) |
Overdrivr | 5:cd94bb58e096 | 299 | break; |
Overdrivr | 5:cd94bb58e096 | 300 | topicSize++; |
Overdrivr | 5:cd94bb58e096 | 301 | cursor++; |
Overdrivr | 5:cd94bb58e096 | 302 | } |
Overdrivr | 5:cd94bb58e096 | 303 | |
Overdrivr | 5:cd94bb58e096 | 304 | if(topicSize == 0) |
Overdrivr | 5:cd94bb58e096 | 305 | return; |
Overdrivr | 5:cd94bb58e096 | 306 | |
Overdrivr | 5:cd94bb58e096 | 307 | // payload = total - header - topic - /0 - crc |
Overdrivr | 5:cd94bb58e096 | 308 | int32_t payloadSize = size - 2 - topicSize - 1 - 2; |
Overdrivr | 5:cd94bb58e096 | 309 | |
Overdrivr | 5:cd94bb58e096 | 310 | if(payloadSize <= 0) |
Overdrivr | 5:cd94bb58e096 | 311 | return; |
Overdrivr | 5:cd94bb58e096 | 312 | |
Overdrivr | 5:cd94bb58e096 | 313 | // Check crc |
Overdrivr | 5:cd94bb58e096 | 314 | uint16_t expected_crc = crc16(storage, size-2); |
Overdrivr | 5:cd94bb58e096 | 315 | uint16_t rcv_crc; |
Overdrivr | 5:cd94bb58e096 | 316 | ptr = (uint8_t*)(&rcv_crc); |
Overdrivr | 5:cd94bb58e096 | 317 | memcpy(ptr,storage+size-2,2); |
Overdrivr | 5:cd94bb58e096 | 318 | |
Overdrivr | 5:cd94bb58e096 | 319 | if(expected_crc != rcv_crc) |
Overdrivr | 5:cd94bb58e096 | 320 | return; |
Overdrivr | 5:cd94bb58e096 | 321 | |
Overdrivr | 5:cd94bb58e096 | 322 | // Store topic |
Overdrivr | 5:cd94bb58e096 | 323 | char * t = (char*)(storage); |
Overdrivr | 5:cd94bb58e096 | 324 | strcpy(topicBuffer, t + 2); |
Overdrivr | 5:cd94bb58e096 | 325 | |
Overdrivr | 5:cd94bb58e096 | 326 | // ptr to beginning of payload |
Overdrivr | 5:cd94bb58e096 | 327 | ptr = (uint8_t*)(storage) + (uint32_t)(2 + topicSize + 1); |
Overdrivr | 5:cd94bb58e096 | 328 | |
Overdrivr | 5:cd94bb58e096 | 329 | TM_msg packet; |
Overdrivr | 5:cd94bb58e096 | 330 | packet.topic = topicBuffer; |
Overdrivr | 5:cd94bb58e096 | 331 | packet.type = (TM_type)head; |
Overdrivr | 5:cd94bb58e096 | 332 | packet.buffer = (void *)(ptr); |
Overdrivr | 5:cd94bb58e096 | 333 | packet.size = (uint32_t)payloadSize; |
Overdrivr | 5:cd94bb58e096 | 334 | |
Overdrivr | 7:d224bddd5405 | 335 | // Try update variable if found in hash table |
Overdrivr | 7:d224bddd5405 | 336 | try_update_hashtable(&packet); |
Overdrivr | 7:d224bddd5405 | 337 | |
Overdrivr | 7:d224bddd5405 | 338 | // Call global handler |
Overdrivr | 5:cd94bb58e096 | 339 | userCallback(statePtr,&packet); |
Overdrivr | 5:cd94bb58e096 | 340 | } |
Overdrivr | 5:cd94bb58e096 | 341 | |
Overdrivr | 5:cd94bb58e096 | 342 | void on_incoming_error(int32_t errCode) |
Overdrivr | 5:cd94bb58e096 | 343 | { |
Overdrivr | 5:cd94bb58e096 | 344 | // TODO : Error management |
Overdrivr | 5:cd94bb58e096 | 345 | } |
Overdrivr | 5:cd94bb58e096 | 346 | |
Overdrivr | 5:cd94bb58e096 | 347 | void emptyCallback(TM_state * s, TM_msg * m) |
Overdrivr | 5:cd94bb58e096 | 348 | { |
Overdrivr | 5:cd94bb58e096 | 349 | // Called only if the user forgot to subscribe a callback |
Overdrivr | 5:cd94bb58e096 | 350 | } |