Merge branch 'master' into feature-branch

2026-06-02 18:19:33 +00:00 · 2019-11-02 18:03:27 +03:00
parent 587b8c7a01 9f7fda0b16
commit 080d7438ec
11 changed files with 272 additions and 254 deletions
--- 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")

@@ -76,7 +77,7 @@ def check(check_name):
            if msgs:
                return msgs
            else:
-                logger.info("[{}]: OK".format(check_name))
+                logger.debug("[{}]: OK".format(check_name))
                return None

        checklist.append(wrapper)
@@ -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")
--- a/Drone/animation_lib.py
+++ b/Drone/animation_lib.py
@@ -24,7 +24,7 @@ def get_id(filepath="animation.csv"):
    try:
        animation_file = open(filepath)
    except IOError:
-        logging.error("File {} can't be opened".format(filepath))
+        logger.error("File {} can't be opened".format(filepath))
        anim_id = "No animation"
        return anim_id
    else:
@@ -35,17 +35,17 @@ def get_id(filepath="animation.csv"):
            row_0 = csv_reader.next()
            if len(row_0) == 1:
                anim_id = row_0[0]
-                print("Got animation_id: {}".format(anim_id))
+                logger.debug("Got animation_id: {}".format(anim_id))
            else:
                anim_id = "Empty id"
-                print("No animation id in file")
+                logger.debug("No animation id in file")
    return anim_id

 def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1):
    try:
        animation_file = open(filepath)
    except IOError:
-        logging.error("File {} can't be opened".format(filepath))
+        logger.error("File {} can't be opened".format(filepath))
        anim_id = "No animation"
        return float('nan'), float('nan')
    else:
@@ -59,14 +59,14 @@ def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1):
                return float('nan'), float('nan')
            if len(row_0) == 1:
                anim_id = row_0[0]
-                print("Got animation_id: {}".format(anim_id))
+                logger.debug("Got animation_id: {}".format(anim_id))
                try:
                    frame_number, x, y, z, yaw, red, green, blue = csv_reader.next()
                except:
                    return float('nan'), float('nan')
            else:
                anim_id = "Empty id"
-                print("No animation id in file")
+                logger.debug("No animation id in file")
                try:
                    frame_number, x, y, z, yaw, red, green, blue = row_0
                except:
@@ -90,9 +90,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati
            row_0 = csv_reader.next()
            if len(row_0) == 1:
                anim_id = row_0[0]
-                print("Got animation_id: {}".format(anim_id))
+                logger.debug("Got animation_id: {}".format(anim_id))
            else:
