Skip to content

Commit 37045e1

Browse files
krichardssonkrichardsson
committed
#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.
1 parent 5ab4c28 commit 37045e1

File tree

1 file changed

+66
-62
lines changed

1 file changed

+66
-62
lines changed

‎src/cfclient/ui/tabs/QualisysTab.py

Lines changed: 66 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -200,7 +200,6 @@ def __init__(self, tabWidget, helper, *args):
200200

201201
self.flying_enabled = False
202202
self.switch_flight_mode(FlightModeStates.DISCONNECTED)
203-
self.cf_ready_to_fly = False
204203
self.path_pos_threshold = 0.2
205204
self.circle_pos_threshold = 0.1
206205
self.circle_radius = 1.5
@@ -211,37 +210,37 @@ def __init__(self, tabWidget, helper, *args):
211210
self.new_path = []
212211
self.recording = False
213212
self.land_for_recording = False
214-
self.default_flight_paths = [[
215-
"Path 1: Sandbox", [0.0, -1.0, 1.0, 0.0], [0.0, 1.0, 1.0, 0.0]
216-
],
217-
[
218-
"Path 2: Height Test",
219-
[0.0, 0.0, 0.5, 0.0],
220-
[0.0, 0.0, 1.0, 0.0],
221-
[0.0, 0.0, 1.5, 0.0],
222-
[0.0, 0.0, 2.0, 0.0],
223-
[0.0, 0.0, 2.3, 0.0],
224-
[0.0, 0.0, 1.8, 0.0],
225-
[0.0, 0.0, 0.5, 0.0],
226-
[0.0, 0.0, 0.3, 0.0],
227-
[0.0, 0.0, 0.15, 0.0]
228-
],
229-
[
230-
"Path 3: 'Spiral'",
231-
[0.0, 0.0, 1.0, 0.0],
232-
[0.5, 0.5, 1.0, 0.0],
233-
[0.0, 1.0, 1.0, 0.0],
234-
[-0.5, 0.5, 1.0, 0.0],
235-
[0.0, 0.0, 1.0, 0.0],
236-
[0.5, 0.5, 1.2, 0.0],
237-
[0.0, 1.0, 1.4, 0.0],
238-
[-0.5, 0.5, 1.6, 0.0],
239-
[0.0, 0.0, 1.8, 0.0],
240-
[0.5, 0.5, 1.5, 0.0],
241-
[0.0, 1.0, 1.0, 0.0],
242-
[-0.5, 0.5, 0.5, 0.0],
243-
[0.0, 0.0, 0.25, 0.0]
244-
]]
213+
self.default_flight_paths = [
214+
[
215+
"Path 1: Sandbox",
216+
[0.0, -1.0, 1.0, 0.0],
217+
[0.0, 1.0, 1.0, 0.0]],
218+
[
219+
"Path 2: Height Test",
220+
[0.0, 0.0, 0.5, 0.0],
221+
[0.0, 0.0, 1.0, 0.0],
222+
[0.0, 0.0, 1.5, 0.0],
223+
[0.0, 0.0, 2.0, 0.0],
224+
[0.0, 0.0, 2.3, 0.0],
225+
[0.0, 0.0, 1.8, 0.0],
226+
[0.0, 0.0, 0.5, 0.0],
227+
[0.0, 0.0, 0.3, 0.0],
228+
[0.0, 0.0, 0.15, 0.0]],
229+
[
230+
"Path 3: 'Spiral'",
231+
[0.0, 0.0, 1.0, 0.0],
232+
[0.5, 0.5, 1.0, 0.0],
233+
[0.0, 1.0, 1.0, 0.0],
234+
[-0.5, 0.5, 1.0, 0.0],
235+
[0.0, 0.0, 1.0, 0.0],
236+
[0.5, 0.5, 1.2, 0.0],
237+
[0.0, 1.0, 1.4, 0.0],
238+
[-0.5, 0.5, 1.6, 0.0],
239+
[0.0, 0.0, 1.8, 0.0],
240+
[0.5, 0.5, 1.5, 0.0],
241+
[0.0, 1.0, 1.0, 0.0],
242+
[-0.5, 0.5, 0.5, 0.0],
243+
[0.0, 0.0, 0.25, 0.0]]]
245244

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

483+
def _update_flight_status(self):
484+
prev_flying_enabled = self.flying_enabled
485+
self.flying_enabled = self._cf is not None and \
486+
self._qtm_connection is not None
487+
488+
if not prev_flying_enabled and self.flying_enabled:
489+
self.switch_flight_mode(FlightModeStates.GROUNDED)
490+
t = threading.Thread(target=self.flight_controller)
491+
t.start()
492+
493+
if prev_flying_enabled and not self.flying_enabled:
494+
self.switch_flight_mode(FlightModeStates.DISCONNECTED)
495+
484496
def _is_discovering(self, discovering):
485497
if discovering:
486498
self.qtmIpBox.clear()
@@ -815,8 +827,7 @@ async def setup_qtm_connection(self):
815827
self.discoverQTM.setEnabled(False)
816828
self.qtmIpBox.setEnabled(False)
817829

