Agronode_feeder/feeder_lib/cra/sensob/sensob.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <syslog.h>
#include <stdint.h>
#include <math.h>

#include <confuse.h>

#include "libfixmath/libfixmath/fixmath.h"

//#include "../../status.h"
#include "../../../feeder.h"

#include "sensob.h"


typedef struct sensob_item_t
{
	uint8_t flag;
	uint32_t sensor;
	uint32_t timestamp;
	uint8_t type;
} __attribute__((packed)) sensob_item_t;

typedef struct sensob_header_t
{
	uint8_t delimiter;
	uint64_t id;
	uint16_t seq;
	uint16_t size;
} __attribute__((packed)) sensob_header_t;

typedef struct sensob_footer_t
{
	uint16_t crc;
} __attribute__((packed)) sensob_footer_t;

typedef struct sensob_ack_t
{
        uint8_t delimiter;
	uint16_t seq;
} __attribute__((packed)) sensob_ack_t;


typedef struct sensoblite_t
{
    union
    {
        uint32_t raw;
//        struct sensoblite_timespec_t timespec;
    } timespec;
} __attribute__((packed)) sensoblite_t;

typedef struct sensobliteitem_t
{
    uint32_t sensor_id;
    uint32_t value;
} __attribute__((packed)) sensobliteitem_t;

typedef struct sensoblite_ack_t
{
    uint32_t timestamp;
    uint32_t dummy;
} __attribute__((packed)) sensoblite_ack_t;


typedef struct sensobtinymessage_t
{
    uint8_t sensor_id_meta;
    uint16_t tm_diff;
} __attribute__((packed)) sensobtinymessage_t;

typedef struct sensob_item_position_data_t
{
        double lat;
        double lon;
        double alt;
} __attribute__((packed)) sensob_item_position_data_t;

uint8_t sensob_types_len[] =
{
	sizeof(uint8_t), 
	sizeof(int8_t), 
	sizeof(uint16_t), 
	sizeof(int16_t), 
	sizeof(uint32_t), 
	sizeof(int32_t), 
	sizeof(uint64_t), 
	sizeof(int64_t), 
	sizeof(float), 
	sizeof(double), 
	sizeof(time_t),
	sizeof(uint8_t), 
	sizeof(sensob_item_position_data_t),
	sizeof(uint32_t),
	sizeof(fix16_t)
};

// todo: oifovat podle endianes
uint16_t byteSwap16(uint16_t value)
{
    uint16_t swapped;
    swapped = (((0x00FF) & (value >> 8)) |
              ((0xFF00) & (value << 8)));
    return swapped;
}

uint32_t byteSwap32(uint32_t value)
{
    uint32_t swapped;
    swapped = (((0x000000FF) & (value >> 24)) |
              ((0x0000FF00) & (value >> 8)) |
              ((0x00FF0000) & (value << 8)) |
              ((0xFF000000) & (value << 24)));
    return swapped;
}

uint64_t byteSwap64(uint64_t value)
{
    uint64_t swapped;
    swapped = (((0x00000000000000FFULL) & (value >> 56)) |
              ((0x000000000000FF00ULL) & (value >> 40)) |
              ((0x0000000000FF0000ULL) & (value >> 24)) |
              ((0x00000000FF000000ULL) & (value >> 8)) |
              ((0x000000FF00000000ULL) & (value << 8)) |
              ((0x0000FF0000000000ULL) & (value << 24)) |
              ((0x00FF000000000000ULL) & (value << 40)) |
              ((0xFF00000000000000ULL) & (value << 56)));
    return swapped;
}

//todo: kontrolvat jestli to jde - tj len=2,4,8,16 ...
void memSwap(uint8_t * data, uint8_t len)
{
	uint8_t temp;
	uint8_t i;

	for (i = 0; i < len / 2; i++)
	{
		temp = data[i];
		data[i] = data[len - 1 - i];
		data[len - 1 - i] = temp;
	}
}


