"""Interface to CodeAIR's Flight Control Computer
See also `flight_lib/flight_lib`
* fly = `MotionCommander`
* fly_high = `HighLevelCommander`
* cf_commander = `Commander`
High-level flying and sensors:
------------------------------
**Basic Hover:**
>>> fly.take_off() # Ascend to default altitude at default speed
>>> fly.steady(4) # Hold for 4 seconds
>>> fly.land() # Descend at default speed
Low-level control and flight parameter access:
----------------------------------------------
**Watch Flight Controller console output on bootup:**
>>> crtp.console_stdout(True)
>>> reset_controller()
**Get sensor data:**
>>> get_data(RANGERS)
**Set and Get parameters (e.g., m1 speed parameter):**
>>> set_param('motorPowerSet.m1', 5000)
>>> get_param('motorPowerSet.m1')
"""
import board
import time
import stm32_boot
import digitalio
import flightdb
import crtp
from flightdb import read_crtp, CRTP_PORT_PLATFORM
from utils import BytesIO_ReadOnly
from cflib import commander, motion_commander, crtpstack, high_level_commander
[docs]def load_fw(file=None, prog=None):
"""Load new firmware into the flight controller. By default uses Crazyflie controller firmware.
Optional progress callback.
"""
if file is None:
print("Extracting default firmware image.")
from zlib import decompress
from stm_fw import stm_fw_gz
bin = decompress(stm_fw_gz(), 31)
file = BytesIO_ReadOnly(bin)
elif type(file) == str:
print("Updating STM32 firmware with file: ", file)
# Release UART so we can access bootloader
crtp.deinit()
st = stm32_boot.STM32boot(prog)
stat = st.enter_boot()
if not stat:
print("Error: Failed to enter bootloader.")
return False
ver = st.get_version() # returns 0x31 => Version 3.1
print("Bootloader v%s" % '.'.join(hex(ver)[2:]))
print("Erasing...")
stat = st.mass_erase() # returns True after ~16 seconds
if not stat:
print("Error: Erase failed.")
return False
print("Writing...")
stat = st.write_flash(file) # returns True after ~34 seconds
if not stat:
print("Error: Write failed.")
return False
st.exit_boot() # exits BOOT mode and resets STM32
st.deinit()
crtp.init() # restore CRTP control of UART
print("Done!")
return True
[docs]def reset_controller():
"""Reset the flight controller CPU"""
STM_NRST = board.D33
nrst = digitalio.DigitalInOut(STM_NRST)
nrst.direction = digitalio.Direction.OUTPUT
nrst.value = 0
time.sleep(0.1)
nrst.value = 1
time.sleep(0.1)
nrst.deinit()
# Instantiate database singletons
log_manager = flightdb.LogManager()
param_manager = flightdb.ParamManager(log_manager)
# Data item tuples, pass to 'get_data()'
# Complete list here: https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/logs/
GYRO = ('gyro.xRaw', 'gyro.yRaw', 'gyro.zRaw',) #:
RANGERS = ('range.front', 'range.up', 'range.zrange',) #:
FLOW = ('motion.deltaX', 'motion.deltaY',) #:
ACCEL = ('acc.x', 'acc.y', 'acc.z',) #:
MAG = ('mag.x', 'mag.y', 'mag.z',) #:
BAROMETER = ('baro.asl', 'baro.temp', 'baro.pressure',) #:
STATE_EST_POS = ('stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z', 'stateEstimate.yaw') #:
STATE_EST_VEL = ('stateEstimate.vx', 'stateEstimate.vy', 'stateEstimate.vz',) #:
current_log_items = None # Active tuple of log items
[docs]def get_data(log_items):
"""Get a sensor value tuple from the Flight Controller data log.
This will register for periodic logging of the given log items.
Pass 'None' to stop data logging packets from Flight Controller.
Args:
log_items (tuple): Tuple of str names `Log Values <https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/logs/>`_.
Provided reference tuples: `GYRO`, `RANGERS`, `FLOW`, `ACCEL`, `MAG`, `BAROMETER`
"""
global current_log_items
if log_items is None:
log_manager.enable(False)
current_log_items = None
return None
if current_log_items != log_items:
log_manager.register_items(log_items)
log_manager.enable(True)
current_log_items = log_items
crtp.log_read() # Flush
log_manager.fetch_log_data()
return tuple(log_manager.cache[item] for item in current_log_items)
[docs]def get_param(name):
"""Get a single parameter value from Flight Controller.
Note: these differ from log data.
Args:
name (str): See `Param Names <https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/params/>`_
"""
return param_manager.get(name)
[docs]def set_param(name, value):
"""Set a single parameter in Flight Controller"""
return param_manager.set(name, value)
[docs]def version():
"""Get version of flight controller firmware
"""
crtp.flush()
crtp.send(CRTP_PORT_PLATFORM, 1,'\x02')
dat = read_crtp(CRTP_PORT_PLATFORM, 1)
if dat:
return dat[1:].decode('utf-8')
else:
return "timeout"
[docs]def motor_test(do_run):
"""Run the motors at low speed."""
if do_run:
param_manager.set("motorPowerSet.enable", 1)
param_manager.set("motorPowerSet.m1", 5000)
param_manager.set("motorPowerSet.m2", 5000)
param_manager.set("motorPowerSet.m3", 5000)
param_manager.set("motorPowerSet.m4", 5000)
else:
param_manager.set("motorPowerSet.enable", 0)
#---- Initialize cflib flight control ----
_cf_driver = crtpstack.CRTPPacket()
cf_commander = commander.Commander(_cf_driver)
fly = motion_commander.MotionCommander(cf_commander, set_param)
fly_high = high_level_commander.HighLevelCommander()