def run(self):
global indexMin
while True:
setLoiterSpeedAndGetACK(toAPLoitSpeed)
#setLoiterAccelerateAndGetACK(LOIT_ACC_MAX)
time.sleep(0.001)
#min8Array[0], min8Array[1], min8Array[2], min8Array[3], min8Array[4], min8Array[5], min8Array[6], min8Array[7], terrainHeight = doLpf()
# print lpf[0]
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
0, #min distance
6000, #max distance
terrainHeight, # current
3, #type
4, #id
25, #ori
0, #cova
0, #fov
0, #fov
dumpy)
master.mav.send(mss)
#singleThreadSendMin(indexMin)
#time.sleep(0.001)
#print "done sent"
indexMin = indexMin + 1
if indexMin >= 8:
indexMin = 0
singleThreadSendMin(indexMin)
def run(self):
global indexMin
while True:
#min8Array[0], min8Array[1], min8Array[2], min8Array[3], min8Array[4], min8Array[5], min8Array[6], min8Array[7], terrainHeight = doLpf()
setLoiterSpeedAndGetACK(toAPLoitSpeed)
setWaypointSpeedAndGetACK(toAPWpSpeed) # need to check this
#setLoiterAccelerateAndGetACK(LOIT_ACC_MAX)
time.sleep(0.005)
# sending terrain....................
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
0, #min distance
6000, #max distance
terrainHeight, # current
3, #type
4, #id
25, #ori
0, #cova
0, #fov
0, #fov
dumpy)
# sending terrain end ....................
master.mav.send(mss)
time.sleep(0.005)
indexMin = indexMin + 1
if indexMin >= 8:
indexMin = 0
#if indexMin % 2 == 0:
singleThreadSendMin(indexMin)
def run(self):
global indexMin
while True:
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
0, #min distance
6000, #max distance
terrainHeight, # current
3, #type
4, #id
25, #ori
0, #cova
0, #fov
0, #fov
dumpy)
# sending terrain end ....................
master.mav.send(mss)
time.sleep(0.005)
indexMin = indexMin + 1
if indexMin >= 8:
indexMin = 0
#if indexMin % 2 == 0:
singleThreadSendMin(indexMin)
def singleThreadSendMin(inx):
msss = mavlink2.MAVLink_distance_sensor_message(
0, #time
10, #min distance
maxDist, #max distance
min8Array[inx], # current
3, #type
1, #id
inx, #ori
255)
master.mav.send(msss)
def parseSec(setors):
global sectors
global msss
global mss
global beepPeriod
global beepDuty
global beeperUsedFor
global terrainHeight
global partFront
global partRight
global partRear
global partLeft
global min0
global min1
global min2
global min3
global min4
global min5
global min6
global min7
global min8Array
global toAPLoitSpeed
global toAPAccMax
now = datetime.now()
timeStart = long(now.strftime("%H%M%S%f"))
timeStart = timeFrom00(timeStart)
while True:
data, addr = sock.recvfrom(2048) # buffer size is 1024 bytes
#print data
if data != None:
break
#print data
#return
abc = data.split(',')
#print abc
if abc[0] == 'E' and abc[1] == ' nomr72':
print('One of MR72 disconnected')
for i in range(8):
min8Array[i] = maxDist + 1
return 'E'
elif abc[0] == 'E' and abc[1] == ' nonra24':
#print ('NRA24 disconnected')
return 'E'
elif abc[0] == 'H':
terrainHeight = float(abc[1])
print(terrainHeight)
if terrainHeight >= 5000:
terrainHeight = 5000
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
0, #min distance
6000, #max distance
terrainHeight, # current
3, #type
4, #id
25, #ori
0, #cova
0, #fov
0, #fov
dumpy)
master.mav.send(mss)
time.sleep(0.001)
return 'H'
else:
abcc = []
for i in range(1, len(abc) - 1):
abcc.append(int(float(abc[i]) * 100))
#aa.append(i*100)
if abcc[i - 1] >= 9999:
abcc[i - 1] = maxDist + 1
#print abcc[i-1]
#print abcc
if abc[0] == 'front':
# abcc.reverse() # quay lai, nguoc voi chieu default
partFront = np.asarray(abcc)
partFront = np.pad(partFront, (0, 248),
'constant',
constant_values=maxDist + 1)
partFront = np.roll(partFront, 33)
if abc[0] == 'left':
# abcc.reverse()
partLeft = np.asarray(abcc)
partLeft = np.pad(partLeft, (0, 248),
'constant',
constant_values=maxDist + 1)
partLeft = np.roll(partLeft, 123)
if abc[0] == 'rear':
# abcc.reverse()
partRear = np.asarray(abcc)
partRear = np.pad(partRear, (0, 248),
'constant',
constant_values=maxDist + 1)
partRear = np.roll(partRear, 213)
if abc[0] == 'right':
# abcc.reverse()
partRight = np.asarray(abcc)
partRight = np.pad(partRight, (0, 248),
'constant',
constant_values=maxDist + 1)
partRight = np.roll(partRight, 303)
#print partFront.size
#print partLeft.size
#print partRight.size
#print partRear.size
cp1 = np.fmin(partFront, partRight)
cp2 = np.fmin(partRear, partLeft)
obstaclesAround = np.minimum(cp1, cp2)
#obstaclesAroundMM = np.roll(obstaclesAround, -22)
#print partRight
#print partFront
abc2 = abcc
#print abc2
abc.reverse()
#print(obstaclesAround[22:66])
#print '\n'
#min8Array[1] = min(obstaclesAround[22:67])
#min8Array[0] = min(obstaclesAround[67:112])
#min8Array[7] = min(obstaclesAround[112:157])
#min8Array[6] = min(obstaclesAround[157:202])
#min8Array[5] = min(obstaclesAround[202:247])
#min8Array[4] = min(obstaclesAround[247:292])
#min8Array[3] = min(obstaclesAround[292:337])
#min8Array[2] = min(min(obstaclesAround[337:360]), min(obstaclesAround[0:22]))
minAll = min(obstaclesAround[0:360])
#obstaclesAroundQuanti = np.asarray(obstaclesAround)
#obstaclesAroundQuanti[range(22, 67)] = min8Array[1]
#obstaclesAroundQuanti[range(67, 112)] = min8Array[0]
#obstaclesAroundQuanti[range(112, 157)] = min8Array[7]
#obstaclesAroundQuanti[range(157, 202)] = min8Array[6]
#obstaclesAroundQuanti[range(202, 247)] = min8Array[5]
#obstaclesAroundQuanti[range(247, 292)] = min8Array[4]
#obstaclesAroundQuanti[range(292, 337)] = min8Array[3]
#obstaclesAroundQuanti[range(337, 360)] = min8Array[2]
#obstaclesAroundQuanti[range(0, 22)] = min8Array[2]
#print min8Array[0], min8Array[1], min8Array[2], min8Array[3], min8Array[4], min8Array[5], min8Array[6], min8Array[7]
#return
#if inLoiter and terrainHeight >= 120: #used for ket hop voi radar dia hinh
#if inLoiter and terrainHeight >= 120 and abs(attitudePitch) < 7 and abs(attitudeRoll) < 7 : #used for ket hop voi radar dia hinh
if inLoiter and abs(attitudePitch) < 7 and abs(
attitudeRoll) < 7: #used for ket hop voi radar dia hinh
min8Array[1] = min(obstaclesAround[22:67])
min8Array[0] = min(obstaclesAround[67:112])
min8Array[7] = min(obstaclesAround[112:157])
min8Array[6] = min(obstaclesAround[157:202])
min8Array[5] = min(obstaclesAround[202:247])
min8Array[4] = min(obstaclesAround[247:292])
min8Array[3] = min(obstaclesAround[292:337])
min8Array[2] = min(min(obstaclesAround[337:360]),
min(obstaclesAround[0:22]))
minAll = min(obstaclesAround[0:360])
obstaclesAroundQuanti = np.asarray(obstaclesAround)
obstaclesAroundQuanti[range(22, 67)] = min8Array[1]
obstaclesAroundQuanti[range(67, 112)] = min8Array[0]
obstaclesAroundQuanti[range(112, 157)] = min8Array[7]
obstaclesAroundQuanti[range(157, 202)] = min8Array[6]
obstaclesAroundQuanti[range(202, 247)] = min8Array[5]
obstaclesAroundQuanti[range(247, 292)] = min8Array[4]
obstaclesAroundQuanti[range(292, 337)] = min8Array[3]
obstaclesAroundQuanti[range(337, 360)] = min8Array[2]
obstaclesAroundQuanti[range(0, 22)] = min8Array[2]
tempDist1 = distanceToStopinManual + float(
distanceToWarn - distanceToStopinManual) / 2
tempDist2 = distanceToStopinManual + float(
distanceToWarn - distanceToStopinManual) / 6
#print distanceToStopinManual,minAll,tempDist1
if (distanceToStopinManual <= minAll <= tempDist1
and beeperUsedFor[1]):
beepDuty = 50
beepPeriod = 0.5
elif (distanceToStopinManual <= minAll <= tempDist2
and beeperUsedFor[1]):
beepDuty = 50
beepPeriod = 0.2
elif (minAll < distanceToStopinManual and beeperUsedFor[1]):
beepDuty = 100
beepPeriod = 1
elif beeperUsedFor[1]:
beepDuty = 0
beepPeriod = 1
if rcAvoid > 1700:
rcRollNorm = round((float(rcRoll) - 1500) / 500, 2)
rcPitchNorm = round((float(rcPitch) - 1500) / 500, 2)
#print '----'
#print rcRoll, rcPitch
rcHoriMag = math.sqrt(pow(rcRollNorm, 2) + pow(rcPitchNorm, 2))
if rcRollNorm == 0.00 or rcRollNorm == -0.00:
rcHoriAngle = math.copysign(90.00, rcPitchNorm)
else:
rcHoriAngle = (math.atan2(rcPitchNorm, rcRollNorm) *
180.00 / math.pi)
if rcHoriAngle < 0:
rcHoriAngle = rcHoriAngle + 359
rcHoriAngle = int(round(rcHoriAngle))
#print rcHoriAngle
#print rcHoriMag
#return
if rcHoriMag >= 0.05: # filtering magitude of RC stick
minSecNum = maxDist + 1
fooIndex = -1
tempSecNum = 0
# filter, lay xung quanh do + - 15 do
#for i in range (-22,23):
# if rcHoriAngle + i < 0:
# tempSecNum = rcHoriAngle + i + 360
# elif rcHoriAngle + i > 359:
# tempSecNum = rcHoriAngle + i - 360
# else:
# tempSecNum = rcHoriAngle + i
# if (minSecNum > obstaclesAroundQuanti[tempSecNum]):
# minSecNum = obstaclesAroundQuanti[tempSecNum]
# fooIndex = tempSecNum
#print obstaclesAroundQuanti[rcHoriAngle]
#print obstaclesAroundQuanti
#return
#if fooIndex!= -1:
if obstaclesAroundQuanti[rcHoriAngle] < distanceToWarn:
# this is used for dieu chinh RC Override, in progress
ratioToRc = float(
(obstaclesAroundQuanti[rcHoriAngle] -
distanceToStopinManual) /
(distanceToWarn - distanceToStopinManual))
print(obstaclesAroundQuanti[rcHoriAngle])
print(ratioToRc)
if ratioToRc <= 0.00:
ratioToRc = 0
#agn = fooIndex*1.53 + 34
ltSpeed = (ratioToRc * LOIT_SPEED)
if ltSpeed <= 20:
ltSpeed = 20
ltAcc = (ratioToRc * LOIT_ACC_MAX)
if ltAcc <= 100:
ltAcc = 100
#print ltAcc
print(ltSpeed)
print('----------------')
toAPLoitSpeed = ltSpeed
#toAPAccMax = ltAcc
else:
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
else:
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
else:
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
return 'DS'
else:
for i in range(8):
min8Array[i] = maxDist + 1
if inAuto or inRTL:
if (minAll < distanceToStopinAuto):
while True:
master.mav.command_long_send(
master.target_system, master.target_component,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0,
0, currLat, currLon, currAlt + 200
) # MUST CHECK mm or m not continuos lift up because of staying in this state just 1 moment
time.sleep(0.01)
msg = master.recv_match()
if not msg:
continue
if msg.get_type() == 'COMMAND_ACK':
if msg.command == 17 and msg.result == 0:
break
print('Obs in Auto, change to Loiter')
# MAV_CMD_NAV_LOITER_UNLIM
#print len(abc)
"""
msss = mavlink2.MAVLink_obstacle_distance_message(
0, #time
3, #sensor type
#abc[2:len(abc) - 2], #distance range in cm
abc,
1, #angular width
10, #min distance
maxDist, #max distance
1.53,
-55.08,
12
)
time.sleep(0.01)
master.mav.send(msss)
"""
# print msss
#return 'O'
now = datetime.now()
timeEnd = long(now.strftime("%H%M%S%f"))
timeEnd = timeFrom00(timeEnd)
def parseSec(setors):
global sectors
global msss
global mss
while True:
data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
if data != None:
break
#print data
abc = data.split(',')
#print abc
if abc[0] == 'E' and abc[1] == ' nomr72':
#print ('MR72 disconnected')
return 'E'
elif abc[0] == 'E' and abc[1] == ' nonra24':
#print ('NRA24 disconnected')
return 'E'
elif abc[0] == 'H':
terrainHeight = float(abc[1])
print(terrainHeight)
if terrainHeight >= 5000:
terrainHeight = 5000
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
0, #min distance
6000, #max distance
terrainHeight, # current
3, #type
4, #id
25, #ori
0, #cova
0, #fov
0, #fov
dumpy
)
master.mav.send(mss)
return 'H'
else:
aa = []
for i in range (len(abc)):
abc[i] = int(float(abc[i]) * 100)
aa.append(i*100)
if abc[i] >= 9999:
abc[i] = maxDist + 1
abc.reverse()
if inLoiter:
min1 = min(abc[0:20])
min2 = min(abc[21:50])
min3 = min(abc[51:71])
#print min1, min2, min3
msss = mavlink1.MAVLink_distance_sensor_message(
0, #time
10, #min distance
maxDist, #max distance
min1, # current
3, #type
1, #id
7, #ori
255)
master.mav.send(msss)
time.sleep(0.01)
msss = mavlink1.MAVLink_distance_sensor_message(
0, #time
10, #min distance
maxDist, #max distance
min2, # current
3, #type
1, #id
0, #ori
255)
master.mav.send(msss)
time.sleep(0.01)
msss = mavlink2.MAVLink_distance_sensor_message(
0, #time
10, #min distance
maxDist, #max distance
min3, # current
3, #type
1, #id
1, #ori
255)
master.mav.send(msss)
#time.sleep(0.01)
return 'DS'
#print len(abc)
msss = mavlink2.MAVLink_obstacle_distance_message(
0, #time
3, #sensor type
#abc[2:len(abc) - 2], #distance range in cm
abc,
1, #angular width
10, #min distance
maxDist, #max distance
1.53,
-55.08,
12
)
time.sleep(0.01)
master.mav.send(msss)
# print msss
return 'O'
time.sleep(0.5)
distance = []
dumpy =[0,0,0,0]
for i in range(72):
distance.append(1000)
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
10, #min distance
10000, #max distance
200, # current
3, #type
1, #id
0, #ori
0, #cova
0, #fov
0, #fov
dumpy
)
connectTo650(master)
time.sleep(0.5)
"""
master.mav.param_set_send(
master.target_system, master.target_component,
def parseSec(setors):
global sectors
global msss
global mss
global beepPeriod
global beepDuty
global beeperUsedFor
global terrainHeight
while True:
data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
if data != None:
break
#print data
#return
abc = data.split(',')
#print abc
if abc[0] == 'E' and abc[1] == ' nomr72':
#print ('MR72 disconnected')
return 'E'
elif abc[0] == 'E' and abc[1] == ' nonra24':
#print ('NRA24 disconnected')
return 'E'
elif abc[0] == 'H':
terrainHeight = float(abc[1])
#print(terrainHeight)
if terrainHeight >= 5000:
terrainHeight = 5000
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
0, #min distance
6000, #max distance
terrainHeight, # current
3, #type
4, #id
25, #ori
0, #cova
0, #fov
0, #fov
dumpy
)
master.mav.send(mss)
return 'H'
else:
aa = []
for i in range (len(abc)):
abc[i] = int(float(abc[i]) * 100)
aa.append(i*100)
if abc[i] >= 9999:
abc[i] = maxDist + 1
for i in range(360):
if i < 34 or i >= 144:
continue
obstaclesAround[i] = abc[int(round((i - 34) / 1.53))]
abc2 = abc
abc.reverse()
min1 = min(abc[0:20])
min2 = min(abc[21:50])
min3 = min(abc[51:71])
minAll = min(abc[0:71])
if inLoiter: #and terrainHeight >= 120: used for ket hop voi radar dia hinh
tempDist1 = distanceToStopinManual + float(distanceToWarn - distanceToStopinManual)/2
tempDist2 = distanceToStopinManual + float(distanceToWarn - distanceToStopinManual)/6
#print distanceToStopinManual,minAll,tempDist1
if (distanceToStopinManual<= minAll <= tempDist1
and beeperUsedFor[1]):
beepDuty = 50
beepPeriod = 0.5
elif (distanceToStopinManual<= minAll <= tempDist2
and beeperUsedFor[1]):
beepDuty = 50
beepPeriod = 0.2
elif (minAll < distanceToStopinManual
and beeperUsedFor[1]):
beepDuty = 100
beepPeriod = 1
elif beeperUsedFor[1] :
beepDuty = 0
beepPeriod = 1
"""
rcJoyLRNew = -1000 + 2000 * ((float(rcJoyLR) - 1000) / 1000)
rcJoyFBNew = -1000 + 2000 * ((float(rcJoyFB) - 1000) / 1000)
print rcJoyLRNew, rcJoyFBNew
msss = mavlink1.MAVLink_manual_control_message(
master.target_component,
rcJoyLRNew,
rcJoyFBNew,
0,
0,
0)
master.mav.send(msss)
"""
if rcAvoid > 1700:
rcRollNorm = round((float(rcRoll) - 1500) /500,2)
rcPitchNorm = round((float(rcPitch) - 1500)/500,2)
#print rcRollNorm, rcPitchNorm
if rcRollNorm == 0.00 or rcRollNorm == -0.00:
rcHoriAngle = math.copysign(90.00, rcPitchNorm)
else:
rcHoriAngle = (math.atan2(rcPitchNorm,rcRollNorm) * 180.00 /math.pi)
#print rcHoriAngle
if rcHoriAngle > 144 or rcHoriAngle < 34:
secFromAngle = -1
else:
secFromAngle = int((rcHoriAngle-34)/1.53)
if secFromAngle < 0:
secFromAngle = 0
if secFromAngle > 71:
secFromAngle = 71
#print secFromAngle
if secFromAngle != -1:
minSecNum = maxDist +1
fooIndex = -1
tempSecNum = 0
for i in range (-4,5):
if secFromAngle + i < 0:
tempSecNum = 0
elif secFromAngle + i > 71:
tempSecNum = 71
else:
tempSecNum = secFromAngle + i
if (minSecNum > abc2[tempSecNum]):
minSecNum = abc2[tempSecNum]
fooIndex = tempSecNum
#print fooIndex
if fooIndex!= -1:
if abc2[fooIndex] < distanceToWarn:
# this is used for dieu chinh RC Override, in progress
print fooIndex
ratioToRc = (abc2[fooIndex] - distanceToStopinManual) / (distanceToWarn - distanceToStopinManual)
print ratioToRc
if ratioToRc <= 0.00:
ratioToRc = 0
#agn = fooIndex*1.53 + 34
ltSpeed = (ratioToRc*LOIT_SPEED)
ltAcc = (ratioToRc*LOIT_ACC_MAX)
print ltAcc
print ltSpeed
print '----------------'
"""
master.mav.param_set_send(
master.target_system, master.target_component,
b'LOIT_SPEED',
ltSpeed,
mavutil.mavlink.MAV_PARAM_TYPE_UINT16
)
"""
setLoiterSpeedAndGetACK(ltSpeed)
setLoiterAccelerateAndGetACK(ltAcc)
#time.sleep(0.01)
"""
master.mav.param_set_send(
master.target_system, master.target_component,
b'LOIT_ACC_MAX',
ltAcc,
mavutil.mavlink.MAV_PARAM_TYPE_UINT16
)
"""
"""
rcRollNew =int( 1500 + (rcRoll - 1500) * ratioToRc * math.cos(agn*math.pi/180.00) )
rcPitchNew = int( 1500 + (rcPitch -1500) * ratioToRc * math.sin(agn*math.pi/180.00))
print abc2[fooIndex]
print rcRoll, rcPitch
print rcRollNew, rcPitchNew
print '---------------------------------'
"""
"""
msss = mavlink1.MAVLink_rc_channels_override_message(
master.target_system,
master.target_component,
rcRollNew, #time
rcPitchNew, #min distance
0, #3
0, #4
0, #5
0, #6
0, #7
0)
master.mav.send(msss)
"""
#time.sleep(0.01)
else:
#pass
"""master.mav.param_set_send(
master.target_system, master.target_component,
b'LOIT_SPEED',
LOIT_SPEED,
mavutil.mavlink.MAV_PARAM_TYPE_UINT16
)
"""
setLoiterSpeedAndGetACK(LOIT_SPEED)
setLoiterAccelerateAndGetACK(LOIT_ACC_MAX)
"""
#time.sleep(0.01)
master.mav.param_set_send(
master.target_system, master.target_component,
b'LOIT_ACC_MAX',
LOIT_ACC_MAX,
mavutil.mavlink.MAV_PARAM_TYPE_UINT16
)
"""
else:
#pass
"""
master.mav.param_set_send(
master.target_system, master.target_component,
b'LOIT_SPEED',
LOIT_SPEED,
mavutil.mavlink.MAV_PARAM_TYPE_UINT16
)
"""
setLoiterSpeedAndGetACK(LOIT_SPEED)
setLoiterAccelerateAndGetACK(LOIT_ACC_MAX)
"""
#time.sleep(0.01)
master.mav.param_set_send(
master.target_system, master.target_component,
b'LOIT_ACC_MAX',
LOIT_ACC_MAX,
mavutil.mavlink.MAV_PARAM_TYPE_UINT16
)
"""
else:
"""
master.mav.param_set_send(
master.target_system, master.target_component,
b'LOIT_SPEED',
LOIT_SPEED,
mavutil.mavlink.MAV_PARAM_TYPE_UINT16
)
#time.sleep(0.01)
"""
setLoiterSpeedAndGetACK(LOIT_SPEED)
setLoiterAccelerateAndGetACK(LOIT_ACC_MAX)
"""
master.mav.param_set_send(
master.target_system, master.target_component,
b'LOIT_ACC_MAX',
LOIT_ACC_MAX,
mavutil.mavlink.MAV_PARAM_TYPE_UINT16
)
"""
#print abc2[fooIndex], distanceToStopinManual, distanceToWarn
#print min1, min2, min3
"""
msss = mavlink1.MAVLink_distance_sensor_message(
0, #time
10, #min distance
maxDist, #max distance
min1, # current
3, #type
1, #id
7, #ori
255)
master.mav.send(msss)
time.sleep(0.01)
"""
msss = mavlink1.MAVLink_distance_sensor_message(
0, #time
10, #min distance
maxDist, #max distance
min2, # current
3, #type
1, #id
0, #ori
255)
master.mav.send(msss)
#time.sleep(0.01)
"""
msss = mavlink2.MAVLink_distance_sensor_message(
0, #time
10, #min distance
maxDist, #max distance
min3, # current
3, #type
1, #id
1, #ori
255)
master.mav.send(msss)
#time.sleep(0.01)
"""
return 'DS'
if inAuto or inRTL:
if min1 < distanceToStopinAuto or min2 < distanceToStopinAuto or min3 < distanceToStopinAuto:
while True:
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,
0,
0, 0, 0, 0, currLat, currLon, currAlt + 2) # not continuos lift up because of staying in this state just 1 moment
time.sleep(0.01)
msg = master.recv_match()
if not msg:
continue
if msg.get_type() == 'COMMAND_ACK':
if msg.command == 17 and msg.result == 0:
break
print 'Obs in Auto, change to Loiter'
# MAV_CMD_NAV_LOITER_UNLIM
#print len(abc)
"""
msss = mavlink2.MAVLink_obstacle_distance_message(
0, #time
3, #sensor type
#abc[2:len(abc) - 2], #distance range in cm
abc,
1, #angular width
10, #min distance
maxDist, #max distance
1.53,
-55.08,
12
)
time.sleep(0.01)
master.mav.send(msss)
"""
# print msss
return 'O'
def parseSec(setors):
global sectors
global msss
global mss
global beepPeriod
global beepDuty
global beeperUsedFor
global terrainHeight
global partFront
global partRight
global partRear
global partLeft
global min0
global min1
global min2
global min3
global min4
global min5
global min6
global min7
global min8Array
global toAPLoitSpeed
global toAPAccMax
now = datetime.now()
timeStart = long(now.strftime("%H%M%S%f"))
timeStart = timeFrom00(timeStart)
#while True:
#data,address = sock.recvfrom(100) # buffer size is 1024 bytes
#while data:
# print data
# data = sock.recvfrom(100)
#print data
#if data != None:
# break
data, address = sock.recvfrom(100)
#print data
#return
abc = data.split(',')
#print abc
if abc[0] == 'E' and abc[1] == ' nomr72':
#print ('MR72 disconnected')
return 'E'
elif abc[0] == 'E' and abc[1] == ' nonra24':
#print ('NRA24 disconnected')
return 'E'
elif abc[0] == 'H':
terrainHeight = float(abc[1])
#print(terrainHeight)
if terrainHeight >= 5000:
terrainHeight = 5000
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
0, #min distance
6000, #max distance
terrainHeight, # current
3, #type
4, #id
25, #ori
0, #cova
0, #fov
0, #fov
dumpy)
master.mav.send(mss)
return 'H'
else:
abcc = []
for i in range(1, len(abc) - 1):
abcc.append(int(float(abc[i]) * 100))
#aa.append(i*100)
if abcc[i - 1] >= 9999:
abcc[i - 1] = maxDist
#print abcc[i-1]
#print abcc
if abc[0] == 'front':
#pass
abcc.reverse()
#partFront[0:36] = np.asarray(abcc)
#print partFront
partFrontTemp[0:33] = abcc[0]
partFrontTemp[33:78] = abcc[1]
partFrontTemp[78:112] = abcc[2]
partFront = np.roll(partFrontTemp, 33)
if abc[0] == 'left':
partLeftTemp[0:33] = abcc[0]
partLeftTemp[33:78] = abcc[1]
partLeftTemp[78:112] = abcc[2]
partLeft = np.roll(partLeftTemp, 123)
if abc[0] == 'rear':
abcc.reverse()
partRearTemp[0:33] = abcc[0]
partRearTemp[33:78] = abcc[1]
partRearTemp[78:112] = abcc[2]
partRear = np.roll(partRearTemp, 213)
#pass
if abc[0] == 'right':
#pass
partRightTemp[0:33] = abcc[0]
partRightTemp[33:78] = abcc[1]
partRightTemp[78:112] = abcc[2]
partRight = np.roll(partRightTemp, 303)
#print partFront.size
#print partLeft.size
#print partRight.size
#print partRear.size
cp1 = np.fmin(partFront, partLeft)
cp2 = np.fmin(partRear, partRight)
obstaclesAround = np.minimum(cp1, cp2)
#print obstaclesAround
#return
#obstaclesAroundMM = np.roll(obstaclesAround, -22)
#print partRight
#print partFront
#abc2 = abcc
#print abc2
#abc.reverse()
#print(obstaclesAround[22:66])
#print '\n'
min8Array[0] = min(obstaclesAround[22:67])
min8Array[7] = min(obstaclesAround[67:112])
min8Array[6] = min(obstaclesAround[112:157])
min8Array[5] = min(obstaclesAround[157:202])
min8Array[4] = min(obstaclesAround[202:247])
min8Array[3] = min(obstaclesAround[247:292])
min8Array[2] = min(obstaclesAround[292:337])
min8Array[1] = min(min(obstaclesAround[337:360]),
min(obstaclesAround[0:22]))
minAll = min(obstaclesAround[0:360])
print min8Array[0], min8Array[1], min8Array[2], min8Array[
3], min8Array[4], min8Array[5], min8Array[6], min8Array[7]
#print minAll
"""
obstaclesAroundPrime = np.where(obstaclesAround==10000, 0, obstaclesAround)
print '1', obstaclesAroundPrime[0:59]
print '2', obstaclesAroundPrime[60:119]
print '3', obstaclesAroundPrime[120:179]
print '4', obstaclesAroundPrime[180:239]
print '5', obstaclesAroundPrime[240:299]
print '6', obstaclesAroundPrime[300:359]
return
"""
if inLoiter: #and terrainHeight >= 120: used for ket hop voi radar dia hinh
tempDist1 = distanceToStopinManual + float(
distanceToWarn - distanceToStopinManual) / 2
tempDist2 = distanceToStopinManual + float(
distanceToWarn - distanceToStopinManual) / 6
#print distanceToStopinManual,minAll,tempDist1
if (distanceToStopinManual <= minAll <= tempDist1
and beeperUsedFor[1]):
beepDuty = 50
beepPeriod = 0.5
elif (distanceToStopinManual <= minAll <= tempDist2
and beeperUsedFor[1]):
beepDuty = 50
beepPeriod = 0.2
elif (minAll < distanceToStopinManual and beeperUsedFor[1]):
beepDuty = 100
beepPeriod = 1
elif beeperUsedFor[1]:
beepDuty = 0
beepPeriod = 1
if rcAvoid > 1700:
#time.sleep(0.005)
rcRollNorm = round((float(rcRoll) - 1500) / 500, 2)
rcPitchNorm = round((float(rcPitch) - 1500) / 500, 2)
#print '----'
print rcRoll, rcPitch
rcHoriMag = math.sqrt(pow(rcRollNorm, 2) + pow(rcPitchNorm, 2))
if rcRollNorm == 0.00 or rcRollNorm == -0.00:
rcHoriAngle = math.copysign(90.00, rcPitchNorm)
else:
rcHoriAngle = (math.atan2(rcPitchNorm, rcRollNorm) *
180.00 / math.pi)
if rcHoriAngle < 0:
rcHoriAngle = rcHoriAngle + 359
rcHoriAngle = int(round(rcHoriAngle))
#print rcHoriAngle
#print rcHoriMag
#print secFromAngle
#print 'dbh'
if rcHoriMag >= 0.05:
minSecNum = maxDist + 1
fooIndex = -1
tempSecNum = 0
# filter, lay xung quanh do + - 15 do
for i in range(-10, 11):
if rcHoriAngle + i < 0:
tempSecNum = rcHoriAngle + i + 360
elif rcHoriAngle + i > 359:
tempSecNum = rcHoriAngle + i - 360
else:
tempSecNum = rcHoriAngle + i
if (minSecNum > obstaclesAround[tempSecNum]):
minSecNum = obstaclesAround[tempSecNum]
fooIndex = tempSecNum
#print fooIndex
if fooIndex != -1:
if obstaclesAround[fooIndex] < distanceToWarn:
# this is used for dieu chinh RC Override, in progress
print fooIndex
ratioToRc = (obstaclesAround[fooIndex] -
distanceToStopinManual) / (
distanceToWarn -
distanceToStopinManual)
print ratioToRc
if ratioToRc <= 0.00:
ratioToRc = 0
#agn = fooIndex*1.53 + 34
ltSpeed = (ratioToRc * LOIT_SPEED)
ltAcc = (ratioToRc * LOIT_ACC_MAX)
print ltAcc
print ltSpeed
print '----------------'
#setLoiterSpeedAndGetACK(ltSpeed)
#setLoiterAccelerateAndGetACK(ltAcc)
toAPLoitSpeed = ltSpeed
toAPAccMax = ltAcc
else:
#pass
#setLoiterSpeedAndGetACK(LOIT_SPEED)
#setLoiterAccelerateAndGetACK(LOIT_ACC_MAX)
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
else:
#pass
#setLoiterSpeedAndGetACK(LOIT_SPEED)
#setLoiterAccelerateAndGetACK(LOIT_ACC_MAX)
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
else:
#setLoiterSpeedAndGetACK(LOIT_SPEED)
#setLoiterAccelerateAndGetACK(LOIT_ACC_MAX)
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
return 'DS'
if inAuto or inRTL:
if (min0 < distanceToStopinAuto or min1 < distanceToStopinAuto
or min2 < distanceToStopinAuto
or min3 < distanceToStopinAuto
or min4 < distanceToStopinAuto
or min5 < distanceToStopinAuto
or min6 < distanceToStopinAuto
or min7 < distanceToStopinAuto):
while True:
master.mav.command_long_send(
master.target_system, master.target_component,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0,
0, currLat, currLon, currAlt + 2
) # not continuos lift up because of staying in this state just 1 moment
time.sleep(0.01)
msg = master.recv_match()
if not msg:
continue
if msg.get_type() == 'COMMAND_ACK':
if msg.command == 17 and msg.result == 0:
break
print 'Obs in Auto, change to Loiter'
# MAV_CMD_NAV_LOITER_UNLIM
#print len(abc)
"""
msss = mavlink2.MAVLink_obstacle_distance_message(
0, #time
3, #sensor type
#abc[2:len(abc) - 2], #distance range in cm
abc,
1, #angular width
10, #min distance
maxDist, #max distance
1.53,
-55.08,
12
)
time.sleep(0.01)
master.mav.send(msss)
"""
# print msss
#return 'O'
now = datetime.now()
timeEnd = long(now.strftime("%H%M%S%f"))
timeEnd = timeFrom00(timeEnd)
def parseSec(setors):
global sectors
global msss
global mss
global beepPeriod
global beepDuty
global beeperUsedFor
global terrainHeight
global partFront
global partRight
global partRear
global partLeft
global min0
global min1
global min2
global min3
global min4
global min5
global min6
global min7
global min8Array
global toAPLoitSpeed
global toAPAccMax
now = datetime.now()
timeStart = long(now.strftime("%H%M%S%f"))
timeStart = timeFrom00(timeStart)
while True:
data, addr = sock.recvfrom(2048) # buffer size is 1024 bytes
#print data
if data != None:
break
#print data
#return
abc = data.split(',')
#print abc
if abc[0] == 'E' and abc[1] == ' nomr72':
print('One of MR72 disconnected')
for i in range(8):
min8Array[i] = maxDist + 1
return 'E'
elif abc[0] == 'E' and abc[1] == ' nonra24':
#print ('NRA24 disconnected')
return 'E'
elif abc[0] == 'H':
terrainHeight = float(abc[1])
print(terrainHeight)
if terrainHeight >= 5000:
terrainHeight = 5000
mss = mavlink2.MAVLink_distance_sensor_message(
0, #time
0, #min distance
6000, #max distance
terrainHeight, # current
3, #type
4, #id
25, #ori
0, #cova
0, #fov
0, #fov
dumpy)
master.mav.send(mss)
time.sleep(0.001)
return 'H'
else:
abcc = []
for i in range(1, len(abc) - 1):
abcc.append(int(float(abc[i]) * 100))
#aa.append(i*100)
if abcc[i - 1] >= 9999:
abcc[i - 1] = maxDist + 1
#print abcc[i-1]
#print abcc
if abc[0] == 'front':
# abcc.reverse() # quay lai, nguoc voi chieu default
partFront = np.asarray(abcc)
partFront = np.pad(partFront, (0, 248),
'constant',
constant_values=maxDist + 1)
partFront = np.roll(partFront, 33)
if abc[0] == 'left':
# abcc.reverse()
partLeft = np.asarray(abcc)
partLeft = np.pad(partLeft, (0, 248),
'constant',
constant_values=maxDist + 1)
partLeft = np.roll(partLeft, 123)
if abc[0] == 'rear':
# abcc.reverse()
partRear = np.asarray(abcc)
partRear = np.pad(partRear, (0, 248),
'constant',
constant_values=maxDist + 1)
partRear = np.roll(partRear, 213)
if abc[0] == 'right':
# abcc.reverse()
partRight = np.asarray(abcc)
partRight = np.pad(partRight, (0, 248),
'constant',
constant_values=maxDist + 1)
partRight = np.roll(partRight, 303)
#print partFront.size
#print partLeft.size
#print partRight.size
#print partRear.size
cp1 = np.fmin(partFront, partRight)
cp2 = np.fmin(partRear, partLeft)
obstaclesAround = np.minimum(cp1, cp2)
#obstaclesAroundMM = np.roll(obstaclesAround, -22)
#print partRight
#print partFront
abc2 = abcc
#print abc2
abc.reverse()
print(obstaclesAround)
return
#print '\n'
minAll = min(obstaclesAround[0:360])
minSecNum = maxDist + 1
fooIndex = -1
tempSecNum = 0
for i in range(-10, 10):
if rcHoriAngle + i < 0:
tempSecNum = rcHoriAngle + i + 360
elif rcHoriAngle + i > 359:
tempSecNum = rcHoriAngle + i - 360
else:
tempSecNum = rcHoriAngle + i
if (minSecNum >= int(obstaclesAround[tempSecNum])):
minSecNum = int(obstaclesAround[tempSecNum])
fooIndex = tempSecNum
#if fooIndex!= -1:
print rcHoriAngle
print obstaclesAround[fooIndex]
print '------------------'
#if inBreak and fooIndex!= 1:
if inBreak and rcHoriMag >= 0.1:
if obstaclesAround[fooIndex] >= distanceToStopinManual:
master.set_mode(5)
return
if inLoiter:
tempDist1 = distanceToStopinManual + float(
distanceToWarn - distanceToStopinManual) / 2
tempDist2 = distanceToStopinManual + float(
distanceToWarn - distanceToStopinManual) / 6
#print distanceToStopinManual,minAll,tempDist1
if (distanceToStopinManual <= minAll <= tempDist1
and beeperUsedFor[1]):
beepDuty = 50
beepPeriod = 0.5
elif (distanceToStopinManual <= minAll <= tempDist2
and beeperUsedFor[1]):
beepDuty = 50
beepPeriod = 0.2
elif (minAll < distanceToStopinManual and beeperUsedFor[1]):
beepDuty = 100
beepPeriod = 1
elif beeperUsedFor[1]:
beepDuty = 0
beepPeriod = 1
if rcAvoid > 1700:
if rcHoriMag >= 0.05: # filtering magitude of RC stick
if obstaclesAround[fooIndex] <= distanceToStopinManual:
master.set_mode(17)
return
# filter, lay xung quanh do + - 15 do
if obstaclesAround[fooIndex] < distanceToWarn:
ratioToRc = float(
(obstaclesAround[rcHoriAngle] -
distanceToStopinManual) /
(distanceToWarn - distanceToStopinManual))
#print (obstaclesAround[rcHoriAngle])
#print (ratioToRc)
if ratioToRc <= 0.00:
ratioToRc = 0
#agn = fooIndex*1.53 + 34
ltSpeed = (ratioToRc * LOIT_SPEED)
ltAcc = (ratioToRc * LOIT_ACC_MAX)
#print ltAcc
#print (ltSpeed)
#print ('----------------')
toAPLoitSpeed = ltSpeed
toAPAccMax = ltAcc
else:
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
else:
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
else:
toAPLoitSpeed = LOIT_SPEED
toAPAccMax = LOIT_ACC_MAX
return 'DS'
if inAuto or inRTL:
if (minAll < distanceToStopinAuto):
while True:
master.mav.command_long_send(
master.target_system, master.target_component,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0,
0, currLat, currLon, currAlt + 200
) # MUST CHECK mm or m not continuos lift up because of staying in this state just 1 moment
time.sleep(0.01)
msg = master.recv_match()
if not msg:
continue
if msg.get_type() == 'COMMAND_ACK':
if msg.command == 17 and msg.result == 0:
break
print('Obs in Auto, change to Loiter')
now = datetime.now()
timeEnd = long(now.strftime("%H%M%S%f"))
timeEnd = timeFrom00(timeEnd)
