Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 2203 → Rev 2204

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