-                print("No animation id in file")
+                logger.debug("No animation id in file")
                frame_number, x, y, z, yaw, red, green, blue = row_0
                imported_frames.append({
                    'number': int(frame_number),
--- a/Drone/client.py
+++ b/Drone/client.py
@@ -17,7 +17,6 @@ current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfra
 parent_dir = os.path.dirname(current_dir)
 sys.path.insert(0, parent_dir) 

-logging.basicConfig(level=logging.DEBUG)
 logger = logging.getLogger(__name__)

 import messaging_lib as messaging
@@ -31,7 +30,7 @@ class Client(object):
        self.selector = selectors.DefaultSelector()
        self.client_socket = None

-        self.server_connection = messaging.ConnectionManager()
+        self.server_connection = messaging.ConnectionManager("pi")

        self.server_host = None
        self.server_port = None
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -1,4 +1,5 @@
 import os
+import sys
 import time
 import math
 import rospy
@@ -6,6 +7,7 @@ import datetime
 import logging
 import threading
 import subprocess
+import ConfigParser
 from collections import namedtuple

 from FlightLib import FlightLib
@@ -27,20 +29,46 @@ static_bloadcaster = tf2_ros.StaticTransformBroadcaster()
 Telemetry = namedtuple("Telemetry", "git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position")
 telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS')

-# logging.basicConfig(  # TODO all prints as logs
-#    level=logging.DEBUG, # INFO
-#    format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
-#    handlers=[
-#        logging.StreamHandler(),
-#    ])
+logging.basicConfig(  # TODO all prints as logs
+   level=logging.DEBUG, # INFO
+   stream=sys.stdout,
+   format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
+   handlers=[
+       logging.StreamHandler(sys.stdout),
+   ])
+
+handler = logging.StreamHandler(sys.stdout)
+handler.setLevel(logging.DEBUG)
+formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s")
+handler.setFormatter(formatter)

 logger = logging.getLogger(__name__)
+logger.setLevel(logging.DEBUG)
+logger.addHandler(handler)

+animation_logger = logging.getLogger('animation_lib')
+animation_logger.setLevel(logging.INFO)
+animation_logger.addHandler(handler)

-# import ros_logging
+client_logger = logging.getLogger('client')
+client_logger.setLevel(logging.DEBUG)
+client_logger.addHandler(handler)
+
+messaging_logger = logging.getLogger('messaging_lib')
+messaging_logger.setLevel(logging.INFO)
+messaging_logger.addHandler(handler)
+
+tasking_logger = logging.getLogger('tasking_lib')
+tasking_logger.setLevel(logging.INFO)
+tasking_logger.addHandler(handler)
+
+flightlib_logger = logging.getLogger('FlightLib')
+flightlib_logger.setLevel(logging.INFO)
+flightlib_logger.addHandler(handler)

 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 +93,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:
+            rospy.logerror("No floor frame!")
+            self.FLOOR_FRAME_EXISTS = False
        self.RESTART_DHCPCD = self.config.getboolean('PRIVATE', 'restart_dhcpcd')

    def on_broadcast_bind(self):
@@ -73,38 +112,31 @@ class CopterClient(client.Client):
        restart_service("chrony")

    def start(self, task_manager_instance):
-        client.logger.info("Init ROS node")
+        rospy.loginfo("Init ROS node")
        rospy.init_node('clever_show_client')
        if self.USE_LEDS:
            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)
+                rospy.logerror("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))
@@ -117,7 +149,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
        with open(path, 'r') as f:
            raw_content = f.read()
    except IOError as e:
-        print("Reading error {}".format(e))
+        logger.error("Reading error {}".format(e))
        return False

    content = raw_content.split(" ")
@@ -125,11 +157,11 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
    try:
        current_ip = content[ip_index]
    except IndexError:
-        print("Something wrong with config")
+        logger.error("Something wrong with config")
        return False

    if "." not in current_ip:
-        print("That's not ip!")
+        logger.debug("That's not ip!")
        return False

    if current_ip != ip:
@@ -139,7 +171,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
            with open(path, 'w') as f:
                f.write(" ".join(content))
        except IOError:
-            print("Error writing")
+            logger.error("Error writing")
            return False

    return True
@@ -151,7 +183,7 @@ def configure_hostname(hostname):
        with open(path, 'r') as f:
            raw_content = f.read()
    except IOError as e:
-        print("Reading error {}".format(e))
+        logger.error("Reading error {}".format(e))
        return False

    current_hostname = str(raw_content)
@@ -162,7 +194,7 @@ def configure_hostname(hostname):
            with open(path, 'w') as f:
                f.write(content)
        except IOError:
-            print("Error writing")
+            logger.error("Error writing")
            return False

    return True
@@ -174,7 +206,7 @@ def configure_hosts(hostname):
        with open(path, 'r') as f:
            raw_content = f.read()
    except IOError as e:
-        print("Reading error {}".format(e))
+        logger.error("Reading error {}".format(e))
        return False

    index_start = raw_content.find("127.0.1.1", )
@@ -187,7 +219,7 @@ def configure_hosts(hostname):
            with open(path, 'w') as f:
                f.write(content)
        except IOError:
-            print("Error writing")
+            logger.error("Error writing")
            return False

    return True
@@ -202,7 +234,7 @@ def configure_bashrc(hostname):
        with open(path, 'r') as f:
            raw_content = f.read()
    except IOError as e:
-        print("Reading error {}".format(e))
+        logger.error("Reading error {}".format(e))
        return False

    index_start = raw_content.find("ROS_HOSTNAME='", ) + 14
@@ -215,7 +247,7 @@ def configure_bashrc(hostname):
            with open(path, 'w') as f:
                f.write(content)
        except IOError:
-            print("Error writing")
+            logger.error("Error writing")
            return False

    return True
@@ -270,7 +302,7 @@ def _response_animation_id(*args, **kwargs):
    # Load animation
    result = animation.get_id()
    if result != 'No animation':
-        print ("Saving corrected animation")
+        logger.debug ("Saving corrected animation")
        frames = animation.load_animation(os.path.abspath("animation.csv"),
                                            x0=client.active_client.X0 + client.active_client.X0_COMMON,
                                            y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
@@ -284,7 +316,7 @@ def _response_animation_id(*args, **kwargs):
                                            check_takeoff=client.active_client.TAKEOFF_CHECK,
                                            check_land=client.active_client.LAND_CHECK,
                                            )
-        print("Start action: {}".format(start_action))
+        logger.debug("Start action: {}".format(start_action))
        # Save corrected animation
        animation.save_corrected_animation(corrected_frames)
    return result
@@ -338,6 +370,7 @@ def _calibrate_level(*args, **kwargs):
@messaging.message_callback("test")
 def _command_test(*args, **kwargs):
    logger.info("logging info test")
+    rospy.logdebug("ros logdebug test")
    print("stdout test")

@messaging.message_callback("move_start")
@@ -346,20 +379,20 @@ def _command_move_start_to_current_position(*args, **kwargs):
                                        x_ratio=client.active_client.X_RATIO,
                                        y_ratio=client.active_client.Y_RATIO,
                                        )
-    print("x_start = {}, y_start = {}".format(x_start, y_start))
+    logger.debug("x_start = {}, y_start = {}".format(x_start, y_start))
    if not math.isnan(x_start):
        telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
-        print("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
-        if not math.isnan(x_telem):
+        logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
+        if not math.isnan(telem.x):
            client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start)
            client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
            client.active_client.rewrite_config()
            client.active_client.load_config()
-            print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+            logger.info ("Set start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
        else:
-            print ("Wrong telemetry")
+            logger.debug ("Wrong telemetry")
    else:
-        print("Wrong animation file")
+        logger.debug("Wrong animation file")

@messaging.message_callback("reset_start")
 def _command_reset_start(*args, **kwargs):
@@ -367,7 +400,7 @@ def _command_reset_start(*args, **kwargs):
    client.active_client.config.set('PRIVATE', 'y0', 0)
    client.active_client.rewrite_config()
    client.active_client.load_config()
-    print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+    logger.info ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))

