class CircleTrajectory:
LAT = 45.0
LON = -74.0
ALT = 1.0
SPEED = 1.0 # M/S
RADIUS = 10.0 # Meters
ORIGIN = Lla(toRadian(LAT), toRadian(LON), ALT)
sinLon = math.sin(ORIGIN.lon)
cosLon = math.cos(ORIGIN.lon)
sinLat = math.sin(ORIGIN.lat)
cosLat = math.cos(ORIGIN.lat)
def _computeVelocity(self, posOnCircle):
ve = -self.SPEED * math.sin(posOnCircle)
vn = self.SPEED * math.cos(posOnCircle)
return Ecef(-self.sinLon * ve - self.sinLat * self.cosLon * vn,
self.cosLon * ve - self.sinLat * self.sinLon * vn,
self.cosLat * vn)
def generateEcefWithDynamics(self, elapsedTime):
time = elapsedTime / 1000.0
posOnCircle = time * self.SPEED / self.RADIUS
e = math.cos(posOnCircle) * self.RADIUS
n = math.sin(posOnCircle) * self.RADIUS
llaPos = self.ORIGIN.addEnu(Enu(e, n, 0))
position = llaPos.toEcef()
return position, self._computeVelocity(posOnCircle)
def send(self, duration):
origin = Lla(toRadian(CircleTrajectory.LAT),
toRadian(CircleTrajectory.LON), CircleTrajectory.ALT)
for i in range(0, duration * 100):
elapsedTime = 10 * i # time in msec
enu = self._generateEnu(elapsedTime)
attitude = Attitude(-math.atan2(enu.north, enu.east), 0,
toRadian(-5))
self.sim.pushTrackLlaNed(elapsedTime, origin.addEnu(enu), attitude)
def readRow(row):
if len(row) < 4: raise StopIteration('Found invalid csv row at line %d' % (reader.line_num))
# Each row has [Speed (m/s), Lat (deg), Lon (deg), Alt (m)] as string
speed = float(row[0])
lat = float(row[1])
lon = float(row[2])
alt = float(row[3])
lla = Lla(toRadian(lat), toRadian(lon), alt)
return [speed, lla]
def __init__(self, speed=1.0, latDeg=45.0, lonDeg=-74.0, alt=1.0):
self.SPEED = speed # M/S
self.LAT = latDeg
self.LONG = lonDeg
self.ALT = alt
self.ORIGIN = Lla(toRadian(self.LAT), toRadian(self.LONG), self.ALT)
self.sinLon = math.sin(self.ORIGIN.lon)
self.cosLon = math.cos(self.ORIGIN.lon)
self.sinLat = math.sin(self.ORIGIN.lat)
self.cosLat = math.cos(self.ORIGIN.lat)
def readRow(row): # Each row has [Time, X (km), Y (km), Z (km), Yaw (deg), Pitch (deg), Roll (deg)] as string time = datetime.strptime(row[0], '%d %b %Y %H:%M:%S.%f') x = float(row[1]) * 1000 y = float(row[2]) * 1000 z = float(row[3]) * 1000 yaw = toRadian(float(row[4])) pitch = toRadian(float(row[5])) roll = toRadian(float(row[6])) xyz = Ecef(x, y, z) ypr = Attitude(yaw, pitch, roll) yield time yield xyz yield ypr
def send(self, duration):
origin = Lla(toRadian(CircleTrajectory.LAT), toRadian(CircleTrajectory.LON), CircleTrajectory.ALT)
for i in range(0,duration*100):
elapsedTime = 10 * i # time in msec
enu = self._generateEnu(elapsedTime)
self.sim.pushTrackLla(elapsedTime, origin.addEnu(enu))
def pushRouteNode(sim, speedKmh, latDeg, lonDeg): #Push Route LLA in radian with speed in m/s sim.pushRouteLla(speedKmh/3.6, Lla(toRadian(latDeg), toRadian(lonDeg), 0));
def pushTrackNode(sim, timestampSec, latDeg, lonDeg):
#Push Track LLA in radian with timestamp in ms
sim.pushTrackLla(timestampSec * 1000,
Lla(toRadian(latDeg), toRadian(lonDeg), 0))
def generateEcef(self, duration, rate):
print('Generating ECEF circle trajectory for ' + str(duration) +
' seconds and a rate of ' + str(rate) + 'Hz')
origin = Lla(toRadian(CircleTrajectory.LAT),
toRadian(CircleTrajectory.LON), CircleTrajectory.ALT)
elapsedTime = 0
filename = 'circle_' + str(rate) + 'Hz_ecef.csv'
print('Creating file ' + filename)
with codecs.open(filename, 'wb', encoding="utf-8") as csvfile:
ecefwriter = csv.writer(csvfile,
delimiter=',',
quotechar='#',
quoting=csv.QUOTE_MINIMAL)
ecefwriter.writerow(['Timestamp (ms)', 'X (m)', 'Y (m)', 'Z (m)'])
# -- Sending positions --
while 1 == 1:
if elapsedTime > duration * 1000:
break
enu = self._generateEnu(elapsedTime)
lla = origin.addEnu(enu)
ecef = lla.toEcef()
ecefwriter.writerow(
[str(elapsedTime),
str(ecef.x),
str(ecef.y),
str(ecef.z)])
elapsedTime += 1000 // rate
#6-degree-of-freedom
elapsedTime = 0
filename = 'circle_' + str(rate) + 'Hz_6dof_ecef.csv'
print('Creating file ' + filename)
with codecs.open(filename, 'wb', encoding="utf-8") as csvfile:
ecefwriter = csv.writer(csvfile,
delimiter=',',
quotechar='#',
quoting=csv.QUOTE_MINIMAL)
ecefwriter.writerow([
'Timestamp (ms)', 'X (m)', 'Y (m)', 'Z (m)', 'Yaw (deg)',
'Pitch (deg)', 'Roll (deg)'
])
# -- Sending positions --
while 1 == 1:
if elapsedTime > duration * 1000:
return
enu = self._generateEnu(elapsedTime)
lla = origin.addEnu(enu)
ecef = lla.toEcef()
attitude = Attitude(-math.atan2(enu.north, enu.east), 0,
toRadian(-5))
ecefwriter.writerow([
str(elapsedTime),
str(ecef.x),
str(ecef.y),
str(ecef.z),
str(attitude.yawDeg()),
str(attitude.pitchDeg()),
str(attitude.rollDeg())
])
elapsedTime += 1000 // rate
