Agronode_feeder/feeder_lib/wli/wli.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <stdint.h>
#include <syslog.h>
#include <stdarg.h>
//#include <unistd.h>


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

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

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

#define WLI_STX 0x02
#define WLI_ETX 0x03
#define WLI_PCLASS_APPLICATION 0x31
#define WLI_PCLASS_PRESENTATION 0x32
#define WLI_PCLASS_KEEPALIVE 0x34

#define ESCAPE_HDR 0xDB
#define ESCAPE_STX 0xD2
#define ESCAPE_ETX 0xD3
#define ESCAPE_DB  0xDD

// Classes
//#define APP_CLASS          0x31
//#define PRESENTATION_CLASS 0x32
// #define KEEP_ALIVE_CLASS   0x34
	//
	// 	// APP MSG Types
	// 	#define APP_GPS_MSG     0xC9
	// 	#define APP_NO_GPS_MSG  0xE4
	// 	#define APP_LOC_BOUND   0xCD
	// 	#define APP_IO_STATE    0xCB
	// 	#define APP_LOGIN       0xFE
	// 	#define APP_LOGOUT      0xFD
	// 	#define APP_LOW_BAT     0xFB
	// 	#define APP_EMERGENCY   0x0a
	// 	#define APP_IN_COVERAGE 0xF8
	// 	#define APP_ACK         0xFC
	// 	#define APP_IP_CHANGED  0x17
	//
// GPS FIELD NUMBERS
#define FLD_GPS_FORMAT       1
#define FLD_GPS_LONG 	     2
#define FLD_GPS_SHORT        5
#define FLD_GPS_REASON       6
#define FLD_GPS_STOP_LEN     7
#define FLD_GPS_STOP_TIME    8
#define FLD_GPS_DISTANCE     9
#define FLD_GPS_START_STATE  10
#define FLD_GPS_MSG_NUM      19
#define FLD_GPS_ACC_ALRM_CNT 20
#define FLD_GPS_A2D          22
#define FLD_GPS_ENGINE_STATE 26
#define FLD_GPS_NUM_SAT      27
#define FLD_GPS_SV_SNR       28
#define FLD_GPS_SAT_USED     29
#define FLD_GPS_HDOP         30
#define FLD_GPS_CELL_MCC     35
#define FLD_GPS_CELL_MNC     36
#define FLD_GPS_CELL_LAC     37
#define FLD_GPS_CELL_ID      38
#define FLD_GPS_GSM_RSSI     39

// COMMON FIELD NUMBERS
#define FLD_GLB_PWR          246
#define FLD_GLB_SERIAL_NUM   247
#define FLD_GLB_BAT_CONV     249
#define FLD_GLB_INTERN_TEMP  250
#define FLD_GLB_BAT_AT_STRT  251
#define FLD_GLB_BAT_NOW      252
#define FLD_GLB_TIME         255

// Reserved Fields to be ignored
#define FLD_GLB_RESERVED_248 248
#define FLD_GLB_RESERVED_40  40



typedef struct wli_packet_t
{
    uint8_t stx;
    uint8_t packet_class;
    /* class specific data */
    uint8_t data[1];
    uint8_t etx;
} __attribute__((packed)) wli_packet_t;


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

void arraySwap4(uint8_t * array)
{
    uint8_t temp;

    temp = array[0];
    array[0] = array[3];
    array[3] = temp;
    temp = array[1];
    array[1] = array[2];
    array[2] = temp;
}

//----------------------------------------------------------------------------------------------

#define TYPE_START 0
#define TYPE_POS 1
#define TYPE_OBS 2

typedef struct rec_t
{
    struct rec_t * next;
    uint8_t type;
    time_t timestamp;
} rec_t;

typedef struct pos_t
{
    struct rec_t * next;
    uint8_t type;
    time_t timestamp;
    float lat;
    float lon;
} pos_t;

