diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c index 2ae5219f759dc07013a31f06c0fa708f3206128e..b2f201bdc7efa5105a79726daabe8d2b8da54141 100644 --- a/groundStation/src/backend/backend.c +++ b/groundStation/src/backend/backend.c @@ -174,9 +174,6 @@ int main(int argc, char **argv) err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__); } - // create vrpnTracker instance - tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP); - // open the log file if(createLogFile(argc, argv[1])) { @@ -196,8 +193,12 @@ int main(int argc, char **argv) // Tell the quad we are ready to send it vrpn data sendStartPacket(); - // this function will be called whenever tracker receives data - ucart_vrpn_tracker_addCallback(tracker, cb); + if(!getenv(NOVRPN_ENV)){ + // create vrpnTracker instance + tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP); + // this function will be called whenever tracker receives data + ucart_vrpn_tracker_addCallback(tracker, cb); + } struct timeval timeout = { .tv_sec = 1, @@ -257,7 +258,6 @@ int main(int argc, char **argv) } } } else if (get_client_index(fd) > -1) { - printf("receiving from client\n"); client_recv(fd); } } @@ -584,21 +584,31 @@ static void client_recv(int fd) { break; } buffer[newline] = '\0'; - + printf("Client(%d) : '%s'\n",fd, buffer); unsigned char * packet; // printf("newline =%li, Client sent: '%s'\n", newline, buffer); - if(formatCommand(buffer, &packet) != -1) { - printf("Backend sees as: %f\n", getFloat(packet, 7)); - } else { + if(formatCommand(buffer, &packet) == -1) { printf("Could not recognize command '%s'\n", buffer); } - writeQuad((char *) packet, len); + + int datalen = (packet[6] << 8) | (packet[5]); + printf("data length = %d\n", datalen); + printf("packet = '"); + for (int i = 0; i < datalen+7+1; ++i) + { + printf("%x ", packet[i]); + } + printf("'\n"); + + writeQuad((char *) packet, datalen +7); //free(packet); char * rest = &buffer[newline] + 1; size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest); /* Delete parsed data and move the rest to the left */ memmove(buffer, rest, restLen); + + break; } } diff --git a/groundStation/src/frontend/frontend_common.c b/groundStation/src/frontend/frontend_common.c index 4fbe4f27992db4dd078e3e203a321529885a0f3f..7da484f0b120e9a888bad0dc57c0b4093aba1141 100644 --- a/groundStation/src/frontend/frontend_common.c +++ b/groundStation/src/frontend/frontend_common.c @@ -51,7 +51,7 @@ struct backend_conn * ucart_backendConnect() conn->len = 0; conn->buf = NULL; - conn->socket = fdopen(s, "rw"); + conn->socket = fdopen(s, "r+"); if (conn->socket == NULL) { perror("fdopen"); goto fail_malloc_conn; diff --git a/groundStation/src/frontend/frontend_getpid.c b/groundStation/src/frontend/frontend_getpid.c index c1bb9b46444ad7e5dbd037c34747d51c6a040c94..09b2e62b728d766a4bcac108d9be43270a4741be 100644 --- a/groundStation/src/frontend/frontend_getpid.c +++ b/groundStation/src/frontend/frontend_getpid.c @@ -22,31 +22,25 @@ int frontend_getpid( struct backend_conn * conn, struct frontend_pid_data * pid_data) { - char line[10] = "getpid \0"; - char pid_option[2]; + char line[100]; switch (pid_data->pid) { case PITCH : - strncpy(pid_option, "p\0", 2); + strncpy(line, "getpitchp\ngetpitchd\n\0", 21); break; case ROLL : - strncpy(pid_option, "i\0", 2); + strncpy(line, "getrollp\ngetrolld\n\0", 19); break; case YAW : - strncpy(pid_option, "d\0", 2); + strncpy(line, "getyawp\ngetyawd\n\0", 17); break; default : return 1; } - strncat(line, pid_option, 2); - int size; if((size = ucart_backendWrite(conn, line)) < 0 ) { - printf("nope! %d \n", size); return 1; } - printf("we send size bytes\n"); - return 0; } \ No newline at end of file