/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 ); |