typedef struct obs_t
{
    struct rec_t * next;
    uint8_t type;
    time_t timestamp;
    uint64_t sensor_id;
    float val;
} obs_t;


int recAdd(struct rec_t * list, struct rec_t * rec)
{
    if (list == NULL)
	return -1;

    while (list->next != NULL)
	list = list->next;

    list->next = rec;
    return 0;
}

struct rec_t * recAlloc(int type)
{
    struct rec_t * rec;
    
    switch (type)
    {
	case TYPE_POS:
	    rec = malloc(sizeof(struct pos_t));
	break;
	case TYPE_OBS:
	    rec = malloc(sizeof(struct obs_t));
	break;
	default:
	    return NULL;
	break;
    }
    if (rec == NULL)
	return NULL;
    
    rec->type = type;
    rec->next = NULL;
    return rec;
}

void recCheckTimestamp(struct rec_t * rec, time_t timestamp)
{
    while (rec != NULL)
    {
	if (rec->timestamp == 0)
	    rec->timestamp = timestamp;
	rec = rec->next;
    }
}

int wliPwr(char * b, struct rec_t * list)
{
    char *s;
    int i;
    struct obs_t * obs;
    
    i = 0;
    while (s = strchr(b, ','), s != NULL)//todo: ignoruje posledni parametr
    {
	*s = '\0';
	if (i == 3)	//interal voltage
	{
	    if (obs = (struct obs_t *)recAlloc(TYPE_OBS), obs == NULL)
		return -1;
	    obs->timestamp = 0;	//ted jeste nevim, musime pockat na doparseni celeho paketu a vyplnit az pri odevzdani dat
	    obs->sensor_id = 360190000;
	    obs->val = ((float)atoi(b)) / 100.0;
	    recAdd(list, (struct rec_t *)obs);
	}
	b = s + 1;
	i++;
    }
    return 0;
}

int wliTemp(char * b, struct rec_t * list)
{
    struct obs_t * obs;
    
    if (obs = (struct obs_t *)recAlloc(TYPE_OBS), obs == NULL)
	return -1;
    obs->timestamp = 0;	//ted jeste nevim, musime pockat na doparseni celeho paketu a vyplnit az pri odevzdani dat
    obs->sensor_id = 340290000;
    obs->val = (0.98 * ((float)atoi(b))) - 60.0;
    recAdd(list, (struct rec_t *)obs);
    return 0;
}

