Skip to content

Commit

Permalink
FlightTab: Add Command Based Flight Control
Browse files Browse the repository at this point in the history 
If connected and a positioning deck (bcFlow, bcFlow2, bcLightHouse4,
bcDWM1000) is found we enable a simple interface to send high level
commands to the Crazyflie.

This is to make the "first encounter" with the Crazyflie and the client
more enjoyable, and to enable a user to fly around with the drone
without any real experience.
  • Loading branch information
@jonasdn
jonasdn committed Mar 5, 2021
1 parent e37779d commit 1e94ebf
Show file tree
Hide file tree
Showing 2 changed files with 290 additions and 1 deletion.
99 changes: 99 additions & 0 deletions src/cfclient/ui/tabs/FlightTab.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@
"""

import logging
import time
from enum import Enum

from PyQt5 import uic
from PyQt5.QtCore import Qt, pyqtSignal
Expand All @@ -40,6 +42,8 @@

from cfclient.utils.config import Config
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.position_hl_commander import PositionHlCommander

from cfclient.utils.input import JoystickReader

Expand Down Expand Up @@ -79,6 +83,17 @@
control. Requires a flow deck. Uses body-fixed coordinates."""


class CommanderAction(Enum):
TAKE_OFF = 1
LAND = 2
UP = 3
DOWN = 4
LEFT = 5
RIGHT = 6
FORWARD = 7
BACK = 8


class FlightTab(Tab, flight_tab_class):
uiSetupReadySignal = pyqtSignal()

Expand Down Expand Up @@ -162,6 +177,34 @@ def __init__(self, tabWidget, helper, *args):
self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
self.uiSetupReadySignal.connect(self.uiSetupReady)
self.isInCrazyFlightmode = False

# Command Based Flight Control
self._hlCommander = None
self.commanderTakeOffButton.clicked.connect(
lambda: self._flight_command(CommanderAction.TAKE_OFF)
)
self.commanderLandButton.clicked.connect(
lambda: self._flight_command(CommanderAction.LAND)
)
self.commanderLeftButton.clicked.connect(
lambda: self._flight_command(CommanderAction.LEFT)
)
self.commanderRightButton.clicked.connect(
lambda: self._flight_command(CommanderAction.RIGHT)
)
self.commanderForwardButton.clicked.connect(
lambda: self._flight_command(CommanderAction.FORWARD)
)
self.commanderBackButton.clicked.connect(
lambda: self._flight_command(CommanderAction.BACK)
)
self.commanderUpButton.clicked.connect(
lambda: self._flight_command(CommanderAction.UP)
)
self.commanderDownButton.clicked.connect(
lambda: self._flight_command(CommanderAction.DOWN)
)

self.uiSetupReady()

