#154 Updates to power switch

cflib/utils/power_switch.py

@@ -23,14 +23,17 @@
 This class is used to turn the power of the Crazyflie on and off via
 a Crazyradio.
 """
-import sys
 import time
 
 import cflib.crtp
 from cflib.drivers.crazyradio import Crazyradio
 
 
 class PowerSwitch:
+    BOOTLOADER_CMD_ALLOFF = 0x01
+    BOOTLOADER_CMD_SYSOFF = 0x02
+    BOOTLOADER_CMD_SYSON = 0x03
+
     def __init__(self, uri):
         self.uri = uri
         uri_parts = cflib.crtp.RadioDriver.parse_uri(uri)
@@ -42,17 +45,17 @@ def __init__(self, uri):
     def platform_power_down(self):
         """ Power down the platform, both NRF and STM MCUs.
             Same as turning off the platform with the power button."""
-        self._send((0xf3, 0xfe, 0x01))
+        self._send(self.BOOTLOADER_CMD_ALLOFF)
 
     def stm_power_down(self):
         """ Power down the STM MCU, the NRF will still be powered and handle
             basic radio communication. The STM can be restarted again remotely.
             Note: the power to expansion decks is also turned off. """
-        self._send((0xf3, 0xfe, 0x02))
+        self._send(self.BOOTLOADER_CMD_SYSOFF)
 
     def stm_power_up(self):
         """ Power up (boot) the STM MCU and decks."""
-        self._send((0xf3, 0xfe, 0x03))
+        self._send(self.BOOTLOADER_CMD_SYSON)
 
     def stm_power_cycle(self):
         """ Restart the STM MCU by powering it off and on.
@@ -61,7 +64,9 @@ def stm_power_cycle(self):
         time.sleep(1)
         self.stm_power_up()
 
-    def _send(self, packet):
+    def _send(self, cmd):
+        packet = (0xf3, 0xfe, cmd)
+
         cr = Crazyradio(devid=self.devid)
         cr.set_channel(self.channel)
         cr.set_data_rate(self.datarate)
@@ -80,5 +85,5 @@ def _send(self, packet):
         cr.close()
 
         if not success:
-            print('Failed to connect to Crazyflie at', self.uri)
-            sys.exit(-1)
+            raise Exception(
+                'Failed to connect to Crazyflie at {}'.format(self.uri))

sys_test/swarm_test_rig/rig_support.py

@@ -23,8 +23,7 @@
 #  MA  02110-1301, USA.
 import time
 
-from cflib.crtp import RadioDriver
-from cflib.drivers.crazyradio import Crazyradio
+from cflib.utils.power_switch import PowerSwitch
 
 
 class RigSupport:
@@ -43,35 +42,15 @@ def __init__(self):
         ]
 
     def restart_devices(self, uris):
-        def send_packets(uris, value, description):
-            for uri in uris:
-                devid, channel, datarate, address = RadioDriver.parse_uri(uri)
-                radio.set_channel(channel)
-                radio.set_data_rate(datarate)
-                radio.set_address(address)
-
-                received_packet = False
-                for i in range(10):
-                    result = radio.send_packet((0xf3, 0xfe, value))
-                    if result.ack is True:
-                        received_packet = True
-                        # if i > 0:
-                        #     print('Lost packets', i, uri)
-                        break
-
-                if not received_packet:
-                    raise Exception('Failed to turn device {}, for {}'.
-                                    format(description, uri))
-
         print('Restarting devices')
 
-        BOOTLOADER_CMD_SYSOFF = 0x02
-        BOOTLOADER_CMD_SYSON = 0x03
+        for uri in uris:
+            PowerSwitch(uri).stm_power_down()
+
+        time.sleep(1)
 
-        radio = Crazyradio()
-        send_packets(uris, BOOTLOADER_CMD_SYSOFF, 'off')
-        send_packets(uris, BOOTLOADER_CMD_SYSON, 'on')
+        for uri in uris:
+            PowerSwitch(uri).stm_power_up()
 
         # Wait for devices to boot
         time.sleep(8)
-        radio.close()