int wliGpsShort(uint8_t * binVal, struct rec_t * list)
{
    int minutsInDay, minutes, hours, dayOfMonth, year, month;
    struct tm gps_tm;
    struct pos_t * pos;
    struct obs_t * obs;
    float speed;
    float course;
    
    if (pos = (struct pos_t *)recAlloc(TYPE_POS), pos == NULL)
	return -1;
	
    pos->type = TYPE_POS;
    pos->next = NULL;
    pos->lat = ((float)((binVal[0]<<24) + (binVal[1]<<16) + (binVal[2]<<8) + binVal[3]))/600000;
    pos->lon = ((float)((binVal[4]<<24) + (binVal[5]<<16) + (binVal[6]<<8) + binVal[7]))/600000;
//    fprintf(stdout, "Latitude(deg):%.4f, Longitude(deg):%.4f", pos->lat, pos->lon);

// Speed over Ground, the value is 0.1 knots, so divide by 10 to get it by knots
// To convert to MPH: divide by 1.151
// To convert to KPH: divide by 1.852 - yes, converted

    speed = (((binVal[8]<<8) + binVal[9])/10) / 1.852;
//    fprintf(stdout, ", Speed (km/h): %.2f", pos->speed);

    // Get date & time of GPS message
    minutsInDay = ((binVal[10] & 0x7)<<8) + binVal[11];
    minutes = minutsInDay%60;
    hours = minutsInDay/60;
    dayOfMonth = ((binVal[10] & 0xF8)>>3);
    month = (binVal[12] & 0x0F);
    year = 2000 + ((binVal[12] & 0xF0)>>4);
    // ***** Year bug workaround (Alex: May 2013 ) ********
    
    // In short high precision extended format (4) year is sent using 4 bits (0...15).
    // We suppose that the century is 20 always
    // As a result in 2016,2032 etc the Year sending value will be 0 + 2000 = 2000 
    // The workaround is based on the following: 
    //  when the WLI unit is running and communcates with host,the UTC year received from GPS may be:
    //  same as host computer year;
    //  one year after then the computer UTC year ( e.g., due to computer clock delay at Dec 31 - Jan 01)
    //  one year before then the computer UTC year ( e.g., due to GPS sending  delay)
    // The workaround algorithm implements the assertion that GPS UTC time can not be more then 14 years before
    // current UTC time as set on the computer

    // Current terminal time is sent as time_t value. This parameter value is the number of seconds
    // elapsed since midnight (00:00:00), January 1, 1970, coordinated universal time, according to the
    // system clock.
    time_t ltime;
    time(&ltime);
    struct tm gmt;
    // Breakdown of the time is done using gmtime_r and NOT gmtime, as gmtime_r is NOT thread safe.
    gmtime_r(&ltime, &gmt);
    // Calculate current year (full value, e.g., 2013). tm_year field of struct tm is a year since 1900
    short shCurrentYear = gmt.tm_year + 1900; 
    int DeltaYear = (int)(shCurrentYear - year);
    while (DeltaYear > 14)
    {
	year += 16;
	DeltaYear -= 16;
    }
    // ***** Year bug workaround end (Alex: May 2013 ) ********
    gps_tm.tm_sec = binVal[13];
    gps_tm.tm_min = minutes;
    gps_tm.tm_hour = hours;
    gps_tm.tm_mday = dayOfMonth;
    gps_tm.tm_mon = month - 1;
    gps_tm.tm_year = year - 1900;
    gps_tm.tm_isdst = 0;
    pos->timestamp = timegm(&gps_tm);
    
//    fprintf(stdout, ", Time: %02d/%02d/%04d %2d:%2d:%2d", month, dayOfMonth, year, hours, minutes, binVal[13]);

    // Course is tenth of degrees, so divided by 10 to get it in degrees
    course = ((float)(binVal[14]<<8) + binVal[15])/10;
    //fprintf(stdout, ", Course (deg): %.2f\n", pos->course);

    feederLog(LOG_DEBUG, "wli:, Latitude(deg):%.4f, Longitude(deg):%.4f, Speed (km/h): %.2f, Course (deg): %.2f, Timestamp (sec from 1.1.1970): %ld\n", pos->lat, pos->lon, speed, course, pos->timestamp);
    
    recAdd(list, (struct rec_t *)pos);

    if (obs = (struct obs_t *)recAlloc(TYPE_OBS), obs == NULL)
	return -1;
    obs->timestamp = pos->timestamp;
    obs->sensor_id = 440090000;
    obs->val = speed;
    recAdd(list, (struct rec_t *)obs);
    
    if (obs = (struct obs_t *)recAlloc(TYPE_OBS), obs == NULL)
	return -1;
    obs->timestamp = pos->timestamp;
    obs->sensor_id = 710030000;
    obs->val = course;
    recAdd(list, (struct rec_t *)obs);

    return 0;
}


void logHex(char * buffer, unsigned char * data, unsigned int length)
{
    unsigned int i;

    buffer[0] = 0;
    for (i = 0; i < length; i++)
    {
	sprintf(buffer + strlen(buffer), "%02X ", data[i]);
    }
}

typedef struct wli_application_header_t
{
    uint16_t sequence;
    uint16_t length;
    uint16_t checksum;
} __attribute__((packed)) wli_application_header_t;

unsigned int
wliApplication(struct wli_application_header_t * p)
{
    feederLog(LOG_DEBUG, "wli: Application message\n");
    feederLog(LOG_DEBUG, "wli: message sequence %d\n", byteSwap16(p->sequence));
    feederLog(LOG_DEBUG, "wli: app message len %d\n", byteSwap16(p->length));
    
    return 0;
}