818-
if self.cf_ready_to_fly:
819-
self.switch_flight_mode(FlightModeStates.GROUNDED)
830+
self._update_flight_status()
820831

821832
self._ui_update_timer.start(200)
822833

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

834845
self._ui_update_timer.stop()
846+
self._update_flight_status()
835847

836848
self._qtm_connection = None
837849
logger.info(reason)
@@ -844,10 +856,7 @@ async def on_qtm_disconnect(self, reason):
844856
self.connectQtmButton.setEnabled(True)
845857
self.connectQtmButton.setText('Connect QTM')
846858
self.qtmStatus = ': not connected : {}'.format(
847-
reason if reason is not None else ''
848-
)
849-
850-
self.switch_flight_mode(FlightModeStates.DISCONNECTED)
859+
reason if reason is not None else '')
851860

852861
def on_qtm_event(self, event):
853862
logger.info(event)
@@ -938,8 +947,7 @@ def _flight_mode_land_entered(self):
938947
def _flight_mode_path_entered(self):
939948
self.path_index = 1
940949

941-
current = self.flight_paths[self.pathSelector.
942-
currentIndex()]
950+
current = self.flight_paths[self.pathSelector.currentIndex()]
943951
self.current_goal_pos = Position(
944952
current[self.path_index][0],
945953
current[self.path_index][1],
@@ -991,6 +999,7 @@ def _flight_mode_disconnected_entered(self):
991999

9921000
def flight_controller(self):
9931001
try:
1002+
logger.info('Starting flight controller thread')
9941003
self._cf.param.set_value('stabilizer.estimator', '2')
9951004
self.reset_estimator(self._cf)
9961005

@@ -1048,8 +1057,8 @@ def flight_controller(self):
10481057
if self.valid_cf_pos.distance_to(
10491058
Position(self.current_goal_pos.x,
10501059
self.current_goal_pos.y,
1051-
(self.current_goal_pos.z / self.land_rate
1052-
))) < self.path_pos_threshold:
1060+
self.current_goal_pos.z / self.land_rate
1061+
)) < self.path_pos_threshold:
10531062
self.land_rate *= 1.1
10541063

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

10751084
if position_hold_timer > self.position_hold_timelimit:
10761085

1077-
current = self.flight_paths[self.pathSelector.
1078-
currentIndex()]
1086+
current = self.flight_paths[
1087+
self.pathSelector.currentIndex()]
10791088

10801089
self.path_index += 1
10811090
if self.path_index == len(current):
@@ -1271,6 +1280,8 @@ def flight_controller(self):
12711280
logger.error(err)
12721281
self.cfStatus = str(err)
12731282

1283+
logger.info('Terminating flight controller thread')
1284+
12741285
def save_current_position(self):
12751286
if self.recording:
12761287
# Restart the timer
@@ -1285,15 +1296,9 @@ def _connected(self, link_uri):
12851296
"""Callback when the Crazyflie has been connected"""
12861297

12871298
self._cf = self._helper.cf
1299+
self._update_flight_status()
12881300

1289-
if not self.flying_enabled:
1290-
self.flying_enabled = True
1291-
self.cfStatus = ": connecting..."
1292-
t = threading.Thread(target=self.flight_controller)
1293-
t.start()
1294-
1295-
self.uri = link_uri
1296-
logger.debug("Crazyflie connected to {}".format(self.uri))
1301+
logger.debug("Crazyflie connected to {}".format(link_uri))
12971302

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

13041309
logger.info("Crazyflie disconnected from {}".format(link_uri))
13051310
self.cfStatus = ': not connected'
1306-
self.flying_enabled = False
1307-
self.cf_ready_to_fly = False
13081311
self._cf = None
1312+
self._update_flight_status()
13091313

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

13671371
if (max_x - min_x) < threshold and (
13681372
max_y - min_y) < threshold and (
1369-
max_z - min_z) < threshold:
1370-
logger.info(
1371-
"Position found with error in, x: {}, y: {}, z: {}".
1372-
format(max_x - min_x, max_y - min_y, max_z - min_z))
1373+
max_z - min_z) < threshold:
1374+
logger.info("Position found with error in, x: {}, y: {}, "
1375+
"z: {}".format(max_x - min_x,
1376+
max_y - min_y,
1377+
max_z - min_z))
13731378

13741379
self.cfStatus = ": connected"
13751380

13761381
self.switch_flight_mode(FlightModeStates.GROUNDED)
1377-
self.cf_ready_to_fly = True
13781382

13791383
break
13801384

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

13901394
def switch_flight_mode(self, mode):
13911395
# Handles the behaviour of switching between flight modes
1392-
13931396
self.flight_mode = mode
13941397

13951398
# Handle client input control.
13961399
# Disable gamepad input if we are not grounded
13971400
if self.flight_mode in [
1398-
FlightModeStates.GROUNDED, FlightModeStates.DISCONNECTED,
1399-
FlightModeStates.RECORD
1401+
FlightModeStates.GROUNDED,
1402+
FlightModeStates.DISCONNECTED,
1403+
FlightModeStates.RECORD
14001404
]:
14011405
self._helper.mainUI.disable_input(False)
14021406
else:

0 commit comments

Comments
 (0)