Agronode_feeder/feeder_lib/acm4/acm4.c

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

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


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

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

typedef struct acm4_header_t {
    uint8_t start[14];
    char imei[16];
    uint8_t end[6];
} acm4_header_t;

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

static struct acm4_header_t acm4_header_valid = {
    {0x00, 0x0a, 0x02, 0x00, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
     0x20,
     0x20, 0x31},
    "1234567890123456",
    {0x20, 0x20, 0x20, 0x20, 0x20, 0x20}
};

#define ACM4_SHORT_LAT_DG(data) ((data[0] & 0xFE) >> 1)
#define ACM4_SHORT_LAT_MIN(data) (((data[0] & 0x01) << 5) | ((data[1] & 0xF8) >> 3))
#define ACM4_SHORT_LAT_SEC(data) (((data[1] & 0x07) << 4) | ((data[2] & 0xF0) >> 4))
#define ACM4_SHORT_LAT_SEC_HUN(data) (((data[2] & 0x0F) << 3) | ((data[3] & 0xE0) >> 5))
#define ACM4_SHORT_LON_DG(data) (((data[3] & 0x1F) << 3) | ((data[4] & 0xE0) >> 5))
#define ACM4_SHORT_LON_MIN(data) (((data[4] & 0x1F) << 1) | ((data[5] & 0x80) >> 7))
#define ACM4_SHORT_LON_SEC(data) (data[5] & 0x7F)
#define ACM4_SHORT_LON_SEC_HUN(data) ((data[6] & 0xFE) >> 1)

#define ACM4_SHORT_GPS_VALID(data) (data[6] & 0x01)
#define ACM4_SHORT_LON_SIGN(data) ((data[7] & 0x80) >> 7)
#define ACM4_SHORT_LAT_SIGN(data) ((data[7] & 0x40) >> 6)

#define ACM4_SHORT_DAY(data) (data[7] & 0x1F)
#define ACM4_SHORT_MON(data) ((data[8] & 0xF0) >> 4)
//#define ACM4_SHORT_YEAR(data) ((data[8] & 0x0F) + 2003)Year can not hold more than 16 years (4 bit wide). For year greater than 2018 next line is valid (workaround)
#define ACM4_SHORT_YEAR(data) ((data[8] & 0x0F) + 2019)
#define ACM4_SHORT_HOUR(data) ((data[9] & 0xF8) >> 3)
#define ACM4_SHORT_MIN(data) (((data[9] & 0x07) << 3) | ((data[10] & 0xE0) >> 5))
#define ACM4_SHORT_SEC(data) (((data[10] & 0x1F) << 1) | ((data[11] & 0x80) >> 7))

#define ACM4_SHORT_SPEED(data) (((data[11] & 0x7F) << 1) | ((data[12] & 0x80) >> 7))
#define ACM4_SHORT_AN(data) ((data[12] & 0x60) >> 5)
#define ACM4_SHORT_IN(data) (((((uint16_t)data[12]) & 0x001F) << 5) | ((((uint16_t)data[13]) & 0x00F8) >> 3))

#define ACM4_SHORT_ARB(data) (data[14] & 0x07)
#define ACM4_SHORT_ARB1(data) (data[15])
#define ACM4_SHORT_ARB2(data) (data[16])

#define ACM4_SHORT_SIZE 28

#define ARB_OUTPUTS_STATUS	0x00
#define ARB_POWER_MJ		0x01
#define ARB_AN_INPUTS_STATUS	0x02
#define ARB_SYSTEM_INFO		0x03
#define ARB_TEMPERATURE		0x04
#define ARB_USER1		0x05
#define ARB_USER2		0x06
#define ARB_BYTE1517_OLD_BB	0x07


typedef struct acm4_dev_t {
    char imei[17];
    uint8_t *data;
    uint16_t size;
    double lat, lon;
    uint16_t speed, an, in;
    uint8_t arb;
    uint8_t arb1, arb2;
    time_t ts;
    int obs;
    int alert;
    int fault;
    struct acm4_dev_t *next;
} acm4_dev_t;