unsigned int
wliPresentation(char * p)
{
    (void)p;
    
    feederLog(LOG_DEBUG, "wli: Presentation message\n");
    
    return 0;
}

enum { STATE_OUT = 0, STATE_PACKET_CLASS, STATE_APPLICATION, STATE_PRESENTATION, STATE_KEEPALIVE, STATE_APP_LENGTH, STATE_APP_CHECKSUM, STATE_MESS_TYPE, STATE_FIELD_NO, STATE_FIELD_VAL, STATE_FIELD_BIN, STATE_FIELD_BIN_LEN, STATE_FIELD_TEXT } wli_state;
#define STATE_BUFFER_SIZE 64

int wliStateMachine(uint8_t c, struct rec_t * list, unsigned long long int *id, time_t * timestamp)
{
    static unsigned int state = STATE_OUT;
    static uint8_t b[STATE_BUFFER_SIZE];
    static uint8_t bl = 0;
    static uint8_t esc = 0;
    static uint8_t bin_len = 0;
    static uint8_t field_no = 0;
    
    
    if (c == WLI_ETX)
    {
        feederLog(LOG_DEBUG, "wli: end of message\n");
	state = STATE_OUT;
	bl = 0;
	return 0;
    }
    
    if (esc)
    {
	if (c == 0xDD)
	    c = 0xDB;
	else
	    c -= 0xD0;
	esc = 0;
    }
    else if (c == 0xDB)
    {
	esc = 1;
	return 0;
    }
    
    b[bl++] = c;
    if (bl == STATE_BUFFER_SIZE)
	bl = 0;
    
//    feederLog(LOG_DEBUG, "wli: byte in %d, state %d \n", c, state);
    switch (state)
    {
	case STATE_OUT:
	    if (c == WLI_STX)
		state = STATE_PACKET_CLASS;
	break;
	case STATE_PACKET_CLASS:
	    switch (c)
	    {
		case WLI_PCLASS_APPLICATION:
		    state = STATE_APPLICATION;
		    bl = 0;
		break;
		case WLI_PCLASS_PRESENTATION:
		    state = STATE_PRESENTATION;
		    bl = 0;
		break;
	        case WLI_PCLASS_KEEPALIVE:
	    	    state = STATE_KEEPALIVE;
		break;
		default:
		    feederLog(LOG_ERR, "wli: bad packet class\n");
		return -1;
	    }
	break;
	case STATE_APPLICATION:
	    if (bl == sizeof(struct wli_application_header_t))
	    {
		wliApplication((struct wli_application_header_t *)b);
		bl = 0;
		state = STATE_MESS_TYPE;
	    }
	break;
	case STATE_MESS_TYPE:
	    if (bl == 2)
	    {
		feederLog(LOG_DEBUG, "wli: message type %d\n", b[0]);
		bl = 0;
		state = STATE_FIELD_NO;
	    }
	break;
	case STATE_FIELD_NO:
	    if (bl == 2)
	    {
		field_no = b[0];
		feederLog(LOG_DEBUG, "wli: field number %d\n", field_no);
		bl = 0;
		state = STATE_FIELD_VAL;
	    }
	break;
	case STATE_FIELD_VAL:
	    if (bl == 1)
	    {
		if (c == 0xFF)
		{
		    bl = 0;
		    state = STATE_FIELD_BIN_LEN;
		}
		else
		{
		    state = STATE_FIELD_TEXT;
		}
	    }
	break;
	case STATE_FIELD_TEXT:
	    if (c == 0)
	    {
		switch (field_no)
		{
		    case FLD_GLB_SERIAL_NUM:
			* id = atol((char *)b);
		    break;
		    case FLD_GLB_PWR:
			wliPwr((char *)b, list);
		    break;
		    case FLD_GLB_TIME:
			*timestamp = (time_t)atol((char *)b);
			recCheckTimestamp(list, *timestamp);
		    break;
		    case FLD_GLB_INTERN_TEMP:
			wliTemp((char *)b, list);
		    break;
		}
	    
		feederLog(LOG_DEBUG, "wli: field text val: \"%s\"\n", b);
		bl = 0;
		state = STATE_FIELD_NO;
	    }
	break;
	case STATE_FIELD_BIN_LEN:
	    if (bl == 2)
	    {
		bin_len = byteSwap16(*((uint16_t *)b));
		bl = 0;
		state = STATE_FIELD_BIN;
	    }
	break;
	case STATE_FIELD_BIN:
	    if (bl == bin_len + 1)
	    {
		if (field_no == FLD_GPS_SHORT)
		{
		    wliGpsShort(b, list);
		}
		feederLog(LOG_DEBUG, "wli: field bin len: %d\n", bin_len);
		bl = 0;
		state = STATE_FIELD_NO;
	    }
	break;
	
    }
    return 0;
}


