Quad = Multirotor.Multirotor() # default is quadrotor
idx = 0
dt = 0.005
T = 1.3
numsteps = 3
maxInd = int(math.ceil(T/dt))
stateHist = np.zeros((numsteps*maxInd,13))
# command motor
commands = [.5,.5,.5,.5]
#commands = [0,0,0,0]
# add wind
windvel = np.zeros((3,1))
# run for a while
controller = QuadrotorController.Controller()
EKF = KinematicEKF.kinematicQuadEKF()
AttEstimator = KinematicEKF.AttitudeComplementaryFilter()
reference = [0.,0.,0.,3.]
time = 0.
period = dt
'''
for jj in range(int(numsteps)):
for ii in range(0,maxInd):
state = Quad.updateState(dt,commands,windVelocity=windvel)
#stateHist[idx,:] = state
idx = idx+1
publishState(idx*dt,state)
if(jj <= 1):
pass
commands = [.3,.3, .3, .4]
#commands = [0, 0, 0, 0]
else:
pose.velocity.y = state[0, 4]
pose.velocity.z = state[0, 5]
pose.rotational_velocity.x = state[0, 10]
pose.rotational_velocity.y = state[0, 11]
pose.rotational_velocity.z = state[0, 12]
socketEKF.send(pose.SerializeToString())
############################################################################
# here's the real script
############################################################################
EKF = KinematicEKF.kinematicQuadEKF()
AttitudeFilt = KinematicEKF.AttitudeComplementaryFilter()
lastTime = 0.
# Set up ZMQ subscriber/poller stuff
poller = zmq.Poller()
poller.register(IMUReceiver, zmq.POLLIN)
#poller.register(VICONReceiver, zmq.POLLIN)
IMUdata = state_pb2.ImuPb()
VICONdata = state_pb2.PosePb()
firstPass = True
firstPlot = True
ii = 0
jj = 0
while (True):
print 'loop!'
# read in data from zmq
###################################
Quad = Multirotor.Multirotor(fuselageMass = 0.5) # default is quadrotor
idx = 0
dt = 0.002
T = 1.3
numsteps = 3
maxInd = int(math.ceil(T/dt))
stateHist = np.zeros((numsteps*maxInd,13))
commands = [0.5,0.5,0.5,0.5]
# add wind
#windvel = np.array([[30.,30.,0]]).T
windvel = np.zeros((3,1))
# run for a whil
controller = QuadrotorController.Controller()
EKF = KinematicEKF.kinematicQuadEKF()
AttEstimator = KinematicEKF.AttitudeComplementaryFilter(initialAttitude=AQ.Quaternion(np.array(attEst)), timeConstant = 10.,Kbias = .00)
reference = [0.,0.,5.,0.]
time = 0.
lastTime = 0.
startTime = clock.time()
period = dt
Quad.stateVector[0,2] = 0. # initial height
Quad.stateVector[0,6:10] = AQ.Quaternion(np.array([0,0,0])).q
#np.random.seed([])
accMeas = np.zeros((3,1))
def drawQuad(att = [0,0,0], pos = np.zeros((3,1)), color = [1.,0.,0.,1.]):
global Quad
