Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Time of week in EXTERNAL_POSE message #3225

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Time of week in EXTERNAL_POSE message #3225

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
15 changes: 11 additions & 4 deletions sw/ground_segment/python/natnet3.x/natnet2ivy.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,7 @@
import sys
from os import path, getenv
from time import time, sleep
import datetime
import numpy as np
from pyquaternion import Quaternion as Quat
from collections import deque
Expand Down Expand Up @@ -299,7 +300,10 @@ def receiveRigidBodyList( rigidBodyList, stamp ):
timestamp[i] = stamp
continue # too early for next message
timestamp[i] = stamp


today = datetime.datetime.today()
utc_time = datetime.datetime.utcnow()
tow = ((today.weekday()+1)*24*3600 + utc_time.hour*3600 + utc_time.minute*60 + utc_time.second+18)*1000 + utc_time.microsecond/1000
vel = compute_velocity(i)

# Rotate position and velocity according to the quaternions found above
Expand All @@ -322,7 +326,8 @@ def receiveRigidBodyList( rigidBodyList, stamp ):
msg['enu_xd'] = vel[0]
msg['enu_yd'] = vel[1]
msg['enu_zd'] = vel[2]
msg['tow'] = int(1000. * stamp) # TODO convert to GPS itow ?
# msg['tow'] = int(1000. * stamp) # TODO convert to GPS itow ?
msg['tow'] = int(tow) # Time of week in ms
# convert quaternion to psi euler angle
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
Expand All @@ -337,12 +342,14 @@ def receiveRigidBodyList( rigidBodyList, stamp ):
# TODO calculate everything
msg = PprzMessage("datalink", "EXTERNAL_POSE_SMALL")
msg['ac_id'] = id_dict[i]
msg['timestamp'] = int(1000. * stamp) # Time in ms
# msg['timestamp'] = int(1000. * stamp) # Time in ms
msg['timestamp'] = int(tow) # Time of week in ms
ivy.send(msg)
else:
msg = PprzMessage("datalink", "EXTERNAL_POSE")
msg['ac_id'] = id_dict[i]
msg['timestamp'] = int(1000. * stamp) # Time in ms
# msg['timestamp'] = int(1000. * stamp) # Time in ms
msg['timestamp'] = int(tow) # Time of week in ms
msg['enu_x'] = pos[0]
msg['enu_y'] = pos[1]
msg['enu_z'] = pos[2]
Expand Down