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)