#define SENSOB_TYPE_UINT8	0
#define SENSOB_TYPE_INT8	1
#define SENSOB_TYPE_UINT16	2
#define SENSOB_TYPE_INT16	3
#define SENSOB_TYPE_UINT32	4
#define SENSOB_TYPE_INT32	5
#define SENSOB_TYPE_UINT64	6
#define SENSOB_TYPE_INT64	7
#define SENSOB_TYPE_FLOAT	8
#define SENSOB_TYPE_DOUBLE	9
#define SENSOB_TYPE_TIMESTAMP	10
#define SENSOB_TYPE_ERROR	11
#define SENSOB_TYPE_POSITION	12
#define SENSOB_TYPE_ALERT	13
#define SENSOB_TYPE_FIX16	14



static uint8_t * ld = NULL;
static uint8_t * pd = NULL;
static uint16_t sd = 0;

static uint16_t seq = 0;


uint8_t * findDelimiter(uint8_t * data, uint16_t len)
{
    uint8_t * res;

    for (res = data; res < data + len; res++)
    {
	if (*res == 0xff)
	    return res;
    }
    return NULL;
}


#define SENSOBLITE_MASK_FIX16 0x00000000
#define SENSOBLITE_MASK_FLOAT 0x40000000
#define SENSOBLITE_MASK_INT32 0x80000000
#define SENSOBLITE_MASK_RES   0xC0000000

int
sensoblite_parse(uint8_t * data,
        uint16_t length, time_t * tm,
        double *result_array, uint64_t * sensors, unsigned int *type)
{
    struct sensoblite_t * sensoblite = (struct sensoblite_t *)data;
    struct sensobliteitem_t * sensobliteitem;
    

    uint32_t val_swapped;
    float val_float;
    uint8_t i;
    int l;
    
    *tm = byteSwap32(sensoblite->timespec.raw);
    
    l = (length - sizeof(struct sensoblite_t)) / sizeof(struct sensobliteitem_t);
    feederLog(LOG_DEBUG, "sensoblite: Analyzing data, size %d   %d\n", l, length);

    i = 0;
    while (i < l)
    {
	sensobliteitem = (struct sensobliteitem_t *)(data + sizeof(struct sensoblite_t) + (i * sizeof(struct sensobliteitem_t)));
	
	sensors[i] = byteSwap32(sensobliteitem->sensor_id);
	if ((sensors[i] & 0xC0000000) == SENSOBLITE_MASK_FIX16)
	{
	    if (((fix16_t)(byteSwap32((uint32_t)(sensobliteitem->value)))) == fix16_overflow)
		result_array[i] = NAN;
	    else
		result_array[i] = fix16_to_dbl((fix16_t)(byteSwap32((uint32_t)(sensobliteitem->value))));
	}
	if ((sensors[i] & 0xC0000000) == SENSOBLITE_MASK_FLOAT)
	{
	    val_swapped = byteSwap32((uint32_t)(sensobliteitem->value));
	//result_array[0] = byteSwap32((uint32_t)(sensobliteitem->value));
	    memcpy(&val_float, &val_swapped, 4);
	    result_array[i] = val_float;
	}
	if ((sensors[i] & 0xC0000000) == SENSOBLITE_MASK_INT32)
	{
	    result_array[i] = (float)(byteSwap32((uint32_t)(sensobliteitem->value)));
	}
        sensors[i] &= 0xBFFFFFFF;
        i++;
    }
    *type = VALUES_TYPE_OBS;

    return l;
}


typedef struct meta_u_t
{
    uint64_t id;
    double multi;
} meta_u_t;

typedef struct sensobtinymeta_t
{
    struct meta_u_t u[32];
} sensobtinymeta_t;

