bool AsixDOFPawn::OurTick(float DeltaSeconds)
{
bool ret = false;
char buf[BUFLEN];
struct sockaddr_in si_other;
int slen = sizeof(si_other) , recv_len = 0;
char temp_str[256];
double *telem;
if (m_socket == -1)
{
return ret;
}
if ((recv_len = recvtimeout(AsixDOFPawn::m_socket, buf, BUFLEN, 10 ,(struct sockaddr *) &si_other, (socklen_t *)&slen)) == -1)
{
printf("error is : %s\n", strerror(errno));
}
if (recv_len > 0)
{
telem = (double*) buf;
double utm_x = telem[0];
double utm_y = telem[1];
double alt = telem[2];
double roll = telem[3];
double pitch = telem[4];
double yaw = telem[5];
double vx = telem[6];
double vy = telem[7];
double vz = telem[8];
m_vx = vx;
m_vy = vy;
m_vz = vz;
m_Deltaroll = roll - m_roll;
m_Deltapitch = pitch - m_pitch;
m_Deltayaw = yaw - m_yaw;
//printf("Delta --> %f %f %f \n",m_Deltaroll, m_Deltapitch, m_Deltayaw );
m_roll = roll;
m_pitch = pitch;
m_yaw = yaw;
/*
double vzp = vx*sin(pitch* PI / 180.0);
double vzr = vy*sin(roll* PI / 180.0);
m_vz = vz + vzp + vzr;
m_vx = vx + vx * cos(pitch* PI / 180.0);
m_vy = vy + vy * cos(roll* PI / 180.0);
*/
printf("6 --> %f %f %f %f %f %f \n",m_vx, m_vy, m_vz, pitch, roll, yaw );
ret = true;
}
return ret;
}
int request(const int sock, const struct sockaddr_in6 *client_addr, const char *request, bool sse, double timeout) {
ssize_t ret;
char buffer[8192];
ret = sendto(sock, request, strlen(request), 0, (struct sockaddr *)client_addr, sizeof(struct sockaddr_in6));
if (ret < 0) {
perror("Error in sendto()");
exit(EXIT_FAILURE);
}
struct timeval tv_timeout, tv_offset;
tv_timeout.tv_sec = (int) timeout;
tv_timeout.tv_usec = ((int) (timeout * 1000000)) % 1000000;
getclock(&tv_offset);
while (1) {
ret = recvtimeout(sock, buffer, sizeof(buffer), 0, &tv_timeout, &tv_offset);
if (ret < 0)
break;
if (sse)
fputs("event: neighbour\ndata: ", stdout);
fwrite(buffer, sizeof(char), ret, stdout);
if (sse)
fputs("\n\n", stdout);
else
fputs("\n", stdout);
fflush(stdout);
}
return 0;
}
