def __init__(self):
print 'Starting MAV'
self.mav = MAV()
self.airspeeds = [20, 19, 18, 17, 16, 15, 14, 13, 12]
self.count = 0
self.defaultAirspeed = 15
self.mav.waypointCallback = self.wp_cb
def __init__(self):
print 'Starting MAV'
self.mav = MAV()
self.mav.vfrCallback = self.altCallback
self.powered = True
self.airspeeds = [20, 19, 18, 17, 16, 15, 14, 13, 12]
self.count = 0
self.defaultAirspeed = 15
def run(self):
self.mav = MAV()
time.sleep(15)
self.mav.waypointCallback = self.wp_cb
while True:
time.sleep(5)
self.mav.close()
print 'done'
# p.join()
self.pool.join()
class glideSlope():
def __init__(self):
print 'Starting MAV'
self.mav = MAV()
self.airspeeds = [20, 19, 18, 17, 16, 15, 14, 13, 12]
self.count = 0
self.defaultAirspeed = 15
self.mav.waypointCallback = self.wp_cb
def wp_cb(self, x):
print self.count
if x.seq == 7:
if self.count >= len(self.airspeeds):
print 'not tests left'
pass
else:
self.mav.setParam('ARSPD_FBW_MAX',
self.airspeeds[self.count] + 1.)
self.mav.setParam(
'TRIM_ARSPD_CM', self.airspeeds[self.count] *
100) ## Units for this are in cm/2, hence the 100 term
self.mav.setParam('THR_MAX', 0)
self.mav.setParam('TECS_SPDWEIGHT', 2)
print 'Triggering'
print 'Set Airspeed to {}'.format(self.airspeeds[self.count])
self.count += 1
if x.seq == 8:
self.mav.setParam('THR_MAX', 100)
self.mav.setParam('TECS_SPDWEIGHT', 1)
self.mav.setParam('ARSPD_FBW_MAX', 18.0)
print 'Terminating'
else:
pass
def altitude_cb(self, x):
if x.altitude < 20:
self.mav.setParam('THR_MAX', 100)
self.mav.setParamself.mav.setParam(
'TRIM_ARSPD_CM', self.defaultAirspeed *
100) ## Units for this are in cm/2, hence the 100 term
self.mav.setParam('TECS_SPDWEIGHT', 1)
self.mav.setParam('ARSPD_FBW_MAX', 18.0)
print 'Altitude Panic!! THR_MAX set to 100'
def run(self):
while True:
time.sleep(5)
class glideApplication():
def __init__(self):
self.airspeed_queue = Queue(100)
self.pitch_queue = Queue(100)
self.alt_queue = Queue(100)
self.airspeeds = [15,14,13,12,11,10,9]
self.event = Event()
self.count = 0
self.broadcastQueues = []
time.sleep(5)
self.datastores = {'airspeed':{'queue':self.airspeed_queue, 'title': 'Airspeed', 'dis':'Airspeed (m/s)'},\
'pitch':{'queue':self.pitch_queue, 'title': 'Pitch', 'dis':'Pitch Angle (deg)'},\
'alt':{'queue':self.alt_queue, 'title': 'Altitude', 'dis':'Altitude (m)'} }
for i in self.datastores:
self.broadcastQueues.append(Queue())
p = Process(target=f, args=(self.datastores[i]['queue'], self.broadcastQueues[-1],self.datastores[i]['title'],self.datastores[i]['dis'] ))
p.start()
time.sleep(5)
def run(self):
self.mav = MAV()
time.sleep(15)
self.mav.waypointCallback = self.wp_cb
while True:
time.sleep(5)
self.mav.close()
print 'done'
# p.join()
self.pool.join()
def broadcast(self, value):
for q in self.broadcastQueues:
q.put(value)
def ntun(self, x):
self.airspeed_queue.put([x[0],x[2]])
self.alt_queue.put([x[0],x[3]])
if x[3]< 20:
print x[3]
print 'Too low!!!'
self.mav.setParam('THR_MAX', 100)
def ahrs2cb(self, x):
self.pitch_queue.put( [x[0], np.degrees(x[1]) ] )
def wp_cb(self, x):
if x.seq == 6:
print 'Triggering'
self.mav.vfrCallback = self.ntun
self.mav.ahrs2Callback = self.ahrs2cb
self.mav.setParam('TECS_SPDWEIGHT', 2)
# self.mav.setParam('TECS_PTCH_DAMP', 0)
# self.mav.setParam('PTCH2SRV_RMAX_UP', 2 )
# self.mav.setParam('PTCH2SRV_TCONST', .7)
# self.mav.setParam('PTCH2SRV_P', 5)
# self.mav.setParam('PTCH2SRV_I', 0.05)
# self.mav.setParam('PTCH2SRV_D', 0.05)
# self.mav.setParam('PTCH2SRV_RMAX_DOWN', 3 )
print 'Setting min airspeed to: {}'.format(self.airspeeds[self.count])
# self.mav.setParam('TECS_SINK_MAX', self.sinkrate[self.count])
self.mav.setParam('ARSPD_FBW_MIN', self.airspeeds[self.count])
self.mav.setParam('THR_MAX', 0)
if x.seq == 7:
self.count +=1
# self.mav.setParam('PTCH2SRV_RMAX_UP', 0 )
self.mav.setParam('THR_MAX', 100)
self.mav.setParam('TECS_SPDWEIGHT', 1)
# self.mav.setParam('TECS_PTCH_DAMP', 0)
print 'Terminating'
self.mav.vfrCallback = None
self.mav.ahrs2Callback = None
time.sleep(1)
self.broadcast('terminate')
else:
pass
class glideApplication():
def __init__(self):
self.airspeedq = Queue(100)
self.pitchq = Queue(100)
self.climbq = Queue(100)
self.spdweight = [0, 1, 2]
self.sinkrate = [10, 9, 8, 7, 6, 5, 4, 3, 2, 1]
self.airspeeds = [15, 14, 13, 12, 11, 10, 9]
self.event = Event()
self.count = 0
self.broadcastQueues = []
self.plots = {'airspeed':{'plotobj':self.airspeedq, 'title': 'Airspeed', 'ylabel':'Airspeed (m/s)'},\
'pitch':{'plotobj':self.pitchq, 'title': 'Pitch', 'ylabel':'Pitch Angle (deg)'},\
'climb':{'plotobj':self.climbq, 'title': 'Altitude', 'ylabel':'Altitude (m)'} }
for i in self.plots:
self.broadcastQueues.append(Queue())
p = Process(target=f,
args=(self.plots[i]['plotobj'],
self.broadcastQueues[-1], self.plots[i]['title'],
self.plots[i]['ylabel']))
p.start()
time.sleep(5)
def run(self):
self.mav = MAV()
time.sleep(15)
self.mav.waypointCallback = self.wp_cb
while True:
time.sleep(5)
self.mav.close()
print 'done'
# p.join()
self.pool.join()
def broadcast(self, value):
for q in self.broadcastQueues:
q.put(value)
def vfrcb(self, x):
self.airspeedq.put([x[0], x[2]])
self.climbq.put([x[0], x[3]])
if x[3] < 600:
print x[3]
print 'Too low!!!'
self.mav.setParam('THR_MAX', 100)
def ahrs2cb(self, x):
self.pitchq.put([x[0], np.degrees(x[1])])
def wp_cb(self, x):
if x.seq == 3:
print 'Triggering'
self.mav.vfrCallback = self.vfrcb
self.mav.ahrs2Callback = self.ahrs2cb
self.mav.setParam('TECS_SPDWEIGHT', 2)
# self.mav.setParam('TECS_PTCH_DAMP', 0)
# self.mav.setParam('PTCH2SRV_RMAX_UP', 2 )
self.mav.setParam('PTCH2SRV_TCONST', .4)
self.mav.setParam('PTCH2SRV_IMAX', 3500)
self.mav.setParam('PTCH2SRV_P', 1.373599)
self.mav.setParam('PTCH2SRV_I', 0.1173599)
self.mav.setParam('PTCH2SRV_D', 0.09388795)
# self.mav.setParam('PTCH2SRV_RMAX_DOWN', 3 )
print 'Setting min airspeed to: {}'.format(
self.airspeeds[self.count])
# self.mav.setParam('TECS_SINK_MAX', self.sinkrate[self.count])
self.mav.setParam('ARSPD_FBW_MIN', self.airspeeds[self.count])
self.mav.setParam('THR_MAX', 0)
if x.seq == 4:
self.count += 1
# self.mav.setParam('PTCH2SRV_RMAX_UP', 0 )
self.mav.setParam('THR_MAX', 100)
self.mav.setParam('TECS_SPDWEIGHT', 1)
# self.mav.setParam('TECS_PTCH_DAMP', 0)
print 'Terminating'
self.mav.vfrCallback = None
self.mav.ahrs2Callback = None
time.sleep(1)
self.broadcast('terminate')
else:
pass
class glideSlope():
def __init__(self):
print 'Starting MAV'
self.mav = MAV()
self.mav.vfrCallback = self.altCallback
self.powered = True
self.airspeeds = [20, 19, 18, 17, 16, 15, 14, 13, 12]
self.count = 0
self.defaultAirspeed = 15
def altCallback(self, x):
print x[3]
if x[3] > 45:
if self.powered:
self.mav.setParam('ARSPD_FBW_MAX',
self.airspeeds[self.count] + 1.)
self.mav.setParam(
'TRIM_ARSPD_CM', self.airspeeds[self.count] *
100) ## Units for this are in cm/2, hence the 100 term
self.mav.setParam('THR_MAX', 0)
self.mav.setParam('TECS_SPDWEIGHT', 2)
self.powered = False
self.count += 1
if self.count >= len(self.airspeeds):
self.count = 0
if x[3] < 35:
if not self.powered:
self.mav.setParam('THR_MAX', 100)
self.mav.setParam('TECS_SPDWEIGHT', 1)
self.mav.setParam('ARSPD_FBW_MAX', 18.0)
# self.mav.setParam('TRIM_ARSPD_CM', self.defaultAirspeed * 100.) ## Units for this are in cm/2, hence the 100 term
self.powered = True
print 'Altitude Panic!! THR_MAX set to 100'
def run(self):
while True:
time.sleep(5)