Repairs in code

[hydro.git] / app-stefic / sensor / definitions.c

diff --git a/app-stefic/sensor/definitions.c b/app-stefic/sensor/definitions.c
index 3a16c11f1f27b3cb198f6d4c89c9b055cbc9d37d..59da0b836a65527179fdd57b46d1e3ac3a717a01 100644 (file)
--- a/app-stefic/sensor/definitions.c
+++ b/app-stefic/sensor/definitions.c
@@ -5,11 +5,6 @@
 #include <system_def.h>
 #include <cpu_def.h>
 
-// #include <lpciap.h>
-// #include <keyval_id.h>
-// #include <keyval_loc.h>
-// #include <lpciap_kvpb.h>
-
 #include "definitions.h"
 
 //variables
@@ -18,23 +13,36 @@ long sn = SN;
 #ifndef UL_WITHOUT_HANDLE
 uloi_coninfo_t *coninfo=&uloi_coninfo_global;
 ul_dyac_t *ul_dyac=&ul_dyac_global;
-// kvpb_block_t *kvpb_block=&kvpb_block_global;
 #endif
 
 // LED variables
-int led1 = 200;
-int led2 = 100;
+int led1 = 500;
+int led2 = 300;
 
 void long2buf(uchar *buf,unsigned long mod){
-  buf[0]=mod>>0;
-  buf[1]=mod>>8;
-  buf[2]=mod>>16;
-  buf[3]=mod>>24;
+    buf[0]=mod>>0;
+    buf[1]=mod>>8;
+    buf[2]=mod>>16;
+    buf[3]=mod>>24;
 }
 
 void int2buf(uchar *buf,int mod){
-  buf[0]=mod>>0;
-  buf[1]=mod>>8;
+    buf[0]=mod>>0;
+    buf[1]=mod>>8;
+}
+
+int buf2int(uchar *buf){
+    int i;
+    i = (uchar) buf[0];
+    i = ((uchar) buf[1]<<8) | i;
+    return i;
+}
+
+uint buf2uint(uchar *buf){
+    uint i;
+    i = (uchar) buf[0];
+    i = ((uchar) buf[1]<<8) | i;
+    return i;
 }
 
 mstime_t current_time(){
@@ -73,25 +81,6 @@ void blink(void){
 }
 
 int all_init(void){
-    // kvpb init
-//     kvpb_block->base=(uint8_t*)KEYVAL_START;
-//     kvpb_block->size=KEYVAL_PAGE_LEN;
-//     kvpb_block->flags=KVPB_DESC_DOUBLE|KVPB_DESC_CHUNKWO;
-//     kvpb_block->chunk_size=KVPB_CHUNK_SIZE;
-//     kvpb_block->erase=lpcisp_kvpb_erase;
-//     kvpb_block->copy=lpcisp_kvpb_copy;
-//     kvpb_block->flush=lpcisp_kvpb_flush;
-//     printf("Keyval ready\n");
-
-    /***********************************/
-    // set configuration for device
-//     kvpb_get_key(kvpb_block,KVPB_KEYID_ULAN_ADDR,1,&uaddr);
-//     kvpb_get_key(kvpb_block,KVPB_KEYID_ULAN_SN,4,&usn);
-// 
-//     kvpb_get_key(kvpb_block,KVPB_KEYID_ADC_CFG,sizeof(adc_cal_t),&adccal);
-//     kvpb_get_key(kvpb_block,KVPB_KEYID_CON_PARAM,sizeof(pid_con_param_t),&con_param);
-//     kvpb_get_key(kvpb_block,KVPB_KEYID_APPL_PARAM,sizeof(appl_param_t),&appl_param);
-//     printf("Keyval variables read\n");
 
     //opening file descriptor
     ul_fd = ul_open(NULL,NULL);
@@ -109,7 +98,7 @@ int all_init(void){
         return addfilt;
     }
 
-    setmyaddr();
+    ul_setmyadr(ul_fd,99);
     ul_setidstr(ul_fd,ul_idstr);
 
     // uLan object interface init
@@ -160,13 +149,82 @@ void heartbeat(void){
     free(buf);
 }
 
+void accept_SDO(void) {
+        // setting lan configuration
+        if ((ul_inepoll(ul_fd)>0) && (ul_acceptmsg(ul_fd, &msginfo)>=0)) {
+            if (msginfo.cmd == UL_CMD_PDO){
+                process_PDO();
+                ul_freemsg(ul_fd);
+                printf("accept PDO msg: sadr: %i, dadr: %i\n", msginfo.sadr, msginfo.dadr);
+            } else if (msginfo.sadr != 99) {
+                if (!(msginfo.flg&(UL_BFL_PROC | UL_BFL_FAIL))) {
+                    //waiting for msg from bus
+                    if (uloi_process_msg(ULOI_ARG_coninfo (uloi_objdes_array_t*)&uloi_objdes_main, &msginfo)<0) {
+                        //checking dynamic adresation
+                        if (uldy_process_msg(ULDY_ARG_ul_dyac &msginfo)>=0){
+                            printf("message recieved - UL_CMD_NCS, sadr: %i, dadr: %i, cmd: %i\n",msginfo.sadr, msginfo.dadr, msginfo.cmd);
+                            printf("server addr :%i\n",ul_dyac->ul_dysa);
+                        } else {
+                            ul_freemsg(ul_fd);
+                            printf("problem in uldy_process_msg\n");
+                        }
+                    } else {
+                        printf("ULOI message processed: sadr: %i, dadr: %i\n", msginfo.sadr, msginfo.dadr);
+                    }
+                } else {
+                    ul_freemsg(ul_fd);
+                }
+            }
+        }
+}
+
+void process_PDO(void){
+uchar * buf;
+int buf_len = 0;
+
+int position = 3;
+
+uint cid = 0;
+int len_data = 0;
+int data = 0;
+
+    buf_len = msginfo.len;
+    buf = (uchar *) malloc(buf_len*sizeof(uchar));
+    ul_read(ul_fd, buf, buf_len);
+    while(1){
+        if ((position+2) > buf_len) break;
+        cid = take_cid(&buf[position]);
+        position += 2;
+
+        if ((position+2) > buf_len) break;
+        len_data = take_len(&buf[position]);
+        position += 2;
+
+        if ((position+len_data) > buf_len) break;
+        data = take_data(&buf[position], len_data);
+        position += len_data;
+
+        check_PDO(cid, data);
+    }
+}
+
+uint take_cid(uchar * buf){
+    return buf2uint(buf);
+}
+
+int take_len(uchar * buf){
+    return buf2int(buf);
+}
+
+int take_data(uchar * buf, int len){
+    if (len == 2) return buf2int(buf);
+    return 0;
+}
+
 char ul_save_sn(uint32_t usn){
-//   kvpb_set_key(kvpb_block,KVPB_KEYID_ULAN_SN,sizeof(unsigned long),&usn);
   return 0;
 }
 
 char ul_save_adr(uint8_t uaddr){
-//   unsigned int v=uaddr;
-//   kvpb_set_key(kvpb_block,KVPB_KEYID_ULAN_ADDR,sizeof(unsigned int),&v);
   return 0;
 }