From fc57f6abaedae4e5ef1445573ef72290ee002baf Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Mon, 20 Jan 2020 18:16:12 +0000 Subject: [PATCH 1/2] Client: Add current task information to telemetry --- Drone/copter_client.py | 19 ++++++++++--------- Drone/tasking_lib.py | 19 +++++++++++++++++-- 2 files changed, 27 insertions(+), 11 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 18a0f3d..c9d1aa5 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -690,12 +690,13 @@ class Telemetry: "animation_id": None, "battery": None, "armed": False, - "system_status": None, - "calibration_status": None, + "fcu_status": None, + "cal_status": None, "mode": None, "selfcheck": None, "current_position": None, "start_position": None, + "task": None, "time": None, } @@ -779,6 +780,7 @@ class Telemetry: def update_telemetry_fast(self): self.start_position = self.get_start_position() + self.task = task_manager.get_current_task() try: self.ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id) if self.ros_telemetry.connected: @@ -802,8 +804,8 @@ class Telemetry: self.animation_id = animation.get_id() self.git_version = self.get_git_version() try: - self.calibration_status = get_calibration_status() - self.system_status = get_sys_status() + self.cal_status = get_calibration_status() + self.fcu_status = get_sys_status() self.battery = self.get_battery(self.ros_telemetry) except rospy.ServiceException: rospy.logdebug("Some service is unavailable") @@ -825,8 +827,8 @@ class Telemetry: def reset_telemetry_values(self): self.battery = float('nan'), float('nan') - self.calibration_status = 'NO_FCU' - self.system_status = 'NO_FCU' + self.cal_status = 'NO_FCU' + self.fcu_status = 'NO_FCU' self.mode = 'NO_FCU' self.selfcheck = ['NO_FCU'] self.current_position = 'NO_POS' @@ -900,15 +902,14 @@ class Telemetry: if client.active_client.config.telemetry_transmit and client.active_client.connected: self.transmit_message() - if client.active_client.config.telemetry_log_resources: - self.log_cpu_and_memory() - rate.sleep() def _slow_update_loop(self): rate = rospy.Rate(1) while not rospy.is_shutdown(): self.update_telemetry_slow() + if client.active_client.config.telemetry_log_resources: + self.log_cpu_and_memory() rate.sleep() def start_loop(self): diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index 68b06de..1827c3c 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -89,6 +89,21 @@ class TaskManager(object): def get_last_task_name(self): return self._last_task + + def get_current_task(self): + try: + start_time, priority, count, task = self.task_queue[0] + except IndexError as e: + logger.debug("Task queue checking exception: {}".format(e)) + return "No task" + else: + if self._running_event.is_set(): + time_to_start = start_time - time.time() + if time_to_start > 0: + return "{} in {:.1f} s".format(task.func.__name__,time_to_start) + return task.func.__name__ + else: + return "paused" def start(self): #print("Task manager is started") @@ -138,7 +153,7 @@ class TaskManager(object): with self._task_queue_lock: try: start_time, priority, count, task = self.task_queue[0] - except Exception as e: + except IndexError as e: logger.debug("Task queue checking exception: {}".format(e)) self._timeshift = 0.0 self._wait_interrupt_event.clear() @@ -180,7 +195,7 @@ class TaskManager(object): if time.time() > start_time: try: start_time_n, priority_n, count_n, task_n = self.task_queue[0] - except Exception as e: + except IndexError as e: logger.warning("Timeout checking exception: {}".format(e)) self._timeshift = 0.0 self._wait_interrupt_event.clear() From eba6412d44a4b238767c079ee64fda1c7fb03236 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Mon, 20 Jan 2020 21:19:51 +0300 Subject: [PATCH 2/2] Server: Add ability to view task information in table --- Server/copter_table_models.py | 111 ++++++---------------------------- Server/server_qt.py | 7 ++- 2 files changed, 21 insertions(+), 97 deletions(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 2f5da09..8f8ba84 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -96,94 +96,11 @@ def check_pos_status(item): def check_start_pos_status(item): return item != 'NO_POS' - @ModelChecks.col_check(10) -def check_time_delta(item): - return abs(item) < ModelChecks.time_delta_max - - -class ModelChecks: - checks_dict = {} - takeoff_checklist = (3, 4, 6, 7, 8) - - @classmethod - def col_check(cls, col): - def inner(f): - def wrapper(item): - if item is not None: - return f(item) - return None - - cls.checks_dict[col] = wrapper - return wrapper - - return inner - - @classmethod - def all_checks(cls, copter_item): - for col, check in cls.checks_dict.items(): - if not check(copter_item[col]): - return False - return True - - @classmethod - def takeoff_checks(cls, copter_item): - for col in cls.takeoff_checklist: - if not cls.checks_dict[col](copter_item[col]): - return False - return True - - -@ModelChecks.col_check(1) -def check_ver(item): - return True # TODO git version! - - -@ModelChecks.col_check(2) -def check_anim(item): - return str(item) != 'No animation' - - -@ModelChecks.col_check(3) -def check_bat(item): - if item == "NO_INFO": - return False - return item[1]*100 > ModelChecks.battery_min - - -@ModelChecks.col_check(4) -def check_sys_status(item): - return item == "STANDBY" - - -@ModelChecks.col_check(5) -def check_cal_status(item): - return item == "OK" - - -@ModelChecks.col_check(6) -def check_mode(item): - return (item != "NO_FCU") and not ("CMODE" in item) - - -@ModelChecks.col_check(7) def check_selfcheck(item): - return item == "OK" + return True - -@ModelChecks.col_check(8) -def check_pos_status(item): - if item == 'NO_POS': - return False - return not math.isnan(item[0]) - - -@ModelChecks.col_check(9) -def check_start_pos_status(item): - return item != 'NO_POS' - - -@ModelChecks.col_check(10) +@ModelChecks.col_check(11) def check_time_delta(item): return abs(item) < ModelChecks.time_delta_max @@ -192,21 +109,22 @@ columns_names = {'copter_id': 'copter ID', 'git_ver': 'version', 'animation_id': ' animation ID ', 'battery': ' battery ', - 'system_status': ' system ', - 'calibration_status': 'sensors', + 'fcu_status': 'FCU status', + 'cal_status': 'sensors', 'mode': ' mode ', 'selfcheck': ' checks ', 'current_position': 'current x y z yaw frame_id', 'start_position': ' start x y z ', + 'last_task': 'last task', 'time_delta': 'dt' } class CopterData: class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('git_ver', None), ('animation_id', None), - ('battery', None), ('system_status', None), ('calibration_status', None), + ('battery', None), ('fcu_status', None), ('cal_status', None), ('mode', None), ('selfcheck', None), ('current_position', None), - ('start_position', None), ('time_delta', None), ('client', None)]) + ('start_position', None), ('last_task', None), ('time_delta', None), ('client', None)]) def __init__(self, **kwargs): self.attrs_dict = self.class_basic_attrs.copy() @@ -370,11 +288,16 @@ def view_selfcheck(value): return value @ModelFormatter.col_format(10, ModelFormatter.PLACE_FORMATTER) +def view_last_task(value): + if value is None: + return 'No task' + return value + +@ModelFormatter.col_format(11, ModelFormatter.PLACE_FORMATTER) def place_time_delta(value): - return abs(value - time.time()) + return abs(value - time.time()) - -@ModelFormatter.col_format(10, ModelFormatter.VIEW_FORMATTER) +@ModelFormatter.col_format(11, ModelFormatter.VIEW_FORMATTER) def view_time_delta(value): return "{:.3f}".format(value) @@ -389,8 +312,8 @@ class CopterDataModel(QtCore.QAbstractTableModel): def __init__(self, checks=ModelChecks, formatter=ModelFormatter, parent=None): super(CopterDataModel, self).__init__(parent) # self.headers = list(columns_names.values()) # todo - self.headers = (' copter ID ', ' version ', ' animation ID ', ' battery ', ' system ', ' sensors ', - ' mode ', ' checks ', ' current x y z yaw frame_id ', ' start x y z ', ' dt ') + self.headers = (' copter ID ', ' version ', ' animation ID ', ' battery ', ' fcu_status ', ' sensors ', + ' mode ', ' checks ', ' current x y z yaw frame_id ', ' start x y z ', ' task ', ' dt ') self.data_contents = [] self.checks = checks diff --git a/Server/server_qt.py b/Server/server_qt.py index 241bf47..438d0ba 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -220,13 +220,14 @@ class MainWindow(QtWidgets.QMainWindow): "git_version": 1, "animation_id": 2, "battery": 3, - "system_status": 4, - "calibration_status": 5, + "fcu_status": 4, + "cal_status": 5, "mode": 6, "selfcheck": 7, "current_position": 8, "start_position": 9, - "time": 10, + "task": 10, + "time": 11, } for key, value in telems.items():