CopterExpress/clever-show
1
0
Fork 0
mirror of https://github.com/CopterExpress/clever-show.git synced 2026-05-26 15:13:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

Drone: Fix GPS delta position counting

Browse Source
This commit is contained in:
Arthur Golubtsov
2020-05-21 16:51:57 +00:00
parent 1f70520ac5
commit bef932871f
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Drone/copter_client.py
View File

@@ -44,7 +44,7 @@ def azi(x, y):
return 90 - math.atan2(y,x)*180/math.pi
def get_xy(dist, azi):
return dist*math.sin(azi), dist*math.cos(azi)
return dist*math.sin(math.radians(azi)), dist*math.cos(math.radians(azi))
static_broadcaster = tf2_ros.StaticTransformBroadcaster()
@@ -162,7 +162,8 @@ class CopterClient(client.Client):
lat = float(self.config.gps_frame_lat)
lon = float(self.config.gps_frame_lon)
geo_delta = Earth.Inverse(telem.lat, telem.lon, lat, lon)
dx, dy = get_xy(geo_delta['s12'], geo_delta['azi2'])
logger.info("dist: {} | azi: {}".format(geo_delta['s12'], geo_delta['azi1']))
dx, dy = get_xy(geo_delta['s12'], geo_delta['azi1'])
gps_dx = telem.x + dx
gps_dy = telem.y + dy
logger.info("GPS frame dx: {} | dy: {}".format(gps_dx, gps_dy))