/*
static struct sensobtinymeta_t metas[24] =//todo: konfigurovat toto a casovou zonu devicy v conf, nebo to bude posilat noda
{
    {{{100000000,1.0}}},						//0
    {{{340340092,1.0},{340350004,1.0},{360200000,1.0}}},		//1
    {{{360200000,1.0}}},						//2
    {{{340070001,1.0},{410050001,1.0},{460010001,1.0}}},		//3
    {{{480020001,1.0},{490010001,1.0}}},				//4
    {{{470020001,1.0},{470010001,1.0}}},				//5
    {{{540090004,10.0},{550020004,10.0},{340370004,10.0}}},		//6
    {{{620030000,1.0},{480080000,1.0},{820010000,1.0},{830010000,1.0},{470160000,1.0},{470180000,1.0},{470170000,1.0},{340420000,1.0},{460080000,1.0},{460090000,1.0},{410180000,1.0},{840010000,1.0},{840020000,1.0}}},		//7
    {{{710010000,1.0},{340260000,1.0}}},	//8
    {{{620030000,1.0},{480080000,1.0},{470160000,1.0},{470180000,1.0},{340420000,1.0},{460090000,1.0},{410180000,1.0}}},	//9
    {{{820010000,1.0},{830010000,1.0},{470170000,1.0},{460080000,1.0},{840010000,1.0},{840020000,1.0}}},		//10
    {{{690020001,1.0},{340230001,1.0},{700020001,1.0}}},		//11
    
    {{{0,0.0}}}
};
*/

static struct sensobtinymeta_t metas[24];//todo: konfigurovat toto a casovou zonu devicy v conf, nebo to bude posilat noda

#define SENSOB_TINY_DATA_SIZE 3
int
sensobtiny_parse(uint8_t * data,
        uint16_t length, 
        uint32_t time_of_receive,
        time_t * tm,
        double *result_array, uint64_t * sensors, unsigned int *type)
{
    uint8_t meta_id;
    uint16_t i, j;
    fix16_t val;
    struct sensobtinymessage_t * sensobtinymessage = (struct sensobtinymessage_t *)data;
    struct tm t;


    static cfg_t * cfg = NULL;
    cfg_t * cfg_tiny, * cfg_item;
    
    (void)length;


    cfg_opt_t cfg_items[] = 
    {
        CFG_INT((char *)"id", 0, CFGF_NONE),
        CFG_FLOAT((char *)"multi", 0, CFGF_NONE),
	CFG_END()
    };

    cfg_opt_t cfg_tinys[] = 
    {
        CFG_INT((char *)"no", 0, CFGF_NONE),
        CFG_SEC((char *)"item:", cfg_items, CFGF_MULTI),
	CFG_END()
    };

    cfg_opt_t cfg_opts[] = 
    {
        CFG_SEC((char *)"tiny:", cfg_tinys, CFGF_MULTI),
	CFG_END()
    };

    if (cfg == NULL)
    {
        cfg = cfg_init(cfg_opts, CFGF_NONE);
        if (cfg_parse(cfg, "sensob.conf") != CFG_SUCCESS)
	{
    	    feederLog(LOG_WARNING, "sensobtiny: Can not read sensob.conf file\n");
    	    cfg = NULL;
	}
	else
	{
	    for (i = 0; i < 24; i++)
		metas[i].u[0].id = 0;
	    for (i = 0; i < cfg_size(cfg, "tiny:"); i++)
	    {
		cfg_tiny = cfg_getnsec(cfg, "tiny:", i);
//		meta_id = atoi(cfg_title(cfg_tiny));
		meta_id = cfg_getint(cfg_tiny, "no");
		for (j = 0; j < cfg_size(cfg_tiny, "item:"); j++)
		{
		    cfg_item = cfg_getnsec(cfg_tiny, "item:", j);
		    metas[meta_id].u[j].id = cfg_getint(cfg_item, "id");
		    metas[meta_id].u[j].multi = cfg_getfloat(cfg_item, "multi");
		}
	    }
	}
    }




    feederLog(LOG_DEBUG, "sensobtiny: Analyzing data, size %d\n", length);
    
    meta_id = sensobtinymessage->sensor_id_meta - 0xA0;
    /*
    localtime_r((time_t *)&time_of_receive, &t);
    time_of_receive = mktime(&t);
    */
    *tm = (time_of_receive / 60) * 60  - (byteSwap16(sensobtinymessage->tm_diff) * 60);
    
    localtime_r((time_t *)tm, &t);//todo: z konfigurace
    *tm = timegm(&t);
    
    length -= sizeof(struct sensobtinymessage_t);
    data += sizeof(struct sensobtinymessage_t);
    feederLog(LOG_DEBUG, "sensobtiny: time of receive %lld, diff %lld, timestamp %lld\n", time_of_receive, byteSwap16(sensobtinymessage->tm_diff), *tm);
    feederLog(LOG_DEBUG, "sensobtiny: meta id %lld, diff %lld, timestamp %lld\n", meta_id);
    i = 0;
    
    if (metas[meta_id].u[0].id != 0)
    {
        while (length > 0)
	{
    	    feederLog(LOG_DEBUG, "sensobtiny: field %d\n", i);

	
    	    sensors[i] = metas[meta_id].u[i].id;
    	    val = 0;
    	    memSwap(data, SENSOB_TINY_DATA_SIZE);
    	    memcpy(((uint8_t *)&val) + 1, data, SENSOB_TINY_DATA_SIZE);
    	    feederLog(LOG_DEBUG, "sensobtiny: field val 0x%LX\n", (uint32_t)val);
    	    data += SENSOB_TINY_DATA_SIZE;
    	    if (val == fix16_overflow)
    	    {
    		result_array[i] = NAN;
    	    }
    	    else
//    	    	val = fix16_div(val, fix16_from_dbl(metas[meta_id].u[i].multi));//todo:
	    {
        	result_array[i] = round(fix16_to_dbl(val) * 100.0) / 100.0;
        	result_array[i] /= metas[meta_id].u[i].multi;
    	    }
    	    i++;
    	    length -= SENSOB_TINY_DATA_SIZE;
	}
    }
    else
	feederLog(LOG_DEBUG, "sensobtiny: Unknowen meta id %d\n", meta_id);
    
    *type = VALUES_TYPE_OBS;
/*    
    *tm = byteSwap32(sensoblite->timespec.raw);
    sensors[0] = byteSwap32(sensoblite->sensor_id);
    result_array[0] = fix16_to_dbl((fix16_t)(byteSwap32((uint32_t)(sensoblite->value))));

    *type = VALUES_TYPE_OBS;
*/
    return i;
}

