/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/followme/followme.c |
---|
149,8 → 149,10 |
// Hier Höhenanzeigefehler Korrigieren |
#define AltimeterAdjust 1.5 |
positionOffset followMeOffset; |
// Flags |
//uint8_t COSD_FLAGS2 = 0; |
// |
213,7 → 215,6 |
} |
//--------------------- |
// 2. Follow Me |
//--------------------- |
415,7 → 416,6 |
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; |
timer_mk_timeout = MK_TIMEOUT; |
432,7 → 432,6 |
// } |
// Sourcecode aus NaviCtrl/V2.10e/uart1.c |
// FOLLOW_ME |
// else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
470,10 → 469,12 |
// } |
// |
#ifdef USE_FOLLOWME_STEP2 |
sendFollowMeData(&FollowMe, &send_followme_counter, &NMEA_GPGGA_counter_old); |
#else |
//----------------------------------------- |
// neue NMEA Daten? |
//----------------------------------------- |
523,6 → 524,8 |
} |
} |
#endif |
//----------------------------------------- |
// TASTEN |
//----------------------------------------- |
536,7 → 539,6 |
break; |
} |
//----------------------------------------- |
//----------------------------------------- |
if( !abo_timer ) |
550,8 → 552,6 |
abo_timer = ABO_TIMEOUT; |
} |
//-------------------------- |
// Daten Timeout vom MK? |
//-------------------------- |
567,25 → 567,64 |
} // end: while( (receiveNMEA) ); |
OSD_active = false; |
GPSMouse_Disconnect(); |
} |
//--------------------- |
// BEENDEN |
//--------------------- |
OSD_active = false; |
//--------------------- |
// GPS beenden |
//--------------------- |
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 |
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 |
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; |
} |
*tNMEA_GPGGA_counter_old = NMEA.Counter; |
} |
} |
} |
// FollowMeStep2: |
// |
#ifdef USE_FOLLOWME_STEP2 |
void Debug_GPS (void) |
{ |
594,7 → 633,6 |
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 ) |
{ |
601,21 → 639,11 |
return; |
} |
while( true ) |
{ |
NMEApos.lat = NMEA.Latitude; |
NMEApos.lon = NMEA.Longitude; |
//######## Quell Koordinaten ############################## |
NMEApos.lat = NMEA.Latitude; |
NMEApos.lon = NMEA.Longitude; |
//######################################################### |
//----------------------------------------- |
// Screen redraw |
//----------------------------------------- |
if( redraw ) |
{ |
lcd_cls(); |
642,8 → 670,12 |
writex_gpspos( 1, 2, NMEApos.lat , MNORMAL,0,0 ); // GPS-Maus: Latitude |
writex_gpspos(10, 2, NMEApos.lon, MNORMAL, 0,0 ); // GPS-Maus: Longitude |
followme_calculate_offset(&NMEApos, &NMEATarget, 10, 10); |
//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 |
666,7 → 698,7 |
#endif |
#endif // FOLLOW_ME_STEP2 |
#endif // #ifdef USE_FOLLOWME |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/followme/followme.h |
---|
8,7 → 8,11 |
#ifndef FOLLOWME_H_ |
#define FOLLOWME_H_ |
#include "../mk-data-structs.h" |
#include <inttypes.h> |
void FollowMe (void); |
void Debug_GPS (void); |
void sendFollowMeData(Point_t *tFollowMe, uint32_t *tsend_followme_counter, uint32_t *tNMEA_GPGGA_counter_old); |
#endif /* FOLLOWME_H_ */ |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gps.c |
---|
123,27 → 123,72 |
} |
// Berechnet die Position der Kopters für FollowMeStep2 |
// Momentan wird die gleich Position ausgegeben |
// Fügt den Startpostition einen Offset hinzu und gibt es als Zielposition zurück |
// |
// Benutzt die c_cos_8192 der FC |
// TODO: move to followme.c |
// TODO: *8192 optimieren |
uint8_t followme_calculate_offset( |
const nmeaPOS *pPktPos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
int d_lat, /**< Distance lat(m) */ |
int d_long /**< Distance long(m) */ |
uint8_t followme_add_offset( |
const nmeaPOS *pPktPos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
positionOffset *pFollowMeOffset /**< Position Offset in Millimeters */ |
) |
{ |
nmeaPOS pktPos = *pPktPos; |
positionOffset followMeOffset = * pFollowMeOffset; |
target_pos->lat = pktPos.lat + ( d_lat * ( LAT_DIV / FOLLOWME_M2DEG ) ); |
target_pos->lon = pktPos.lon + ( d_long * ( LONG_DIV / FOLLOWME_M2DEG ) * 8192 ) / abs ( c_cos_8192( (int16_t)(pktPos.lat / LONG_DIV ) ) ); |
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 ) ) ); |
return 1; |
} |
// 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, |
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; |
} |
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 |
---|
25,6 → 25,14 |
} nmeaPOS; |
typedef struct _positionOffset |
{ |
uint16_t offset_lat; |
uint16_t offset_long; |
}positionOffset; |
extern nmeaPOS NMEApos; |
extern nmeaPOS NMEATarget; |
45,13 → 53,18 |
); |
uint8_t followme_calculate_offset( |
const nmeaPOS *pkt_pos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
int d_lat, /**< Distance lat(m) */ |
int d_lon /**< Distance long(m) */ |
uint8_t followme_add_offset( |
const nmeaPOS *pPktPos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
positionOffset *followMeOffset /**< Position Offset in Millimeters */ |
); |
uint8_t followme_calculate_offset( |
uint16_t radius, |
int16_t angle, |
positionOffset *followMeOffset |
); |
#endif // #define GPS_H_ |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/mymath.c |
---|
57,7 → 57,6 |
7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \ |
8161, 8172, 8181, 8187, 8191, 8192}; |
//double c_cos_8192(int16_t angle) |
int16_t c_cos_8192(int16_t angle) |
{ |
int8_t m,n; |
86,6 → 85,33 |
return (sinus * m * n); |
} |
int16_t c_sin_8192(int16_t angle) |
{ |
int8_t m, n; |
int16_t sinus; |
// 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); |
} |
// ---------------------------------------------------------------------------------------------- |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/mymath.h |
---|
10,6 → 10,7 |
uint16_t sqrt32(uint32_t qzahl); |
int16_t c_cos_8192(int16_t angle); |
int16_t c_sin_8192(int16_t angle); |
int16_t my_atan2(int32_t y, int32_t x); |
#endif |
#endif |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/main.h |
---|
291,7 → 291,7 |
#define _MAIN_H |
// Softwareversion des PKT |
#define PKTSWVersion "3.85_f" // PKT Version |
#define PKTSWVersion "3.85_f_fm2" // PKT Version |
//######################################################################### |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/setup/setup.c |
---|
1849,8 → 1849,10 |
//if( itemid == FME_REFRESH ) { Config.FM_Refresh = Edit_generic( Config.FM_Refresh ,250,60000, Show_uint3,1 ,NULL,NULL); } |
// FollowMeStep2: |
if( itemid == FME_DISTANCE) { Config.FM_Distance= Edit_generic( Config.FM_Distance , 0, 100, Show_uint3,1 ,strGet(STR_METERS),NULL); } |
if( itemid == FME_AZIMUTH ) { Config.FM_Azimuth = Edit_generic( Config.FM_Azimuth , 0, 360, Show_uint3,1 ,PSTR("Grad"),NULL); } |
// FollowMe: |
if( itemid == FME_SPEED ) { Config.FM_Speed = Edit_generic( Config.FM_Speed , 0, 100, Show_uint3,1 ,PSTR("0.1 m/s") ,NULL); } |
if( itemid == FME_RADIUS ) { Config.FM_Radius = Edit_generic( Config.FM_Radius , 1, 20, Show_uint3,1 ,strGet(STR_METERS),NULL); } |