Added forgotten files.

[fpga/rpi-motor-control.git] / pmsm-control / test_sw / udp_cli.c

diff --git a/pmsm-control/test_sw/udp_cli.c b/pmsm-control/test_sw/udp_cli.c
new file mode 100644 (file)
index 0000000..fd9db7a
--- /dev/null
+++ b/pmsm-control/test_sw/udp_cli.c
@@ -0,0 +1,106 @@
+/**
+ * \brief Implementace klienta pro UDP komunikaci.
+ * \file udp_cli.c
+ * \author Martin Prudek
+ * 
+ *     Implementace klienta pro prijmani referencni pozice pro rizeni motoru.
+ *     Pouziva protokol UDP.
+ */
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+#include <unistd.h>
+
+#include "udp_cli.h"
+
+
+#define MAX_BUF 1400
+#define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
+
+
+/**
+ * 
+ * Funkce spusti sekvenci,
+ * ktera pravidelne aktualizuje pozici vzdaleneho moturu
+ * port 45789 
+ */
+void* start_reading_remote_position(void * param)
+{
+
+       char* ipbuff;
+       int delka;
+       int sockd;
+       struct sockaddr_in my_addr, srv_addr;
+       char buf[MAX_BUF];
+       char code[MAX_BUF];
+       int count;
+       int addrlen;
+       int port=45789;
+       struct timespec t;
+       int interval = 2000000; /*500hz*/
+       struct remote_pos_st *rpst=(struct remote_pos_st*)param;
+       
+
+       ipbuff=rpst->ip;
+  
+       /* create a UDP socket */
+       sockd = socket(AF_INET, SOCK_DGRAM, 0);
+       if (sockd == -1)
+       {
+               perror("Socket creation error");
+               exit(1);
+       }
+
+
+       /* Configure client address */
+       my_addr.sin_family = AF_INET;
+       my_addr.sin_addr.s_addr = INADDR_ANY;
+       my_addr.sin_port = 0;
+
+       bind(sockd, (struct sockaddr*)&my_addr, sizeof(my_addr));
+
+       /*inicializa bufferu*/
+       memset(buf,0,MAX_BUF);
+       buf[MAX_BUF-1]='\0';
+
+       /* server address */
+       srv_addr.sin_family = AF_INET;
+       inet_aton(ipbuff, &srv_addr.sin_addr);
+       srv_addr.sin_port = htons(port);
+       
+       strcpy(code,"pr"); /*position request code */
+       delka=strlen(code)+1;
+       clock_gettime(CLOCK_MONOTONIC ,&t);
+       while(1){  
+               /* wait until next shot */
+               clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
+
+               sendto(sockd, code, delka, 0,(struct sockaddr*)&srv_addr, sizeof(srv_addr));
+               addrlen = sizeof(srv_addr);
+               count = recvfrom(sockd, buf, MAX_BUF, 0,(struct sockaddr*)&srv_addr, &addrlen);
+
+               sem_wait(rpst->semaphore);
+               if (rpst->stop){
+                       sem_post(rpst->semaphore);
+                       break;
+               }
+               *rpst->rem_pos=*((int*)buf)*rpst->factor;
+               sem_post(rpst->semaphore);
+
+               /* calculate next shot */
+               t.tv_nsec += interval;
+               while (t.tv_nsec >= NSEC_PER_SEC) {
+                       t.tv_nsec -= NSEC_PER_SEC;
+                       t.tv_sec++;
+
+               }
+       }
+       close(sockd);
+       return (void *)0;
+}
+