@messaging.message_callback("set_z_to_ground")
 def _command_set_z(*args, **kwargs):
@@ -375,14 +408,14 @@ def _command_set_z(*args, **kwargs):
    client.active_client.config.set('PRIVATE', 'z0', telem.z)
    client.active_client.rewrite_config()
    client.active_client.load_config()
-    print ("Set z offset to {:.2f}".format(client.active_client.Z0))
+    logger.info ("Set z offset to {:.2f}".format(client.active_client.Z0))

@messaging.message_callback("reset_z_offset")
 def _command_reset_z(*args, **kwargs):
    client.active_client.config.set('PRIVATE', 'z0', 0)
    client.active_client.rewrite_config()
    client.active_client.load_config()
-    print ("Reset z offset to {:.2f}".format(client.active_client.Z0))
+    logger.info ("Reset z offset to {:.2f}".format(client.active_client.Z0))


@messaging.message_callback("update_repo")
@@ -407,7 +440,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):
@@ -437,7 +472,7 @@ def _copter_flip(*args, **kwargs):

@messaging.message_callback("takeoff")
 def _command_takeoff(*args, **kwargs):
-    print("Takeoff at {}".format(datetime.datetime.now()))
+    logger.info("Takeoff at {}".format(datetime.datetime.now()))
    task_manager.add_task(0, 0, animation.takeoff,
                          task_kwargs={
                              "z": client.active_client.TAKEOFF_HEIGHT,
@@ -452,7 +487,7 @@ def _command_takeoff_z(*args, **kwargs):
    z_str = kwargs.get("z", None)
    if z_str is not None:
        telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
-        print("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now()))
+        logger.info("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now()))
        task_manager.add_task(0, 0, FlightLib.reach_point,
                          task_kwargs={
                              "x": telem.x,
@@ -508,12 +543,12 @@ def _play_animation(*args, **kwargs):
    start_time = float(kwargs["time"])
    # Check if animation file is available
    if animation.get_id() == 'No animation':
-        print("Can't start animation without animation file!")
+        logger.error("Can't start animation without animation file!")
        return

    task_manager.reset(interrupt_next_task=False)
    
-    print("Start time = {}, wait for {} seconds".format(start_time, start_time-time.time()))
+    logger.info("Start time = {}, wait for {} seconds".format(start_time, start_time-time.time()))
    # Load animation
    frames = animation.load_animation(os.path.abspath("animation.csv"),
                                        x0=client.active_client.X0 + client.active_client.X0_COMMON,
@@ -556,7 +591,6 @@ def _play_animation(*args, **kwargs):
        frame_time = rfp_time + client.active_client.RFP_TIME

    elif start_action == 'arm':
-        print ("Start_time")
        # Calculate start time
        start_time += start_delay
        # Arm
@@ -579,6 +613,7 @@ def _play_animation(*args, **kwargs):
                        )
        # Calculate first frame start time
        frame_time += client.active_client.FRAME_DELAY # TODO Think about arming time   
+    logger.debug(task_manager.task_queue)
    # Play animation file
    for frame in corrected_frames:
        point, color, yaw = animation.convert_frame(frame)
@@ -603,7 +638,6 @@ def _play_animation(*args, **kwargs):
                        "use_leds": client.active_client.USE_LEDS,
                    },
                    )
-    #print(task_manager.task_queue)

 def telemetry_loop():
    global telemetry
@@ -611,60 +645,67 @@ 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 = '{}'.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())
+                    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:
+                rospy.logdebug("Some service is unavailable")
+            except AttributeError as e:
+                rospy.logdebug(e)
+            except rospy.TransportException as e:
+                rospy.logdebug(e)
+        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)})
-            except Exception as e:
-                print e
+            except AttributeError as e:
+                rospy.logdebug(e)
+
        rate.sleep()

 def create_telemetry_message(telemetry):