static struct rec_t list;

int init(void * param)
{
    (void)param;

    list.type = TYPE_START;
    list.next = NULL;

    return 0;
}

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)
{
    unsigned int ret = 0;
    unsigned int l;
    char buffer[1024];
//    struct wli_packet_t * p;
    unsigned int i;
    struct rec_t * rec, * rec_f;
    static time_t timestamp;

    (void)lib_data;
    (void)socket;
    
    if (data != NULL)
    {
        feederLog(LOG_DEBUG, "wli: Incoming data: len %d: \n", length);
        l = length;
        while (l > 0)
        {
	    logHex(buffer, data + length - l, (l > 16) ? 16:l);
	    feederLog(LOG_DEBUG, "%s\n", buffer);
	    l -= (l > 16) ? 16:l;
	}

	for (i = 0; i < length; i++)
	{
	    if (wliStateMachine(data[i], &list, id, &timestamp) < 0)
		return 0;
	}
    }
    
    rec = &list;
    if (rec->next != NULL)
    {
	feederLog(LOG_DEBUG, "wli: new record type %d\n", rec->next->type);
	if (rec->next->type == TYPE_POS)
	{
	    struct pos_t * pos;
	    
	    pos = (struct pos_t *)rec->next;
	    result_array[0] = (float)pos->lat;
	    result_array[1] = (float)pos->lon;
	    sensors[0] = sensors[1] = 0x10;
	    *type = VALUES_TYPE_POS;
	    ret = 2;
//			    *id = atol(aunit->imei);
	    *tm = pos->timestamp;
	}
	if (rec->next->type == TYPE_OBS)
	{
	    struct obs_t * obs;
	    
	    obs = (struct obs_t *)rec->next;
	    result_array[0] = (float)obs->val;
	    sensors[0] = obs->sensor_id;
	    *type = VALUES_TYPE_OBS;
	    ret = 1;
//			    *id = atol(aunit->imei);
			feederLog(LOG_DEBUG, "wli: timestmap %ld, %ld\n", timestamp, obs->timestamp);

	    if (obs->timestamp == 0)
		*tm = timestamp;
	    else
		*tm = obs->timestamp;
	}
	
	rec_f = rec->next;
	rec->next = rec->next->next;
	free(rec_f);
	
    }
    return ret;
}

int reply(void *lib_data, int socket, unsigned char *data)
{
    unsigned int temp_reply_len = 0;

    (void)lib_data;
    (void)socket;
    (void)data;

    feederLog(LOG_DEBUG, "wli: replying\n");
    return temp_reply_len;
}

int open(void *lib_data, int socket)
{
    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "wli: socket %d opened\n", socket);

    return 0;
}

int close(void *lib_data, int socket)
{
    (void)lib_data;
    (void)socket;
    
    feederLog(LOG_DEBUG, "wli: socket %d closed\n", socket);
    return 0;
}