app-stefic/sensor/definitions.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <ul_lib/ulan.h>
   5 #include <system_def.h>
   6 #include <cpu_def.h>
   7
   8 #include "definitions.h"
   9
  10 //variables
  11 long sn = SN;
  12
  13 #ifndef UL_WITHOUT_HANDLE
  14 uloi_coninfo_t *coninfo=&uloi_coninfo_global;
  15 ul_dyac_t *ul_dyac=&ul_dyac_global;
  16 #endif
  17
  18 // LED variables
  19 int led1 = 500;
  20 int led2 = 300;
  21
  22 void long2buf(uchar *buf,unsigned long mod){
  23     buf[0]=mod>>0;
  24     buf[1]=mod>>8;
  25     buf[2]=mod>>16;
  26     buf[3]=mod>>24;
  27 }
  28
  29 void int2buf(uchar *buf,int mod){
  30     buf[0]=mod>>0;
  31     buf[1]=mod>>8;
  32 }
  33
  34 int buf2int(uchar *buf){
  35     int i;
  36     i = (uchar) buf[0];
  37     i = ((uchar) buf[1]<<8) | i;
  38     return i;
  39 }
  40
  41 uint buf2uint(uchar *buf){
  42     uint i;
  43     i = (uchar) buf[0];
  44     i = ((uchar) buf[1]<<8) | i;
  45     return i;
  46 }
  47
  48 mstime_t current_time(){
  49     mstime_t ret = 0;
  50
  51     #ifdef OS_POSIX
  52         struct timespec tp;
  53         if(clock_gettime(CLOCK_REALTIME, &tp) == 0){
  54             ret = tp.tv_sec*1000;
  55             ret += tp.tv_nsec/1000000;
  56         }
  57     #else
  58         lt_mstime_update();
  59         ret = actual_msec;
  60     #endif
  61
  62    return ret;
  63 }
  64
  65 void blink(void){
  66     #ifndef OS_POSIX
  67     if(led1 == 0) IO0SET=LED1_BIT;
  68     else if(led1 == 1) IO0CLR=LED1_BIT;
  69     else if((current_time()-led1_time) > led1){
  70         IO0PIN=IO0PIN^LED1_BIT;
  71         led1_time+=led1;
  72     }
  73
  74     if(led2 == 0) IO0SET=LED2_BIT;
  75     else if(led2 == 1) IO0CLR=LED2_BIT;
  76     else if((current_time()-led2_time) > led2){
  77         IO0PIN=IO0PIN^LED2_BIT;
  78         led2_time+=led2;
  79     }
  80     #endif
  81 }
  82
  83 int all_init(void){
  84
  85     //opening file descriptor
  86     ul_fd = ul_open(NULL,NULL);
  87     ul_fd1 = ul_open(NULL,NULL);
  88     if ((ul_fd == UL_FD_INVALID) || (ul_fd1 == UL_FD_INVALID)){
  89         printf("error in creating ul_fd\n");
  90         return -1;
  91     }
  92
  93     //memory set and filter set
  94     memset(&msginfo,0,sizeof(msginfo));
  95     addfilt=ul_addfilt(ul_fd,&msginfo);
  96     if(addfilt<0) {
  97         printf("spy_messages : add filter failed\n");
  98         return addfilt;
  99     }
 100
 101     ul_setmyadr(ul_fd,99);
 102     ul_setidstr(ul_fd,ul_idstr);
 103
 104     // uLan object interface init
 105     coninfo->cmd = UL_CMD_OISV;
 106     coninfo->sn = SN;
 107     coninfo->bsn = 0;
 108     coninfo->ul_fd = ul_fd;
 109     coninfo->ul_fd1 = ul_fd1;
 110
 111     oiinit();
 112
 113     // uLan dyac init
 114     uldy_init(ul_dyac,ul_fd,ul_save_sn,ul_save_adr,(char*)ul_idstr,sn);
 115
 116     return 0;
 117
 118 }
 119
 120 void send_sn(void){
 121     int msgsend;
 122
 123     //.preparing of msg
 124     uchar buf_out[ULDY_TMBUF_LEN];
 125     buf_out[0]=ULNCS_ADR_RQ;
 126     long2buf(&buf_out[1],sn);
 127
 128     //address set and sign on the ulan
 129     msgsend = ul_send_command(ul_fd, msginfo.dadr, UL_CMD_NCS, UL_BFL_NORE, (void*) buf_out, ULDY_TMBUF_LEN);
 130     printf("[I] sn was send\n");
 131     ul_freemsg(ul_fd);
 132 }
 133
 134 void heartbeat(void){
 135     //int period;
 136     int msgsend, i;
 137     int len = HEART_BUF_LEN;
 138     uchar *buf;
 139
 140     //.preparing of msg
 141     buf = (uchar *) malloc(len*sizeof(uchar));  // NULL NULL NULL CID_HEART status_val
 142     for (i=0; i<=2; i++) buf[i] = 0;
 143     int2buf(&buf[3],HEART_CID);
 144     int2buf(&buf[5],2);
 145     int2buf(&buf[7],status_val);
 146
 147     msgsend = ul_send_command(ul_fd, ul_dyac->ul_dysa, UL_CMD_PDO, UL_BFL_NORE, (void*) buf, HEART_BUF_LEN);
 148     printf("[I] BEAT\n");
 149     free(buf);
 150 }
 151
 152 void accept_SDO(void) {
 153         // setting lan configuration
 154         if ((ul_inepoll(ul_fd)>0) && (ul_acceptmsg(ul_fd, &msginfo)>=0)) {
 155             if (msginfo.cmd == UL_CMD_PDO){
 156                 process_PDO();
 157                 ul_freemsg(ul_fd);
 158                 printf("accept PDO msg: sadr: %i, dadr: %i\n", msginfo.sadr, msginfo.dadr);
 159             } else if (msginfo.sadr != 99) {
 160                 if (!(msginfo.flg&(UL_BFL_PROC | UL_BFL_FAIL))) {
 161                     //waiting for msg from bus
 162                     if (uloi_process_msg(ULOI_ARG_coninfo (uloi_objdes_array_t*)&uloi_objdes_main, &msginfo)<0) {
 163                         //checking dynamic adresation
 164                         if (uldy_process_msg(ULDY_ARG_ul_dyac &msginfo)>=0){
 165                             printf("message recieved - UL_CMD_NCS, sadr: %i, dadr: %i, cmd: %i\n",msginfo.sadr, msginfo.dadr, msginfo.cmd);
 166                             printf("server addr :%i\n",ul_dyac->ul_dysa);
 167                         } else {
 168                             ul_freemsg(ul_fd);
 169                             printf("problem in uldy_process_msg\n");
 170                         }
 171                     } else {
 172                         printf("ULOI message processed: sadr: %i, dadr: %i\n", msginfo.sadr, msginfo.dadr);
 173                     }
 174                 } else {
 175                     ul_freemsg(ul_fd);
 176                 }
 177             }
 178         }
 179 }
 180
 181 void process_PDO(void){
 182 uchar * buf;
 183 int buf_len = 0;
 184
 185 int position = 3;
 186
 187 uint cid = 0;
 188 int len_data = 0;
 189 int data = 0;
 190
 191     buf_len = msginfo.len;
 192     buf = (uchar *) malloc(buf_len*sizeof(uchar));
 193     ul_read(ul_fd, buf, buf_len);
 194     while(1){
 195         if ((position+2) > buf_len) break;
 196         cid = take_cid(&buf[position]);
 197         position += 2;
 198
 199         if ((position+2) > buf_len) break;
 200         len_data = take_len(&buf[position]);
 201         position += 2;
 202
 203         if ((position+len_data) > buf_len) break;
 204         data = take_data(&buf[position], len_data);
 205         position += len_data;
 206
 207         check_PDO(cid, data);
 208     }
 209 }
 210
 211 uint take_cid(uchar * buf){
 212     return buf2uint(buf);
 213 }
 214
 215 int take_len(uchar * buf){
 216     return buf2int(buf);
 217 }
 218
 219 int take_data(uchar * buf, int len){
 220     if (len == 2) return buf2int(buf);
 221     return 0;
 222 }
 223
 224 char ul_save_sn(uint32_t usn){
 225   return 0;
 226 }
 227
 228 char ul_save_adr(uint8_t uaddr){
 229   return 0;
 230 }