UIUC flight model contribution. This is based on LaRCsim, but can read

[flightgear.git] / src / Network / fg_serial.cxx

diff --git a/src/Network/fg_serial.cxx b/src/Network/fg_serial.cxx
index 87dcb207ef462b84493dd049102a742da64dc352..e1897b59c8920a7417e357325ca478cca798f0b6 100644 (file)
--- a/src/Network/fg_serial.cxx
+++ b/src/Network/fg_serial.cxx
@@ -21,22 +21,23 @@
 // $Id$
 
 
-#include <Include/compiler.h>
+#include <simgear/compiler.h>
 
 #include STL_STRING
 
-#include <Debug/logstream.hxx>
+#include <simgear/debug/logstream.hxx>
+#include <simgear/serial/serial.hxx>
+
 #include <Aircraft/aircraft.hxx>
-#include <Include/fg_constants.h>
-#include <Serial/serial.hxx>
-#include <Time/fg_time.hxx>
 
 #include "fg_serial.hxx"
 
 FG_USING_STD(string);
 
 
-FGSerial::FGSerial() {
+FGSerial::FGSerial() :
+    save_len(0)
+{
 }
 
 
@@ -62,27 +63,84 @@ bool FGSerial::open( FGProtocol::fgProtocolDir dir ) {
 }
 
 
-// read data from port
-bool FGSerial::read( char *buf, int *length ) {
-    // read a chunk
-    *length = port.read_port( buf );
+// Read data from port.  If we don't get enough data, save what we did
+// get in the save buffer and return 0.  The save buffer will be
+// prepended to subsequent reads until we get as much as is requested.
+
+int FGSerial::read( char *buf, int length ) {
+    int result;
+
+    // read a chunk, keep in the save buffer until we have the
+    // requested amount read
+
+    char *buf_ptr = save_buf + save_len;
+    result = port.read_port( buf_ptr, length - save_len );
     
-    // just in case ...
-    buf[ *length ] = '\0';
+    if ( result + save_len == length ) {
+       strncpy( buf, save_buf, length );
+       save_len = 0;
 
-    return true;
+       return length;
+    }
+    
+    return 0;
+}
+
+
+// read data from port
+int FGSerial::readline( char *buf, int length ) {
+    int result;
+
+    // read a chunk, keep in the save buffer until we have the
+    // requested amount read
+
+    char *buf_ptr = save_buf + save_len;
+    result = port.read_port( buf_ptr, FG_MAX_MSG_SIZE - save_len );
+    save_len += result;
+
+    // look for the end of line in save_buf
+    int i;
+    for ( i = 0; i < save_len && save_buf[i] != '\n'; ++i );
+    if ( save_buf[i] == '\n' ) {
+       result = i + 1;
+    } else {
+       // no end of line yet
+       return 0;
+    }
+
+    // we found an end of line
+
+    // copy to external buffer
+    strncpy( buf, save_buf, result );
+    buf[result] = '\0';
+    FG_LOG( FG_IO, FG_INFO, "fg_serial line = " << buf );
+
+    // shift save buffer
+    for ( i = result; i < save_len; ++i ) {
+       save_buf[ i - result ] = save_buf[i];
+    }
+    save_len -= result;
+
+    return result;
 }
 
+
 // write data to port
-bool FGSerial::write( char *buf, int length ) {
+int FGSerial::write( char *buf, int length ) {
     int result = port.write_port( buf, length );
 
     if ( result != length ) {
        FG_LOG( FG_IO, FG_ALERT, "Error writing data: " << device );
-       return false;
     }
 
-    return true;
+    return result;
+}
+
+
+// write null terminated string to port
+int FGSerial::writestring( char *str ) {
+    int length = strlen( str );
+    return write( str, length );
 }