diff --git a/examples/lps_reboot_to_bootloader.py b/examples/lps_reboot_to_bootloader.py new file mode 100644 index 0000000000000000000000000000000000000000..3dd4f17be7e0ae5187bb8b0d26cc0fb6f7170f35 --- /dev/null +++ b/examples/lps_reboot_to_bootloader.py @@ -0,0 +1,108 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2017 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# 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, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +""" +Simple example that connects to the first Crazyflie found, and then sends the +reboot signals to 6 anchors ID from 0 to 5. The reset signals is sent +10 times in a row to make sure all anchors are reset to bootloader. +""" +import logging +import time +from threading import Thread + +import cflib +from cflib.crazyflie import Crazyflie +from lpslib.lopoanchor import LoPoAnchor + +logging.basicConfig(level=logging.ERROR) + + +class LpsRebootToBootloader: + """Example that connects to a Crazyflie and ramps the motors up/down and + the disconnects""" + + def __init__(self, link_uri): + """ Initialize and run the example with the specified link_uri """ + + self._cf = Crazyflie() + + self._cf.connected.add_callback(self._connected) + self._cf.disconnected.add_callback(self._disconnected) + self._cf.connection_failed.add_callback(self._connection_failed) + self._cf.connection_lost.add_callback(self._connection_lost) + + self._cf.open_link(link_uri) + + print('Connecting to %s' % link_uri) + + def _connected(self, link_uri): + """ This callback is called form the Crazyflie API when a Crazyflie + has been connected and the TOCs have been downloaded.""" + + # Start a separate thread to do the motor test. + # Do not hijack the calling thread! + Thread(target=self._reboot_thread).start() + + def _connection_failed(self, link_uri, msg): + """Callback when connection initial connection fails (i.e no Crazyflie + at the specified address)""" + print('Connection to %s failed: %s' % (link_uri, msg)) + + def _connection_lost(self, link_uri, msg): + """Callback when disconnected after a connection has been made (i.e + Crazyflie moves out of range)""" + print('Connection to %s lost: %s' % (link_uri, msg)) + + def _disconnected(self, link_uri): + """Callback when the Crazyflie is disconnected (called in all cases)""" + print('Disconnected from %s' % link_uri) + + def _reboot_thread(self): + + anchors = LoPoAnchor(self._cf) + + print('Sending reboot signal to all anchors 10 times in a row ...') + for retry in range(10): + for anchor_id in range(6): + anchors.reboot(anchor_id, anchors.REBOOT_TO_BOOTLOADER) + time.sleep(0.1) + + self._cf.close_link() + + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + # Scan for Crazyflies and use the first one found + print('Scanning interfaces for Crazyflies...') + available = cflib.crtp.scan_interfaces() + print('Crazyflies found:') + for i in available: + print(i[0]) + + if len(available) > 0: + le = LpsRebootToBootloader(available[0][0]) + else: + print('No Crazyflies found, cannot run example') diff --git a/lpslib/lopoanchor.py b/lpslib/lopoanchor.py index 975225b1248f52714fec339fea79c82a78f23f5f..aa7fb4da324a8c19613460ad09d1c842fded0d26 100644 --- a/lpslib/lopoanchor.py +++ b/lpslib/lopoanchor.py @@ -29,6 +29,10 @@ import struct class LoPoAnchor(): LPP_TYPE_POSITION = 1 + LPP_TYPE_REBOOT = 2 + + REBOOT_TO_BOOTLOADER = 0 + REBOOT_TO_FIRMWARE = 1 def __init__(self, crazyflie): """ @@ -49,3 +53,7 @@ class LoPoAnchor(): data = struct.pack('<Bfff', LoPoAnchor.LPP_TYPE_POSITION, x, y, z) self.crazyflie.loc.send_short_lpp_packet(anchor_id, data) + + def reboot(self, anchor_id, mode): + data = struct.pack('<BB', LoPoAnchor.LPP_TYPE_REBOOT, mode) + self.crazyflie.loc.send_short_lpp_packet(anchor_id, data)