From 86611d1f6b8d1a7cd46c357f27e67bc5e6d44ddb Mon Sep 17 00:00:00 2001
From: Agis Zisimatos <azisi@windowslive.com>
Date: Wed, 7 Jan 2015 00:35:14 +0200
Subject: [PATCH] Implement get position command using custom and test code.

---
 Arduino/SatNOGS.ino | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/Arduino/SatNOGS.ino b/Arduino/SatNOGS.ino
index 8a02ad6..a954461 100644
--- a/Arduino/SatNOGS.ino
+++ b/Arduino/SatNOGS.ino
@@ -163,8 +163,21 @@ void cmd_proc(int &stepAz, int &stepEl)
   while (Serial.available() > 0)
   {
     incomingByte = Serial.read();
+    /* XXX: Get position using custom and test code */
+    if (incomingByte == '!')
+    {
+      /* Get position */
+      Serial.print("TM");
+      Serial.print(1);
+      Serial.print(" ");
+      Serial.print("AZ");
+      Serial.print(10*step2deg(AZstepper.currentPosition()), 1);
+      Serial.print(" ");
+      Serial.print("EL");
+      Serial.println(10*step2deg(ELstepper.currentPosition()), 1);
+    }
     /*new data*/
-    if (incomingByte == '\n')
+    else if (incomingByte == '\n')
     {
       p = buffer;
       buffer[counter] = 0;