#include #include #include #include #include #include #include //#include #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(<ime); struct tm gmt; // Breakdown of the time is done using gmtime_r and NOT gmtime, as gmtime_r is NOT thread safe. gmtime_r(<ime, &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, ×tamp) < 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; }