Client: Add additional checks for copter state requests

Drone/copter_client.py

@@ -98,8 +98,12 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
 
 @messaging.request_callback("selfcheck")
 def _response_selfcheck():
-    check = FlightLib.selfcheck()
-    return check if check else "OK"
+    if check_state_topic(wait_new_status=True):
+        check = FlightLib.selfcheck()
+        return check if check else "OK"
+    else:
+        stop_subscriber()
+        return "NOT_CONNECTED_TO_FCU"
 
 
 @messaging.request_callback("anim_id")
@@ -124,12 +128,20 @@ def _response_animation_id():
 
 @messaging.request_callback("batt_voltage")
 def _response_batt():
-    return FlightLib.get_telemetry('body').voltage
+    if check_state_topic(wait_new_status=True):
+        return FlightLib.get_telemetry('body').voltage
+    else:
+        stop_subscriber()
+        return "NOT_CONNECTED_TO_FCU"
 
 
 @messaging.request_callback("cell_voltage")
 def _response_cell():
-    return FlightLib.get_telemetry('body').cell_voltage
+    if check_state_topic(wait_new_status=True):
+        return FlightLib.get_telemetry('body').cell_voltage
+    else:
+        stop_subscriber()
+        return "NOT_CONNECTED_TO_FCU"
 
 @messaging.request_callback("sys_status")
 def _response_sys_status():
@@ -141,11 +153,13 @@ def _response_cal_status():
 
 @messaging.request_callback("calibrate_gyro")
 def _calibrate_gyro():
-    return calibrate('gyro')
+    calibrate('gyro')
+    return get_calibration_status()
 
 @messaging.request_callback("calibrate_level")
 def _calibrate_level():
-    return calibrate('level')
+    calibrate('level')
+    return get_calibration_status()
 
 
 @messaging.message_callback("test")

Drone/mavros_mavlink.py

@@ -15,16 +15,18 @@ def state_callback(data):
     global system_status
     system_status = data.system_status
 
-def check_state_topic():
+def check_state_topic(wait_new_status = False):
     global system_status, heartbeat_sub, heartbeat_sub_status
     # Check subscriber
     if (not heartbeat_sub) or (not heartbeat_sub_status):
-        rospy.loginfo('Not subscribed to topic')
-        return False
+        start_subscriber()
+        system_status = -1
+    if wait_new_status:
+        system_status = -1
     # Wait for heartbeat
     start_time = time.time()
     while system_status == -1:
-        if time.time() - start_time > 3.:
+        if time.time() - start_time > 1.:
             rospy.loginfo("Not connected to fcu. Check connection.")
             return False
         rospy.sleep(0.1)
@@ -35,6 +37,7 @@ def reboot_fcu():
     if check_state_topic():
         rospy.loginfo("Send reboot message to fcu")
         send_command_long(False, mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, 1, 0, 0, 0, 0, 0, 0)
+        stop_subscriber()
         return True
     return False