self.ratePidRadioButton.clicked.connect(
Expand Down Expand Up @@ -252,6 +295,33 @@ def uiSetupReady(self):
self.flightModeCombo.setCurrentIndex(flightComboIndex)
self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

def _flight_command(self, action):
if self._hlCommander is None:
return

if action == CommanderAction.TAKE_OFF:
#
# Reset the Kalman filter before taking off, to avoid
# positional confusion.
#
self.helper.cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(1)
self._hlCommander.take_off()
elif action == CommanderAction.LAND:
self._hlCommander.land()
elif action == CommanderAction.LEFT:
self._hlCommander.left(0.5)
elif action == CommanderAction.RIGHT:
self._hlCommander.right(0.5)
elif action == CommanderAction.FORWARD:
self._hlCommander.forward(0.5)
elif action == CommanderAction.BACK:
self._hlCommander.back(0.5)
elif action == CommanderAction.UP:
self._hlCommander.up(0.5)
elif action == CommanderAction.DOWN:
self._hlCommander.down(0.5)

def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Log error",
"Error when starting log config [%s]: %s" % (
Expand Down Expand Up @@ -319,6 +389,23 @@ def _change_input_labels(self, using_hover_assist):
self.inputRollLabel.setText(roll)
self.inputYawLabel.setText(yaw)

def _update_flight_commander(self, connected):
if not connected:
self.commanderBox.setEnabled(False)
return

# flowV1 flowV2 LightHouse LPS
position_decks = ['bcFlow', 'bcFlow2', 'bcLighthouse4', 'bcDWM1000']
for deck in position_decks:
if int(self.helper.cf.param.values['deck'][deck]) == 1:
self.commanderBox.setEnabled(True)
break
else:
self.commanderBox.setToolTip(
'You need a positioning deck to use Command Based Flight'
)
self.commanderBox.setEnabled(False)

def connected(self, linkURI):
# MOTOR & THRUST
lg = LogConfig("Motors", Config().get("ui_update_period"))
Expand All @@ -328,6 +415,15 @@ def connected(self, linkURI):
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")

sync = SyncCrazyflie(linkURI, cf=self.helper.cf)
self._hlCommander = PositionHlCommander(
sync,
x=0.0, y=0.0, z=0.0,
default_velocity=0.3,
default_height=0.5,
controller=PositionHlCommander.CONTROLLER_PID
)

try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
Expand Down Expand Up @@ -392,6 +488,8 @@ def disconnected(self, linkURI):
self._assist_mode_combo.setEnabled(False)
self._assist_mode_combo.clear()

self._update_flight_commander(False)

def minMaxThrustChanged(self):
self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
Expand Down Expand Up @@ -543,6 +641,7 @@ def alt2_updated(self, state):
def _all_params_updated(self):
self._ring_populate_dropdown()
self._populate_assisted_mode_dropdown()
self._update_flight_commander(True)

def _ring_populate_dropdown(self):
try:
Expand Down
192 changes: 191 additions & 1 deletion src/cfclient/ui/tabs/flightTab.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -702,7 +702,181 @@
</property>
</widget>
</item>
<item row="0" column="14" rowspan="8">
<item row="0" column="14" rowspan="6">
<widget class="QGroupBox" name="commanderBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="Enabled">
<bool>False</bool>
</property>
<property name="title">
<string>Command Based Flight Control</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
<layout class="QGridLayout" name="horizontalLayout">
<property name="leftMargin">
<number>2</number>
</property>
<property name="topMargin">
<number>2</number>
</property>
<property name="rightMargin">
<number>2</number>
</property>
<property name="bottomMargin">
<number>2</number>
</property>
<property name="spacing">
<number>5</number>
</property>
<item row="0" column="0">
<widget class="QPushButton" name="commanderTakeOffButton">
<property name="text">
<string>Take off</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="commanderLandButton">
<property name="text">
<string>Land</string>
</property>
</widget>
</item>
<item row="0" column="1" rowspan="2" colspan="2">
<layout class="QGridLayout" name="horizontalLayout">
<property name="sizeConstraint">
<enum>QLayout::SetDefaultConstraint</enum>
</property>
<property name="leftMargin">
<number>10</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<property name="spacing">
<number>0</number>
</property>
<item row="1" column="0">
<widget class="QPushButton" name="commanderLeftButton">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>30</width>
<height>30</height>
</size>
</property>
<property name="text">
<string>←</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QPushButton" name="commanderForwardButton">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>30</width>
<height>30</height>
</size>
</property>
<property name="text">
<string>↑</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QPushButton" name="commanderBackButton">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>30</width>
<height>30</height>
</size>
</property>
<property name="text">
<string>↓</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QPushButton" name="commanderRightButton">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>30</width>
<height>30</height>
</size>
</property>
<property name="text">
<string>→</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="5" column="1">
<widget class="QPushButton" name="commanderUpButton">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Up</string>
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QPushButton" name="commanderDownButton">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Down</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="0" column="15" rowspan="8">
<layout class="QGridLayout" name="gridLayout_7"/>
</item>
<item row="1" column="7" rowspan="7">
Expand Down Expand Up @@ -809,6 +983,22 @@
</property>
</widget>
</item>
<item row="1" column="13">
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
Expand Down

0 comments on commit 1e94ebf

Please sign in to comment.