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?
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 | 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 | } |