import numpy as np
import sys
sys.path.append('..') # add parent directory
import satelliteParam as P
from signalGenerator import signalGenerator
from satelliteAnimation import satelliteAnimation
from dataPlotter import dataPlotter
# instantiate reference input classes
reference = signalGenerator(amplitude=0.5, frequency=0.1)
thetaRef = signalGenerator(amplitude=2.0 * np.pi, frequency=0.1)
phiRef = signalGenerator(amplitude=0.5, frequency=0.1)
tauRef = signalGenerator(amplitude=5, frequency=.5)
# instantiate the simulation plots and animation
dataPlot = dataPlotter()
animation = satelliteAnimation()
t = P.t_start # time starts at t_start
while t < P.t_end: # main simulation loop
# set variables
r = reference.square(t)
theta = thetaRef.sin(t)
phi = phiRef.sin(t)
tau = tauRef.sawtooth(t)
# update animation
state = np.array([[theta], [phi], [0.0], [0.0]])
animation.update(state)
dataPlot.update(t, r, state, tau)
(opts, args) = parser.parse_args()
if len(args) != 1:
print("usage: position_estimate.py <file>")
sys.exit(1)
filename = args[0]
dev = ublox.UBlox(filename)
rtcmfile = open('rtcm2.dat', mode='wb')
if opts.plot:
reference = util.ParseLLH(opts.reference).ToECEF()
plotter = dataPlotter.dataPlotter(reference)
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
# get get position the receiver calculated. We use this to check the calculations
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
if opts.plot:
plotter.plotPosition(pos, 0)
plotter.plotPosition(satinfo.average_position, 1)
