def _connected(self, link_uri):
# stoping the regular crtp link
self._cf.link.device_flag.clear()
self._dev_handle = self._cf.link.cradio.handle
self._send_vendor_setup(SET_RADIO_ARC, 0, 0, ())
self._use_drake_controller = USE_DRAKE_CONTROLLER
self._use_pos_control = CTRL_USE_POSITION_CONTROL
# state estimator
self._state_estimator = StateEstimator(
listen_to_vicon=SE_LISTEN_TO_VICON,
vicon_channel=SE_VICON_CHANNEL,
publish_to_lcm=SE_PUBLISH_TO_LCM,
use_rpydot=SE_USE_RPYDOT,
use_ekf=SE_USE_EKF,
use_ukf=SE_USE_UKF,
delay_comp=SE_DELAY_COMP)
# controller
self._control_input_updated_flag = Event()
self._controller = Controller(
control_input_type=CTRL_INPUT_TYPE,
listen_to_lcm=CTRL_LISTEN_TO_LCM,
control_input_updated_flag=self._control_input_updated_flag,
listen_to_extra_input=CTRL_LISTEN_TO_EXTRA_INPUT,
publish_to_lcm=CTRL_PUBLISH_TO_LCM,
pos_control=CTRL_USE_POSITION_CONTROL)
# Transmitter thread (handles all comm with the crazyflie)
Thread(target=self._transmitter_thread).start()
if STARTUP_NANOKONTROL:
Thread(target=nanokontrol.main).start()
def _connected(self, link_uri):
# stoping the regular crtp link
self._cf.link.device_flag.clear()
self._dev_handle = self._cf.link.cradio.handle
self._send_vendor_setup(SET_RADIO_ARC, 0, 0, ())
self._use_drake_controller = USE_DRAKE_CONTROLLER
self._use_pos_control = CTRL_USE_POSITION_CONTROL
# state estimator
self._state_estimator = StateEstimator(listen_to_vicon=SE_LISTEN_TO_VICON,
vicon_channel=SE_VICON_CHANNEL,
publish_to_lcm=SE_PUBLISH_TO_LCM,
use_rpydot=SE_USE_RPYDOT,
use_ekf=SE_USE_EKF,
use_ukf=SE_USE_UKF,
delay_comp=SE_DELAY_COMP)
# controller
self._control_input_updated_flag = Event()
self._controller = Controller(control_input_type=CTRL_INPUT_TYPE,
listen_to_lcm=CTRL_LISTEN_TO_LCM,
control_input_updated_flag=self._control_input_updated_flag,
listen_to_extra_input=CTRL_LISTEN_TO_EXTRA_INPUT,
publish_to_lcm=CTRL_PUBLISH_TO_LCM,
pos_control=CTRL_USE_POSITION_CONTROL)
# Transmitter thread (handles all comm with the crazyflie)
Thread(target=self._transmitter_thread).start()
if STARTUP_NANOKONTROL:
Thread(target=nanokontrol.main).start()
class SimpleClient:
def __init__(self, link_uri):
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print "Connecting to %s" % link_uri
def _connected(self, link_uri):
# stoping the regular crtp link
self._cf.link.device_flag.clear()
self._dev_handle = self._cf.link.cradio.handle
self._send_vendor_setup(SET_RADIO_ARC, 0, 0, ())
self._use_drake_controller = USE_DRAKE_CONTROLLER
self._use_pos_control = CTRL_USE_POSITION_CONTROL
# state estimator
self._state_estimator = StateEstimator(listen_to_vicon=SE_LISTEN_TO_VICON,
vicon_channel=SE_VICON_CHANNEL,
publish_to_lcm=SE_PUBLISH_TO_LCM,
use_rpydot=SE_USE_RPYDOT,
use_ekf=SE_USE_EKF,
use_ukf=SE_USE_UKF,
delay_comp=SE_DELAY_COMP)
# controller
self._control_input_updated_flag = Event()
self._controller = Controller(control_input_type=CTRL_INPUT_TYPE,
listen_to_lcm=CTRL_LISTEN_TO_LCM,
control_input_updated_flag=self._control_input_updated_flag,
listen_to_extra_input=CTRL_LISTEN_TO_EXTRA_INPUT,
publish_to_lcm=CTRL_PUBLISH_TO_LCM,
pos_control=CTRL_USE_POSITION_CONTROL)
# Transmitter thread (handles all comm with the crazyflie)
Thread(target=self._transmitter_thread).start()
if STARTUP_NANOKONTROL:
Thread(target=nanokontrol.main).start()
def _connection_failed(self, link_uri, msg):
print "Connection to %s failed: %s" % (link_uri, msg)
def _connection_lost(self, link_uri, msg):
print "Connection to %s lost: %s" % (link_uri, msg)
def _disconnected(self, link_uri):
print "Disconnected from %s" % link_uri
def _transmitter_thread(self):
sensor_request_pk = CRTPPacket()
sensor_request_pk.port = CRTPPort.SENSORS
control_input_pk = CRTPPacket()
control_input_pk.port = CRTPPort.OFFBOARDCTRL
vicon_yaw = 0.0
if SE_LISTEN_TO_VICON:
use_vicon_yaw = 1
else:
use_vicon_yaw = 0
imu_lc = lcm.LCM()
while True:
#t0 = time.time()
sensor_request_pk.data = struct.pack('<fi',vicon_yaw,use_vicon_yaw)
sensor_request_dataout = self._pk_to_dataout(sensor_request_pk)
datain = self._write_read_usb(sensor_request_dataout)
sensor_packet = self._datain_to_pk(datain)
if not sensor_packet:
continue
try:
imu_reading = struct.unpack('<7f',sensor_packet.data)
except:
continue
self._state_estimator.add_imu_reading(imu_reading)
# msg = crazyflie_imu_t()
# msg.omegax = imu_reading[0]
# msg.omegay = imu_reading[1]
# msg.omegaz = imu_reading[2]
# msg.alphax = imu_reading[3]
# msg.alphay = imu_reading[4]
# msg.alphaz = imu_reading[5]
# imu_lc.publish('crazyflie_imu', msg.encode())
self._control_input_updated_flag.clear()
xhat = self._state_estimator.get_xhat()
vicon_yaw = xhat[5]
if self._use_drake_controller:
# wait for Drake to give us the control input
self._control_input_updated_flag.wait(0.005)
control_input = self._controller.get_control_input(xhat=xhat)
if self._use_pos_control:
control_input_pk.data = struct.pack('<7f',*control_input)
else:
control_input_pk.data = struct.pack('<5fi',*control_input)
control_input_dataout = self._pk_to_dataout(control_input_pk)
self._write_usb(control_input_dataout)
if not(self._use_pos_control):
# TODO: position control could still update the state
# estimator about the last input sent
self._state_estimator.add_input(control_input[0:4])
#tf = time.time()
#time.sleep(max(0.0,(1.0/TXRX_FREQUENCY)-float(tf-t0)))
def _pk_to_dataout(self,pk):
dataOut = array.array('B')
dataOut.append(pk.header)
for X in pk.data:
if type(X) == int:
dataOut.append(X)
else:
dataOut.append(ord(X))
return dataOut
def _datain_to_pk(self,dataIn):
if dataIn != None:
if dataIn[0] != 0:
data = dataIn[1:]
if (len(data) > 0):
packet = CRTPPacket(data[0], list(data[1:]))
return packet
def _write_usb(self, dataout):
try:
self._dev_handle.write(endpoint=1, data=dataout, timeout=0)
except usb.USBError:
pass
def _write_read_usb(self, dataout):
datain = None
try:
self._dev_handle.write(endpoint=1, data=dataout, timeout=0)
datain = self._dev_handle.read(0x81, 64, timeout=5)
except usb.USBError:
pass
return datain
def _send_vendor_setup(self, request, value, index, data):
self._dev_handle.ctrl_transfer(usb.TYPE_VENDOR, request, wValue=value,
wIndex=index, timeout=1000, data_or_wLength=data)
class SimpleClient:
def __init__(self, link_uri):
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print "Connecting to %s" % link_uri
def _connected(self, link_uri):
# stoping the regular crtp link
self._cf.link.device_flag.clear()
self._dev_handle = self._cf.link.cradio.handle
self._send_vendor_setup(SET_RADIO_ARC, 0, 0, ())
self._use_drake_controller = USE_DRAKE_CONTROLLER
self._use_pos_control = CTRL_USE_POSITION_CONTROL
# state estimator
self._state_estimator = StateEstimator(
listen_to_vicon=SE_LISTEN_TO_VICON,
vicon_channel=SE_VICON_CHANNEL,
publish_to_lcm=SE_PUBLISH_TO_LCM,
use_rpydot=SE_USE_RPYDOT,
use_ekf=SE_USE_EKF,
use_ukf=SE_USE_UKF,
delay_comp=SE_DELAY_COMP)
# controller
self._control_input_updated_flag = Event()
self._controller = Controller(
control_input_type=CTRL_INPUT_TYPE,
listen_to_lcm=CTRL_LISTEN_TO_LCM,
control_input_updated_flag=self._control_input_updated_flag,
listen_to_extra_input=CTRL_LISTEN_TO_EXTRA_INPUT,
publish_to_lcm=CTRL_PUBLISH_TO_LCM,
pos_control=CTRL_USE_POSITION_CONTROL)
# Transmitter thread (handles all comm with the crazyflie)
Thread(target=self._transmitter_thread).start()
if STARTUP_NANOKONTROL:
Thread(target=nanokontrol.main).start()
def _connection_failed(self, link_uri, msg):
print "Connection to %s failed: %s" % (link_uri, msg)
def _connection_lost(self, link_uri, msg):
print "Connection to %s lost: %s" % (link_uri, msg)
def _disconnected(self, link_uri):
print "Disconnected from %s" % link_uri
def _transmitter_thread(self):
sensor_request_pk = CRTPPacket()
sensor_request_pk.port = CRTPPort.SENSORS
control_input_pk = CRTPPacket()
control_input_pk.port = CRTPPort.OFFBOARDCTRL
vicon_yaw = 0.0
if SE_LISTEN_TO_VICON:
use_vicon_yaw = 1
else:
use_vicon_yaw = 0
imu_lc = lcm.LCM()
while True:
#t0 = time.time()
sensor_request_pk.data = struct.pack('<fi', vicon_yaw,
use_vicon_yaw)
sensor_request_dataout = self._pk_to_dataout(sensor_request_pk)
datain = self._write_read_usb(sensor_request_dataout)
sensor_packet = self._datain_to_pk(datain)
if not sensor_packet:
continue
try:
imu_reading = struct.unpack('<7f', sensor_packet.data)
except:
continue
self._state_estimator.add_imu_reading(imu_reading)
# msg = crazyflie_imu_t()
# msg.omegax = imu_reading[0]
# msg.omegay = imu_reading[1]
# msg.omegaz = imu_reading[2]
# msg.alphax = imu_reading[3]
# msg.alphay = imu_reading[4]
# msg.alphaz = imu_reading[5]
# imu_lc.publish('crazyflie_imu', msg.encode())
self._control_input_updated_flag.clear()
xhat = self._state_estimator.get_xhat()
vicon_yaw = xhat[5]
if self._use_drake_controller:
# wait for Drake to give us the control input
self._control_input_updated_flag.wait(0.005)
control_input = self._controller.get_control_input(xhat=xhat)
if self._use_pos_control:
control_input_pk.data = struct.pack('<7f', *control_input)
else:
control_input_pk.data = struct.pack('<5fi', *control_input)
control_input_dataout = self._pk_to_dataout(control_input_pk)
self._write_usb(control_input_dataout)
if not (self._use_pos_control):
# TODO: position control could still update the state
# estimator about the last input sent
self._state_estimator.add_input(control_input[0:4])
#tf = time.time()
#time.sleep(max(0.0,(1.0/TXRX_FREQUENCY)-float(tf-t0)))
def _pk_to_dataout(self, pk):
dataOut = array.array('B')
dataOut.append(pk.header)
for X in pk.data:
if type(X) == int:
dataOut.append(X)
else:
dataOut.append(ord(X))
return dataOut
def _datain_to_pk(self, dataIn):
if dataIn != None:
if dataIn[0] != 0:
data = dataIn[1:]
if (len(data) > 0):
packet = CRTPPacket(data[0], list(data[1:]))
return packet
def _write_usb(self, dataout):
try:
self._dev_handle.write(endpoint=1, data=dataout, timeout=0)
except usb.USBError:
pass
def _write_read_usb(self, dataout):
datain = None
try:
self._dev_handle.write(endpoint=1, data=dataout, timeout=0)
datain = self._dev_handle.read(0x81, 64, timeout=5)
except usb.USBError:
pass
return datain
def _send_vendor_setup(self, request, value, index, data):
self._dev_handle.ctrl_transfer(usb.TYPE_VENDOR,
request,
wValue=value,
wIndex=index,
timeout=1000,
data_or_wLength=data)
