Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 2204 → Rev 2205

/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/eeprom/eeprom.h
258,7 → 258,7
uint8_t OSD_ScreenMode; // Variante des OSD-Screen
uint8_t OSD_LipoBar; // Bargraphanzeige für MK Lipo
uint8_t PKT_Baudrate; // Baudrate für BT und Wi232
uint16_t FM_Azimuth; // Azimuth für FollowMe 4.8.2015 CB
int16_t FM_Azimuth; // Azimuth für FollowMe 4.8.2015 CB
uint16_t FM_Speed; // FollowMe Speed in m/s *0.1
uint16_t FM_Radius; // Waypoint Tolerance Radius in meter
uint8_t HWSound; // Hardware Sounderweiterung an PD7
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/followme/followme.c
633,12 → 633,22
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep
redraw = true;
 
//#define OFFLINE
#ifndef OFFLINE
int retcode = GPSMouse_Connect(); // Abfrage der GPS-Daten zum testen Quick an Dirty ;-)
if( retcode <= 0 )
{
return;
}
#endif
 
/* // DEBUG
NMEApos.lat = 520000000;
NMEApos.lon = 0;
Config.FM_Azimuth = 90;
Config.FM_Distance = 10000;
*/
 
while( true )
{
NMEApos.lat = NMEA.Latitude;
655,44 → 665,50
redraw = false;
}
 
//#################
//# DISTANCE TO TARGET
//#################
writex_gpspos( 1, 2, NMEApos.lat, MNORMAL, 0,0 ); // GPS-Maus: Latitude
writex_gpspos(10, 2, NMEApos.lon, MNORMAL, 0,0 ); // GPS-Maus: Longitude
 
//lcdx_printf_at_P( 0, 1, MNORMAL,0,0, PSTR("Distance: %3d Meter"), Config.FM_Distance );
followme_calculate_offset(Config.FM_Distance, Config.FM_Azimuth, &followMeOffset);
 
//#################
//# TARGET Azimuth
//#################
//lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR(" Azimuth: %3d Grad"), Config.FM_Azimuth);
// DEBUG
writex_gpspos( 1, 6, (int32_t)Config.FM_Azimuth*100 , MNORMAL, 0, 0 );
writex_gpspos( 10, 6, (int32_t)Config.FM_Distance*100 , MNORMAL, 0, 0 );
writex_gpspos( 1, 7, (int32_t)followMeOffset.offset_lat*100 , MNORMAL, 0, 0 );
writex_gpspos( 10, 7, (int32_t)followMeOffset.offset_long*100 , MNORMAL, 0, 0 );
 
 
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;
//followMeOffset.offset_long = 10000;
 
followme_calculate_offset(Config.FM_Distance, Config.FM_Azimuth, &followMeOffset);
followme_add_offset(&NMEApos, &NMEATarget, &followMeOffset);
 
writex_gpspos( 1, 4, (int32_t)NMEATarget.lat , MNORMAL, 0,0 ); // Ziel Latitude
writex_gpspos(10, 4, (int32_t)NMEATarget.lon , MNORMAL, 0,0 ); // Ziel Longitude
writex_gpspos( 1, 4, (int32_t)NMEATarget.lat , MNORMAL, 0, 0 ); // Ziel Latitude
writex_gpspos(10, 4, (int32_t)NMEATarget.lon , MNORMAL, 0, 0 ); // Ziel Longitude
 
 
if( get_key_press(1 << KEY_ESC) )
{
#ifndef OFFLINE
GPSMouse_Disconnect();
#endif
break;
}
 
 
if( get_key_press(1 << KEY_ENTER) )
{
redraw = true;
redraw = true;
// break;
}
}
 
if( get_key_press(1 << KEY_MINUS) )
{
Config.FM_Azimuth -= 10;
redraw = true;
}
if( get_key_press(1 << KEY_PLUS) )
{
Config.FM_Azimuth += 10;
redraw = true;
}
}
}
 
 
 
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gps.c
141,50 → 141,34
nmeaPOS pktPos = *pPktPos;
positionOffset followMeOffset = * pFollowMeOffset;
target_pos->lat = pktPos.lat + ( followMeOffset.offset_lat * 1000 * ( LAT_DIV / FOLLOWME_M2DEG ) );
target_pos->lon = pktPos.lon + ( followMeOffset.offset_long * 1000 * ( LONG_DIV / FOLLOWME_M2DEG ) * 8192 ) / abs ( c_cos_8192( (int16_t)(pktPos.lat / LONG_DIV ) ) );
 
target_pos->lat = pktPos.lat + ( ( followMeOffset.offset_lat * ( LAT_DIV / FOLLOWME_M2DEG ) ) ) / 1000;
target_pos->lon = pktPos.lon + ( ( followMeOffset.offset_long * ( LONG_DIV / FOLLOWME_M2DEG ) * (8192/1000) ) / my_abs( c_cos_8192( (pktPos.lat / LONG_DIV ) ) ) );
return 1;
}
 
int16_t my_abs(int16_t input)
{
if(input < 0)
return -input;
else
return input;
}
 
// Rechnet einen Offset aus Radius und Winkel nach Lat/Long
// !!! Vorzeichen der Quadranten noch zu verifizeiren!!!
// Benutzt die c_cos_8192 und c_sin_8192 der FC
// TODO: move to followme.c
// TODO: /8192 optimieren
 
uint8_t followme_calculate_offset(
uint16_t radius,
int16_t angle,
int32_t radius, // in mm
int16_t angle, // in Grad °
positionOffset *followMeOffset
)
{
angle %= 360; // map angle to 0° - 360°
 
if(angle < 90) // Q1
{
followMeOffset->offset_lat = ( radius * c_cos_8192( angle ) ) / 8192;
followMeOffset->offset_long = ( radius * c_sin_8192( angle ) ) / 8192;
}
else
if(angle < 180) // Q2
{
followMeOffset->offset_lat = ( radius * c_cos_8192( angle ) ) / 8192;
followMeOffset->offset_long = - ( radius * c_sin_8192( angle ) ) / 8192;
followMeOffset->offset_lat = ( radius * c_cos_8192( angle ) ) / 8192;
followMeOffset->offset_long = ( radius * c_sin_8192( angle ) ) / 8192;
 
}
else
if(angle < 270) // Q3
{
followMeOffset->offset_lat = - ( radius * c_cos_8192( angle ) ) / 8192;
followMeOffset->offset_long = - ( radius * c_sin_8192( angle ) ) / 8192;
}
else // Q4
{
followMeOffset->offset_lat = - ( radius * c_cos_8192( angle ) ) / 8192;
followMeOffset->offset_long = ( radius * c_sin_8192( angle ) ) / 8192;
}
return 1;
}
 
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gps.h
27,8 → 27,8
 
typedef struct _positionOffset
{
uint16_t offset_lat;
uint16_t offset_long;
int32_t offset_lat;
int32_t offset_long;
}positionOffset;
 
 
60,11 → 60,13
);
 
uint8_t followme_calculate_offset(
uint16_t radius,
int32_t radius,
int16_t angle,
positionOffset *followMeOffset
);
 
int16_t my_abs(int16_t input);
 
 
 
#endif // #define GPS_H_
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/mymath.c
48,7 → 48,7
 
// aus Orginal MK source code
// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
const uint16_t sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \
const int16_t sinlookup[91] = { 0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \
1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \
3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \
4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \
57,34 → 57,14
7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \
8161, 8172, 8181, 8187, 8191, 8192};
 
 
int16_t c_cos_8192(int16_t angle)
{
int8_t m,n;
int16_t sinus;
return (c_sin_8192(90 - angle));
}
 
angle = 90 - angle;
// avoid negative angles
if (angle < 0)
{
m = -1;
angle = -angle;
}
else m = +1;
 
// fold angle to interval 0 to 359
angle %= 360;
 
// check quadrant
if (angle <= 90) n = 1; // first quadrant
else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
else {angle = 360 - angle; n = -1;} //fourth quadrant
// get lookup value
sinus = sinlookup[angle];
// calculate sinus value
return (sinus * m * n);
}
 
int16_t c_sin_8192(int16_t angle)
{
int8_t m, n;
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/menu.c
372,7 → 372,7
#define PKTINFO_en PKTINFO_de
 
 
#ifdef DEBUG_GPS
#ifdef USE_FOLLOWME_STEP2
static const char PKTGPS_de[] PROGMEM = "PKT GPS Debug";
#define PKTGPS_en PKTGPS_de
#endif
714,12 → 714,8
void _DefMenu_Main_NO( void )
{
#ifdef USE_FOLLOWME
#ifdef DEBUG_GPS
MenuCtrl_PushML2_P( ID_DEBUG_GPS , MENU_ITEM, &Debug_GPS , PKTGPS_de , PKTGPS_en );
 
 
#endif
#endif
 
MenuCtrl_PushML2_P( ID_SEARCHMK , MENU_ITEM, NOFUNC , SEARCHMK_de , SEARCHMK_en );