Server: Add calibration functions, improve buttons state behavior

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 @@ def takeoff_ready(self, contents=()):
         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 @@ def update_model(self, index=QtCore.QModelIndex()):
         #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,10 +237,25 @@ def takeoff_checks(copter_item):
             return False
     return True
 
-def not_calibrating_check(copter_item):
+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 True
+    return not calibrating_check(copter_item)
 
 class CopterProxyModel(QtCore.QSortFilterProxyModel):
     def __init__(self, parent=None):

Server/server_qt.py

@@ -74,30 +74,52 @@ def init_model(self):
         # 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))
 
     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.stop_button.clicked.connect(self.land_all)
+
         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)
+        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)
 
         # 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 @@ def _set_copter_data(self, value, col, copter_id):
 
     @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 @@ def pause_resume_selected(self):
         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 @@ def _resume_selected(self, **kwargs):
     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 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 calibrate_gyro(self):
+    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()