WebSVN - Projects - Diff - Rev 2203 and 2204 - /Transportables_Koptertool/PKT/branches/branch

 uint16_t heading_home;
 // Hier Höhenanzeigefehler Korrigieren
+#define AltimeterAdjust 1.5
-#define AltimeterAdjust 1.5
+positionOffset followMeOffset;
+    {
         return;                                                         // Fehler bzgl. BT GPS-Maus -> exit
+    }
     //---------------------
     // 2. Follow Me
             // Target: Lat / Long
             //-----------------
             writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 );                 // GPS-Maus: Latitude
             writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 );                 // GPS-Maus: Longitude
             rxd_buffer_locked = FALSE;
 //        {
 //            NMEA_GetNewData();
 //        }
 // Sourcecode aus NaviCtrl/V2.10e/uart1.c
 //  FOLLOW_ME
 //        else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))
 //                }
 //                UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time
 //        }
 //
+#ifdef USE_FOLLOWME_STEP2
         sendFollowMeData(&FollowMe, &send_followme_counter, &NMEA_GPGGA_counter_old);
 #else
                 NMEA_GPGGA_counter_old = NMEA.Counter;
+            }
+        }
 #endif
         //-----------------------------------------
         // TASTEN
         //-----------------------------------------
         if( get_key_press(1 << KEY_ENTER) )
+        {
             break;
+        }
         //-----------------------------------------
         //-----------------------------------------
         if( !abo_timer )
+        {
             SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1);
             abo_timer = ABO_TIMEOUT;
-        }
+        }
         //--------------------------
         // Daten Timeout vom MK?
         //--------------------------
             // OSD_MK_Connect( MK_CONNECT );
+        }
+    } // end: while( (receiveNMEA) );
+    OSD_active = false;
-    } // end: while( (receiveNMEA) );
+    GPSMouse_Disconnect();
+}
+// Funktion sendFollowMeData angepasst auf Zeiger
+// ausgelagert als Funktion, da auch von FollowMeStep2 verwendet wird
+// TODO: testen mit Simulator und Kopter :-)
+void sendFollowMeData(Point_t *tFollowMe, uint32_t *tsend_followme_counter, uint32_t  *tNMEA_GPGGA_counter_old)
+{
+        Point_t FollowMe = *tFollowMe;
+        if(NMEA_isdataready() && receiveNMEA)
+        {
+                if( NMEA.Counter > *tNMEA_GPGGA_counter_old )
+                {
+                        if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) )
+                        {
+                                FollowMe.Position.Status    = NEWDATA;
+                                FollowMe.Position.Longitude = NMEA.Longitude;
+                                FollowMe.Position.Latitude  = NMEA.Latitude;
+                                FollowMe.Position.Altitude  = 1;                    // 20.7.2015 CB
+                                FollowMe.Heading = -1;                              // invalid heading
-    //---------------------
+                                FollowMe.ToleranceRadius = Config.FM_Radius;        // 5 meter default
-    // BEENDEN
+                                FollowMe.HoldTime = 60;                             // ????? go home after 60s without any update ??????
-    //---------------------
+                                FollowMe.Event_Flag = 1;                            //  20.7.2015 CB
+                                FollowMe.Index = 1;                                 // 2st wp, 0 = Delete List, 1 place at first entry in the list
+                                FollowMe.Type = POINT_TYPE_WP;                      // Typ des Wegpunktes
+                                FollowMe.Name[0] = 'F';                             // Name des Wegpunktes (ASCII)
+                                FollowMe.Name[1] = 'O';
+                                FollowMe.Name[2] = 'L';
+                                FollowMe.Name[3] = 'L';
+                                FollowMe.WP_EventChannelValue = 100;                // set servo value 20.7.2015
+                                FollowMe.AltitudeRate = 0;                          // rate to change the Aetpoint
-    OSD_active = false;
+                                FollowMe.Speed = Config.FM_Speed;                   // rate to change the Position
+                                FollowMe.CamAngle = 255;                            // Camera servo angle in degree (255 -> POI-Automatic)
+                                FollowMe.reserve[0] = 0;                            // reserve
+                                FollowMe.reserve[1] = 0;                            // reserve
+                                SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) );       //'s' = target Position  'w' = Waypoint
                                 *tsend_followme_counter += 1;
+                        }
     //---------------------
-    // GPS beenden
+                        *tNMEA_GPGGA_counter_old = NMEA.Counter;
     //---------------------
     GPSMouse_Disconnect();
+}
-//
+// FollowMeStep2:
 #ifdef USE_FOLLOWME_STEP2
-void Debug_GPS (void)
 #ifdef USE_FOLLOWME_STEP2
-        uint8_t   redraw;
+void Debug_GPS (void)
+{
+        uint8_t   redraw;
-        set_beep( 25, 0xffff, BeepNormal );                                 // kurzer Bestaetigungs-Beep
-        redraw = true;
-    int retcode = GPSMouse_Connect();                                   // Abfrage der GPS-Daten zum testen Quick an Dirty ;-)
-    if( retcode <= 0 )
-    {
-        return;
-    }
-        while( true )
+        set_beep( 25, 0xffff, BeepNormal );                                 // kurzer Bestaetigungs-Beep
         redraw = true;
+                writex_gpspos( 1, 2, NMEApos.lat , MNORMAL,0,0 );                // GPS-Maus: Latitude
+                writex_gpspos(10, 2, NMEApos.lon, MNORMAL, 0,0 );                // GPS-Maus: Longitude
+                //followMeOffset.offset_lat = 10000;
-                writex_gpspos( 1, 2, NMEApos.lat , MNORMAL,0,0 );                // GPS-Maus: Latitude
+                //followMeOffset.offset_long = 10000;
                 writex_gpspos(10, 2, NMEApos.lon, MNORMAL, 0,0 );                // GPS-Maus: Longitude
                 followme_calculate_offset(Config.FM_Distance, Config.FM_Azimuth, &followMeOffset);

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/followme/followme.c - Rev 2203 → 2204