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

c_api/telemetry_core.cpp@6:f5e1b079bffd, 2016-03-09 (annotated)

Committer:
Overdrivr
Date:
Wed Mar 09 13:41:27 2016 +0000
Revision:
6:f5e1b079bffd
Parent:
c_api/telemetry_core.c@5:cd94bb58e096
Child:
7:d224bddd5405
Fixed build.

Who changed what in which revision?

UserRevisionLine numberNew 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 5:cd94bb58e096 4
Overdrivr 5:cd94bb58e096 5 static TM_state * statePtr;
Overdrivr 5:cd94bb58e096 6 static TM_transport * transportPtr;
Overdrivr 5:cd94bb58e096 7 static uint8_t incomingBuffer[INCOMING_BUFFER_SIZE];
Overdrivr 5:cd94bb58e096 8 static uint8_t outgoingBuffer[OUTGOING_BUFFER_SIZE];
Overdrivr 5:cd94bb58e096 9 static char topicBuffer[TOPIC_BUFFER_SIZE];
Overdrivr 5:cd94bb58e096 10
Overdrivr 5:cd94bb58e096 11 static void (*userCallback)(TM_state * s, TM_msg * m);
Overdrivr 5:cd94bb58e096 12
Overdrivr 5:cd94bb58e096 13 uint16_t header(TM_type type);
Overdrivr 5:cd94bb58e096 14 uint16_t topic(const char * topic, uint16_t crc);
Overdrivr 5:cd94bb58e096 15 uint16_t payload(const void * payload, uint32_t size, uint16_t crc);
Overdrivr 5:cd94bb58e096 16 void frame(const char * t, TM_type type, const void * data, uint32_t datasize);
Overdrivr 5:cd94bb58e096 17 void send(void * buf, uint32_t size);
Overdrivr 5:cd94bb58e096 18 void on_incoming_frame(uint8_t * storage, uint32_t size);
Overdrivr 5:cd94bb58e096 19 void on_incoming_error(int32_t errCode);
Overdrivr 5:cd94bb58e096 20 void emptyCallback(TM_state * s, TM_msg * m);
Overdrivr 5:cd94bb58e096 21
Overdrivr 5:cd94bb58e096 22 void init_telemetry(TM_transport * t)
Overdrivr 5:cd94bb58e096 23 {
Overdrivr 5:cd94bb58e096 24 statePtr = NULL;
Overdrivr 5:cd94bb58e096 25 transportPtr = t;
Overdrivr 5:cd94bb58e096 26 userCallback = emptyCallback;
Overdrivr 5:cd94bb58e096 27
Overdrivr 5:cd94bb58e096 28 // Setup framing
Overdrivr 5:cd94bb58e096 29 initialize_framing();
Overdrivr 5:cd94bb58e096 30 incoming_storage(incomingBuffer,INCOMING_BUFFER_SIZE);
Overdrivr 5:cd94bb58e096 31 outgoing_storage(outgoingBuffer, OUTGOING_BUFFER_SIZE);
Overdrivr 5:cd94bb58e096 32 set_on_incoming_frame(on_incoming_frame);
Overdrivr 5:cd94bb58e096 33 set_on_incoming_error(on_incoming_error);
Overdrivr 5:cd94bb58e096 34 }
Overdrivr 5:cd94bb58e096 35
Overdrivr 5:cd94bb58e096 36 void publish(const char * t, const char * msg)
Overdrivr 5:cd94bb58e096 37 {
Overdrivr 5:cd94bb58e096 38 frame(t,TM_string,msg,strlen(msg));
Overdrivr 5:cd94bb58e096 39 }
Overdrivr 5:cd94bb58e096 40
Overdrivr 5:cd94bb58e096 41 void publish_u8(const char * t, uint8_t msg)
Overdrivr 5:cd94bb58e096 42 {
Overdrivr 5:cd94bb58e096 43 void * ptr = (void *)(&msg);
Overdrivr 5:cd94bb58e096 44 frame(t,TM_uint8,ptr,1);
Overdrivr 5:cd94bb58e096 45 }
Overdrivr 5:cd94bb58e096 46
Overdrivr 5:cd94bb58e096 47 void publish_u16(const char * t, uint16_t msg)
Overdrivr 5:cd94bb58e096 48 {
Overdrivr 5:cd94bb58e096 49 void * ptr = (void *)(&msg);
Overdrivr 5:cd94bb58e096 50 frame(t,TM_uint16,ptr,2);
Overdrivr 5:cd94bb58e096 51 }
Overdrivr 5:cd94bb58e096 52
Overdrivr 5:cd94bb58e096 53 void publish_u32(const char * t, uint32_t msg)
Overdrivr 5:cd94bb58e096 54 {
Overdrivr 5:cd94bb58e096 55 void * ptr = (void *)(&msg);
Overdrivr 5:cd94bb58e096 56 frame(t,TM_uint32,ptr,4);
Overdrivr 5:cd94bb58e096 57 }
Overdrivr 5:cd94bb58e096 58
Overdrivr 5:cd94bb58e096 59 void publish_i8(const char * t, int8_t msg)
Overdrivr 5:cd94bb58e096 60 {
Overdrivr 5:cd94bb58e096 61 void * ptr = (void *)(&msg);
Overdrivr 5:cd94bb58e096 62 frame(t,TM_int8,ptr,1);
Overdrivr 5:cd94bb58e096 63 }
Overdrivr 5:cd94bb58e096 64
Overdrivr 5:cd94bb58e096 65 void publish_i16(const char * t, int16_t msg)
Overdrivr 5:cd94bb58e096 66 {
Overdrivr 5:cd94bb58e096 67 void * ptr = (void *)(&msg);
Overdrivr 5:cd94bb58e096 68 frame(t,TM_int16,ptr,2);
Overdrivr 5:cd94bb58e096 69 }
Overdrivr 5:cd94bb58e096 70
Overdrivr 5:cd94bb58e096 71 void publish_i32(const char * t, int32_t msg)
Overdrivr 5:cd94bb58e096 72 {
Overdrivr 5:cd94bb58e096 73 void * ptr = (void *)(&msg);
Overdrivr 5:cd94bb58e096 74 frame(t,TM_int32,ptr,4);
Overdrivr 5:cd94bb58e096 75 }
Overdrivr 5:cd94bb58e096 76
Overdrivr 5:cd94bb58e096 77 void publish_f32(const char * t, float msg)
Overdrivr 5:cd94bb58e096 78 {
Overdrivr 5:cd94bb58e096 79 void * ptr = (void *)(&msg);
Overdrivr 5:cd94bb58e096 80 frame(t,TM_float32,ptr,4);
Overdrivr 5:cd94bb58e096 81 }
Overdrivr 5:cd94bb58e096 82
Overdrivr 5:cd94bb58e096 83 void subscribe(void (*callback)(TM_state* s, TM_msg* m), TM_state * s)
Overdrivr 5:cd94bb58e096 84 {
Overdrivr 5:cd94bb58e096 85 statePtr = s;
Overdrivr 5:cd94bb58e096 86 userCallback = callback;
Overdrivr 5:cd94bb58e096 87 }
Overdrivr 5:cd94bb58e096 88
Overdrivr 5:cd94bb58e096 89 void update_telemetry(float elapsedTime)
Overdrivr 5:cd94bb58e096 90 {
Overdrivr 5:cd94bb58e096 91 // If user forgot to define transport by calling init_telemetry, abort
Overdrivr 5:cd94bb58e096 92 if(!transportPtr)
Overdrivr 5:cd94bb58e096 93 return;
Overdrivr 5:cd94bb58e096 94
Overdrivr 5:cd94bb58e096 95 uint32_t amount = transportPtr->readable();
Overdrivr 5:cd94bb58e096 96 uint32_t i = 0 ;
Overdrivr 5:cd94bb58e096 97 for(i = 0 ; i < amount ; i++)
Overdrivr 5:cd94bb58e096 98 {
Overdrivr 5:cd94bb58e096 99 uint8_t c;
Overdrivr 5:cd94bb58e096 100 transportPtr->read(&c,1);
Overdrivr 5:cd94bb58e096 101 feed(c);
Overdrivr 5:cd94bb58e096 102 }
Overdrivr 5:cd94bb58e096 103 }
Overdrivr 5:cd94bb58e096 104
Overdrivr 5:cd94bb58e096 105 uint16_t header(TM_type type)
Overdrivr 5:cd94bb58e096 106 {
Overdrivr 5:cd94bb58e096 107 // header data
Overdrivr 5:cd94bb58e096 108 uint16_t h = type;
Overdrivr 5:cd94bb58e096 109 uint8_t * ptr = (uint8_t*)(&h);
Overdrivr 5:cd94bb58e096 110
Overdrivr 5:cd94bb58e096 111 // add data to frame
Overdrivr 5:cd94bb58e096 112 append2(h);
Overdrivr 5:cd94bb58e096 113
Overdrivr 5:cd94bb58e096 114 // compute crc and return it
Overdrivr 5:cd94bb58e096 115 return crc16(ptr, 2);
Overdrivr 5:cd94bb58e096 116 }
Overdrivr 5:cd94bb58e096 117
Overdrivr 5:cd94bb58e096 118 uint16_t topic(const char * t, uint16_t crc)
Overdrivr 5:cd94bb58e096 119 {
Overdrivr 5:cd94bb58e096 120 const uint8_t * ptr = (uint8_t*)t;
Overdrivr 5:cd94bb58e096 121 uint32_t i = 0 ;
Overdrivr 5:cd94bb58e096 122 for(i = 0 ; i < strlen(t) ; i++)
Overdrivr 5:cd94bb58e096 123 {
Overdrivr 5:cd94bb58e096 124 // TODO : Replace with Huffman compression
Overdrivr 5:cd94bb58e096 125 append(ptr[i]);
Overdrivr 5:cd94bb58e096 126 crc = crc16_recursive(ptr[i], crc);
Overdrivr 5:cd94bb58e096 127 }
Overdrivr 5:cd94bb58e096 128 // Add NULL character
Overdrivr 5:cd94bb58e096 129 append(0);
Overdrivr 5:cd94bb58e096 130 return crc16_recursive(0,crc);
Overdrivr 5:cd94bb58e096 131 }
Overdrivr 5:cd94bb58e096 132
Overdrivr 5:cd94bb58e096 133 uint16_t payload(const void * p, uint32_t size, uint16_t crc)
Overdrivr 5:cd94bb58e096 134 {
Overdrivr 5:cd94bb58e096 135 const uint8_t * ptr = (uint8_t*)p;
Overdrivr 5:cd94bb58e096 136 uint32_t i = 0 ;
Overdrivr 5:cd94bb58e096 137 for(i = 0 ; i < size ; i++)
Overdrivr 5:cd94bb58e096 138 {
Overdrivr 5:cd94bb58e096 139 append(ptr[i]);
Overdrivr 5:cd94bb58e096 140 crc = crc16_recursive(ptr[i], crc);
Overdrivr 5:cd94bb58e096 141 }
Overdrivr 5:cd94bb58e096 142 return crc;
Overdrivr 5:cd94bb58e096 143 }
Overdrivr 5:cd94bb58e096 144
Overdrivr 5:cd94bb58e096 145 void frame(const char * t, TM_type type, const void * data, uint32_t datasize)
Overdrivr 5:cd94bb58e096 146 {
Overdrivr 5:cd94bb58e096 147 // start new frame
Overdrivr 5:cd94bb58e096 148 begin();
Overdrivr 5:cd94bb58e096 149
Overdrivr 5:cd94bb58e096 150 // header
Overdrivr 5:cd94bb58e096 151 uint16_t crc = header(type);
Overdrivr 5:cd94bb58e096 152
Overdrivr 5:cd94bb58e096 153 // topic
Overdrivr 5:cd94bb58e096 154 crc = topic(t, crc);
Overdrivr 5:cd94bb58e096 155
Overdrivr 5:cd94bb58e096 156 // payload
Overdrivr 5:cd94bb58e096 157 crc = payload(data, datasize, crc);
Overdrivr 5:cd94bb58e096 158
Overdrivr 5:cd94bb58e096 159 // crc
Overdrivr 5:cd94bb58e096 160 append2(crc);
Overdrivr 5:cd94bb58e096 161
Overdrivr 5:cd94bb58e096 162 // complete frame
Overdrivr 5:cd94bb58e096 163 uint32_t bytesAmount = end();
Overdrivr 5:cd94bb58e096 164
Overdrivr 5:cd94bb58e096 165 // send data
Overdrivr 5:cd94bb58e096 166 send(outgoingBuffer, bytesAmount);
Overdrivr 5:cd94bb58e096 167 }
Overdrivr 5:cd94bb58e096 168
Overdrivr 5:cd94bb58e096 169 void send(void * buf, uint32_t size)
Overdrivr 5:cd94bb58e096 170 {
Overdrivr 5:cd94bb58e096 171 // If user forgot to define transport by calling init_telemetry, abort
Overdrivr 5:cd94bb58e096 172 if(!transportPtr)
Overdrivr 5:cd94bb58e096 173 return;
Overdrivr 5:cd94bb58e096 174
Overdrivr 5:cd94bb58e096 175 if(transportPtr->writeable() && size > 0)
Overdrivr 5:cd94bb58e096 176 {
Overdrivr 5:cd94bb58e096 177 transportPtr->write(outgoingBuffer, size);
Overdrivr 5:cd94bb58e096 178 }
Overdrivr 5:cd94bb58e096 179 }
Overdrivr 5:cd94bb58e096 180
Overdrivr 5:cd94bb58e096 181 void on_incoming_frame(uint8_t * storage, uint32_t size)
Overdrivr 5:cd94bb58e096 182 {
Overdrivr 5:cd94bb58e096 183 if(size < 2)
Overdrivr 5:cd94bb58e096 184 return;
Overdrivr 5:cd94bb58e096 185 // Read header
Overdrivr 5:cd94bb58e096 186 uint16_t head;
Overdrivr 5:cd94bb58e096 187 uint8_t * ptr;
Overdrivr 5:cd94bb58e096 188 ptr = (uint8_t*)(&head);
Overdrivr 5:cd94bb58e096 189 memcpy(ptr,storage,2);
Overdrivr 5:cd94bb58e096 190
Overdrivr 5:cd94bb58e096 191 // Read topic
Overdrivr 5:cd94bb58e096 192 uint32_t cursor = 2;
Overdrivr 5:cd94bb58e096 193 uint32_t topicSize = 0;
Overdrivr 5:cd94bb58e096 194 while(cursor < size)
Overdrivr 5:cd94bb58e096 195 {
Overdrivr 5:cd94bb58e096 196 if(storage[cursor] == 0)
Overdrivr 5:cd94bb58e096 197 break;
Overdrivr 5:cd94bb58e096 198 topicSize++;
Overdrivr 5:cd94bb58e096 199 cursor++;
Overdrivr 5:cd94bb58e096 200 }
Overdrivr 5:cd94bb58e096 201
Overdrivr 5:cd94bb58e096 202 if(topicSize == 0)
Overdrivr 5:cd94bb58e096 203 return;
Overdrivr 5:cd94bb58e096 204
Overdrivr 5:cd94bb58e096 205 // payload = total - header - topic - /0 - crc
Overdrivr 5:cd94bb58e096 206 int32_t payloadSize = size - 2 - topicSize - 1 - 2;
Overdrivr 5:cd94bb58e096 207
Overdrivr 5:cd94bb58e096 208 if(payloadSize <= 0)
Overdrivr 5:cd94bb58e096 209 return;
Overdrivr 5:cd94bb58e096 210
Overdrivr 5:cd94bb58e096 211 // Check crc
Overdrivr 5:cd94bb58e096 212 uint16_t expected_crc = crc16(storage, size-2);
Overdrivr 5:cd94bb58e096 213 uint16_t rcv_crc;
Overdrivr 5:cd94bb58e096 214 ptr = (uint8_t*)(&rcv_crc);
Overdrivr 5:cd94bb58e096 215 memcpy(ptr,storage+size-2,2);
Overdrivr 5:cd94bb58e096 216
Overdrivr 5:cd94bb58e096 217 if(expected_crc != rcv_crc)
Overdrivr 5:cd94bb58e096 218 return;
Overdrivr 5:cd94bb58e096 219
Overdrivr 5:cd94bb58e096 220 // Store topic
Overdrivr 5:cd94bb58e096 221 char * t = (char*)(storage);
Overdrivr 5:cd94bb58e096 222 strcpy(topicBuffer, t + 2);
Overdrivr 5:cd94bb58e096 223
Overdrivr 5:cd94bb58e096 224 // ptr to beginning of payload
Overdrivr 5:cd94bb58e096 225 ptr = (uint8_t*)(storage) + (uint32_t)(2 + topicSize + 1);
Overdrivr 5:cd94bb58e096 226
Overdrivr 5:cd94bb58e096 227 TM_msg packet;
Overdrivr 5:cd94bb58e096 228 packet.topic = topicBuffer;
Overdrivr 5:cd94bb58e096 229 packet.type = (TM_type)head;
Overdrivr 5:cd94bb58e096 230 packet.buffer = (void *)(ptr);
Overdrivr 5:cd94bb58e096 231 packet.size = (uint32_t)payloadSize;
Overdrivr 5:cd94bb58e096 232
Overdrivr 5:cd94bb58e096 233 // Call callback
Overdrivr 5:cd94bb58e096 234 userCallback(statePtr,&packet);
Overdrivr 5:cd94bb58e096 235 }
Overdrivr 5:cd94bb58e096 236
Overdrivr 5:cd94bb58e096 237 void on_incoming_error(int32_t errCode)
Overdrivr 5:cd94bb58e096 238 {
Overdrivr 5:cd94bb58e096 239 // TODO : Error management
Overdrivr 5:cd94bb58e096 240 }
Overdrivr 5:cd94bb58e096 241
Overdrivr 5:cd94bb58e096 242 void emptyCallback(TM_state * s, TM_msg * m)
Overdrivr 5:cd94bb58e096 243 {
Overdrivr 5:cd94bb58e096 244 // Called only if the user forgot to subscribe a callback
Overdrivr 5:cd94bb58e096 245 }