int
sensob_parse(uint8_t * data,
        uint16_t length, uint64_t * id, time_t * tm,
        double *result_array, uint64_t * sensors, unsigned int *type)
{
    static struct sensob_header_t * header;
    struct sensob_item_t * item;
    struct sensob_footer_t * footer;
    double val;
    unsigned int ret, i;

    static double lat, lon;		//todo: do lib_data
    static uint8_t pos_flag = 0;
    uint8_t temp[256];
    time_t tm_prev;

    feederLog(LOG_DEBUG, "sensob: Analyzing data, size %d\n", length);

    ret = 0;
    if ((data != NULL) && (length > 0))
    {
	if (ld != NULL)
	{
	    free(ld);
	    ld = pd = NULL;
	    sd = 0;
	}
    
	ld = malloc(length);
	if (ld == NULL)
	{
    	    feederLog(LOG_WARNING, "sensob: Can not allocate data buffer\n");
	    return -1;
	}
	memcpy(ld, data, length);
	pd = NULL;
	sd = length;

	pd = findDelimiter(ld, sd);
        if (pd == NULL)
	{
    	    feederLog(LOG_WARNING, "sensob: No delimiter\n");
    	    free(ld);
    	    ld = NULL;
	    return -1;
	}
	
	header = (sensob_header_t *)pd;
        feederLog(LOG_DEBUG, "sensob: Id %llu, seq %d, size %d\n", header->id, header->seq, header->size);
        
        if (header->size > 100)
	{
    	    feederLog(LOG_WARNING, "sensob: maximum item size of packet overstepped\n");
    	    free(ld);
    	    ld = NULL;
	    return -1;
	}
    
        seq = header->seq;
	pd += sizeof(struct sensob_header_t);
    }

    if (pd == NULL)
    {
	return 0;
    }
    

//    *id = byteSwap64(header->id);
    *id = header->id;

    i = 0;
    tm_prev = 0;
    feederLog(LOG_DEBUG, "sensob: header size %d\n", header->size);
    if (header->size > 0)
    {
	item = (sensob_item_t *)pd;
	
	pd += sizeof(struct sensob_item_t);
	val = 0.0;

	memcpy(temp, pd, sensob_types_len[item->type]);
//	memSwap(temp, sensob_types_len[item->type]);

	switch (item->type)
	{
	    case SENSOB_TYPE_UINT8:
		val = (uint8_t)*pd;
		
		break;
	    case SENSOB_TYPE_FLOAT:
//		val = byteSwap32(*((float *)pd));
		val = *((float *)temp);
		break;
	    case SENSOB_TYPE_DOUBLE:
//		val = byteSwap64(*((double *)pd));
		val = *((double *)temp);
		break;
	    case SENSOB_TYPE_ERROR:
		val = (uint8_t)*pd;
		break;
	    case SENSOB_TYPE_ALERT:
		val = (uint32_t)*pd;
		break;
	    case SENSOB_TYPE_POSITION:
		break;
	    case SENSOB_TYPE_FIX16:
		val = fix16_to_dbl(*((fix16_t *)temp));
		break;
	}
	pd += sensob_types_len[item->type];
	feederLog(LOG_DEBUG, "sensob: item sensor %lu, value %f, type %d\n", item->sensor, val, item->type);
	header->size--;
	
	if (item->type == SENSOB_TYPE_POSITION)
	{
	    lat = val;
	    lon = val;
	    pos_flag = 0;
	    *tm = item->timestamp;		//todo: ted bere posledni cas, musime predelat na casy jednotlivych mereni
	    *type = VALUES_TYPE_POS;
	    result_array[0] = ((sensob_item_position_data_t *) temp)->lat;
	    result_array[1] = ((sensob_item_position_data_t *) temp)->lon;
    	    result_array[2] = 0.0;
    	    result_array[3] = 1.0;
    	    result_array[4] = 0.0;
    	    sensors[0] = sensors[1] = sensors[2] = sensors[3] = sensors[4] = 0x10;

	    ret = 5;
	}
	else
	{
/*	    if ((item->sensor != 4294967295) && (item->timestamp != 4294967295) && (!isnan(val)))	//BEWARE: tohle je kontrola proti lugioho meteoskam - obcas poslou FFFFFFF -> kontrola tamniho posilani*/
//	    if ((!isnan(val)))		//todo: poustime i nan jako oznaceni nevalidniho mereni
	    {
		sensors[i] = item->sensor;
		result_array[i] = val;
		i++;
		*tm = item->timestamp;
		feederLog(LOG_DEBUG, "sensob: Timestamp %lu\n", item->timestamp);
		
	        if ((item->type == SENSOB_TYPE_ERROR) || (item->type == SENSOB_TYPE_ALERT))
	    	    *type = VALUES_TYPE_ALERT;
		else
		    *type = VALUES_TYPE_OBS;
		
		ret = i;
	    }
	}
    }
    if (header->size == 0)
    {
	free(ld);
	ld = pd = NULL;
	sd = 0;
    }

    return ret;
}

int sensob_reply(uint8_t * data)
{
    struct sensob_ack_t * ack;

    if (seq == 0)
	return 0;

    ack = (struct sensob_ack_t *)data;
    ack->delimiter = 0xff;
    ack->seq = seq;
    seq = 0;
//    feederLog(LOG_DEBUG, "sensob: replying\n");

    return sizeof(struct sensob_ack_t);
}