Skip to content

Commit

Permalink
bitcraze#362 Modified the Qualisys tab to only run its worker thread …
Browse files Browse the repository at this point in the history 
…when both QTM and a Crazyflie is connected.

The worker thread switches the estimator the kalman estimator and since the thread only is running when the QTM is connect it will only happen for users with a Qualisys system.
  • Loading branch information
@krichardsson
krichardsson committed Dec 17, 2018
1 parent 5ab4c28 commit 37045e1
Showing 1 changed file with 66 additions and 62 deletions.
128 changes: 66 additions & 62 deletions src/cfclient/ui/tabs/QualisysTab.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,6 @@ def __init__(self, tabWidget, helper, *args):

self.flying_enabled = False
self.switch_flight_mode(FlightModeStates.DISCONNECTED)
self.cf_ready_to_fly = False
self.path_pos_threshold = 0.2
self.circle_pos_threshold = 0.1
self.circle_radius = 1.5
Expand All @@ -211,37 +210,37 @@ def __init__(self, tabWidget, helper, *args):
self.new_path = []
self.recording = False
self.land_for_recording = False
self.default_flight_paths = [[
"Path 1: Sandbox", [0.0, -1.0, 1.0, 0.0], [0.0, 1.0, 1.0, 0.0]
],
[
"Path 2: Height Test",
[0.0, 0.0, 0.5, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 1.5, 0.0],
[0.0, 0.0, 2.0, 0.0],
[0.0, 0.0, 2.3, 0.0],
[0.0, 0.0, 1.8, 0.0],
[0.0, 0.0, 0.5, 0.0],
[0.0, 0.0, 0.3, 0.0],
[0.0, 0.0, 0.15, 0.0]
],
[
"Path 3: 'Spiral'",
[0.0, 0.0, 1.0, 0.0],
[0.5, 0.5, 1.0, 0.0],
[0.0, 1.0, 1.0, 0.0],
[-0.5, 0.5, 1.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.5, 0.5, 1.2, 0.0],
[0.0, 1.0, 1.4, 0.0],
[-0.5, 0.5, 1.6, 0.0],
[0.0, 0.0, 1.8, 0.0],
[0.5, 0.5, 1.5, 0.0],
[0.0, 1.0, 1.0, 0.0],
[-0.5, 0.5, 0.5, 0.0],
[0.0, 0.0, 0.25, 0.0]
]]
self.default_flight_paths = [
[
"Path 1: Sandbox",
[0.0, -1.0, 1.0, 0.0],
[0.0, 1.0, 1.0, 0.0]],
[
"Path 2: Height Test",
[0.0, 0.0, 0.5, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 1.5, 0.0],
[0.0, 0.0, 2.0, 0.0],
[0.0, 0.0, 2.3, 0.0],
[0.0, 0.0, 1.8, 0.0],
[0.0, 0.0, 0.5, 0.0],
[0.0, 0.0, 0.3, 0.0],
[0.0, 0.0, 0.15, 0.0]],
[
"Path 3: 'Spiral'",
[0.0, 0.0, 1.0, 0.0],
[0.5, 0.5, 1.0, 0.0],
[0.0, 1.0, 1.0, 0.0],
[-0.5, 0.5, 1.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.5, 0.5, 1.2, 0.0],
[0.0, 1.0, 1.4, 0.0],
[-0.5, 0.5, 1.6, 0.0],
[0.0, 0.0, 1.8, 0.0],
[0.5, 0.5, 1.5, 0.0],
[0.0, 1.0, 1.0, 0.0],
[-0.5, 0.5, 0.5, 0.0],
[0.0, 0.0, 0.25, 0.0]]]

# The position and rotation of the cf and wand obtained by the
# camera tracking, if it cant be tracked the position becomes Nan
Expand Down Expand Up @@ -481,6 +480,19 @@ def add_transition(mode, child_state, parent):
self._machine.setInitialState(parent_state)
self._machine.start()

def _update_flight_status(self):
prev_flying_enabled = self.flying_enabled
self.flying_enabled = self._cf is not None and \
self._qtm_connection is not None

if not prev_flying_enabled and self.flying_enabled:
self.switch_flight_mode(FlightModeStates.GROUNDED)
t = threading.Thread(target=self.flight_controller)
t.start()

if prev_flying_enabled and not self.flying_enabled:
self.switch_flight_mode(FlightModeStates.DISCONNECTED)

def _is_discovering(self, discovering):
if discovering:
self.qtmIpBox.clear()
Expand Down Expand Up @@ -815,8 +827,7 @@ async def setup_qtm_connection(self):
self.discoverQTM.setEnabled(False)
self.qtmIpBox.setEnabled(False)

if self.cf_ready_to_fly:
self.switch_flight_mode(FlightModeStates.GROUNDED)
self._update_flight_status()