@@ -677,11 +718,7 @@ def create_telemetry_message(telemetry):
 telemetry_thread = threading.Thread(target=telemetry_loop, name="Telemetry getting thread")

 if __name__ == "__main__":
+    
    copter_client = CopterClient()
    task_manager = tasking.TaskManager()
    copter_client.start(task_manager)
-
-    # ros_logging.route_logger_to_ros()
-    # ros_logging.route_logger_to_ros("__main__")
-    # ros_logging.route_logger_to_ros("client")
-    # ros_logging.route_logger_to_ros("messaging")
--- a/Drone/mavros_mavlink.py
+++ b/Drone/mavros_mavlink.py
@@ -78,20 +78,17 @@ 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"
+        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
--- a/Drone/tasking_lib.py
+++ b/Drone/tasking_lib.py
@@ -87,7 +87,7 @@ class TaskManager(object):

    def start(self):
        #print("Task manager is started")
-        #logger.info("Task manager is started")
+        logger.info("Task manager is started")
        self._processor_thread.start()
        self.resume()

@@ -107,7 +107,7 @@ class TaskManager(object):
            self._wait_interrupt_event.set()
            self._task_interrupt_event.set()
        self._running_event.clear()
-        #logger.info("Task queue paused")
+        logger.info("Task queue paused")
        #print("Task queue paused")

    def resume(self, time_to_start_next_task=0.0):
