From 263d4081ab58f6bc933bd106c2c4ec92409b5e5d Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 12 Sep 2019 07:47:42 +0300 Subject: [PATCH] Server: Add calibration functions, improve buttons state behavior --- Server/copter_table_models.py | 43 +++++++++--- Server/server_qt.py | 128 ++++++++++++++++++++++++---------- 2 files changed, 128 insertions(+), 43 deletions(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index d56df25..7367a0e 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -29,7 +29,9 @@ class CopterDataModel(QtCore.QAbstractTableModel): checks = {} selected_ready_signal = QtCore.pyqtSignal(bool) selected_takeoff_ready_signal = QtCore.pyqtSignal(bool) - selected_not_calibrating_signal = QtCore.pyqtSignal(bool) + selected_flip_ready_signal = QtCore.pyqtSignal(bool) + selected_calibrating_signal = QtCore.pyqtSignal(bool) + selected_calibration_ready_signal = QtCore.pyqtSignal(bool) def __init__(self, parent=None): super(CopterDataModel, self).__init__(parent) @@ -56,9 +58,17 @@ class CopterDataModel(QtCore.QAbstractTableModel): contents = contents or self.data_contents return filter(lambda x: takeoff_checks(x), contents) - def not_calibrating(self, contents=()): + def flip_ready(self, contents=()): contents = contents or self.data_contents - return filter(lambda x: not_calibrating_check(x), contents) + return filter(lambda x: flip_checks(x), contents) + + def calibrating(self, contents=()): + contents = contents or self.data_contents + return filter(lambda x: calibrating_check(x), contents) + + def calibration_ready(self, contents=()): + contents = contents or self.data_contents + return filter(lambda x: calibration_ready_check(x), contents) def rowCount(self, n=None): return len(self.data_contents) @@ -100,7 +110,9 @@ class CopterDataModel(QtCore.QAbstractTableModel): #self.modelReset.emit() self.selected_ready_signal.emit(set(self.user_selected()).issubset(self.selfchecked_ready())) self.selected_takeoff_ready_signal.emit(set(self.user_selected()).issubset(self.takeoff_ready())) - self.selected_not_calibrating_signal.emit(set(self.user_selected()).issubset(self.not_calibrating())) + self.selected_flip_ready_signal.emit(set(self.user_selected()).issubset(self.flip_ready())) + self.selected_calibrating_signal.emit(set(self.user_selected()).issubset(self.calibrating())) + self.selected_calibration_ready_signal.emit(set(self.user_selected()).issubset(self.calibration_ready())) self.dataChanged.emit(index, index, (QtCore.Qt.EditRole,)) @QtCore.pyqtSlot() @@ -180,7 +192,7 @@ def check_bat_p(item): def check_sys_status(item): if not item: return None - if item == "MAV_STATE_STANDBY": + if item == "STANDBY": return True else: return False @@ -225,11 +237,26 @@ def takeoff_checks(copter_item): return False return True -def not_calibrating_check(copter_item): - if copter_item[5] == "CALIBRATING": - return False +def flip_checks(copter_item): + for i in range(5): + if 2+i != 4: + if not CopterDataModel.checks[2+i](copter_item[2+i]): + return False + else: + if copter_item[4] != "ACTIVE": + return False return True +def calibrating_check(copter_item): + if copter_item[5] == "CALIBRATING": + return True + return False + +def calibration_ready_check(copter_item): + if not CopterDataModel.checks[4](copter_item[4]): + return False + return not calibrating_check(copter_item) + class CopterProxyModel(QtCore.QSortFilterProxyModel): def __init__(self, parent=None): super(CopterProxyModel, self).__init__(parent) diff --git a/Server/server_qt.py b/Server/server_qt.py index e49fbdd..a4574eb 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -74,6 +74,20 @@ class MainWindow(QtWidgets.QMainWindow): # Connect model signals to UI self.model.selected_ready_signal.connect(self.ui.start_button.setEnabled) self.model.selected_takeoff_ready_signal.connect(self.ui.takeoff_button.setEnabled) + self.model.selected_flip_ready_signal.connect(self.ui.flip_button.setEnabled) + # Connect calibrating signal (testing) + self.model.selected_calibrating_signal.connect(self.ui.check_button.setDisabled) + self.model.selected_calibrating_signal.connect(self.ui.pause_button.setDisabled) + self.model.selected_calibrating_signal.connect(self.ui.stop_button.setDisabled) + self.model.selected_calibrating_signal.connect(self.ui.emergency_button.setDisabled) + self.model.selected_calibrating_signal.connect(self.ui.disarm_button.setDisabled) + self.model.selected_calibrating_signal.connect(self.ui.disarm_all_button.setDisabled) + self.model.selected_calibrating_signal.connect(self.ui.leds_button.setDisabled) + self.model.selected_calibrating_signal.connect(self.ui.land_button.setDisabled) + self.model.selected_calibrating_signal.connect(self.ui.reboot_fcu.setDisabled) + self.model.selected_calibration_ready_signal.connect(self.ui.calibrate_gyro.setEnabled) + self.model.selected_calibration_ready_signal.connect(self.ui.calibrate_level.setEnabled) + def client_connected(self, client: Client): self.signals.add_client_signal.emit(CopterData(copter_id=client.copter_id, client=client)) @@ -81,16 +95,23 @@ class MainWindow(QtWidgets.QMainWindow): def init_ui(self): # Connecting self.ui.check_button.clicked.connect(self.selfcheck_selected) - self.ui.start_button.clicked.connect(self.send_starttime) + self.ui.start_button.clicked.connect(self.send_starttime_selected) self.ui.pause_button.clicked.connect(self.pause_resume_selected) - self.ui.stop_button.clicked.connect(self.stop_all) - self.ui.emergency_button.clicked.connect(self.emergency) + self.ui.stop_button.clicked.connect(self.land_all) - self.ui.leds_button.clicked.connect(self.test_leds) + self.ui.emergency_button.clicked.connect(self.emergency) + self.ui.disarm_button.clicked.connect(self.disarm_selected) + self.ui.disarm_all_button.clicked.connect(self.disarm_all) + + self.ui.leds_button.clicked.connect(self.test_leds_selected) self.ui.takeoff_button.clicked.connect(self.takeoff_selected) - self.ui.land_button.clicked.connect(self.land_all) - self.ui.disarm_button.clicked.connect(self.disarm_all) - self.ui.flip_button.clicked.connect(self.flip) + self.ui.flip_button.clicked.connect(self.flip_selected) + self.ui.land_button.clicked.connect(self.land_selected) + + self.ui.reboot_fcu.clicked.connect(self.reboot_selected) + self.ui.calibrate_gyro.clicked.connect(self.calibrate_gyro_selected) + self.ui.calibrate_level.clicked.connect(self.calibrate_level_selected) + self.ui.action_send_animations.triggered.connect(self.send_animations) self.ui.action_send_configurations.triggered.connect(self.send_configurations) self.ui.action_send_Aruco_map.triggered.connect(self.send_aruco) @@ -98,6 +119,7 @@ class MainWindow(QtWidgets.QMainWindow): # Set most safety-important buttons disabled self.ui.start_button.setEnabled(False) self.ui.takeoff_button.setEnabled(False) + self.ui.flip_button.setEnabled(False) @pyqtSlot() def selfcheck_selected(self): @@ -143,35 +165,13 @@ class MainWindow(QtWidgets.QMainWindow): @confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?") @pyqtSlot() - def send_starttime(self, **kwargs): + def send_starttime_selected(self, **kwargs): dt = self.ui.start_delay_spin.value() + self.selfcheck_selected() for copter in self.model.user_selected(): if all_checks(copter): server.send_starttime(copter.client, dt) - @confirmation_required("This operation will takeoff copters immediately. Proceed?") - @pyqtSlot() - def takeoff_selected(self, **kwargs): - for copter in self.model.user_selected(): - if takeoff_checks(copter): - copter.client.send_message("takeoff") - - @confirmation_required("This operation will flip(!!!) copters immediately. Proceed?") - @pyqtSlot() - def flip(self, **kwargs): - for copter in self.model.user_selected(): - if takeoff_checks(copter): - copter.client.send_message("flip") - - @pyqtSlot() - def test_leds(self): - for copter in self.model.user_selected(): - copter.client.send_message("led_test") - - @pyqtSlot() - def stop_all(self): - Client.broadcast_message("stop") - @pyqtSlot() def pause_resume_selected(self): if self.ui.pause_button.text() == 'Pause': @@ -181,7 +181,6 @@ class MainWindow(QtWidgets.QMainWindow): else: self._resume_selected() - #@confirmation_required("This operation will resume ALL copter tasks with given delay. Proceed?") def _resume_selected(self, **kwargs): time_gap = 0.1 for copter in self.model.user_selected(): @@ -192,16 +191,75 @@ class MainWindow(QtWidgets.QMainWindow): def land_all(self): Client.broadcast_message("land") + @pyqtSlot() + def disarm_selected(self): + for copter in self.model.user_selected(): + copter.client.send_message("disarm") + + @pyqtSlot() + def test_leds_selected(self): + for copter in self.model.user_selected(): + copter.client.send_message("led_test") + @pyqtSlot() def disarm_all(self): Client.broadcast_message("disarm") + @confirmation_required("This operation will takeoff copters immediately. Proceed?") @pyqtSlot() - def calibrate_gyro(self): + def takeoff_selected(self, **kwargs): + for copter in self.model.user_selected(): + if takeoff_checks(copter): + copter.client.send_message("takeoff") + + @confirmation_required("This operation will flip(!!!) copters immediately. Proceed?") + @pyqtSlot() + def flip_selected(self, **kwargs): + for copter in self.model.user_selected(): + if flip_checks(copter): + copter.client.send_message("flip") + + @pyqtSlot() + def land_selected(self): + for copter in self.model.user_selected(): + copter.client.send_message("land") + + @pyqtSlot() + def reboot_selected(self): + for copter in self.model.user_selected(): + copter.client.send_message("reboot_fcu") + + @pyqtSlot() + def calibrate_gyro_selected(self): for copter in self.model.user_selected(): client = copter.client - client.get_response("calibrate_gyro", self._get_calibration_info, callback_args=(copter.copter_id)) - + # Update calibration status + row = self.model.data_contents.index(next(filter( + lambda x: x.copter_id == client.copter_id, self.model.data_contents))) + col = 5 + data = 'CALIBRATING' + self.signals.update_data_signal.emit(row, col, data) + # Send request + client.get_response("calibrate_gyro", self._get_calibration_info, callback_args=(5, copter.copter_id)) + + @pyqtSlot() + def calibrate_level_selected(self): + for copter in self.model.user_selected(): + client = copter.client + # Update calibration status + row = self.model.data_contents.index(next(filter( + lambda x: x.copter_id == client.copter_id, self.model.data_contents))) + col = 5 + data = 'CALIBRATING' + self.signals.update_data_signal.emit(row, col, data) + # Send request + client.get_response("calibrate_level", self._get_calibration_info, callback_args=(5, copter.copter_id)) + + def _get_calibration_info(self, value, col, copter_id): + row = self.model.data_contents.index(next( + filter(lambda x: x.copter_id == copter_id, self.model.data_contents))) + data = str(value) + self.signals.update_data_signal.emit(row, col, data) @pyqtSlot()