def get_rssi(sock, station_no):
#initializing crazyradio
cradio = crazyradio.Crazyradio()
cradio.set_data_rate(cradio.DR_2MPS)
cradio.set_channel(70)
PACKETS_COUNT = 50 # number if packets to send to check whether a drone is present
count = 0
rss = 0
total = 0
while count < PACKETS_COUNT:
pk = cradio.send_packet([0xff, ])
if pk.ack and len(pk.data) > 2 and \
pk.data[0] & 0xf3 == 0xf3 and pk.data[1] == 0x01:
count += 1
rss = pk.data[2]
total += 1
else:
count += 1
drone_present = 0
#check if there is a drone present
if total > 1:
drone_present = 1
#print("Drone detected")
else:
drone_present = 0
#print("No drone")
count = 0
rss = 0
x = 20 # number of rss readings to average
#get x no. of RSS then average
if drone_present:
while count < x:
pk = cradio.send_packet([0xff, ])
if pk.ack and len(pk.data) > 2 and \
pk.data[0] & 0xf3 == 0xf3 and pk.data[1] == 0x01:
count += 1
rss += pk.data[2]
rss = rss / x
rss = int(rss)
else:
rss = 10000
#closing radio
cradio.close()
#display RSS reading
print("RSSI: %d" % rss)
time.sleep(0.2)
return rss
def GetVBat():
cr = crazyradio.Crazyradio()
cr.set_channel(0)
cr.set_data_rate(cr.DR_2MPS)
pk = CRTPPacket()
# Create the log block
pk.set_header(CRTPPort.LOGGING, log.CHAN_SETTINGS)
pk.data = ([log.CMD_CREATE_BLOCK, 0x01, 0x07, 42])
packet = bytearray([pk.header]) + pk.data
retry = True
while retry:
res = cr.send_packet(packet)
if res.ack:
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == pk.header:
if res.data[1] == log.CMD_CREATE_BLOCK:
retry = False
print("Log created with return value 0x{:02x}".format(res.data[2]))
# Start the log block
pk.set_header(CRTPPort.LOGGING, log.CHAN_SETTINGS)
pk.data = ([log.CMD_START_LOGGING, 0x01, 50])
packet = bytearray([pk.header]) + pk.data
retry = True
while retry:
res = cr.send_packet(packet)
if res.ack:
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == pk.header:
if res.data[1] == log.CMD_START_LOGGING:
retry = False
print("Log started with return value 0x{:02x}".format(res.data[2]))
# Now loop forever sending empty CRTP packets, waiting for log data in the ACK.
pk.set_header(CRTPPort.COMMANDER, 0)
roll = 0.0
pitch = 0.0
yaw = 0.0
thrust = 0
pk.data = bytearray(struct.pack("f", roll))
pk.data += bytearray(struct.pack("f", pitch))
pk.data += bytearray(struct.pack("f", yaw))
pk.data += bytearray(struct.pack("H", thrust))
packet = bytearray([pk.header]) + pk.data
logPacket = CRTPPacket()
logPacket.set_header(CRTPPort.LOGGING, log.CHAN_LOGDATA)
while True:
res = cr.send_packet(packet)
if res.ack:
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == logPacket.header:
if res.data[1] == 0x01:
print(struct.unpack("<f", res.data[5:])[0])
def DownloadTOC():
cr = crazyradio.Crazyradio()
cr.set_channel(0)
cr.set_data_rate(cr.DR_2MPS)
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, log.CHAN_TOC)
pk.data = ([log.CMD_TOC_INFO])
packet = bytearray([pk.header]) + pk.data
retry = True
while retry:
print("Sending TOC info request...")
res = cr.send_packet(packet)
if res.ack:
print("ACK received")
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == pk.header:
if res.data[1] == log.CMD_TOC_INFO:
retry = False
print("Received TOC info!")
print("Entire ACK: " + "".join("0x%02x " % b for b in res.data))
tocSize = res.data[2]
#tocChkSum = res.data[3:6]
tocMaxPacket = res.data[7]
tocMaxOps = res.data[8]
print("TOC size={:d}".format(tocSize))
#print("TOC chksum={:d}".format(tocChkSum))
print("TOC MaxPacket={:d}".format(tocMaxPacket))
print("TOC MaxOps={:d}".format(tocMaxOps))
for i in range(0, tocSize):
pk.data = ([log.CMD_TOC_ELEMENT, i])
packet = bytearray([pk.header]) + pk.data
retry = True
while retry:
#print("Sending TOC Element request for {:d}".format(i))
res = cr.send_packet(packet)
if res.ack:
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == pk.header:
if res.data[1] == log.CMD_TOC_ELEMENT:
if res.data[2] == i:
retry = False
strs = bytes.decode(bytes(res.data[4:])).split("\0")
print("Element {:d} of type {:d} is called \"{:s}\" of group \"{:s}\"".
format(res.data[2], res.data[3], strs[1], strs[0]))
def GarbageBytesTest():
cr = crazyradio.Crazyradio()
cr.set_channel(0)
cr.set_data_rate(cr.DR_2MPS)
pk = CRTPPacket()
pk.set_header(CRTPPort.COMMANDER, 0)
roll = 0.0
pitch = 0.0
yaw = 0.0
thrust = 0
pk.data = bytearray(struct.pack("f", roll))
pk.data += bytearray(struct.pack("f", pitch))
pk.data += bytearray(struct.pack("f", yaw))
pk.data += bytearray(struct.pack("H", thrust))
packet = bytearray([pk.header]) + pk.data
garbageBytes = 0
totalSends = 0
run = True
while run:
res = cr.send_packet(packet)
totalSends += 1
if res.ack:
if res.data[0] == 0xF3:
print("".join("0x%02x " % b for b in res.data))
run = False
elif res.data[0] == 0xF7:
print("".join("0x%02x " % b for b in res.data))
run = False
else:
for byte in res.data:
garbageBytes += 1
# print("Received " + garbageBytes + " garbage bytes in " + totalSends + " packets")
print(garbageBytes)
print(totalSends)
This script should be used on a Crazyflie with bluetooth disabled and RSSI
ack packet enabled to get RSSI feedback. To configure the Crazyflie in this
mode build the crazyflie2-nrf-firmware with
```make BLE=0 CONFIG=-DRSSI_ACK_PACKET```.
See https://github.com/bitcraze/crazyflie-lib-python/issues/131 for more
informations.
'''
import argparse
import matplotlib.pyplot as plt
import numpy as np
import cflib.drivers.crazyradio as crazyradio
radio = crazyradio.Crazyradio()
# optional user input
parser = argparse.ArgumentParser(description='Key variables')
parser.add_argument('-try',
'--try',
dest='TRY',
type=int,
default=100,
help='the time to send data for each channel')
# by default my crazyflie uses channel 80
parser.add_argument('-channel',
'--channel',
dest='channel',
type=int,
default=80,
from linuxjsdev import Joystick
CMD_ESTOP_STOP = 0x03
CMD_ESTOP_WD_RESET = 0x04
CF_DATA_CHANNEL = 110
try:
js = Joystick()
js.open(0)
except KeyError:
print("Joystick not found!")
sys.exit(1)
try:
cr = crazyradio.Crazyradio(devid=0)
except Exception:
print("Crazyradio not found!")
sys.exit(1)
cr.set_channel(CF_DATA_CHANNEL)
cr.set_data_rate(cr.DR_2MPS)
cr.set_address(b'\xff\xe7\xe7\xe7\xe7')
cr.set_ack_enable(False)
while True:
jsdata = js.read(0)
cmd_byte = CMD_ESTOP_WD_RESET
if jsdata[1][0] == 1:
cmd_byte = CMD_ESTOP_STOP
cr.handle.write(endpoint=1, data=(0x61, cmd_byte), timeout=1000)
def get_rssi(sock, station_no):
cradio = crazyradio.Crazyradio()
cradio.set_data_rate(cradio.DR_2MPS)
cradio.set_channel(70)
count = 0
#delay = (float(station_no) - 1) / 10
#delay = (float(station_no) - 1)
delay = (station_no - 1) * 2
#addr = 0xff - station_no - 1
#print (delay)
#print (addr)
rss = 0
total = 0
#cradio = crazyradio.Crazyradio()
#cradio.set_data_rate(cradio.DR_2MPS)
#cradio.set_channel(70)
#RSS_list = []
sleep(delay)
while count < 100:
pk = cradio.send_packet([
0xff,
])
if pk.ack and len(pk.data) > 2 and \
pk.data[0] & 0xf3 == 0xf3 and pk.data[1] == 0x01:
#print("RSSI: -{}dBm".format(pk.data[2]))
#print ("RSSI: %d" % pk.data[2])
count += 1
rss = pk.data[2]
#RSS_list.append(rss)
#print("hello")
total += 1
else:
#print("No RSS")
count += 1
#print("hi")
#rss = 10000
drone = 0
#check if there is a drone present
if total > 1:
drone = 1
print("Drone detected")
else:
drone = 0
print("No drone")
count = 0
rss = 0
x = 20
#get x no. of RSS then average
if drone:
#for i in range (0, len(RSS_list)):
# rss_sum += RSS_list[i]
#rss = int(rss_sum / len(RSS_list))
while count < x:
pk = cradio.send_packet([
0xff,
])
if pk.ack and len(pk.data) > 2 and \
pk.data[0] & 0xf3 == 0xf3 and pk.data[1] == 0x01:
count += 1
rss += pk.data[2]
rss = rss / x
rss = int(rss)
else:
rss = 10000
cradio.close()
print("RSSI: %d" % rss)
return rss
def crazyFlieLoop(self, radio_id, drone_id):
# All uneven drones are assigned the channel below
# if drone_id % 2 == 0:
channel = drone_id * 10
#else:
# channel = (drone_id-1)*10
# The complete uri of the crazyfly
URI = 'radio://' + str(radio_id) + '/' + str(
channel) + '/2M/E7E7E7E70' + str(drone_id)
print("Connect to ", URI)
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Open up the logging file, seperated by files per date
datestr = "../../experiments/" + time.strftime("%y%m%d")
if os.path.exists(datestr) == False:
os.mkdir(datestr)
#in
first_run = True
pos_x_int = 0
pos_y_int = 0
rssi_array = []
rssi_heading_array = []
stop_looping = False
start_time = time.time()
outbound = 1
reconnect = True
radio = crazyradio.Crazyradio()
number_count = 0
while reconnect:
self.time_keep_flying = 0
try:
cf = Crazyflie(rw_cache='./cache')
cf.param.add_update_callback(group="gbug",
name="keep_flying",
cb=self.param_keepflying_callback)
cf.param.add_update_callback(group="gbug",
name="outbound",
cb=self.param_outbound_callback)
lg_states = LogConfig(name='kalman_states', period_in_ms=500)
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
#lg_states.add_variable('radio.rssi')
#lg_states.add_variable('radio.rssi_inter')
lg_states.add_variable('stabilizer.yaw')
lg_states.add_variable('gradientbug.rssi_angle')
lg_states.add_variable('gradientbug.state')
lg_states.add_variable('gradientbug.state_wf')
lg_states.add_variable('gradientbug.rssi_beacon')
lg_states.add_variable('gradientbug.rssi_angle_i')
lg_states.add_variable('gradientbug.rssi_i')
# lg_states.add_variable('stateEstimate.z')
lg_states.data_received_cb.add_callback(self.data_received)
cf.param.add_update_callback(group="gbug",
name="sendnum",
cb=self.param_sendnum_callback)
time.sleep(drone_id)
with SyncCrazyflie(URI, cf=cf) as scf:
with SyncLogger(scf, lg_states) as logger_states:
reconnect = False
time.sleep(2)
while 1:
#pk = radio.send_packet((3,4,88,48 ))
#if pk.ack:
#print "got back!"
time.sleep(0.5)
#cf.param.request_param_update("gbug.keep_flying")
#cf.param.request_param_update("gbug.outbound")
time_delta = time.time() - start_time
param_name = "gbug.sendnum"
param_value = str(int(time_delta * 2) % 10)
print("param", str(int(time_delta * 2) % 10))
cf.param.set_value(param_name, param_value)
cf.param.request_param_update(param_name)
number_count = number_count + 1
print(time.time() - self.time_keep_flying)
# if (time.time()-self.time_keep_flying)>5:
# raise Exception('parameter not received')
if (time.time() - start_time) % 5:
if scf.is_link_open() == False:
raise Exception('link is not open')
if (time.time() - self.time_send_num
) > 5 and self.time_send_num is not 0:
raise Exception('lost parameter link')
except Exception as ex:
print ex
reconnect = True
def crazyFlieLoop(self, radio_id, drone_id):
# All uneven drones are assigned the channel below
#if drone_id % 2 == 0:
channel = drone_id * 10
#else:
# channel = (drone_id-1)*10
# The complete uri of the crazyfly
if drone_id < 10:
URI = 'radio://' + str(radio_id) + '/' + str(
channel) + '/2M/E7E7E7E70' + str(drone_id)
else:
URI = 'radio://' + str(radio_id) + '/' + str(
channel) + '/2M/E7E7E7E7' + str(drone_id)
print("Connect to ", URI)
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Open up the logging file, seperated by files per date
datestr = "../../experiments/" + time.strftime("%y%m%d")
if os.path.exists(datestr) == False:
os.mkdir(datestr)
timestr = time.strftime("%Y%m%d_%H%M%S")
fh = open(datestr + "/log_" + timestr + "_" + str(drone_id) + ".txt",
"w")
print("change crazyradio power")
cr = crazyradio.Crazyradio(None, radio_id)
cr.set_power(3)
#in
first_run = True
pos_x_int = 0
pos_y_int = 0
rssi_array = []
rssi_heading_array = []
stop_looping = False
start_time = 0
outbound = 1
reconnect = True
while reconnect:
try:
cf = Crazyflie(rw_cache='./cache')
cf.param.add_update_callback(group="gbug",
name="keep_flying",
cb=self.param_keepflying_callback)
lg_states = LogConfig(name='kalman_states', period_in_ms=100)
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
lg_states.add_variable('radio.rssi')
lg_states.add_variable('radio.rssi_inter')
lg_states.add_variable('stabilizer.yaw')
lg_states.add_variable('gradientbug.rssi_angle')
lg_states.add_variable('gradientbug.state')
lg_states.add_variable('gradientbug.state_wf')
lg_states.add_variable('gradientbug.rssi_beacon')
lg_states.add_variable('gradientbug.rssi_angle_i')
lg_states.add_variable('gradientbug.rssi_i')
# lg_states.add_variable('stateEstimate.z')
lg_states.data_received_cb.add_callback(self.data_received)
time.sleep(drone_id)
with SyncCrazyflie(URI, cf=cf) as scf:
with SyncLogger(scf, lg_states) as logger_states:
reconnect = False
time.sleep(2)
start_time = time.time()
while 1:
#print self.gradientbug_state
#print self.gradientbug_rssi_angle
#print("RSSI BEACON", self.gradientbug_rssi_beacon)
#print("rssi angle",self.gradientbug_rssi_angle)
time.sleep(0.03)
print("time", time.time())
print("RSSI", self.radio_rssi)
print("RSSI _inter", self.radio_rssi_inter)
#param_name = "gbug.keep_flying"
#param_value = "0"
#cf.param.set_value(param_name, param_value)
if first_run:
pos_x_int = self.kalman_states_ox
pos_y_int = self.kalman_states_oy
first_run = False
self.kalman_states_ox = self.kalman_states_ox - pos_x_int
self.kalman_states_oy = self.kalman_states_oy - pos_y_int
if time.time() - start_time > (
0) * 10 and self.keep_flying == 0:
param_name = "gbug.keep_flying"
param_value = "0"
cf.param.set_value(param_name, param_value)
cf.param.request_param_update(
"gbug.keep_flying")
print "don't fly!"
for event in pygame.event.get():
if event.type == KEYDOWN:
stop_looping = True
break
pygame.event.pump()
if stop_looping:
break
param_name = "gbug.keep_flying"
param_value = "0"
cf.param.set_value(param_name, param_value)
param_name = "gbug.keep_flying"
param_value = "0"
cf.param.set_value(param_name, param_value)
param_name = "gbug.keep_flying"
param_value = "0"
cf.param.set_value(param_name, param_value)
print "demo over"
except Exception as ex:
print ex
reconnect = True
time.sleep(2)