From 37045e1fbf281b56265646a310922c8bc9e07172 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 17 Dec 2018 14:36:00 +0100 Subject: [PATCH] #362 Modified the Qualisys tab to only run its worker thread 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. --- src/cfclient/ui/tabs/QualisysTab.py | 128 ++++++++++++++-------------- 1 file changed, 66 insertions(+), 62 deletions(-) diff --git a/src/cfclient/ui/tabs/QualisysTab.py b/src/cfclient/ui/tabs/QualisysTab.py index 4754bf2e..8b64abee 100644 --- a/src/cfclient/ui/tabs/QualisysTab.py +++ b/src/cfclient/ui/tabs/QualisysTab.py @@ -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 @@ -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 @@ -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() @@ -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) @@ -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) @@ -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) @@ -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], @@ -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) @@ -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: @@ -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): @@ -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 @@ -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' @@ -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""" @@ -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 @@ -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: