| 586,172 → 586,176 |
|---|
| void Debug_GPS (void) |
| { |
| |
| uint8_t redraw; |
| uint8_t redraw; |
| |
| double l1; |
| GPS_Pos_t currpos; |
| double l1; |
| GPS_Pos_t currpos; |
| |
| set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
| redraw = true; |
| uint8_t mktimeout = false; |
| set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
| redraw = true; |
| uint8_t mktimeout = false; |
| |
| uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete |
| uint32_t send_followme_counter; |
| Point_t FollowMe; |
| uint32_t NMEA_GPGGA_counter_old = 0; // Merker: zaehlt empfangene GPGGA-Pakete |
| uint32_t send_followme_counter; |
| Point_t FollowMe; |
| |
| |
| while( true ) |
| { |
| while( true ) |
| { |
| |
| //######## Quell Koordinaten ############################## |
| //# |
| // NMEApos.lat = 0x0FB51D1F; //# |
| // NMEApos.lon = 0xE2CF4105; //# |
| //# |
| // NMEApos.lat = 0x1f1db5fb; //# |
| // NMEApos.lon = 0x0541cfe2; //# |
| //######## Quell Koordinaten ############################## |
| //# |
| // NMEApos.lat = 0x0FB51D1F; //# |
| // NMEApos.lon = 0xE2CF4105; //# |
| //# |
| NMEApos.lat = 0x1f1db5fb; //# |
| NMEApos.lon = 0x0541cfe2; //# |
| |
| // l1 = NMEApos.lat; |
| //# |
| //######################################################### |
| // l1 = NMEApos.lat; |
| //# |
| //######################################################### |
| |
| int ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
| /* int ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
| //lcd_cls (); |
| |
| if( ok <= 0 ) |
| { |
| return; // Fehler bzgl. BT GPS-Maus -> exit |
| } |
| |
| if(NMEA_isdataready() && receiveNMEA) |
| { |
| if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
| if( ok <= 0 ) |
| { |
| if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
| return; // Fehler bzgl. BT GPS-Maus -> exit |
| } |
| |
| receiveNMEA = true; |
| |
| if(NMEA_isdataready() && receiveNMEA) |
| { |
| if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
| { |
| if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
| { |
| |
| NMEApos.lat = NMEA.Latitude; |
| NMEApos.lon = NMEA.Longitude; |
| //Config.FM_Refresh |
| /* |
| FollowMe.Position.Status = NEWDATA; |
| FollowMe.Position.Longitude = NMEA.Longitude; |
| FollowMe.Position.Latitude = NMEA.Latitude; |
| FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
| // FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
| NMEApos.lat = NMEA.Latitude; |
| NMEApos.lon = NMEA.Longitude; |
| //Config.FM_Refresh |
| /* |
| FollowMe.Position.Status = NEWDATA; |
| FollowMe.Position.Longitude = NMEA.Longitude; |
| FollowMe.Position.Latitude = NMEA.Latitude; |
| FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
| // FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
| |
| FollowMe.Heading = -1; // invalid heading |
| FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
| FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
| // FollowMe.Event_Flag = 0; // no event |
| 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 = 0; // Will be transferred to the FC and can be used as Poti value there |
| FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
| FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
| 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 |
| FollowMe.Heading = -1; // invalid heading |
| FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
| FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
| // FollowMe.Event_Flag = 0; // no event |
| 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 = 0; // Will be transferred to the FC and can be used as Poti value there |
| FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
| FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
| 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 |
| send_followme_counter++; |
| SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
| send_followme_counter++; |
| |
| //void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
| // SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
| //SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
| */ |
| } |
| //void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
| // SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
| //SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
| |
| NMEA_GPGGA_counter_old = NMEA.Counter; |
| } |
| } |
| |
| //----------------------------------------- |
| // Screen redraw |
| //----------------------------------------- |
| if( redraw ) |
| { |
| lcd_cls(); |
| NMEA_GPGGA_counter_old = NMEA.Counter; |
| } |
| */ |
| //----------------------------------------- |
| // Screen redraw |
| //----------------------------------------- |
| if( redraw ) |
| { |
| lcd_cls(); |
| |
| lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("GPS Test") ); // Titel: oben, mitte |
| lcdx_printf_center_P( 4, MNORMAL, 1,0, PSTR(" Source Lat/Lon") ); |
| lcdx_printf_center_P( 6, MNORMAL, 1,0, PSTR(" Target Lat/Lon") ); |
| lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("GPS Test") ); // Titel: oben, mitte |
| lcdx_printf_center_P( 4, MNORMAL, 1,0, PSTR(" Source Lat/Lon") ); |
| lcdx_printf_center_P( 6, MNORMAL, 1,0, PSTR(" Target Lat/Lon") ); |
| |
| |
| // sprintf(printbuff, "\r\nLat_Source: %ld", NMEA.Latitude); |
| // uart1_puts(printbuff); |
| // |
| // sprintf(printbuff, "\r\nLon_Source: %ld", NMEA.Longitude); |
| // uart1_puts(printbuff); |
| // |
| // |
| // writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 ); // unten links: Latitude |
| // writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 ); |
| // sprintf(printbuff, "\r\nLat_Source: %ld", NMEA.Latitude); |
| // uart1_puts(printbuff); |
| // |
| // sprintf(printbuff, "\r\nLon_Source: %ld", NMEA.Longitude); |
| // uart1_puts(printbuff); |
| // |
| // |
| // writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 ); // unten links: Latitude |
| // writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 ); |
| |
| redraw = false; |
| } |
| redraw = false; |
| } |
| |
| //################# |
| //# DISTANCE TO TARGET |
| //################# |
| //################# |
| //# DISTANCE TO TARGET |
| //################# |
| |
| lcdx_printf_at_P( 0, 1, MNORMAL,0,0, PSTR("Distance: %3d Meter"), Config.FM_Distance ); |
| lcdx_printf_at_P( 0, 1, MNORMAL,0,0, PSTR("Distance: %3d Meter"), Config.FM_Distance ); |
| |
| //################# |
| //# TARGET Azimuth |
| //################# |
| lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR(" Azimuth: %3d Grad"), Config.FM_Azimuth); |
| //################# |
| //# TARGET Azimuth |
| //################# |
| lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR(" Azimuth: %3d Grad"), Config.FM_Azimuth); |
| |
| |
| |
| |
| //----------------- |
| // Original: Lat / Long |
| //----------------- |
| writex_gpspos( 1, 5, NMEApos.lat , MNORMAL,0,0 ); // GPS-Maus: Latitude |
| writex_gpspos(10, 5, NMEApos.lon, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
| //----------------- |
| // Original: Lat / Long |
| //----------------- |
| writex_gpspos( 1, 5, NMEApos.lat , MNORMAL,0,0 ); // GPS-Maus: Latitude |
| writex_gpspos(10, 5, NMEApos.lon, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
| |
| |
| //############################## Test GPS Offset |
| //############################## Test GPS Offset |
| |
| |
| //nmea_move_horz(&NMEApos,&NMEATarget, Config.FM_Azimuth, Config.FM_Distance/1000); // neues Ziel berechnen |
| //nmea_move_horz(&NMEApos,&NMEATarget, Config.FM_Azimuth, Config.FM_Distance/1000); // neues Ziel berechnen |
| |
| followme_calculate_offset(&NMEApos, &NMEATarget, 1, 1); |
| followme_calculate_offset(&NMEApos, &NMEATarget, 1000, 1000); |
| |
| writex_gpspos( 1, 7, (int32_t)NMEATarget.lat , MNORMAL,0,0 ); // Ziel Latitude |
| writex_gpspos(10, 7, (int32_t)NMEATarget.lon , MNORMAL, 0,0 ); // Ziel Longitude |
| writex_gpspos( 1, 7, (int32_t)NMEATarget.lat , MNORMAL, 0,0 ); // Ziel Latitude |
| writex_gpspos(10, 7, (int32_t)NMEATarget.lon , MNORMAL, 0,0 ); // Ziel Longitude |
| |
| |
| |
| //############################## End Test GPS Offset |
| //############################## End Test GPS Offset |
| |
| |
| |
| //----------------------------------------- |
| // TASTEN |
| //----------------------------------------- |
| if( get_key_press(1 << KEY_ESC) ) |
| { |
| break; |
| } |
| //----------------------------------------- |
| // TASTEN |
| //----------------------------------------- |
| if( get_key_press(1 << KEY_ESC) ) |
| { |
| break; |
| } |
| |
| if( get_key_press(1 << KEY_ENTER) ) |
| { |
| redraw = true; |
| // break; |
| } |
| if( get_key_press(1 << KEY_ENTER) ) |
| { |
| redraw = true; |
| // break; |
| } |
| } |
| } |
| } |
| |
| |
| |
| |
| |
| #endif |
| |
| #endif // #ifdef USE_FOLLOWME |