From 05ea4dce7d5d6373955e23b1ef43a835a6025edc Mon Sep 17 00:00:00 2001
From: jjulio1234 <jjulio1234@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Tue, 9 Nov 2010 22:03:09 +0000
Subject: [PATCH] Yellow LED on when GPS position hold mode

git-svn-id: https://arducopter.googlecode.com/svn/trunk@815 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArducopterNG/ArducopterNG.pde | 24 ++++++++++++++++++------
 ArducopterNG/Radio.pde        |  4 ++--
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde
index 4e90ce2088..0bb10fe1b8 100644
--- a/ArducopterNG/ArducopterNG.pde
+++ b/ArducopterNG/ArducopterNG.pde
@@ -250,7 +250,7 @@ void loop()
   currentTime = millis();
 
   // Main loop at 200Hz (IMU + control)
-  if ((currentTime-mainLoop) > 5)    // 200Hz (every 5ms)
+  if ((currentTime-mainLoop) > 5)    // about 200Hz (every 5ms)
   {
     G_Dt = (currentTime-mainLoop)*0.001;   // Microseconds!!!
     mainLoop = currentTime;
@@ -271,8 +271,8 @@ void loop()
       gled_speed = 1200;
       if (AP_mode == AP_NORMAL_MODE)    // Normal mode
         Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw);
-      else                        // Automatic mode : GPS position hold mode
-      Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw);
+      else                              // Automatic mode : GPS position hold mode
+        Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw);
     }
     else {                 // ACRO Mode
       gled_speed = 400;
@@ -292,13 +292,22 @@ void loop()
     // Autopilot mode functions
     if (AP_mode == AP_AUTOMATIC_MODE)
     {
-      if (target_position)
+      digitalWrite(LED_Yellow,HIGH);      // Yellow LED ON : GPS Position Hold MODE
+      if (target_position) 
       {
         #ifdef IsGPS
         if (GPS.NewData)     // New GPS info?
         {
-          read_GPS_data();
-          Position_control(target_lattitude,target_longitude);     // Call GPS position hold routine
+          if (GPS.Fix)
+            {
+            read_GPS_data();    // In Navigation.pde
+            Position_control(target_lattitude,target_longitude);     // Call GPS position hold routine
+            }
+          else
+            {
+            command_gps_roll=0;
+            command_gps_pitch=0;
+            }
         }
         #endif
       }
@@ -320,7 +329,10 @@ void loop()
       }
     }
     else
+      {
+      digitalWrite(LED_Yellow,LOW);
       target_position=0;
+      }
   }
 
   // Medium loop (about 60Hz) 
diff --git a/ArducopterNG/Radio.pde b/ArducopterNG/Radio.pde
index e112113595..fec627763d 100644
--- a/ArducopterNG/Radio.pde
+++ b/ArducopterNG/Radio.pde
@@ -56,7 +56,7 @@ void read_radio()
     // Autopilot mode (only works on Stable mode)
     if (flightMode == STABLE_MODE)
       {
-      if(ch_aux > 1800)
+      if(ch_aux < 1200)
         AP_mode = AP_AUTOMATIC_MODE;   // Automatic mode : GPS position hold mode + altitude hold
       else 
         AP_mode = AP_NORMAL_MODE;   // Normal mode
@@ -123,4 +123,4 @@ void read_radio()
     }
 }
 
-
+