Toverumar/add write param file

cflib/crtp/pcap.py

@@ -1,6 +1,6 @@
 import os
 import struct
-from datetime import datetime
+import time
 from enum import IntEnum
 
 
@@ -139,6 +139,6 @@ def _assemble_record(self, link_type, receive, address, channel, devid, crtp_pac
         )
 
     def _pcap_header(self, len):
-        ts = datetime.now()
-
-        return struct.pack('<LLLL', ts.second, ts.microsecond, len, len)
+        seconds = time.time()
+        u_sec = int((seconds % 1)*1000000)
+        return struct.pack('<LLLL', int(seconds), u_sec, len, len)

cflib/utils/param_file_helper.py

@@ -0,0 +1,57 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2024 Bitcraze AB
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+from threading import Event
+
+from cflib.crazyflie import Crazyflie
+from cflib.localization.param_io import ParamFileManager
+
+
+class ParamFileHelper:
+    '''ParamFileHelper is a helper to synchonously write multiple paramteters
+    from a file and store them in persistent memory'''
+
+    def __init__(self, crazyflie):
+        if isinstance(crazyflie, Crazyflie):
+            self._cf = crazyflie
+            self.persistent_sema = None
+            self.success = False
+        else:
+            raise TypeError('ParamFileHelper only takes a Crazyflie Object')
+
+    def _persistent_stored_callback(self, complete_name, success):
+        self.success = success
+        if not success:
+            print(f'Persistent params: failed to store {complete_name}!')
+        else:
+            print(f'Persistent params: stored {complete_name}!')
+        self.persistent_sema.set()
+
+    def store_params_from_file(self, filename):
+        params = ParamFileManager().read(filename)
+        for param, state in params.items():
+            self.persistent_sema = Event()
+            self._cf.param.set_value(param, state.stored_value)
+            self._cf.param.persistent_store(param, self._persistent_stored_callback)
+            self.persistent_sema.wait()
+            if not self.success:
+                break
+        return self.success

examples/parameters/persistent_params_from_file.py

@@ -0,0 +1,60 @@
+# -*- coding: utf-8 -*-
+#
+# ,---------,       ____  _ __
+# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+# Copyright (C) 2024 Bitcraze AB
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, in version 3.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <http://www.gnu.org/licenses/>.
+"""
+Example to show how to write several persistent parameters from a yaml file.
+The params in the file should be formatted like this;
+
+params:
+  activeMarker.back:
+    default_value: 3
+    is_stored: true
+    stored_value: 30
+type: persistent_param_state
+version: '1'
+"""
+import argparse
+import logging
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.utils import uri_helper
+from cflib.utils.param_file_helper import ParamFileHelper
+
+
+uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument('-f', '--file', type=str, help='The yaml file containing the arguments. ')
+    args = parser.parse_args()
+
+    cflib.crtp.init_drivers()
+
+    cf = Crazyflie(rw_cache='./cache')
+    with SyncCrazyflie(uri, cf=cf) as scf:
+        writer = ParamFileHelper(scf.cf)
+        writer.store_params_from_file(args.file)

test/utils/fixtures/five_params.yaml

@@ -0,0 +1,23 @@
+params:
+  activeMarker.back:
+    default_value: 3
+    is_stored: true
+    stored_value: 10
+  activeMarker.front:
+    default_value: 3
+    is_stored: true
+    stored_value: 10
+  activeMarker.left:
+    default_value: 3
+    is_stored: true
+    stored_value: 10
+  cppm.angPitch:
+    default_value: 50.0
+    is_stored: true
+    stored_value: 55.0
+  ctrlMel.i_range_z:
+    default_value: 0.4000000059604645
+    is_stored: true
+    stored_value: 0.44999998807907104
+type: persistent_param_state
+version: '1'

test/utils/fixtures/single_param.yaml

@@ -0,0 +1,7 @@
+params:
+  activeMarker.back:
+    default_value: 3
+    is_stored: true
+    stored_value: 10
+type: persistent_param_state
+version: '1'

test/utils/test_param_file_helper.py

@@ -0,0 +1,120 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2018 Bitcraze AB
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+import unittest
+from threading import Event
+from unittest.mock import MagicMock
+from unittest.mock import patch
+
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.utils.param_file_helper import ParamFileHelper
+
+
+class ParamFileHelperTests(unittest.TestCase):
+
+    def setUp(self):
+        self.cf_mock = MagicMock(spec=Crazyflie)
+        self.helper = ParamFileHelper(self.cf_mock)
+
+    def test_ParamFileHelper_SyncCrazyflieAsParam_ThrowsException(self):
+        cf_mock = MagicMock(spec=SyncCrazyflie)
+        helper = None
+        try:
+            helper = ParamFileHelper(cf_mock)
+        except Exception:
+            self.assertIsNone(helper)
+        else:
+            self.fail('Expect exception')
+
+    def test_ParamFileHelper_Crazyflie_Object(self):
+        helper = ParamFileHelper(self.cf_mock)
+        self.assertIsNotNone(helper)
+
+    @patch('cflib.crazyflie.Param')
+    def test_ParamFileHelper_writesAndStoresParamFromFileToCrazyflie(self, mock_Param):
+        # Setup
+        cf_mock = MagicMock(spec=Crazyflie)
+        cf_mock.param = mock_Param
+        helper = ParamFileHelper(cf_mock)
+        # Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world
+
+        def mock_wait(self, timeout=None):
+            helper._persistent_stored_callback('activeMarker.back', True)
+            return
+
+        with patch.object(Event, 'wait', new=mock_wait):
+            self.assertTrue(helper.store_params_from_file('test/utils/fixtures/single_param.yaml'))
+            mock_Param.set_value.assert_called_once_with('activeMarker.back', 10)
+            mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback)
+
+    @patch('cflib.crazyflie.Param')
+    def test_ParamFileHelper_writesParamAndFailsToSetPersistantShouldReturnFalse(self, mock_Param):
+        # Setup
+        cf_mock = MagicMock(spec=Crazyflie)
+        cf_mock.param = mock_Param
+        helper = ParamFileHelper(cf_mock)
+        # Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world
+
+        def mock_wait(self, timeout=None):
+            helper._persistent_stored_callback('activeMarker.back', False)
+            return
+
+        with patch.object(Event, 'wait', new=mock_wait):
+            self.assertFalse(helper.store_params_from_file('test/utils/fixtures/single_param.yaml'))
+            mock_Param.set_value.assert_called_once_with('activeMarker.back', 10)
+            mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback)
+
+    @patch('cflib.crazyflie.Param')
+    def test_ParamFileHelper_TryWriteSeveralParamsPersistantShouldBreakAndReturnFalse(self, mock_Param):
+        # Setup
+        cf_mock = MagicMock(spec=Crazyflie)
+        cf_mock.param = mock_Param
+        helper = ParamFileHelper(cf_mock)
+        # Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world
+
+        def mock_wait(self, timeout=None):
+            helper._persistent_stored_callback('activeMarker.back', False)
+            return
+
+        with patch.object(Event, 'wait', new=mock_wait):
+            # Test and assert
+            self.assertFalse(helper.store_params_from_file('test/utils/fixtures/five_params.yaml'))
+            # Assert it breaks directly by checking number of calls
+            mock_Param.set_value.assert_called_once_with('activeMarker.back', 10)
+            mock_Param.persistent_store.assert_called_once_with('activeMarker.back', helper._persistent_stored_callback)
+
+    @patch('cflib.crazyflie.Param')
+    def test_ParamFileHelper_writesAndStoresAllParamsFromFileToCrazyflie(self, mock_Param):
+        # Setup
+        cf_mock = MagicMock(spec=Crazyflie)
+        cf_mock.param = mock_Param
+        helper = ParamFileHelper(cf_mock)
+        # Mock blocking wait and call callback instead. This lets the flow work as it would in the asynch world
+
+        def mock_wait(self, timeout=None):
+            helper._persistent_stored_callback('something', True)
+            return
+        with patch.object(Event, 'wait', new=mock_wait):
+            # Test and  Assert
+            self.assertTrue(helper.store_params_from_file('test/utils/fixtures/five_params.yaml'))
+            self.assertEquals(5, len(mock_Param.set_value.mock_calls))
+            self.assertEquals(5, len(mock_Param.persistent_store.mock_calls))