I'm a student working on a group project. I have tried to become familiar with the developpement of the python client but some points are still dark to me.
Our first goal is to develop a function that allows the drone to safely land. For that, we have created a button in a new tab to activate the function even if we plan to trigger the landing from the controller in the near future.
The idea is :
- create a new configuration to retrieve acc.z and stabilizer.thrust.
- We block user inputs from the controller.
- everytime the cf sends back data, we analyse the acc.z to see if we're falling down and if so, we try to push the drone up
- Once we're not falling anymore, we decrease the thrust a bit to land slowly.
Logs are stacked everytime they are received on a list.
The problem is : I am not really sure to understand the interactions between the controller, the client and the drone. Is there a master process that centralize the commands sent or any process can send its command to the drone ? Am I sending commands correctly to the drone ? I tried to take inspiration from ramp.py, and flyTab.
Here is my code, (I have removed some methods to make it shorter for the post clarity)
Code: Select all
class TakeOffTab(Tab, take_off_tab_class):
"""Tab to do an automatic take off, hold an altitude and emergency landing"""
_connected_signal = pyqtSignal(str)
_disconnected_signal = pyqtSignal(str)
_log_data_signal = pyqtSignal(int, object, object)
_log_error_signal = pyqtSignal(object, str)
_param_updated_signal = pyqtSignal(str, str)
_start_emergency_landing_signal = pyqtSignal(bool)
_current_log = None
_log_stack = []
def __init__(self, tabWidget, helper, *args):
super(TakeOffTab, self).__init__(*args)
self.setupUi(self)
self.i = 0
self.tabName = "Take off"
self.menuName = "Take off Tab"
self.tabWidget = tabWidget
self._helper = helper
#this boolean indicates if the drone is currently doing an emergncy landing
self.in_emergency = False
self.emergency_phase = 0
# Always wrap callbacks from Crazyflie API though QT Signal/Slots
# to avoid manipulating the UI when rendering it
self._connected_signal.connect(self._connected)
self._disconnected_signal.connect(self._disconnected)
self._log_data_signal.connect(self._log_data_received)
self._param_updated_signal.connect(self._param_updated)
self._start_emergency_landing_signal.connect(self._start_emergency_landing)
# Connect the Crazyflie API callbacks to the signals
self._helper.cf.connected.add_callback(
self._connected_signal.emit)
self._helper.cf.disconnected.add_callback(
self._disconnected_signal.emit)
self._helper.inputDeviceReader.emergency_stop_updated.add_callback(
self._start_emergency_landing_signal.emit)
self._takeOffBtn.clicked.connect(self._log_estimated_z)
self._holdAltitudeBtn.clicked.connect(lambda enabled:
self._helper.cf.param.set_value("flightmode.althold", str(enabled)))
self._helper.cf.param.add_update_callback(group="alt", name="enable", cb=self._param_updated_signal.emit)
self._emergencyBtn.clicked.connect(self._start_emergency_landing)
def _connected(self, link_uri):
"""Callback when the Crazyflie has been connected"""
logger.debug("Crazyflie connected to {}".format(link_uri))
self.connectionLbl.setText("connected !")
self._log_stabilizer()
self.i+=1
self.dataLbl.setText(str(self.i))
def _log_stabilizer(self):
# The definition of the logconfig can be made before connecting
self._current_log = LogConfig(name='Stabilizer', period_in_ms=100)
self._current_log.add_variable('stabilizer.roll', 'float')
self._current_log.add_variable('stabilizer.yaw', 'float')
self._current_log.add_variable("stabilizer.yaw", "float")
self._current_log.add_variable("stabilizer.thrust", "uint16_t")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
self._helper.cf.log.add_config(self._current_log)
if self._current_log.valid:
#then we set the callbacks
self._current_log.data_received_cb.add_callback(self._log_data_signal.emit)
self._current_log.start()
def _start_emergency_landing(self):
#if we're already doing the emergency landing, we dismiss the click
if self.in_emergency:
return
else:
# The definition of the acceleration config
self.emergency_phase = 1
self._current_log = LogConfig(name='Emergency', period_in_ms=50)
self._current_log.add_variable('acc.z', 'float')
self._current_log.add_variable('stabilizer.thrust', 'uint16_t')
self._helper.cf.log.add_config(self._current_log)
if self._current_log.valid:
#then we set the callbacks
self._current_log.data_received_cb.add_callback(self._log_data_signal.emit)
self._current_log.start()
self.targetThrust.setEnabled(not enabled)
def _emergency_treatment(self):
# 1 is the phase where we check if we're falling and if so, we thrust up to stop it
if self.emergency_phase == 1:
#if we're falling
if self._log_stack[-1][1]['acc.z'] < 0.97:
#push the motors !
self._helper.cf.commander.send_setpoint(0,0,0,self._log_stack[-1][1]['stabilizer.thrust']+300)
else:
self.emergency_phase = 2
if self.emergency_phase == 2:
#if we're falling
if self._log_stack[-1][1]['acc.z'] > 0.97:
#lower the thrust slightly
self._helper.cf.commander.send_setpoint(0,0,0,self._log_stack[-1][1]['stabilizer.thrust']-100)
elif self._log_stack[-1][1]['acc.z'] < 0.92:
#push up a bit thrusts
self._helper.cf.commander.send_setpoint(0,0,0,self._log_stack[-1][1]['stabilizer.thrust']+100)
else:
#we land slowly but surely
pass
if self._log_stack[-1][1]['stabilizer.thrust'] < 20000:
self._helper.cf.commander.send_setpoint(0,0,0,0)
def _log_data_received(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
#we stack the data that arrives
self._log_stack.append([timestamp, data])
if self.in_emergency:
self._emergency_treatment()
self.dataTextView.setText(repr(data))
self.i+=1
self.dataLbl.setText(str(self.i))
print("data" + '[%d][%s]: %s' % (timestamp, logconf.name, data))
def _param_updated(self, name, value):
"""Callback when the registered parameter get's updated"""
logger.debug("Updated {0} to {1}".format(name, value))
if not self._holdAltitudeBtn.isEnabled():
self._holdAltitudeBtn.setEnabled(True)
self._holdAltitudeBtn.setChecked(eval(str(value)))
def set_alt_hold(self):
self.helper.cf.param.set_value("flightmode.althold", str(enabled))
Thanks in advance for your help !