/*
int
devAdd (uint8_t * id, uint8_t len)
{
  struct acm4_dev_t *new_dev, *last_dev;

  new_dev = malloc (sizeof (struct acm4_dev_t));
  new_dev->data = malloc (2048);        //todo: do define aspon
  memcpy (new_dev->imei, id, (len <= 16) ? len : 16);
  new_dev->imei[16] = 0;
  new_dev->size = 0;
  new_dev->next = NULL;
  new_dev->obs = 0;
  new_dev->lat = new_dev->lon = 0.0;
  new_dev->an = new_dev->in = 0.0;
  new_dev->ts = 0;
  new_dev->arb = 0;
  if (acm4_devs == NULL)
    {
      acm4_devs = new_dev;
    }
  else
    {
      last_dev = acm4_devs;
      while (last_dev->next != NULL)
        {
          last_dev = last_dev->next;
        }
      last_dev->next = new_dev;
    }
  feederLog (LOG_DEBUG, "acm4: id %s added\n", new_dev->imei);
  return 0;
}
*/
/*
uint8_t *
devImeiCmp (struct acm4_dev_t * dev, uint8_t * data, uint16_t len)
{
  uint8_t pattern[8];
  uint8_t *pos;
  uint8_t i;

  for (i = 0; i < 8; i++)
    {
      pattern[i] =
        (((dev->imei[2 * i] - '0') << 4) | (dev->imei[2 * i + 1] - '0'));
    }
  pos = data;
  while (pos = memchr (pos, pattern[0], len), (pos != NULL))
    {
      if (pos >= data + len)
        {
          return NULL;
        }

      if (memcmp (pos, pattern, 8) == 0)
        {
          return pos;
        }
      pos++;
      if (pos >= data + len)
        {
          return NULL;
        }
    }
  return NULL;
}
*/
/*
struct acm4_dev_t *
devFind (uint8_t * data, uint16_t len)
{
  struct acm4_dev_t *dev;
  uint8_t *pos;

  dev = acm4_devs;
  while (dev != NULL)
    {
      pos = devImeiCmp (dev, data, len);
      if (pos != NULL)
        return dev;
      dev = dev->next;
    }
  return NULL;
}
*/

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