self._ui_update_timer.start(200)

Expand All @@ -832,6 +843,7 @@ async def on_qtm_disconnect(self, reason):
"""Callback when QTM has been disconnected"""

self._ui_update_timer.stop()
self._update_flight_status()

self._qtm_connection = None
logger.info(reason)
Expand All @@ -844,10 +856,7 @@ async def on_qtm_disconnect(self, reason):
self.connectQtmButton.setEnabled(True)
self.connectQtmButton.setText('Connect QTM')
self.qtmStatus = ': not connected : {}'.format(
reason if reason is not None else ''
)

self.switch_flight_mode(FlightModeStates.DISCONNECTED)
reason if reason is not None else '')

def on_qtm_event(self, event):
logger.info(event)
Expand Down Expand Up @@ -938,8 +947,7 @@ def _flight_mode_land_entered(self):
def _flight_mode_path_entered(self):
self.path_index = 1

current = self.flight_paths[self.pathSelector.
currentIndex()]
current = self.flight_paths[self.pathSelector.currentIndex()]
self.current_goal_pos = Position(
current[self.path_index][0],
current[self.path_index][1],
Expand Down Expand Up @@ -991,6 +999,7 @@ def _flight_mode_disconnected_entered(self):

def flight_controller(self):
try:
logger.info('Starting flight controller thread')
self._cf.param.set_value('stabilizer.estimator', '2')
self.reset_estimator(self._cf)

Expand Down Expand Up @@ -1048,8 +1057,8 @@ def flight_controller(self):
if self.valid_cf_pos.distance_to(
Position(self.current_goal_pos.x,
self.current_goal_pos.y,
(self.current_goal_pos.z / self.land_rate
))) < self.path_pos_threshold:
self.current_goal_pos.z / self.land_rate
)) < self.path_pos_threshold:
self.land_rate *= 1.1

if self.land_rate > 1000:
Expand All @@ -1074,8 +1083,8 @@ def flight_controller(self):

if position_hold_timer > self.position_hold_timelimit:

current = self.flight_paths[self.pathSelector.
currentIndex()]
current = self.flight_paths[
self.pathSelector.currentIndex()]

self.path_index += 1
if self.path_index == len(current):
Expand Down Expand Up @@ -1271,6 +1280,8 @@ def flight_controller(self):
logger.error(err)
self.cfStatus = str(err)

logger.info('Terminating flight controller thread')

def save_current_position(self):
if self.recording:
# Restart the timer
Expand All @@ -1285,15 +1296,9 @@ def _connected(self, link_uri):
"""Callback when the Crazyflie has been connected"""

self._cf = self._helper.cf
self._update_flight_status()

if not self.flying_enabled:
self.flying_enabled = True
self.cfStatus = ": connecting..."
t = threading.Thread(target=self.flight_controller)
t.start()

self.uri = link_uri
logger.debug("Crazyflie connected to {}".format(self.uri))
logger.debug("Crazyflie connected to {}".format(link_uri))

# Gui
self.cfStatus = ': connected'
Expand All @@ -1303,9 +1308,8 @@ def _disconnected(self, link_uri):

logger.info("Crazyflie disconnected from {}".format(link_uri))
self.cfStatus = ': not connected'
self.flying_enabled = False
self.cf_ready_to_fly = False
self._cf = None
self._update_flight_status()

def _param_updated(self, name, value):
"""Callback when the registered parameter get's updated"""
Expand Down Expand Up @@ -1366,15 +1370,15 @@ def wait_for_position_estimator(self, cf):

if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (
max_z - min_z) < threshold:
logger.info(
"Position found with error in, x: {}, y: {}, z: {}".
format(max_x - min_x, max_y - min_y, max_z - min_z))
max_z - min_z) < threshold:
logger.info("Position found with error in, x: {}, y: {}, "
"z: {}".format(max_x - min_x,
max_y - min_y,
max_z - min_z))

self.cfStatus = ": connected"

self.switch_flight_mode(FlightModeStates.GROUNDED)
self.cf_ready_to_fly = True

break

Expand All @@ -1389,14 +1393,14 @@ def reset_estimator(self, cf):

def switch_flight_mode(self, mode):
# Handles the behaviour of switching between flight modes

self.flight_mode = mode

# Handle client input control.
# Disable gamepad input if we are not grounded
if self.flight_mode in [
FlightModeStates.GROUNDED, FlightModeStates.DISCONNECTED,
FlightModeStates.RECORD
FlightModeStates.GROUNDED,
FlightModeStates.DISCONNECTED,
FlightModeStates.RECORD
]:
self._helper.mainUI.disable_input(False)
else:
Expand Down

0 comments on commit 37045e1

Please sign in to comment.