Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 2195 → Rev 2199

/Transportables_Koptertool/PKT/trunk/followme/followme.c
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