//  acm4_devs = NULL;
//  devAdd ((uint8_t *) imeis[0], strlen (imeis[0]));

    return sizeof(struct acm4_dev_t);
}

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;
    static uint8_t *lpos = NULL;
    struct acm4_header_t *acm4_header_data;
    uint8_t *acm4_short_data = NULL;
    char imei[24];

    uint32_t i, j;
    struct tm time_str;

    struct acm4_dev_t *dev;

    dev = lib_data;

    if (length != 0) {
/*      ldata = malloc (length);
      if (ldata == NULL)
        {
          feederLog (LOG_WARNING,
                     "acm4: Can not allocate data buffer, size %d\n", length);
          return 0;
        }
      memcpy (ldata, data, length);
      lpos = ldata;
      lsize = length;*/

        feederLog(LOG_DEBUG, "In data ");

        for (i = 0; i < length; i++) {
            feederLog(LOG_DEBUG, "0x%02X ", data[i]);
        }
        feederLog(LOG_DEBUG, "\n");


        lpos = data;
        acm4_header_data = (struct acm4_header_t *) lpos;
        if ((memcmp(acm4_header_data->start, acm4_header_valid.start, 14) == 0) && (memcmp(acm4_header_data->end, acm4_header_valid.end, 6) == 0))      //todo: sizeof misto pevne delky 14 a 6
        {
            memcpy(dev->imei, acm4_header_data->imei, 16);      //todo: viz vyse sizeof
            dev->imei[16] = 0;
            feederLog(LOG_DEBUG, "acm4: information block, imei %s\n",
                      dev->imei);
            lpos += sizeof(acm4_header_t);
            dev->size = 0;
        }
        feederLog(LOG_DEBUG, "acm4: data from %s\n", dev->imei);

        memcpy(dev->data + dev->size, lpos, length - (lpos - data));
        dev->size += length - (lpos - data);

/*        
      if ((lpos != NULL) && (ldata != NULL) && (lsize != 0)
          && (lpos < ldata + lsize))
        {
          acm4_short_data = (uint8_t *) lpos;

          ldev = devFind (lpos, lsize - (lpos - ldata));
          if (ldev == NULL)
            feederLog (LOG_WARNING, "acm4: data from unknowen device\n");
          else
            {
              feederLog (LOG_DEBUG, "acm4: data from %s\n", ldev->imei);
              memcpy (ldev->data + ldev->size, lpos, lsize - (lpos - ldata));
              ldev->size += lsize - (lpos - ldata);

            }
        }*/
    }

    *id = 0;
    if ((dev != NULL) && (dev->obs == 0) && (dev->lat == 0.0)
        && (dev->lon == 0.0)) {
        feederLog(LOG_DEBUG, "acm4: data size in dev buffer %d\n",
                  dev->size);

        if (dev->size >= ACM4_SHORT_SIZE) {
            acm4_short_data = (uint8_t *) (dev->data);
            dev->lat = dev->lon = 0.0;

            j = 0;
            for (i = 20; i < 28; i++) {
                imei[j++] = ((acm4_short_data[i] & 0xF0) >> 4) + '0';
                imei[j++] = (acm4_short_data[i] & 0x0F) + '0';
            }
            imei[j] = 0;
            feederLog(LOG_DEBUG, "acm4: imei according data packet %s\n",
                      imei);

            if (strcmp(dev->imei, imei) == 0) {

                if (ACM4_SHORT_GPS_VALID(acm4_short_data) == 1) {
                    dev->lat =
                        ((double) ACM4_SHORT_LAT_DG(acm4_short_data) +
                         (((double) ACM4_SHORT_LAT_MIN(acm4_short_data)
                           + ((double)
                              (ACM4_SHORT_LAT_SEC(acm4_short_data))) *
                           0.01 + ((double)
                                   (ACM4_SHORT_LAT_SEC_HUN
                                    (acm4_short_data)))
                           * 0.01 * 0.01)) / 60.0) *
                        ((ACM4_SHORT_LAT_SIGN(acm4_short_data)) * 2 - 1);
                    dev->lon =
                        ((double) ACM4_SHORT_LON_DG(acm4_short_data) +
                         (((double) ACM4_SHORT_LON_MIN(acm4_short_data) +
                           ((double) (ACM4_SHORT_LON_SEC(acm4_short_data)))
                           * 0.01 +
                           ((double)
                            (ACM4_SHORT_LON_SEC_HUN(acm4_short_data))) *
                           0.01 * 0.01)) / 60.0) *
                        ((ACM4_SHORT_LON_SIGN(acm4_short_data)) * (-2) +
                         1);

                    feederLog(LOG_DEBUG, "acm4: lat %lf\n", dev->lat);
                    feederLog(LOG_DEBUG, "acm4: lon %lf\n", dev->lon);

                } else {
                    feederLog(LOG_WARNING, "acm4: position not valid\n");
                }

                feederLog(LOG_DEBUG,
                          "acm4: %04d/%02d/%02d %02d:%02d:%02d\n",
                          ACM4_SHORT_YEAR(acm4_short_data),
                          ACM4_SHORT_MON(acm4_short_data),
                          ACM4_SHORT_DAY(acm4_short_data),
                          ACM4_SHORT_HOUR(acm4_short_data),
                          ACM4_SHORT_MIN(acm4_short_data),
                          ACM4_SHORT_SEC(acm4_short_data));

                time_str.tm_year = ACM4_SHORT_YEAR(acm4_short_data) - 1900;
                time_str.tm_mon = ACM4_SHORT_MON(acm4_short_data) - 1;
                time_str.tm_mday = ACM4_SHORT_DAY(acm4_short_data);
                time_str.tm_hour = ACM4_SHORT_HOUR(acm4_short_data);
                time_str.tm_min = ACM4_SHORT_MIN(acm4_short_data);
                time_str.tm_sec = ACM4_SHORT_SEC(acm4_short_data);
                dev->ts = timegm(&time_str);

                feederLog(LOG_DEBUG, "acm4: timestamp %ld\n", dev->ts);

                dev->speed = ACM4_SHORT_SPEED(acm4_short_data);
                dev->an = ACM4_SHORT_AN(acm4_short_data);
                dev->in = ACM4_SHORT_IN(acm4_short_data);
                dev->arb = ACM4_SHORT_ARB(acm4_short_data);
                feederLog(LOG_DEBUG, "acm4: arbitrary %d\n", dev->arb);
                dev->arb1 = ACM4_SHORT_ARB1(acm4_short_data);
                dev->arb2 = ACM4_SHORT_ARB2(acm4_short_data);

                feederLog(LOG_DEBUG, "acm4: speed %d\n", dev->speed);
                feederLog(LOG_DEBUG, "acm4: an %d\n", dev->an);
                feederLog(LOG_DEBUG, "acm4: in %d\n", dev->in);
//                if ((dev->lat != 0.0) && (dev->lon != 0.0))		//kdyz nemame pozici neposilame ani info
            	    dev->obs = 1;
/*            	else
            	    dev->obs = 0;*/
                if (dev->in & 0xFC)
            	    dev->alert = 1;
            	else
            	    dev->alert = 0;

            }
            else
            {
        	feederLog(LOG_WARNING, "acm4: imeis are not same\n");
        	dev->fault = 1;
            }
            j = 0;
            for (i = 0; i < dev->size - ACM4_SHORT_SIZE; i++) {
                if ((i == 0) && ((dev->data[i + ACM4_SHORT_SIZE + j] == 0xff) || (dev->data[i + ACM4_SHORT_SIZE + j] == 0x00))) //todo: je to bug ze zdroje?
                {
                    j++;
                }

                dev->data[i] = dev->data[i + ACM4_SHORT_SIZE + j];
//                feederLog(LOG_DEBUG, "0x%02X ", dev->data[i]);
            }
            feederLog(LOG_DEBUG, "\n");

/*              memcpy (dev->data, dev->data + ACM4_SHORT_SIZE,
                      dev->size - ACM4_SHORT_SIZE);*/
    	    if (!dev->fault)
        	dev->size -= ACM4_SHORT_SIZE;
        }
    }

    if (dev != NULL) {
        if ((dev->lat != 0.0) && (dev->lon != 0.0)) {
            result_array[0] = dev->lat;
            result_array[1] = dev->lon;
            result_array[2] = 0.0;
            result_array[3] = 1.0;
            result_array[4] = (double)dev->speed;
            sensors[0] = sensors[1] = sensors[2] = sensors[3] = sensors[4] = 0x10;
            *type = VALUES_TYPE_POS;
            dev->lat = dev->lon = 0.0;
            ret = 5;
        } else {
            if (dev->obs) {
                result_array[0] = (double) dev->speed;
                result_array[1] = (double) dev->an;
                result_array[2] = (double) dev->in;
                result_array[3] = (double) (dev->in & 0x01);
                sensors[0] = TYPE_SPEED * 1000000 + 001 * 10000;
                sensors[1] = TYPE_DIGITAL_INPUT * 10000000 + 002 * 10000;
                sensors[2] = TYPE_DIGITAL_INPUT * 10000000 + 003 * 10000;
                sensors[3] = TYPE_DIGITAL_INPUT * 10000000 + 004 * 10000;

                ret = 4;
                if (dev->arb == ARB_SYSTEM_INFO) {
                    result_array[4] = (double) dev->arb1;
                    sensors[4] = TYPE_STATUS * 10000000 + 005 * 10000;
                    ret++;
                }

                *type = VALUES_TYPE_OBS;
                dev->obs = 0;

        	feederLog(LOG_DEBUG, "Generating observations\n");
            }
            else
            {
        	if (dev->alert)
        	{
        	    j = 0;
        	    for (i = 2; i < 10; i++)
        	    {
        		if (dev->in & (1 << i))
        		{
        		    sensors[j] = TYPE_CAR_ALERT * 10000000 + (i - 1) * 10000;
    			    j++;
        		}
        	    }
        	    *type = VALUES_TYPE_ALERT;
        	    ret = j;
        	    dev->alert = 0;

        	    feederLog(LOG_DEBUG, "Generating alerts\n");
        	}
    	    }
        }

        *tm = dev->ts;
        *id = atoll(dev->imei);
    } else
        ret = 0;

