From b17607e4c31402d75bd9e29ece767da0427d2858 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 31 Oct 2019 16:33:28 +0000 Subject: [PATCH 01/11] FlightLib: Fix check ros services function --- Drone/FlightLib/FlightLib.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 40d019e..6d6e28d 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -22,7 +22,8 @@ get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) landing = rospy.ServiceProxy('/land', Trigger) -services_list = [navigate, set_position, set_rates, set_mode, get_telemetry, arming,landing] +services_list = ['/navigate', '/set_position', '/set_rates', '/mavros/set_mode', + '/get_telemetry', '/mavros/cmd/arming', '/land', '/mavros/param/get'] logger.info("Proxy services inited") @@ -88,15 +89,22 @@ def check(check_name): def _check_nans(*values): return any(math.isnan(x) for x in values) +def check_ros_services_unavailable(): + unavailable_services = [] + for service in services_list: + try: + rospy.wait_for_service(service, timeout=0.1) + except (rospy.ServiceException, rospy.ROSException): + unavailable_services.append(service) + return unavailable_services @check("Ros services") def check_ros_services(): - timeout = 0.1 for service in services_list: try: - service.wait_for_service(timeout=timeout) - except (rospy.ServiceException, rospy.ROSException) as e: - yield ("ROS service {} is not available!".format(service.name)) + rospy.wait_for_service(service, timeout=0.1) + except (rospy.ServiceException, rospy.ROSException): + yield ("ROS service {} is not available!".format(service)) @check("FCU connection") From 370bee5c890b84e3f252b0350cc799d79efd4369 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 31 Oct 2019 16:39:12 +0000 Subject: [PATCH 02/11] mavros_mavlink: Modify get_calibration_status --- Drone/mavros_mavlink.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/Drone/mavros_mavlink.py b/Drone/mavros_mavlink.py index 7f58180..e5dede5 100644 --- a/Drone/mavros_mavlink.py +++ b/Drone/mavros_mavlink.py @@ -78,17 +78,11 @@ def get_calibration_status(): mag_status = get_param('CAL_MAG0_ID') acc_status = get_param('CAL_ACC0_ID') status_text = "" - if gyro_status.success == False: - status_text += "gyro: wrong_param; " - elif gyro_status.value.integer == 0: + if gyro_status.value.integer == 0 and gyro_status.success: status_text += "gyro: uncalibrated; " - if mag_status.success == False: - status_text += "mag: wrong_param; " - elif mag_status.value.integer == 0: + if mag_status.value.integer == 0 and mag_status.success: status_text += "mag: uncalibrated; " - if acc_status.success == False: - status_text += "acc: wrong_param; " - elif acc_status.value.integer == 0: + if acc_status.value.integer == 0 and acc_status.success: status_text += "acc: uncalibrated; " if status_text == "": status_text = "OK" From e5c25eac4fc0c37f9deac57849cbeeaaf9650b0f Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 31 Oct 2019 16:39:59 +0000 Subject: [PATCH 03/11] Client: Fix telemetry thread --- Drone/copter_client.py | 94 +++++++++++++++++++++--------------------- 1 file changed, 48 insertions(+), 46 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 666fa36..617d27b 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -611,55 +611,57 @@ def telemetry_loop(): while not rospy.is_shutdown(): telemetry = telemetry._replace(animation_id = animation.get_id()) telemetry = telemetry._replace(git_version = subprocess.check_output("git log --pretty=format:'%h' -n 1", shell=True)) - try: - ros_telemetry = FlightLib.get_telemetry(client.active_client.FRAME_ID) - except: - pass + x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"), + x_ratio=client.active_client.X_RATIO, + y_ratio=client.active_client.Y_RATIO, + ) + x_delta = client.active_client.X0 + client.active_client.X0_COMMON + y_delta = client.active_client.Y0 + client.active_client.Y0_COMMON + z_delta = client.active_client.Z0 + client.active_client.Z0_COMMON + if not math.isnan(x_start): + telemetry = telemetry._replace(start_position = '{:.2f} {:.2f} {:.2f}'.format(x_start+x_delta, y_start+y_delta, z_delta)) else: - if ros_telemetry.connected == False: - telemetry = telemetry._replace(battery_v = 'nan') - telemetry = telemetry._replace(battery_p = 'nan') - telemetry = telemetry._replace(calibration_status = 'NO_FCU') - telemetry = telemetry._replace(system_status = 'NO_FCU') - telemetry = telemetry._replace(mode = 'NO_FCU') - telemetry = telemetry._replace(selfcheck = 'NO_FCU') - telemetry = telemetry._replace(current_position = 'NO_POS in {}'.format(client.active_client.FRAME_ID)) - telemetry = telemetry._replace(start_position = 'NO_POS') - stop_subscriber() - start_subscriber() - else: - telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage)) - batt_empty_param = get_param('BAT_V_EMPTY') - batt_charged_param = get_param('BAT_V_CHARGED') - if batt_empty_param.success and batt_charged_param.success: - batt_empty = batt_empty_param.value.real - batt_charged = batt_charged_param.value.real - telemetry = telemetry._replace(battery_p = '{:.2f}'.format((ros_telemetry.cell_voltage - batt_empty)/(batt_charged - batt_empty)*100.)) + telemetry = telemetry._replace(start_position = 'NO_POS') + services_unavailable = FlightLib.check_ros_services_unavailable() + if not services_unavailable: + try: + ros_telemetry = FlightLib.get_telemetry(client.active_client.FRAME_ID) + if ros_telemetry.connected: + telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage)) + batt_empty_param = get_param('BAT_V_EMPTY') + batt_charged_param = get_param('BAT_V_CHARGED') + batt_cells_param = get_param('BAT_N_CELLS') + if batt_empty_param.success and batt_charged_param.success and batt_cells_param.success: + batt_empty = batt_empty_param.value.real + batt_charged = batt_charged_param.value.real + batt_cells = batt_cells_param.value.integer + telemetry = telemetry._replace(battery_p = '{:.2f}'.format(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100))) + else: + telemetry = telemetry._replace(battery_p = 'nan') + telemetry = telemetry._replace(calibration_status = get_calibration_status()) + telemetry = telemetry._replace(system_status = get_sys_status()) + telemetry = telemetry._replace(mode = ros_telemetry.mode) + check = FlightLib.selfcheck() + if not check: + check = "OK" + telemetry = telemetry._replace(selfcheck = str(check)) + if not math.isnan(ros_telemetry.x): + telemetry = telemetry._replace(current_position = '{:.2f} {:.2f} {:.2f} {:.1f} {}'.format(ros_telemetry.x, ros_telemetry.y, ros_telemetry.z, + math.degrees(ros_telemetry.yaw), client.active_client.FRAME_ID)) + else: + telemetry = telemetry._replace(current_position = 'NO_POS in {}'.format(client.active_client.FRAME_ID)) else: + telemetry = telemetry._replace(battery_v = 'nan') telemetry = telemetry._replace(battery_p = 'nan') - telemetry = telemetry._replace(calibration_status = get_calibration_status()) - telemetry = telemetry._replace(system_status = get_sys_status()) - telemetry = telemetry._replace(mode = ros_telemetry.mode) - check = FlightLib.selfcheck() - if not check: - check = "OK" - telemetry = telemetry._replace(selfcheck = str(check)) - if not math.isnan(ros_telemetry.x): - telemetry = telemetry._replace(current_position = '{:.2f} {:.2f} {:.2f} {:.1f} {}'.format(ros_telemetry.x, ros_telemetry.y, ros_telemetry.z, - math.degrees(ros_telemetry.yaw), client.active_client.FRAME_ID)) - else: - telemetry = telemetry._replace(current_position = 'NO_POS in {}'.format(client.active_client.FRAME_ID)) - x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"), - x_ratio=client.active_client.X_RATIO, - y_ratio=client.active_client.Y_RATIO, - ) - x_delta = client.active_client.X0 + client.active_client.X0_COMMON - y_delta = client.active_client.Y0 + client.active_client.Y0_COMMON - z_delta = client.active_client.Z0 + client.active_client.Z0_COMMON - if not math.isnan(x_start): - telemetry = telemetry._replace(start_position = '{:.2f} {:.2f} {:.2f}'.format(x_start+x_delta, y_start+y_delta, z_delta)) - else: - telemetry = telemetry._replace(start_position = 'NO_POS') + telemetry = telemetry._replace(calibration_status = 'NO_FCU') + telemetry = telemetry._replace(system_status = 'NO_FCU') + telemetry = telemetry._replace(mode = 'NO_FCU') + telemetry = telemetry._replace(selfcheck = 'NO_FCU') + telemetry = telemetry._replace(current_position = 'NO_POS') + except rospy.ServiceException: + print "Some service is unavailable" + else: + telemetry = telemetry._replace(selfcheck = 'WAIT_ROS') if client.active_client.TELEM_TRANSMIT: try: client.active_client.server_connection.send_message('telem', args={'message':create_telemetry_message(telemetry)}) From 87a45ad1571f218e94e3476bbff2619fd5798146 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 17:16:25 +0300 Subject: [PATCH 04/11] copter_table_models: Add three new rows with git version, mode and start x y z --- Server/copter_table_models.py | 58 +++++++++++++++++++---------------- 1 file changed, 32 insertions(+), 26 deletions(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 62bb977..d011b76 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -13,11 +13,10 @@ ModelStateRole = 999 class CopterData: - class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('anim_id', None), - ('batt_v', None), ('batt_p', None), - ('sys_status', None), ('cal_status', None), ('selfcheck', None), - ('position', None), ("time_delta", None), - ("client", None), ]) + class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('git_ver', None), ('anim_id', None), + ('battery', None), ('sys_status', None), ('cal_status', None), + ('mode', None), ('selfcheck', None), ('position', None), + ('start_pos', None), ('time_delta', None), ('client', None)]) def __init__(self, **kwargs): self.attrs_dict = self.class_basic_attrs.copy() @@ -63,7 +62,7 @@ class StatedCopterData(CopterData): class Checks: all_checks = {} - takeoff_checklist = (2, 3, 4, 5, 6) + takeoff_checklist = (3, 4, 6, 7, 8) class CopterDataModel(QtCore.QAbstractTableModel): @@ -75,8 +74,8 @@ class CopterDataModel(QtCore.QAbstractTableModel): def __init__(self, parent=None): super(CopterDataModel, self).__init__(parent) - self.headers = ('copter ID', ' animation ID ', 'batt V', 'batt %', ' system ', - 'calibration', 'selfcheck', 'current x y z yaw frame_id', 'time delta') + self.headers = ('copter ID', 'version', ' animation ID ', ' battery ', ' system ', 'calibration', + ' mode ', 'selfcheck', 'current x y z yaw frame_id', ' start x y z ', 'dt') self.data_contents = [] self.on_id_changed = None @@ -86,7 +85,6 @@ class CopterDataModel(QtCore.QAbstractTableModel): def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()): rows = len(contents) position = len(self.data_contents) if position == 'last' else position - self.beginInsertRows(parent, position, position + rows - 1) self.data_contents[position:position] = contents @@ -263,25 +261,22 @@ def col_check(col): @col_check(1) +def check_ver(item): + if not item: + return None + return True + +@col_check(2) def check_anim(item): if not item: return None return str(item) != 'No animation' - -@col_check(2) -def check_bat_v(item): - if not item: - return None - return float(item) > 3.2 - - @col_check(3) -def check_bat_p(item): +def check_bat(item): if not item: return None - return float(item) > 30 - + return int(item.split(' ')[1][:-1]) > 30 @col_check(4) def check_sys_status(item): @@ -289,29 +284,40 @@ def check_sys_status(item): return None return item == "STANDBY" - @col_check(5) def check_cal_status(item): if not item: return None return item == "OK" - @col_check(6) +def check_mode(item): + if not item: + return None + return True + + +@col_check(7) def check_selfcheck(item): if not item: return None return item == "OK" -@col_check(7) +@col_check(8) def check_pos_status(item): if not item: return None - return str(item).split(' ')[0] != 'nan' + return item.split(' ')[0] != 'nan' or item.split(' ')[0] != 'NO_POS' + +@col_check(9) +def check_start_pos_status(item): + if not item: + return None + return str(item).split(' ')[0] != 'NO_POS' -@col_check(8) +@col_check(10) def check_time_delta(item): if not item: return None @@ -334,7 +340,7 @@ def takeoff_checks(copter_item): def flip_checks(copter_item): for col in Checks.takeoff_checklist: - if col != 4: + if col != 4 or col != 7: if not Checks.all_checks[col](copter_item[col]): return False else: From 0beb6ca69ae8f45120053d0a9df8ea13cc0926d5 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 17:18:01 +0300 Subject: [PATCH 05/11] Server: Handle new data, fix getting copter client data, remove old requests --- Server/server_qt.py | 81 ++++++--------------------------------------- 1 file changed, 11 insertions(+), 70 deletions(-) diff --git a/Server/server_qt.py b/Server/server_qt.py index b6f819b..370fea7 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -166,52 +166,6 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.takeoff_button.setEnabled(False) self.ui.flip_button.setEnabled(False) - @pyqtSlot() - def selfcheck_selected_old(self): - for copter_data_row in self.model.user_selected(): - client = copter_data_row.client - - client.get_response("anim_id", self.set_copter_data, callback_args=(1, copter_data_row)) - client.get_response("batt_voltage", self.set_copter_data, callback_args=(2, copter_data_row)) - client.get_response("cell_voltage", self.set_copter_data, callback_args=(3, copter_data_row)) - client.get_response("sys_status", self.set_copter_data, callback_args=(4, copter_data_row)) - client.get_response("cal_status", self.set_copter_data, callback_args=(5, copter_data_row)) - client.get_response("selfcheck", self.set_copter_data, callback_args=(6, copter_data_row)) - client.get_response("position", self.set_copter_data, callback_args=(7, copter_data_row)) - client.get_response("time", self.set_copter_data, callback_args=(8, copter_data_row)) - - - def set_copter_data(self, value, col, copter_data_row): - row = self.model.get_row_index(copter_data_row) - if row is None: - logging.error("No such client!") - return - - if col == 1: - data = value - elif col == 2: - data = "{}".format(round(float(value), 3)) - elif col == 3: - batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100 # TODO config - data = "{}".format(round(batt_percent, 3)) - elif col == 4: - data = str(value) - elif col == 5: - data = str(value) - elif col == 6: - data = value - elif col == 7: - data = str(value) - elif col == 8: - data = "{}".format(round(float(value) - time.time(), 3)) - if abs(float(data)) > 1: - copter_data_row.client.send_message("repair_chrony") - else: - logging.error("No column matched for response") - return - - self.signals.update_data_signal.emit(row, col, data, ModelDataRole) - @pyqtSlot() def selfcheck_selected(self): for copter_data_row in self.model.user_selected(): @@ -221,7 +175,6 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot(str) def update_table_data(self, message): fields = message.split('`') - logging.info(fields[8]) # copter_id git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position copter_time copter_id = fields[0] git_version = fields[1] @@ -236,27 +189,16 @@ class MainWindow(QtWidgets.QMainWindow): start_pos = fields[10] copter_time = fields[11] row = self.model.get_row_index(self.model.get_row_by_attr('copter_id', copter_id)) - logging.info("Row = {}".format(row)) - self.signals.update_data_signal.emit(row, 1, animation_id, ModelDataRole) - self.signals.update_data_signal.emit(row, 2, battery_v, ModelDataRole) - self.signals.update_data_signal.emit(row, 3, battery_p, ModelDataRole) + self.signals.update_data_signal.emit(row, 1, git_version, ModelDataRole) + self.signals.update_data_signal.emit(row, 2, animation_id, ModelDataRole) + self.signals.update_data_signal.emit(row, 3, "{}V {}%".format(battery_v, battery_p), ModelDataRole) self.signals.update_data_signal.emit(row, 4, sys_status, ModelDataRole) self.signals.update_data_signal.emit(row, 5, cal_status, ModelDataRole) - self.signals.update_data_signal.emit(row, 6, selfcheck, ModelDataRole) - self.signals.update_data_signal.emit(row, 7, current_pos, ModelDataRole) - self.signals.update_data_signal.emit(row, 8, "{}".format(round(float(copter_time) - time.time(), 3)), ModelDataRole) - - #def set_copter_id(self, value, copter_data_row): - # col = 0 - # row = self.model.get_row_index(copter_data_row) - # if row is None: - # logging.error("No such client!") - # return - # logging.info("SET COPTER ID TO {}".format(value)) - # - # copter_data_row.client.copter_id = value - # self.signals.update_data_signal.emit(row, col, value, ModelDataRole) - # self.signals.update_data_signal.emit(row, col, True, ModelStateRole) + self.signals.update_data_signal.emit(row, 6, mode, ModelDataRole) + self.signals.update_data_signal.emit(row, 7, selfcheck, ModelDataRole) + self.signals.update_data_signal.emit(row, 8, current_pos, ModelDataRole) + self.signals.update_data_signal.emit(row, 9, start_pos, ModelDataRole) + self.signals.update_data_signal.emit(row, 10, "{}".format(round(float(copter_time) - time.time(), 3)), ModelDataRole) @pyqtSlot(QtCore.QModelIndex) def selfcheck_info_dialog(self, index): @@ -400,13 +342,12 @@ class MainWindow(QtWidgets.QMainWindow): print("Selected directory:", path) files = [file for file in glob.glob(path + '/*.csv')] names = [os.path.basename(file).split(".")[0] for file in files] - # print(files) for file, name in zip(files, names): for copter in self.model.user_selected(): if name == copter.copter_id: copter.client.send_file(file, "animation.csv") # TODO config else: - print("Filename has no matches with any drone selected") + logging.info("Filename has no matches with any drone selected") @pyqtSlot() def send_calibrations(self): @@ -422,7 +363,7 @@ class MainWindow(QtWidgets.QMainWindow): if name == copter.copter_id: copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/camera_info/calibration.yaml") else: - print("Filename has no matches with any drone selected") + logging.info("Filename has no matches with any drone selected") @pyqtSlot() def send_configurations(self): @@ -435,7 +376,7 @@ class MainWindow(QtWidgets.QMainWindow): for section in sendable_config.sections(): for option in dict(sendable_config.items(section)): value = sendable_config[section][option] - logging.debug("Got item from config:".format(section, option, value)) + logging.debug("Got item from config: {} {} {}".format(section, option, value)) options.append(ConfigOption(section, option, value)) for copter in self.model.user_selected(): From 1053ffe5bb558590d1f54c878929736980c16e07 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 17:18:29 +0300 Subject: [PATCH 06/11] Server: Update GUI --- Server/server_gui.py | 7 ++++--- Server/server_gui.ui | 9 ++++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/Server/server_gui.py b/Server/server_gui.py index 4743936..e577ef7 100644 --- a/Server/server_gui.py +++ b/Server/server_gui.py @@ -13,7 +13,7 @@ from PyQt5 import QtCore, QtGui, QtWidgets class Ui_MainWindow(object): def setupUi(self, MainWindow): MainWindow.setObjectName("MainWindow") - MainWindow.resize(1220, 761) + MainWindow.resize(1360, 761) self.centralwidget = QtWidgets.QWidget(MainWindow) self.centralwidget.setEnabled(True) self.centralwidget.setObjectName("centralwidget") @@ -35,9 +35,10 @@ class Ui_MainWindow(object): self.tableView.setWordWrap(True) self.tableView.setObjectName("tableView") self.tableView.horizontalHeader().setCascadingSectionResizes(False) - self.tableView.horizontalHeader().setDefaultSectionSize(125) + self.tableView.horizontalHeader().setDefaultSectionSize(50) self.tableView.horizontalHeader().setMinimumSectionSize(50) self.tableView.horizontalHeader().setStretchLastSection(True) + self.tableView.verticalHeader().setVisible(False) self.horizontalLayout.addWidget(self.tableView) self.verticalLayout = QtWidgets.QVBoxLayout() self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize) @@ -186,7 +187,7 @@ class Ui_MainWindow(object): self.gridLayout.addLayout(self.horizontalLayout, 0, 0, 1, 1) MainWindow.setCentralWidget(self.centralwidget) self.menubar = QtWidgets.QMenuBar(MainWindow) - self.menubar.setGeometry(QtCore.QRect(0, 0, 1220, 25)) + self.menubar.setGeometry(QtCore.QRect(0, 0, 1360, 25)) self.menubar.setObjectName("menubar") self.menuOptions = QtWidgets.QMenu(self.menubar) self.menuOptions.setObjectName("menuOptions") diff --git a/Server/server_gui.ui b/Server/server_gui.ui index bb58ced..e391a1b 100644 --- a/Server/server_gui.ui +++ b/Server/server_gui.ui @@ -6,7 +6,7 @@ 0 0 - 1220 + 1360 761 @@ -50,7 +50,7 @@ false - 125 + 50 50 @@ -58,6 +58,9 @@ true + + false + @@ -375,7 +378,7 @@ 0 0 - 1220 + 1360 25 From e50b33611bf1bf918c563f2ad9785247cf1701af Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 14:21:16 +0000 Subject: [PATCH 07/11] Client: Update battery status format --- Drone/copter_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 617d27b..2c54d9f 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -635,7 +635,7 @@ def telemetry_loop(): batt_empty = batt_empty_param.value.real batt_charged = batt_charged_param.value.real batt_cells = batt_cells_param.value.integer - telemetry = telemetry._replace(battery_p = '{:.2f}'.format(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100))) + telemetry = telemetry._replace(battery_p = '{}'.format(int(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100)))) else: telemetry = telemetry._replace(battery_p = 'nan') telemetry = telemetry._replace(calibration_status = get_calibration_status()) From 0b23fd6e532caaffd1dd72975a628a89edbbaf97 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 18:53:24 +0300 Subject: [PATCH 08/11] Server: Modify updating table data --- Server/copter_table_models.py | 7 +++++-- Server/server_qt.py | 9 +++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index d011b76..1b48f36 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -276,7 +276,10 @@ def check_anim(item): def check_bat(item): if not item: return None - return int(item.split(' ')[1][:-1]) > 30 + if item == "NO_INFO": + return False + else: + return float(item.split(' ')[1][:-1]) > 30 @col_check(4) def check_sys_status(item): @@ -294,7 +297,7 @@ def check_cal_status(item): def check_mode(item): if not item: return None - return True + return (item != "NO_FCU") and not ("CMODE" in item) @col_check(7) diff --git a/Server/server_qt.py b/Server/server_qt.py index 370fea7..cbae2b5 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -181,6 +181,10 @@ class MainWindow(QtWidgets.QMainWindow): animation_id = fields[2] battery_v = fields[3] battery_p = fields[4] + if battery_v == 'nan' or battery_p == 'nan': + battery_info = "NO_INFO" + else: + battery_info = "{}V {}%".format(battery_v, battery_p) sys_status = fields[5] cal_status = fields[6] mode = fields[7] @@ -188,17 +192,18 @@ class MainWindow(QtWidgets.QMainWindow): current_pos = fields[9] start_pos = fields[10] copter_time = fields[11] + time_delta = "{}".format(round(float(copter_time) - time.time(), 3)) row = self.model.get_row_index(self.model.get_row_by_attr('copter_id', copter_id)) self.signals.update_data_signal.emit(row, 1, git_version, ModelDataRole) self.signals.update_data_signal.emit(row, 2, animation_id, ModelDataRole) - self.signals.update_data_signal.emit(row, 3, "{}V {}%".format(battery_v, battery_p), ModelDataRole) + self.signals.update_data_signal.emit(row, 3, battery_info, ModelDataRole) self.signals.update_data_signal.emit(row, 4, sys_status, ModelDataRole) self.signals.update_data_signal.emit(row, 5, cal_status, ModelDataRole) self.signals.update_data_signal.emit(row, 6, mode, ModelDataRole) self.signals.update_data_signal.emit(row, 7, selfcheck, ModelDataRole) self.signals.update_data_signal.emit(row, 8, current_pos, ModelDataRole) self.signals.update_data_signal.emit(row, 9, start_pos, ModelDataRole) - self.signals.update_data_signal.emit(row, 10, "{}".format(round(float(copter_time) - time.time(), 3)), ModelDataRole) + self.signals.update_data_signal.emit(row, 10, time_delta, ModelDataRole) @pyqtSlot(QtCore.QModelIndex) def selfcheck_info_dialog(self, index): From e26f7a861a5938617077a15627ef5ad048f8e84f Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 19:12:14 +0300 Subject: [PATCH 09/11] copter_table_models: Update position column check --- Server/copter_table_models.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 1b48f36..dd669bf 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -311,7 +311,7 @@ def check_selfcheck(item): def check_pos_status(item): if not item: return None - return item.split(' ')[0] != 'nan' or item.split(' ')[0] != 'NO_POS' + return item.split(' ')[0] != 'nan' and item.split(' ')[0] != 'NO_POS' @col_check(9) def check_start_pos_status(item): From 4f59f397fe885031f26361b6218871c1b5d0ca76 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 15:56:05 +0000 Subject: [PATCH 10/11] mavros_mavlink: Update get_calibration_status --- Drone/mavros_mavlink.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Drone/mavros_mavlink.py b/Drone/mavros_mavlink.py index e5dede5..feded7c 100644 --- a/Drone/mavros_mavlink.py +++ b/Drone/mavros_mavlink.py @@ -85,7 +85,10 @@ def get_calibration_status(): if acc_status.value.integer == 0 and acc_status.success: status_text += "acc: uncalibrated; " if status_text == "": - status_text = "OK" + if not gyro_status.success or not mag_status.success or not acc_status.success: + status_text = "NO_INFO" + else: + status_text = "OK" else: status_text = status_text[:-2] return status_text From a72c447209b33edac65373427fb63c6748f78abd Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 16:13:35 +0000 Subject: [PATCH 11/11] Client: Modify floor frame broadcast --- Drone/copter_client.py | 54 ++++++++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 23 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 2c54d9f..dded96a 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -6,6 +6,7 @@ import datetime import logging import threading import subprocess +import ConfigParser from collections import namedtuple from FlightLib import FlightLib @@ -41,6 +42,7 @@ logger = logging.getLogger(__name__) class CopterClient(client.Client): def load_config(self): + self.FLOOR_FRAME_EXISTS = False super(CopterClient, self).load_config() self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency') self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit') @@ -65,7 +67,18 @@ class CopterClient(client.Client): self.Z0 = self.config.getfloat('PRIVATE', 'z0') self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds') self.LED_PIN = self.config.getint('PRIVATE', 'led_pin') - + try: + self.FLOOR_DX = self.config.getfloat('FLOOR FRAME', 'x') + self.FLOOR_DY = self.config.getfloat('FLOOR FRAME', 'y') + self.FLOOR_DZ = self.config.getfloat('FLOOR FRAME', 'z') + self.FLOOR_ROLL = self.config.getfloat('FLOOR FRAME', 'roll') + self.FLOOR_PITCH = self.config.getfloat('FLOOR FRAME', 'pitch') + self.FLOOR_YAW = self.config.getfloat('FLOOR FRAME', 'yaw') + self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent') + self.FLOOR_FRAME_EXISTS = True + except ConfigParser.Error: + logger.error("No floor frame!") + self.FLOOR_FRAME_EXISTS = False self.RESTART_DHCPCD = self.config.getboolean('PRIVATE', 'restart_dhcpcd') def on_broadcast_bind(self): @@ -79,32 +92,25 @@ class CopterClient(client.Client): LedLib.init_led(self.LED_PIN) task_manager_instance.start() if self.FRAME_ID == "floor": - try: - self.FLOOR_DX = self.config.getfloat('FLOOR FRAME', 'x') - self.FLOOR_DY = self.config.getfloat('FLOOR FRAME', 'y') - self.FLOOR_DZ = self.config.getfloat('FLOOR FRAME', 'z') - self.FLOOR_ROLL = self.config.getfloat('FLOOR FRAME', 'roll') - self.FLOOR_PITCH = self.config.getfloat('FLOOR FRAME', 'pitch') - self.FLOOR_YAW = self.config.getfloat('FLOOR FRAME', 'yaw') - self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent') - except Exception as e: - raise Exception("Can't make floor frame!") - quit() + if self.FLOOR_FRAME_EXISTS: + self.start_floor_frame_broadcast() else: - trans = TransformStamped() - trans.transform.translation.x = self.FLOOR_DX - trans.transform.translation.y = self.FLOOR_DY - trans.transform.translation.z = self.FLOOR_DZ - trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.FLOOR_ROLL), - math.radians(self.FLOOR_PITCH), - math.radians(self.FLOOR_YAW))) - trans.header.frame_id = self.FLOOR_PARENT - trans.child_frame_id = self.FRAME_ID - static_bloadcaster.sendTransform(trans) + logger.error("Can't make floor frame!") start_subscriber() telemetry_thread.start() super(CopterClient, self).start() + def start_floor_frame_broadcast(self): + trans = TransformStamped() + trans.transform.translation.x = self.FLOOR_DX + trans.transform.translation.y = self.FLOOR_DY + trans.transform.translation.z = self.FLOOR_DZ + trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.FLOOR_ROLL), + math.radians(self.FLOOR_PITCH), + math.radians(self.FLOOR_YAW))) + trans.header.frame_id = self.FLOOR_PARENT + trans.child_frame_id = self.FRAME_ID + static_bloadcaster.sendTransform(trans) def restart_service(name): os.system("systemctl restart {}".format(name)) @@ -407,7 +413,9 @@ def _command_reboot(*args, **kwargs): @messaging.message_callback("service_restart") def _command_service_restart(*args, **kwargs): - restart_service(kwargs["name"]) + service = kwargs["name"] + restart_service(service) + @messaging.message_callback("repair_chrony") def _command_chrony_repair(*args, **kwargs):