| 4,6 → 4,10 |
|---|
| //############################################################################ |
| //# HISTORY gps.c |
| //# |
| //# 20.09.2015 Startet |
| //# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen |
| //# followme_calculate_offset(...) |
| //# |
| //# 03.08.2015 cebra |
| //# - add: Routine um aus gegebenen Koordinaten mit Abstand und Winkel eine ZielKoordinate zu berechnen |
| //# int nmea_move_horz( |
| 121,25 → 125,21 |
|---|
| |
| // Berechnet die Position der Kopters für FollowMeStep2 |
| // Momentan wird die gleich Position ausgegeben |
| // Benutzt die c_cos_8192 der FC |
| // TODO: move to followme.c |
| |
| int followme_calculate_offset( |
| const nmeaPOS *pkt_pos, /**< Start position in radians */ |
| 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) */ |
| int d_long /**< Distance long(m) */ |
| ) |
| { |
| nmeaPOS p1 = *pkt_pos; |
| // only for test the "Debug-Mode" |
| target_pos->lat = ( d_lat * ( LAT_DIV / FOLLOWME_M2DEG ) ); //p1.lat + |
| target_pos->lon = 100 * (uint32_t)( (float)FOLLOWME_ROUND_100 / cos( (float)p1.lat / (float)LONG_DIV ) ) ; // ( d_long * ( LONG_DIV / FOLLOWME_M2DEG ) ); // |
| // 10 * 10000000 / 111111 |
| nmeaPOS pktPos = *pPktPos; |
| |
| 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 ) ) ); |
| |
| |
| // p1.lon + / FOLLOWME_M2DEG / FOLLOWME_ROUND_100 * |
| |
| |
| |
| return 1; |
| } |
| |