/*
  if (ldata != NULL)
    free (ldata);
  ldata = lpos = NULL;
  lsize = 0;*/

    return ret;
}

int reply(void *lib_data, int socket, unsigned char *data)
{
//    char cmd[] = {0x1B, 0x1B, 0x1B, 0x4D, 0x45, 0x4D, 0x4F, 0x52, 0x59, 0xFF, 0xFF, 0xFF};
    char cmd[] = {0x1B, 0x1B, 0x1B, 'M', 'E', 'M', 'O', 'R', 'Y', '_', '0', '0', '0', '0', '1', 0xFF, 0xFF, 0xFF};

    struct acm4_dev_t *dev;

    dev = lib_data;

    feederLog(LOG_DEBUG, "acm4: replying\n");
    
    if (dev->fault)
    {    
//	return 0;	//todo: -1, ale feeder to musi sprvane zpracovat, zatim spis spadne
	return -1;
    }	
//    sprintf(data, "MEMORY_10");

    memcpy(data, dev->imei, 16);
    memcpy(data + 16, cmd, sizeof(cmd));
    
    return 0;//strlen(data);
}

int open(void *lib_data, int socket)
{
    struct acm4_dev_t *dev;

    dev = (struct acm4_dev_t *) lib_data;
    feederLog(LOG_DEBUG, "acm4: socket open\n");

    dev->data = malloc(20480);   //todo: do define aspon a checkovat
    dev->imei[0] = 0;
    dev->size = 0;
    dev->obs = 0;
    dev->lat = dev->lon = 0.0;
    dev->an = dev->in = 0.0;
    dev->ts = 0;
    dev->arb = 0;
    dev->fault = 0;

    return 0;
}

int close(void *lib_data, int socket)
{
    struct acm4_dev_t *dev;

    dev = lib_data;

    feederLog(LOG_DEBUG, "acm4: socket closed\n");

    free(dev->data);
    return 0;
}