ARASH_ASiST_GUI_Final/udp.cpp at main · ivias2000/ARASH_ASiST_GUI_Final · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include <cstring>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <cmath>
#include <typeinfo>
#include <QDebug>

// Declare external variables
extern int log_start;
extern int global_log;
extern QVector<double> qv_counter;
extern QVector<double> qv_xValues;
extern QVector<double> qv_yValues;
extern QVector<double> qv_zValues;
extern QVector<double> qv_phiValues;
extern QVector<double> qv_psiValues;
extern QVector<double> qv_dValues;
extern QList<QVector<double>> existingVectors;
extern QList<QVector<double>> existingVectors2;
extern QList<QVector<double>> loglist;
extern QList<QVector<double>> loglist2;
extern double log_data1[7][60];
extern double log_data2[7][60];

void udp::run() {
    const char* UDP_IP = "0.0.0.0";
    const int UDP_PORT = 5500;

    int sockfd;
    // Structure describing
    struct sockaddr_in server_addr;
    // Initialization
    socklen_t addr_len = sizeof(server_addr);
    // AF_INET IPV4
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    // If PROTOCOL is zero ok and -1 for errors
    if (sockfd < 0) {
        std::cerr << "Error opening socket" << std::endl;
    }
    // memset: fill memory
    memset(&server_addr, 0, sizeof(server_addr));
    server_addr.sin_family = AF_INET;
    // htons: converts the unsigned short integer
    server_addr.sin_port = htons(UDP_PORT);
    // presentation to network       //sin_addr: store it
    inet_pton(AF_INET, UDP_IP, &server_addr.sin_addr);
    // if bind.....
    if (bind(sockfd, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
        std::cerr << "Error binding socket" << std::endl;
    }
    std::cout << "UDP server listening on port " << 5500 << std::endl;
    const double pi = 3.14159265358979323846;
    // float l__0 = 0.006;
    float l__1 = 0.1820;
    float l__3 = 0.05;
    float l__5 = 0.1150;
    // float l__2 = 0.07;
    // float l__7 = 0.0342;
    float theta = 42 * (pi / 180);
    float alpha = 50 * (pi / 180);

    while (true) {
        char data[32];
        // ssize_t: Used for a count of bytes or an error indication
        ssize_t data_len = recvfrom(sockfd, data, sizeof(data), 0, (struct sockaddr*)&server_addr, &addr_len);
        if (data_len < 0) {
            std::cerr << "Error receiving data" << std::endl;
            continue;
        }
        // Working with Raw Memory
        double* np_data = reinterpret_cast<double*>(data);

        float data_q1 = static_cast<float>(np_data[0]);
        float data2_q2 = static_cast<float>(np_data[1]);
        float data3_q3 = static_cast<float>(np_data[2]);
        float counter = static_cast<float>(np_data[3]);
        float phi = static_cast<float>((data_q1) * (pi / 180));
        float psi = static_cast<float>((data2_q2) * (pi / 180));
        float d = static_cast<float>((-data3_q3) / 1000);
        float x = static_cast<float>((sin(theta) * cos(psi + alpha) * d + cos(theta) * cos(phi) * sin(psi + alpha) * d + sin(theta) * (l__1 + l__3 + l__5)));
        float y = static_cast<float>(sin(phi) * sin(psi + alpha) * d);
        float z = static_cast<float>((cos(theta) * cos(psi + alpha) * d - cos(phi) * sin(theta) * sin(psi + alpha) * d + cos(theta) * (l__1 + l__3 + l__5)));
        int a = round(counter * 1000.0);
        float mostafa = a / 1000.0;

        if (global_log == 0) {
            log_data1[0][counter_gui] = counter;
            log_data1[1][counter_gui] = phi;
            log_data1[2][counter_gui] = psi;
            log_data1[3][counter_gui] = d;
            log_data1[4][counter_gui] = x;
            log_data1[5][counter_gui] = y;
            log_data1[6][counter_gui] = z;
        } else if (global_log == 1) {
            log_data2[0][counter_gui] = counter;
            log_data2[1][counter_gui] = phi;
            log_data2[2][counter_gui] = psi;
            log_data2[3][counter_gui] = d;
            log_data2[4][counter_gui] = x;
            log_data2[5][counter_gui] = y;
            log_data2[6][counter_gui] = z;
        }

        counter_gui = counter_gui + 1;

        cnt_plot++;
        if (cnt_plot >= 99) {
            cnt_plot = 0;
            global_log = (global_log + 1) % 2;
            log_start = 1;
            packet_recieve_handler.setValue(x, y, z, mostafa, phi, psi, d);

            qv_counter[counter_plot] = counter;
            qv_xValues[counter_plot] = x;
            qv_yValues[counter_plot] = y;
            qv_zValues[counter_plot] = z;
            qv_phiValues[counter_plot] = phi;
            qv_psiValues[counter_plot] = psi;
            qv_dValues[counter_plot] = d;
            counter_plot = counter_plot + 1;
            if (counter_plot >= 600) {
                counter_plot = 0;
            }
        }

        if (counter_gui >= 60) {
            counter_gui = 0;
        }
    }

    close(sockfd);
}