@@ -118,7 +118,7 @@ class TaskManager(object):
        self._wait_interrupt_event.clear()
        self._task_interrupt_event.clear()
        self._running_event.set()
-        #logger.info("Task queue resumed with timeshift {}".format(self._timeshift))
+        logger.info("Task queue resumed with timeshift {}".format(self._timeshift))
        #print("Task queue resumed with timeshift {}".format(self._timeshift))

    def reset(self, interrupt_next_task=True):
@@ -128,11 +128,13 @@ class TaskManager(object):
            self._reset_event.set()

    def execute_task(self):
+        delta = 0.1
+
        with self._task_queue_lock:
            try:
                start_time, priority, count, task = self.task_queue[0]
            except Exception as e:
-                #print("Task queue checking exception: {}".format(e))
+                logger.debug("Task queue checking exception: {}".format(e))
                self._timeshift = 0.0
                self._wait_interrupt_event.clear()
                self._task_interrupt_event.clear()
@@ -140,24 +142,32 @@ class TaskManager(object):
                return

        task_start_time = start_time + self._timeshift
-        #logger.info("Waiting util task execution time:{}".format(task_start_time))
+        logger.info("Executing task {}".format(task.func.__name__))
+        logger.debug("Waiting util task execution time:{}".format(task_start_time))
        #print("Waiting until task execution time:{}".format(task_start_time))
        wait(task_start_time, self._wait_interrupt_event)

+        if task_start_time - time.time() > 0.01:
+            logger.error("Need for waiting more")
+            self._wait_interrupt_event.clear()
+            return
+
        if not self._wait_interrupt_event.is_set():
            #logger.info("Executing task {}".format(task))
            #print("{} Executing task {}".format(time.time(),task))
            #print("Interrupter is set: {}".format(self._task_interrupt_event.is_set()))
            try:
                task.func(*task.args, interrupter=self._task_interrupt_event, **task.kwargs)
+            
            except Exception as e:
                logger.error("Error '{}' occurred in task {}".format(e, task))
                #print("Error '{}' occurred in task {}".format(e, task))
                if str(e) == 'STOP':
                    self.reset()
+                    logger.error("Return after STOP exception, can't arm!")
                    return
        else:
-            #logger.warning("Task interrupted before execution")
+            logger.error("Task interrupted before execution")
            #print("Task interrupted before execution")
            self._wait_interrupt_event.clear()
            return
@@ -166,14 +176,17 @@ class TaskManager(object):
            try:
                start_time_n, priority_n, count_n, task_n = self.task_queue[0]
            except Exception as e:
-                #print("Timeout checking exception: {}".format(e))
+                logger.warning("Timeout checking exception: {}".format(e))
                self._timeshift = 0.0
                self._wait_interrupt_event.clear()
                self._task_interrupt_event.clear()
                self._running_event.clear()
                return
            if (task_n == task) and (start_time_n == start_time):
-                self.pop_task()
+                try:
+                    self.pop_task()
+                except KeyError as e:
+                    logger.error(str(e))
                #try:
                    #print("Pop {} function!".format(task.func.__name__))
                #except Exception as e:
@@ -182,11 +195,11 @@ class TaskManager(object):
        if self._task_interrupt_event.is_set():
            self._task_interrupt_event.clear()     

-        #logger.info("Execution done")
+        logger.info("Execution done")
        #print("Execution done")

    def _task_processor(self):
-        #logger.info("Tasking thread started")
+        logger.info("Tasking thread started")
        # self._update_queue()  # Initial tick if tasks added before start
        while not self._shutdown_event.is_set():
            self._running_event.wait()
--- 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,25 @@ 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
-
+    if item == "NO_INFO":
+        return False
+    else:
+        return float(item.split(' ')[1][:-1]) > 30

