Mihail Stoyanov / SensorDataParser

SensorDataParser library that supports Sensor Data Streamer binary packets and Sensor Monitor csv packets

SensorDataParser.cpp@1:e9d3bb2384a9, 2013-10-24 (annotated)

Committer:
screamer
Date:
Thu Oct 24 17:02:02 2013 +0000
Revision:
1:e9d3bb2384a9
Parent:
0:145141a10e18 
* fixed - Updated docs etc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
screamer 1:e9d3bb2384a9 1 /**
screamer 1:e9d3bb2384a9 2 * @file SensorDataParser.cpp
screamer 1:e9d3bb2384a9 3 * @brief Parser for Sensor Data Streamer (binary) and Sensor Monitor (cvs) iPhone/Android apps
screamer 1:e9d3bb2384a9 4 * @author Bogdan Marinescu & Mihail Stoyanov
screamer 1:e9d3bb2384a9 5 * @version 1.0
screamer 1:e9d3bb2384a9 6 * @see
screamer 1:e9d3bb2384a9 7 *
screamer 1:e9d3bb2384a9 8 * Copyright (c) 2013
screamer 1:e9d3bb2384a9 9 *
screamer 1:e9d3bb2384a9 10 * Licensed under the Apache License, Version 2.0 (the "License");
screamer 1:e9d3bb2384a9 11 * you may not use this file except in compliance with the License.
screamer 1:e9d3bb2384a9 12 * You may obtain a copy of the License at
screamer 1:e9d3bb2384a9 13 *
screamer 1:e9d3bb2384a9 14 * http://www.apache.org/licenses/LICENSE-2.0
screamer 1:e9d3bb2384a9 15 *
screamer 1:e9d3bb2384a9 16 * Unless required by applicable law or agreed to in writing, software
screamer 1:e9d3bb2384a9 17 * distributed under the License is distributed on an "AS IS" BASIS,
screamer 1:e9d3bb2384a9 18 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
screamer 1:e9d3bb2384a9 19 * See the License for the specific language governing permissions and
screamer 1:e9d3bb2384a9 20 * limitations under the License.
screamer 1:e9d3bb2384a9 21 */
screamer 0:145141a10e18 22
screamer 0:145141a10e18 23 #include "mbed.h"
screamer 0:145141a10e18 24 #include "SensorDataParser.h"
screamer 0:145141a10e18 25
screamer 0:145141a10e18 26 static int parse_sensor_stream(char *buf, SENSOR_DATA *pd) {
screamer 0:145141a10e18 27 pd->ax = *(float*)(buf + 4);
screamer 0:145141a10e18 28 pd->ay = *(float*)(buf + 8);
screamer 0:145141a10e18 29 pd->az = *(float*)(buf + 12);
screamer 0:145141a10e18 30
screamer 0:145141a10e18 31 pd->gx = *(float*)(buf + 16);
screamer 0:145141a10e18 32 pd->gy = *(float*)(buf + 20);
screamer 0:145141a10e18 33 pd->gz = *(float*)(buf + 24);
screamer 0:145141a10e18 34
screamer 0:145141a10e18 35 pd->tx = *(double*)(buf + 28);
screamer 0:145141a10e18 36 pd->ty = *(double*)(buf + 36);
screamer 0:145141a10e18 37 pd->tz = *(double*)(buf + 44);
screamer 0:145141a10e18 38
screamer 0:145141a10e18 39 pd->hm = *(double*)(buf + 52);
screamer 0:145141a10e18 40 pd->ht = *(double*)(buf + 60);
screamer 0:145141a10e18 41
screamer 0:145141a10e18 42 pd->latitude = *(double*)(buf + 68);
screamer 0:145141a10e18 43 pd->longitude = *(double*)(buf + 76);
screamer 0:145141a10e18 44 pd->altitude = *(double*)(buf + 84);
screamer 0:145141a10e18 45
screamer 0:145141a10e18 46 pd->proximity = buf[92];
screamer 0:145141a10e18 47
screamer 0:145141a10e18 48 pd->touch1 = buf[93];
screamer 0:145141a10e18 49 pd->touch1x = *(int*)(buf + 94);
screamer 0:145141a10e18 50 pd->touch1y = *(int*)(buf + 98);
screamer 0:145141a10e18 51
screamer 0:145141a10e18 52 pd->touch2 = buf[102];
screamer 0:145141a10e18 53 pd->touch2x = *(int*)(buf + 103);
screamer 0:145141a10e18 54 pd->touch2y = *(int*)(buf + 107);
screamer 0:145141a10e18 55
screamer 0:145141a10e18 56 return 1;
screamer 0:145141a10e18 57 }
screamer 0:145141a10e18 58
screamer 0:145141a10e18 59 static int parse_sensor_csv(char *s, double *pdest, int maxn) {
screamer 0:145141a10e18 60 if (NULL == s || strlen(s) == 0)
screamer 0:145141a10e18 61 return 0;
screamer 0:145141a10e18 62 int crt = 0;
screamer 0:145141a10e18 63 char *p = s, *temp;
screamer 0:145141a10e18 64
screamer 0:145141a10e18 65 while (crt < maxn) {
screamer 0:145141a10e18 66 temp = strchr(p, ',');
screamer 0:145141a10e18 67 if (temp)
screamer 0:145141a10e18 68 *temp = 0;
screamer 0:145141a10e18 69 while (*p == ' ')
screamer 0:145141a10e18 70 p ++;
screamer 0:145141a10e18 71 if (sscanf(p, "%lf", pdest + crt) != 1) {
screamer 0:145141a10e18 72 return 0;
screamer 0:145141a10e18 73 }
screamer 0:145141a10e18 74 crt ++;
screamer 0:145141a10e18 75 if (NULL == temp)
screamer 0:145141a10e18 76 break;
screamer 0:145141a10e18 77 else
screamer 0:145141a10e18 78 p = temp + 1;
screamer 0:145141a10e18 79 }
screamer 0:145141a10e18 80 return crt;
screamer 0:145141a10e18 81 }
screamer 0:145141a10e18 82
screamer 0:145141a10e18 83 int parse_sensor_packet(char *buf, SENSOR_DATA *pd) {
screamer 0:145141a10e18 84 if (buf[0] == 'F' || buf[1] == 'S' || buf[3] == 107) { // SensorStream binary packet
screamer 0:145141a10e18 85 parse_sensor_stream(buf, pd);
screamer 0:145141a10e18 86
screamer 0:145141a10e18 87 return 1;
screamer 0:145141a10e18 88 } else {
screamer 0:145141a10e18 89 int nvals, i;
screamer 0:145141a10e18 90 double vals[MAX_CSV_VALS];
screamer 0:145141a10e18 91
screamer 0:145141a10e18 92 if ((nvals = parse_sensor_csv(buf, vals, MAX_CSV_VALS)) != 0) { // SensorMonitor or generic CSV packet
screamer 0:145141a10e18 93 // Format: timestamp, [sensor_id, x, y, z]+
screamer 0:145141a10e18 94 // sensor_id: 3-acc, 4-gyr, 5-mag
screamer 0:145141a10e18 95 if ((nvals - 1) % 3 != 0)
screamer 0:145141a10e18 96 return 0;
screamer 0:145141a10e18 97 i = 0;
screamer 0:145141a10e18 98 pd->ax = vals[i+1];
screamer 0:145141a10e18 99 pd->ay = vals[i+2];
screamer 0:145141a10e18 100 pd->az = vals[i+3];
screamer 0:145141a10e18 101
screamer 0:145141a10e18 102 pd->touch1 = 0;
screamer 0:145141a10e18 103 pd->touch2 = 0;
screamer 0:145141a10e18 104
screamer 0:145141a10e18 105 return 2;
screamer 0:145141a10e18 106 };
screamer 0:145141a10e18 107 };
screamer 0:145141a10e18 108 return 0;
screamer 0:145141a10e18 109 }