Client: Add current task information to telemetry

Drone/copter_client.py

@@ -690,12 +690,13 @@ class Telemetry:
         "animation_id": None,
         "battery": None,
         "armed": False,
-        "system_status": None,
-        "calibration_status": None,
+        "fcu_status": None,
+        "cal_status": None,
         "mode": None,
         "selfcheck": None,
         "current_position": None,
         "start_position": None,
+        "task": None,
         "time": None,
     }
 
@@ -779,6 +780,7 @@ class Telemetry:
 
     def update_telemetry_fast(self):
         self.start_position = self.get_start_position()
+        self.task = task_manager.get_current_task()
         try:
             self.ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
             if self.ros_telemetry.connected:
@@ -802,8 +804,8 @@ class Telemetry:
         self.animation_id = animation.get_id()
         self.git_version = self.get_git_version()
         try:
-            self.calibration_status = get_calibration_status()
-            self.system_status = get_sys_status()
+            self.cal_status = get_calibration_status()
+            self.fcu_status = get_sys_status()
             self.battery = self.get_battery(self.ros_telemetry)
         except rospy.ServiceException:
             rospy.logdebug("Some service is unavailable")
@@ -825,8 +827,8 @@ class Telemetry:
 
     def reset_telemetry_values(self):
         self.battery = float('nan'), float('nan')
-        self.calibration_status = 'NO_FCU'
-        self.system_status = 'NO_FCU'
+        self.cal_status = 'NO_FCU'
+        self.fcu_status = 'NO_FCU'
         self.mode = 'NO_FCU'
         self.selfcheck = ['NO_FCU']
         self.current_position = 'NO_POS'
@@ -900,15 +902,14 @@ class Telemetry:
             if client.active_client.config.telemetry_transmit and client.active_client.connected:
                 self.transmit_message()
 
-            if client.active_client.config.telemetry_log_resources:
-                self.log_cpu_and_memory()
-
             rate.sleep()
 
     def _slow_update_loop(self):
         rate = rospy.Rate(1)
         while not rospy.is_shutdown():
             self.update_telemetry_slow()
+            if client.active_client.config.telemetry_log_resources:
+                self.log_cpu_and_memory()
             rate.sleep()
 
     def start_loop(self):

Drone/tasking_lib.py

@@ -89,6 +89,21 @@ class TaskManager(object):
 
     def get_last_task_name(self):
         return self._last_task
+    
+    def get_current_task(self):
+        try:
+            start_time, priority, count, task = self.task_queue[0]
+        except IndexError as e:
+            logger.debug("Task queue checking exception: {}".format(e))
+            return "No task"
+        else:
+            if self._running_event.is_set():
+                time_to_start = start_time - time.time()
+                if time_to_start > 0:
+                    return "{} in {:.1f} s".format(task.func.__name__,time_to_start)
+                return task.func.__name__
+            else:
+                return "paused"
 
     def start(self):
         #print("Task manager is started")
@@ -138,7 +153,7 @@ class TaskManager(object):
         with self._task_queue_lock:
             try:
                 start_time, priority, count, task = self.task_queue[0]
-            except Exception as e:
+            except IndexError as e:
                 logger.debug("Task queue checking exception: {}".format(e))
                 self._timeshift = 0.0
                 self._wait_interrupt_event.clear()
@@ -180,7 +195,7 @@ class TaskManager(object):
         if time.time() > start_time:
             try:
                 start_time_n, priority_n, count_n, task_n = self.task_queue[0]
-            except Exception as e:
+            except IndexError as e:
                 logger.warning("Timeout checking exception: {}".format(e))
                 self._timeshift = 0.0
                 self._wait_interrupt_event.clear()