@col_check(4)
 def check_sys_status(item):
@@ -289,30 +287,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 (item != "NO_FCU") and not ("CMODE" in item)
+
+
+@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' and 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
@@ -335,7 +343,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:
--- 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, 26))
+        self.menubar.setGeometry(QtCore.QRect(0, 0, 1360, 25))
        self.menubar.setObjectName("menubar")
        self.menuOptions = QtWidgets.QMenu(self.menubar)
        self.menuOptions.setObjectName("menuOptions")
--- a/Server/server_gui.ui
+++ b/Server/server_gui.ui
@@ -6,7 +6,7 @@
   <rect>
    <x>0</x>
    <y>0</y>
-    <width>1220</width>
+    <width>1360</width>
    <height>761</height>
   </rect>
  </property>
@@ -50,7 +50,7 @@
         <bool>false</bool>
        </attribute>
        <attribute name="horizontalHeaderDefaultSectionSize">
-         <number>125</number>
+         <number>50</number>
        </attribute>
        <attribute name="horizontalHeaderMinimumSectionSize">
         <number>50</number>
@@ -58,6 +58,9 @@
        <attribute name="horizontalHeaderStretchLastSection">
         <bool>true</bool>
        </attribute>
+        <attribute name="verticalHeaderVisible">
+         <bool>false</bool>
+        </attribute>
       </widget>
      </item>
      <item>
@@ -375,8 +378,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-     <width>1220</width>
-     <height>26</height>
+     <width>1360</width>
+     <height>25</height>
    </rect>
   </property>
   <widget class="QMenu" name="menuOptions">
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -169,51 +169,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():
@@ -223,13 +178,16 @@ 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]
        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]
@@ -237,28 +195,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))
-        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, 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, 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, time_delta, ModelDataRole)

    @pyqtSlot(QtCore.QModelIndex)
    def selfcheck_info_dialog(self, index):
@@ -408,13 +356,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):
@@ -430,7 +377,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):
@@ -443,7 +390,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():
--- a/messaging_lib.py
+++ b/messaging_lib.py
@@ -5,6 +5,7 @@ import json
 import socket
 import struct
 import random
+import inspect
 import logging
 import threading
 import collections
@@ -34,7 +35,7 @@ def get_ip_address():
            ip_socket.connect(("8.8.8.8", 80))
            return ip_socket.getsockname()[0]
    except OSError:
-        logging.warning("No network connection detected, using localhost")
+        logger.warning("No network connection detected, using localhost")
        return "localhost"


@@ -194,7 +195,7 @@ class ConnectionManager(object):
    messages_callbacks = {}
    requests_callbacks = {}

-    def __init__(self):
+    def __init__(self, whoami = "computer"):
        self.selector = None
        self.socket = None
        self.addr = None
@@ -204,6 +205,8 @@ class ConnectionManager(object):
        self._recv_buffer = b""
        self._send_buffer = b""

+        self.whoami = whoami
+
        self._send_queue = collections.deque()
        self._received_queue = collections.deque()
        self._request_queue = collections.OrderedDict()
@@ -228,7 +231,7 @@ class ConnectionManager(object):
            raise ValueError("Invalid events mask mode {}.".format(mode))

        key = self.selector.modify(self.socket, events, data=self)
-        logging.debug("Switched selector of {} to mode {}".format(self.addr, key.events))
+        logger.debug("Switched selector of {} to mode {}".format(self.addr, key.events))
        return key

    def connect(self, client_selector, client_socket, client_addr):
@@ -390,7 +393,9 @@ class ConnectionManager(object):
                logger.error("File {} can not be written due error: {}".format(filepath, error))
            else:
                logger.info("File {} successfully received ".format(filepath))
-                os.system("chown pi:pi {}".format(filepath))
+                if self.whoami == "pi":
+                    logger.info("Return rights to pi:pi after file transfer")
+                    os.system("chown pi:pi {}".format(filepath))

    def write(self):
        with self._send_lock: