File content as of revision 1:e9d3bb2384a9:

/**
 * @file    SensorDataParser.cpp
 * @brief   Parser for Sensor Data Streamer (binary) and Sensor Monitor (cvs) iPhone/Android apps
 * @author  Bogdan Marinescu & Mihail Stoyanov
 * @version 1.0
 * @see     
 *
 * Copyright (c) 2013
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include "SensorDataParser.h"

static int parse_sensor_stream(char *buf, SENSOR_DATA *pd) {
    pd->ax = *(float*)(buf + 4);
    pd->ay = *(float*)(buf + 8);
    pd->az = *(float*)(buf + 12);
        
    pd->gx = *(float*)(buf + 16);
    pd->gy = *(float*)(buf + 20);
    pd->gz = *(float*)(buf + 24);
        
    pd->tx = *(double*)(buf + 28);
    pd->ty = *(double*)(buf + 36);
    pd->tz = *(double*)(buf + 44);
    
    pd->hm = *(double*)(buf + 52);
    pd->ht = *(double*)(buf + 60);
        
    pd->latitude  = *(double*)(buf + 68);
    pd->longitude = *(double*)(buf + 76);
    pd->altitude  = *(double*)(buf + 84);
    
    pd->proximity  = buf[92];
    
    pd->touch1  = buf[93];
    pd->touch1x = *(int*)(buf + 94);
    pd->touch1y = *(int*)(buf + 98);
    
    pd->touch2  = buf[102];
    pd->touch2x = *(int*)(buf + 103);
    pd->touch2y = *(int*)(buf + 107);
    
    return 1;
}

static int parse_sensor_csv(char *s, double *pdest, int maxn) {
    if (NULL == s || strlen(s) == 0)
        return 0;
    int crt = 0;
    char *p = s, *temp;
    
    while (crt < maxn) {
        temp = strchr(p, ',');
        if (temp)
            *temp = 0;
        while (*p == ' ')
            p ++;
        if (sscanf(p, "%lf", pdest + crt) != 1) {
            return 0;
        }
        crt ++;
        if (NULL == temp)
            break;
        else
            p = temp + 1;
    }
    return crt;
}

int parse_sensor_packet(char *buf, SENSOR_DATA *pd) {
    if (buf[0] == 'F' || buf[1] == 'S' || buf[3] == 107) {  // SensorStream binary packet
        parse_sensor_stream(buf, pd);
        
        return 1;
    } else {
        int nvals, i;
        double vals[MAX_CSV_VALS];
        
        if ((nvals = parse_sensor_csv(buf, vals, MAX_CSV_VALS)) != 0) { // SensorMonitor or generic CSV packet
            // Format: timestamp, [sensor_id, x, y, z]+
            // sensor_id: 3-acc, 4-gyr, 5-mag
            if ((nvals - 1) % 3 != 0)
                return 0;
            i = 0;
            pd->ax = vals[i+1];
            pd->ay = vals[i+2];
            pd->az = vals[i+3];
            
            pd->touch1 = 0;
            pd->touch2 = 0;
            
            return 2;
        };
    };
    return 0;
}