From 3f8fcec988a7a27044812840c134320bb181d4ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ram=C3=B3n=20Roche?= Date: Fri, 26 Feb 2016 14:38:38 -0800 Subject: [PATCH 1/5] add monotonic lib --- requirements.txt | 1 + setup.py | 1 + 2 files changed, 2 insertions(+) diff --git a/requirements.txt b/requirements.txt index 3257caf2b..c0deaab1d 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,3 +6,4 @@ psutil>=3.0.0 mock>=1.3.0 six>=1.9.0 dronekit-sitl>=3.0,<=3.99999 +monotonic==0.6 diff --git a/setup.py b/setup.py index 7bc24f124..4972966db 100644 --- a/setup.py +++ b/setup.py @@ -12,6 +12,7 @@ author='3D Robotics', install_requires=[ 'pymavlink>=1.1.62', + 'monotonic==0.6' ], author_email='tim@3drobotics.com, kevinh@geeksville.com', classifiers=[ From 5d9edb31d6c3120137a8e8bf41fb84a2d712d9d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ram=C3=B3n=20Roche?= Date: Fri, 26 Feb 2016 14:39:34 -0800 Subject: [PATCH 2/5] use monotonic clock on heartbeat checks --- dronekit/__init__.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 5a0d38fc4..476fe34a9 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -31,6 +31,7 @@ """ from __future__ import print_function +import monotonic import time import socket import sys @@ -1296,18 +1297,18 @@ def listener(self, name, msg): @handler.forward_loop def listener(_): # Send 1 heartbeat per second - if time.time() - self._heartbeat_lastsent > 1: + if monotonic.monotonic() - self._heartbeat_lastsent > 1: self._master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0) - self._heartbeat_lastsent = time.time() + self._heartbeat_lastsent = monotonic.monotonic() # Timeouts. if self._heartbeat_started: - if self._heartbeat_error and self._heartbeat_error > 0 and time.time( + if self._heartbeat_error and self._heartbeat_error > 0 and monotonic.monotonic( ) - self._heartbeat_lastreceived > self._heartbeat_error: raise APIException('No heartbeat in %s seconds, aborting.' % self._heartbeat_error) - elif time.time() - self._heartbeat_lastreceived > self._heartbeat_warning: + elif monotonic.monotonic() - self._heartbeat_lastreceived > self._heartbeat_warning: if self._heartbeat_timeout == False: errprinter('>>> Link timeout, no heartbeat in last %s seconds' % self._heartbeat_warning) @@ -1316,7 +1317,7 @@ def listener(_): @self.on_message(['HEARTBEAT']) def listener(self, name, msg): self._heartbeat_system = msg.get_srcSystem() - self._heartbeat_lastreceived = time.time() + self._heartbeat_lastreceived = monotonic.monotonic() if self._heartbeat_timeout: errprinter('>>> ...link restored.') self._heartbeat_timeout = False @@ -1326,7 +1327,7 @@ def listener(self, name, msg): @handler.forward_loop def listener(_): if self._heartbeat_lastreceived: - self._last_heartbeat = time.time() - self._heartbeat_lastreceived + self._last_heartbeat = monotonic.monotonic() - self._heartbeat_lastreceived self.notify_attribute_listeners('last_heartbeat', self.last_heartbeat) @property @@ -2044,7 +2045,7 @@ def initialize(self, rate=4, heartbeat_timeout=30): self._handler.start() # Start heartbeat polling. - start = time.time() + start = monotonic.monotonic() self._heartbeat_error = heartbeat_timeout or 0 self._heartbeat_started = True self._heartbeat_lastreceived = start From 66b3f6ba52410e7c7474e92dc1694e11e1ad457f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ram=C3=B3n=20Roche?= Date: Fri, 26 Feb 2016 14:43:29 -0800 Subject: [PATCH 3/5] use monotonic clock on parameter actions --- dronekit/__init__.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 476fe34a9..511def394 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1232,7 +1232,7 @@ def listener(self, name, msg): self._params_loaded = False self._params_start = False self._params_map = {} - self._params_last = time.time() # Last new param. + self._params_last = monotonic.monotonic() # Last new param. self._params_duration = start_duration self._parameters = Parameters(self) @@ -1246,7 +1246,7 @@ def listener(_): self._params_loaded = True self.notify_attribute_listeners('parameters', self.parameters) - if not self._params_loaded and time.time() - self._params_last > self._params_duration: + if not self._params_loaded and monotonic.monotonic() - self._params_last > self._params_duration: c = 0 for i, v in enumerate(self._params_set): if v == None: @@ -1255,7 +1255,7 @@ def listener(_): if c > 50: break self._params_duration = repeat_duration - self._params_last = time.time() + self._params_last = monotonic.monotonic() @self.on_message(['PARAM_VALUE']) def listener(self, name, msg): @@ -1273,7 +1273,7 @@ def listener(self, name, msg): try: if msg.param_index < msg.param_count and msg: if self._params_set[msg.param_index] == None: - self._params_last = time.time() + self._params_last = monotonic.monotonic() self._params_duration = start_duration self._params_set[msg.param_index] = msg self._params_map[msg.param_id] = msg.param_value @@ -2386,11 +2386,11 @@ def set(self, name, value, retries=3, wait_ready=False): remaining = retries while True: self._vehicle._master.param_set_send(name, value) - tstart = time.time() + tstart = monotonic.monotonic() if remaining == 0: break remaining -= 1 - while time.time() - tstart < 1: + while monotonic.monotonic() - tstart < 1: if name in self._vehicle._params_map and self._vehicle._params_map[name] == value: return True time.sleep(0.1) From 7761a53c6c7ecf2d8098f21240785dff380e8e34 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ram=C3=B3n=20Roche?= Date: Fri, 26 Feb 2016 14:47:32 -0800 Subject: [PATCH 4/5] use monotonic clock on wait_ready for attributes --- dronekit/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 511def394..1105297a9 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -2133,10 +2133,10 @@ def wait_ready(self, *types, **kwargs): # Wait for these attributes to have been set. await = set(types) - start = time.time() + start = monotonic.monotonic() while not await.issubset(self._ready_attrs): time.sleep(0.1) - if time.time() - start > timeout: + if monotonic.monotonic() - start > timeout: if raise_exception: raise APIException('wait_ready experienced a timeout after %s seconds.' % timeout) From 6776725d83e4a092ca22fe02220ae6a399c9e279 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ram=C3=B3n=20Roche?= Date: Mon, 29 Feb 2016 09:59:19 -0800 Subject: [PATCH 5/5] update version check for monotonic package we can modify this if we need to later --- requirements.txt | 2 +- setup.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/requirements.txt b/requirements.txt index c0deaab1d..7dde864c7 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,4 +6,4 @@ psutil>=3.0.0 mock>=1.3.0 six>=1.9.0 dronekit-sitl>=3.0,<=3.99999 -monotonic==0.6 +monotonic<1.0 diff --git a/setup.py b/setup.py index 4972966db..b34ff24c4 100644 --- a/setup.py +++ b/setup.py @@ -12,7 +12,7 @@ author='3D Robotics', install_requires=[ 'pymavlink>=1.1.62', - 'monotonic==0.6' + 'monotonic<1.0' ], author_email='tim@3drobotics.com, kevinh@geeksville.com', classifiers=[