SensorDataParser.cpp

00001 /**
00002  * @file    SensorDataParser.cpp
00003  * @brief   Parser for Sensor Data Streamer (binary) and Sensor Monitor (cvs) iPhone/Android apps
00004  * @author  Bogdan Marinescu & Mihail Stoyanov
00005  * @version 1.0
00006  * @see     
00007  *
00008  * Copyright (c) 2013
00009  *
00010  * Licensed under the Apache License, Version 2.0 (the "License");
00011  * you may not use this file except in compliance with the License.
00012  * You may obtain a copy of the License at
00013  *
00014  *     http://www.apache.org/licenses/LICENSE-2.0
00015  *
00016  * Unless required by applicable law or agreed to in writing, software
00017  * distributed under the License is distributed on an "AS IS" BASIS,
00018  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00019  * See the License for the specific language governing permissions and
00020  * limitations under the License.
00021  */
00022 
00023 #include "mbed.h"
00024 #include "SensorDataParser.h"
00025 
00026 static int parse_sensor_stream(char *buf, SENSOR_DATA *pd) {
00027     pd->ax = *(float*)(buf + 4);
00028     pd->ay = *(float*)(buf + 8);
00029     pd->az = *(float*)(buf + 12);
00030         
00031     pd->gx = *(float*)(buf + 16);
00032     pd->gy = *(float*)(buf + 20);
00033     pd->gz = *(float*)(buf + 24);
00034         
00035     pd->tx = *(double*)(buf + 28);
00036     pd->ty = *(double*)(buf + 36);
00037     pd->tz = *(double*)(buf + 44);
00038     
00039     pd->hm = *(double*)(buf + 52);
00040     pd->ht = *(double*)(buf + 60);
00041         
00042     pd->latitude  = *(double*)(buf + 68);
00043     pd->longitude = *(double*)(buf + 76);
00044     pd->altitude  = *(double*)(buf + 84);
00045     
00046     pd->proximity  = buf[92];
00047     
00048     pd->touch1  = buf[93];
00049     pd->touch1x = *(int*)(buf + 94);
00050     pd->touch1y = *(int*)(buf + 98);
00051     
00052     pd->touch2  = buf[102];
00053     pd->touch2x = *(int*)(buf + 103);
00054     pd->touch2y = *(int*)(buf + 107);
00055     
00056     return 1;
00057 }
00058 
00059 static int parse_sensor_csv(char *s, double *pdest, int maxn) {
00060     if (NULL == s || strlen(s) == 0)
00061         return 0;
00062     int crt = 0;
00063     char *p = s, *temp;
00064     
00065     while (crt < maxn) {
00066         temp = strchr(p, ',');
00067         if (temp)
00068             *temp = 0;
00069         while (*p == ' ')
00070             p ++;
00071         if (sscanf(p, "%lf", pdest + crt) != 1) {
00072             return 0;
00073         }
00074         crt ++;
00075         if (NULL == temp)
00076             break;
00077         else
00078             p = temp + 1;
00079     }
00080     return crt;
00081 }
00082 
00083 int parse_sensor_packet(char *buf, SENSOR_DATA *pd) {
00084     if (buf[0] == 'F' || buf[1] == 'S' || buf[3] == 107) {  // SensorStream binary packet
00085         parse_sensor_stream(buf, pd);
00086         
00087         return 1;
00088     } else {
00089         int nvals, i;
00090         double vals[MAX_CSV_VALS];
00091         
00092         if ((nvals = parse_sensor_csv(buf, vals, MAX_CSV_VALS)) != 0) { // SensorMonitor or generic CSV packet
00093             // Format: timestamp, [sensor_id, x, y, z]+
00094             // sensor_id: 3-acc, 4-gyr, 5-mag
00095             if ((nvals - 1) % 3 != 0)
00096                 return 0;
00097             i = 0;
00098             pd->ax = vals[i+1];
00099             pd->ay = vals[i+2];
00100             pd->az = vals[i+3];
00101             
00102             pd->touch1 = 0;
00103             pd->touch2 = 0;
00104             
00105             return 2;
00106         };
00107     };
00108     return 0;
00109 }