Agronode_feeder/feeder_lib/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 "libfixmath/libfixmath/fixmath.h"

//#include "../../status.h"
#include "../../feeder.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 sensob_item_position_data_t
{
        float lat;
        float lon;
        float 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 jestly 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;
static uint8_t * pd;
static uint16_t sd;

static uint16_t seq = 0;

static void (* feederLog)(int priority, const char * fmt, ...);

int setLog(void * func)
{
    feederLog = func;
    return 0;
}

int init(void * param)
{
    ld = pd = NULL;
    sd = 0;
    return 0;
}

/*
#define STATUS_NONE	0


void stateMachine(uint8_t c)
{
    static uint8_t status = STATUS_NONE;
    static uint6
    
    switch (status)
    {
	case STATUS_NONE:
	    if (c == 0xff)
		status = STATUS_DELIMITER;
		size = 1;
		exp
	    break;
	case STATUS_DELIMITER:
	    
	default:
	    break;
    }
}*/

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


unsigned int
process(void *lib_data, int socket, unsigned char *data,
        unsigned int length, unsigned long long int *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))
    {
	ld = malloc(length);
	if (ld == NULL)
	{
    	    feederLog(LOG_WARNING, "sensob: Can not allocate data buffer\n");
	    
	    return 0;
	}
	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 0;
	}
	
	header = (sensob_header_t *)pd;
//        feederLog(LOG_DEBUG, "sensob: Id %llu, seq %d, size %d\n", byteSwap64(header->id), header->seq, header->size);
        feederLog(LOG_DEBUG, "sensob: Id %llu, seq %d, size %d\n", header->id, header->seq, header->size);
        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);
    while (header->size > 0)
    {
	item = (sensob_item_t *)pd;
	if ((tm_prev != 0) && (item->timestamp != tm_prev))
	    break;
	
	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:
		if (*((fix16_t *)temp) == fix16_overflow)
		    val = NAN;
		else
		    val = fix16_to_dbl(*((fix16_t *)temp));
		break;
	}
	pd += sensob_types_len[item->type];
	feederLog(LOG_DEBUG, "sensob: item sensor %lu, value %G, type %d\n", item->sensor, val, item->type);
	header->size--;
	
	if (item->type == SENSOB_TYPE_POSITION)
	{
	    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] = ((sensob_item_position_data_t *) temp)->alt;
    	    result_array[3] = 1.0;
    	    result_array[4] = 0.0;
    	    sensors[0] = sensors[1] = sensors[2] = sensors[3] = sensors[4] = 0x10;

	    ret = 5;
	    break;
	}
	else if ((item->sensor == 510010000) || (item->sensor == 510020000))//todo: reflektujme i pripadne serial (posledni 4 nuly)
	{
	    if (item->sensor == 510010000)
	    {
		lat = val;
		pos_flag |= 1;
	    }
	    if (item->sensor == 510020000)
	    {
		lon = val;
		pos_flag |= 2;
	    }
	    if (pos_flag == 3)
	    {
		pos_flag = 0;
		*tm = item->timestamp;		//todo: ted bere posledni cas, musime predelat na casy jednotlivych mereni
	        *type = VALUES_TYPE_POS;
	        result_array[0] = lat;
	        result_array[1] = 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;
		break;
	    }
	}
	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: pustime nan??? */
	    {
/*		if (item->sensor == 450050000)		//BEWARE: this is hack for Kofola - je nastavena blba kalibrace prutokomera takze ji tady prepocteme ... a rovnou i na litro vteriny
		{
		    val = (val / 144.0 * 84.0) / 3.6;
		}
*/	
		sensors[i] = item->sensor;
		result_array[i] = val;
		i++;
		*tm = item->timestamp;		//todo: ted bere posledni cas, musime predelat na casy jednotlivych mereni - urgentni
		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;
		break;
	    }
/*	    else
    		feederLog(LOG_WARNING, "sensob: Invalid item\n");*/ //todo: vyse if na NaN

//	    break;		//todo: viz timestamp flag, pokud nebude musi se brat posledni znamej timestamp
	}
    }
    if (header->size == 0)
    {
	free(ld);
	ld = pd = NULL;
	sd = 0;
    }

    return ret;

/*
    if (data != NULL)
    {
    
    if ((data[0] != ':') && (data[0] != ';'))
	raw = 1;
    else
	raw = 0;
	
    if (!raw)
    {
	switch (data[0])
	{
	    case ':':compressed = 1;
		break;
	    case ';':compressed = 0;
		break;
	}
		
        dlength = ((unsigned long)data[1]) << 24;
	dlength += ((unsigned long)data[2]) << 16;
	dlength += ((unsigned long)data[3]) << 8;
	dlength += ((unsigned long)data[4]);
	ddata = malloc(dlength + 1);
	if (ddata == NULL)
	{
	    feederLog(LOG_WARNING, "senso: Can not allocate decompress buffer\n");
	    return 0;
	}
	feederLog(LOG_DEBUG, "senso: Packet length %ld, data length %ld%s\n", length, dlength, compressed ? ", compressed" : "");

	length -= HEADER_SIZE;
	if (ddata != NULL)
	{
	    if (compressed)
	    {
	        dres = uncompress((unsigned char *)ddata, (uLongf *)&dlength, data + HEADER_SIZE, length);
	    }
	    else
	    {
		memcpy(ddata, data + HEADER_SIZE, length);
		dres = Z_OK;
	    }
	    if (dres == Z_OK)
	    {
		ddata[dlength] = 0;
		feederLog(LOG_DEBUG, "senso: Data, cr: %f%% : \n%s", ((double)length / (double)dlength) * 100.0, ddata);
		message = ddata;
    	    }
	    else
	    {
		zerr(dres);
	    }
	}
    }
    else
    {
	ddata = malloc(length + 1);
	if (ddata == NULL)
	{
	    feederLog(LOG_WARNING, "Can not allocate data buffer\n");
	    return 0;
	}
	memcpy(ddata, data, length);
	ddata[length] = 0;
	feederLog(LOG_DEBUG, "Data raw: %s\n", data);
	message = ddata;
    }
    }


    if (message != NULL)
    {
	c = strchr(message, '\n');
	*c = 0;
	*id = dataCheck(message, tm, &result_array[0], (unsigned long int *)&sensors[0]);
	message = c + 1;
	ret = 1;
	if (strlen(message) == 0)
	{
	    free(ddata);
	    message = ddata = NULL;
	}
    }
    
    *type = VALUES_TYPE_OBS;
    return ret;*/
    return 0;
}

int reply(void *lib_data, int socket, unsigned char *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);
}

int cmd(void *lib_data, int socket, unsigned char *cmd, unsigned int cmd_len, unsigned char *data)
{
    (void)lib_data;
    (void)socket;

    memcpy(data, cmd, cmd_len);
    return cmd_len;
}