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

@@ -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;
@@ -292,14 +292,23 @@ void loop()
     // Autopilot mode functions
     if (AP_mode == AP_AUTOMATIC_MODE)
     {
+      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();
+          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
       }
       else   // First time we enter in GPS position hold we capture the target position as the actual position
@@ -320,8 +329,11 @@ void loop()
       }
     }
     else
+      {
+      digitalWrite(LED_Yellow,LOW);
       target_position=0;
       }
+  }
 
   // Medium loop (about 60Hz) 
   if ((currentTime-mediumLoop)>=17){

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