blocking user inputs and sending commands

Firmware/software/electronics/mechanics
Post Reply
IMT-DEV-Drone
Beginner
Posts: 3
Joined: Thu Feb 09, 2017 1:23 pm

blocking user inputs and sending commands

Post by IMT-DEV-Drone » Thu Apr 20, 2017 3:24 pm

Hi,
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))
Just for information, what is the difference between a param and the incomming log (as there are two different callbacks) ?

Thanks in advance for your help !

Post Reply