From a3de6667ca8cb2e14e4c56c278126fd808bbd4ae Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Thu, 13 Oct 2011 07:25:30 +0800 Subject: [PATCH 1/2] fastserial tcp --- libraries/Desktop/Desktop.mk | 2 +- libraries/Desktop/support/FastSerial.cpp | 118 +++++++++++++++++++++-- 2 files changed, 109 insertions(+), 11 deletions(-) diff --git a/libraries/Desktop/Desktop.mk b/libraries/Desktop/Desktop.mk index c3e214dfb4..e04a8e86f4 100644 --- a/libraries/Desktop/Desktop.mk +++ b/libraries/Desktop/Desktop.mk @@ -171,7 +171,7 @@ else endif # these are library objects we don't want in the desktop build (maybe we'll add them later) -NODESKTOP := FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp RC_Channel/RC_Channel_aux.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +NODESKTOP := FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp # # Find sketchbook libraries referenced by the sketch. diff --git a/libraries/Desktop/support/FastSerial.cpp b/libraries/Desktop/support/FastSerial.cpp index fbca781a2c..adc7f3687d 100644 --- a/libraries/Desktop/support/FastSerial.cpp +++ b/libraries/Desktop/support/FastSerial.cpp @@ -36,6 +36,35 @@ #include #include +#include +#include +#include +#include +#include +#include +#include + + + + int list_s; /* listening socket */ + int conn_s; /* connection socket */ + short int port = 5760; /* port number */ + struct sockaddr_in servaddr, cli_addr; /* socket address structure */ + char buffer[256]; /* character buffer */ + socklen_t clilen; + + +int myread(void) +{ + return read(conn_s,buffer,1); +} + +int mywrite(uint8_t c) +{ + return write(conn_s,(char *) &c,1); +} + + #if defined(UDR3) # define FS_MAX_PORTS 4 #elif defined(UDR2) @@ -76,7 +105,53 @@ void FastSerial::begin(long baud) fcntl(0, F_SETFL, v | O_NONBLOCK); v = fcntl(1, F_GETFL, 0); fcntl(1, F_SETFL, v | O_NONBLOCK); - } + + /* Create the listening socket */ + + if ( (list_s = socket(AF_INET, SOCK_STREAM, 0)) < 0 ) { + fprintf(stderr, "ECHOSERV: Error creating listening socket.\n"); + exit(EXIT_FAILURE); + } + + int val = 1; + + setsockopt(list_s, SOL_SOCKET, SO_REUSEADDR, &val, sizeof val); + + /* Set all bytes in socket address structure to + zero, and fill in the relevant data members */ + + memset(&servaddr, 0, sizeof(servaddr)); + + servaddr.sin_family = AF_INET; + servaddr.sin_addr.s_addr = INADDR_ANY; + servaddr.sin_port = htons(port + _u2x); + + + /* Bind our socket addresss to the + listening socket, and call listen() */ + + if ( bind(list_s, (struct sockaddr *) &servaddr, sizeof(servaddr)) < 0 ) { + fprintf(stderr, "ECHOSERV: Error calling bind()\n"); + exit(EXIT_FAILURE); + } + + if ( listen(list_s, 5) < 0 ) { + fprintf(stderr, "ECHOSERV: Error calling listen()\n"); + exit(EXIT_FAILURE); + } + clilen = sizeof(cli_addr); + + fprintf(stdout, "Listerning on port %i\n",port + _u2x); + fflush(stdout); + + if ( (conn_s = accept(list_s, NULL, NULL) ) < 0 ) { + fprintf(stderr, "ECHOSERV: Error calling accept()\n"); + exit(EXIT_FAILURE); + } + + fcntl(conn_s, F_SETFL, O_NONBLOCK); + } + } void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace) @@ -92,9 +167,22 @@ int FastSerial::available(void) { if (_u2x != 0) return 0; int num_ready; - if (ioctl(0, FIONREAD, &num_ready) == 0) { - return num_ready; - } + + fd_set socks; + struct timeval t; + FD_ZERO(&socks); + FD_SET(conn_s, &socks); + t.tv_sec = 0; + t.tv_usec = 500; + if (int ans = select(conn_s + 1, &socks, NULL, NULL,&t)) + { + + //FD_ZERO(&socks); + //FD_SET(conn_s, &socks); + //if (FD_ISSET(conn_s, &socks)) { + return 1; +// } + } return 0; } @@ -109,10 +197,17 @@ int FastSerial::read(void) return -1; } char c; - if (fread(&c, 1, 1, stdin) != 1) { - return -1; - } - return (int)c; + int n = myread(); + if (n == 0) { + + if ( (conn_s = accept(list_s, NULL, NULL) ) < 0 ) { + fprintf(stderr, "ECHOSERV: Error calling accept()\n"); + exit(EXIT_FAILURE); + } + + fcntl(conn_s, F_SETFL, O_NONBLOCK); + } + return (int)buffer[0]; } int FastSerial::peek(void) @@ -129,8 +224,11 @@ void FastSerial::write(uint8_t c) if (_u2x != 0) { return; } - fwrite(&c, 1, 1, stdout); - fflush(stdout); + mywrite(c); + if (c >= '\n' && c <= 128) { + fwrite(&c, 1, 1, stdout); + fflush(stdout); + } } // Buffer management /////////////////////////////////////////////////////////// From c7c8c309bed583c35eca95e57b54b0db1712b6d2 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Thu, 13 Oct 2011 18:34:32 +0800 Subject: [PATCH 2/2] AC2 fix guide mode fly off --- ArduCopter/ArduCopter.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0b7393187b..a1e3857942 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1046,6 +1046,8 @@ static void update_navigation() case GUIDED: wp_control = WP_MODE; + // check if we are close to point > loiter + verify_nav_wp(); update_auto_yaw(); update_nav_wp();