From e0834ed0f23ea3103c83370577064ae9cbf49038 Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sat, 16 Apr 2011 04:52:01 +0000
Subject: [PATCH] my quicky GCS debug library

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1886 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 libraries/GCS_SIMPLE/GCS_SIMPLE.cpp | 72 +++++++++++++++++++++++++++--
 libraries/GCS_SIMPLE/GCS_SIMPLE.h   | 47 +++++++++++++++----
 2 files changed, 106 insertions(+), 13 deletions(-)

diff --git a/libraries/GCS_SIMPLE/GCS_SIMPLE.cpp b/libraries/GCS_SIMPLE/GCS_SIMPLE.cpp
index a66f0d6b94..16600bef4b 100644
--- a/libraries/GCS_SIMPLE/GCS_SIMPLE.cpp
+++ b/libraries/GCS_SIMPLE/GCS_SIMPLE.cpp
@@ -1,6 +1,6 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 //
-//  DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
+//  DIYDrones Custom ; GPS driver for ArduPilot and ArduPilotMega.
 //  Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
 //
 //  This library is free software; you can redistribute it and / or
@@ -31,6 +31,13 @@ GCS_SIMPLE::GCS_SIMPLE(Stream *s) : _port(s)
 // unrecognised messages.
 //
 
+void
+GCS_SIMPLE::ack(void)
+{
+    buff_pointer = 0;
+    flush(1);
+}
+
 bool
 GCS_SIMPLE::read(void)
 {
@@ -46,6 +53,8 @@ GCS_SIMPLE::read(void)
         // read the next byte
         data = _port->read();
 
+    	//_port->write(data);
+
         restart:
         switch(_step){
             case 0:
@@ -64,12 +73,12 @@ GCS_SIMPLE::read(void)
                 goto restart;
 
             case 2:
-                length = data;
+                _length = data;
                 _step++;
                 break;
 
             case 3:
-                id = data;
+                _id = data;
                 _step++;
                 break;
 
@@ -79,9 +88,66 @@ GCS_SIMPLE::read(void)
                     _step = 0;
                     parsed = true;
                 }
+
+                index       =  _buffer.msg.index;
+                id          =  _buffer.msg.id;
+                p1          =  _buffer.msg.p1;
+                altitude    =  _buffer.msg.altitude;
+                latitude    =  _buffer.msg.latitude;
+                longitude   =  _buffer.msg.longitude;
                 break;
             }
     }
     return parsed;
 }
 
+
+// Add binary values to the buffer
+void
+GCS_SIMPLE::write_byte(uint8_t val)
+{
+	mess_buffer[buff_pointer++] = val;
+}
+
+void
+GCS_SIMPLE::write_int(int val )
+{
+	int_out.value = val;
+	mess_buffer[buff_pointer++] = int_out.bytes[0];
+	mess_buffer[buff_pointer++] = int_out.bytes[1];
+}
+
+void
+GCS_SIMPLE::write_float(float val)
+{
+	double_out.float_value = val;
+	mess_buffer[buff_pointer++] = double_out.bytes[0];
+	mess_buffer[buff_pointer++] = double_out.bytes[1];
+	mess_buffer[buff_pointer++] = double_out.bytes[2];
+	mess_buffer[buff_pointer++] = double_out.bytes[3];
+}
+
+void
+GCS_SIMPLE::write_long(long val)
+{
+	double_out.long_value = val;
+	mess_buffer[buff_pointer++] = double_out.bytes[0];
+	mess_buffer[buff_pointer++] = double_out.bytes[1];
+	mess_buffer[buff_pointer++] = double_out.bytes[2];
+	mess_buffer[buff_pointer++] = double_out.bytes[3];
+}
+
+void
+GCS_SIMPLE::flush(uint8_t msg_id)
+{
+	_port->print("4D");  			// This is the message preamble
+	_port->write(buff_pointer);  	// Length
+	_port->write(msg_id);  				// id
+
+	for (uint8_t i = 0; i < buff_pointer; i++) {
+		_port->write(mess_buffer[i]);
+	}
+
+	buff_pointer = 0;
+}
+
diff --git a/libraries/GCS_SIMPLE/GCS_SIMPLE.h b/libraries/GCS_SIMPLE/GCS_SIMPLE.h
index c5477e465c..3de255e2a6 100644
--- a/libraries/GCS_SIMPLE/GCS_SIMPLE.h
+++ b/libraries/GCS_SIMPLE/GCS_SIMPLE.h
@@ -17,16 +17,26 @@
 
 #include <GCS_MAVLink.h>    // MAVLink GCS definitions
 
-
-
 class GCS_SIMPLE {
 public:
-    //GCS_SIMPLE();
     GCS_SIMPLE(Stream *s);
     bool    read(void);
+    void    ack(void);
+    void    write_byte(uint8_t val);
+    void    write_int(int val);
+    void    write_float(float val);
+    void    write_long(long val);
+    void    flush(uint8_t msg_id);
+
 	Stream	*_port;			///< port the GPS is attached to
 
-private:
+    int8_t      index;
+    int8_t      id;
+    int8_t      p1;
+    int32_t     altitude;
+    int32_t     latitude;
+    int32_t     longitude;
+
 #pragma pack(1)
     struct diyd_mtk_msg {
         int8_t      index;
@@ -38,17 +48,34 @@ private:
     };
 #pragma pack(pop)
 
-    // State machine state
-    uint8_t     _step;
-    uint8_t     _payload_counter;
-    uint8_t     length;
-    uint8_t     id;
-
     // Receive buffer
     union {
         diyd_mtk_msg    msg;
         uint8_t         bytes[];
     } _buffer;
+
+private:
+    // State machine state
+    // incoming
+    uint8_t     _step;
+    uint8_t     _payload_counter;
+    uint8_t     _length;
+    uint8_t     _id;
+
+    // outgoing
+    union d_out{
+        uint8_t bytes[4];
+        int32_t long_value;
+        float float_value;
+    } double_out;
+
+    union i_out {
+        uint8_t bytes[2];
+        int16_t value;
+    } int_out;
+
+    uint8_t mess_buffer[50];
+    uint8_t buff_pointer;
 };
 
 #endif  // GCS_SIMPLE_H