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

Drone: Add GPS frame static broadcast

Browse Source
This commit is contained in:
Arthur Golubtsov
2020-05-21 20:07:47 +03:00
parent bef932871f
commit 72e1d1e916
1 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
Drone/copter_client.py
View File

@@ -142,7 +142,7 @@ class CopterClient(client.Client):
trans.transform.translation.z = 0
trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,0))
trans.header.frame_id = "map"
trans.child_frame_id = self.config.copter_frame_id
trans.child_frame_id = "gps"
static_broadcaster.sendTransform(trans)
gps_frame_thread = threading.Thread(target=self.gps_frame_broadcast_loop, name="GPS frame broadcast thread")
gps_frame_thread.start()
@@ -155,10 +155,6 @@ class CopterClient(client.Client):
if math.isnan(telem.lat) or math.isnan(telem.lon) or math.isnan(telem.x) or math.isnan(telem.y):
logger.info("Can't get position from telemetry")
else:
#pos_init = Earth.Direct(telem.lat, telem.lon, azi(-telem.x,-telem.y), dist(telem.x,telem.y))
#lat_init = pos_init['lat2']
#lon_init = pos_init['lon2']
#logger.info("Initial lat: {} | lon: {}".format(lat_init, lon_init))
lat = float(self.config.gps_frame_lat)
lon = float(self.config.gps_frame_lon)
geo_delta = Earth.Inverse(telem.lat, telem.lon, lat, lon)
@@ -167,6 +163,15 @@ class CopterClient(client.Client):
gps_dx = telem.x + dx
gps_dy = telem.y + dy
logger.info("GPS frame dx: {} | dy: {}".format(gps_dx, gps_dy))
trans = TransformStamped()
trans.transform.translation.x = gps_dx
trans.transform.translation.y = gps_dy
trans.transform.translation.z = 0
trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,
math.radians(self.config.gps_frame_yaw)))
trans.header.frame_id = "map"
trans.child_frame_id = "gps"
static_broadcaster.sendTransform(trans)
rate.sleep()