Crazyflie 集群无人机

# cflib 0.1.34 in Python, connected with SyncCrazyflie: cf = scf.cf
cf.param.set_value('stabilizer.estimator', 2)     # extended Kalman: either deck forces it anyway
cf.param.set_value('stabilizer.controller', 1)    # PID (0, auto select, picks PID too)
cf.param.set_value('lighthouse.systemType', 2)    # Lighthouse V2 base stations
cf.param.set_value('lighthouse.method', 1)        # sweep angles straight into the Kalman filter (default)
cf.param.set_value('posCtlPid.rLimit', 20)        # most roll the position controller asks for, degrees
cf.param.set_value('posCtlPid.pLimit', 20)        # and pitch
cf.param.set_value('posCtlPid.xVelMax', 1)        # m/s it may ask for along x
cf.param.set_value('posCtlPid.yVelMax', 1)        # along y
cf.param.set_value('posCtlPid.zVelMax', 1)        # up and down
cf.param.set_value('supervisor.tmblChckEn', 1)    # motors off if it tumbles
cf.param.set_value('pm.lowVoltage', 3.2)          # V: LED1 goes solid red
cf.param.set_value('pm.criticalLowVoltage', 3.0)  # V

# Read-only: 1 means the deck was found at power-on. get_value returns a string
print(cf.param.get_value('deck.bcFlow2'))
print(cf.param.get_value('deck.bcLighthouse4'))
print(cf.param.get_value('stabilizer.estimator'))

# PROPS OFF. The override reaches the motors only while the supervisor lets them run (upright and armed)
cf.param.set_value('motorPowerSet.enable', 1)
cf.param.set_value('motorPowerSet.m1', 10000)     # M1, front right: CCW seen from above
cf.param.set_value('motorPowerSet.m1', 0)
cf.param.set_value('motorPowerSet.m2', 10000)     # M2, rear right: CW
cf.param.set_value('motorPowerSet.m2', 0)
cf.param.set_value('motorPowerSet.m3', 10000)     # M3, rear left: CCW
cf.param.set_value('motorPowerSet.m3', 0)
cf.param.set_value('motorPowerSet.m4', 10000)     # M4, front left: CW
cf.param.set_value('motorPowerSet.m4', 0)
cf.param.set_value('motorPowerSet.enable', 0)     # back to the stabilizer

# Spins each motor in turn and measures the vibration: a chipped prop or a worn motor stands out
cf.param.set_value('health.startPropTest', 1)

# led.bitmask: bit 7 takes the LEDs over from the status blinks, bits 0-5 choose which are lit
cf.param.set_value('led.bitmask', 138)    # 128 + 8 + 2: both green
cf.param.set_value('led.bitmask', 148)    # 128 + 16 + 4: both red
cf.param.set_value('led.bitmask', 129)    # 128 + 1: the left blue
cf.param.set_value('led.bitmask', 0)      # bit 7 clear: the status blinks are back

import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander
​
URI = 'radio://0/80/2M/E7E7E7E701'
​
cflib.crtp.init_drivers()
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
    if scf.cf.param.get_value('deck.bcFlow2') != '1':
        raise SystemExit('no Flow deck: nothing would hold the position')
    scf.cf.supervisor.send_arming_request(True)
    time.sleep(1.0)
    with MotionCommander(scf, default_height=1.0) as mc:   # takes off to 1 m here
        mc.forward(0.6, velocity=0.4)                     # metres and m/s, measured by the Flow deck
        mc.turn_left(90)                                  # degrees, 72 deg/s
        mc.forward(0.6, velocity=0.4)
        mc.back(0.6)                                      # 0.2 m/s when none is given
    # leaving the with block lands it

# Inside the same with SyncCrazyflie(...) as scf: block, armed. Setpoints at 10 Hz: miss them for
# 0.5 s and it levels, for 2 s and the motors stop
cf = scf.cf
for _ in range(30):                                   # 3 s: up to 0.8 m, level
    cf.commander.send_zdistance_setpoint(0, 0, 0, 0.8)  # roll, pitch (deg), yaw rate (deg/s), height (m)
    time.sleep(0.1)
for _ in range(20):                                   # 2 s leaning 5 degrees: it drifts, the height holds
    cf.commander.send_zdistance_setpoint(0, 5, 0, 0.8)
    time.sleep(0.1)
for i in range(30):                                   # 3 s back down
    cf.commander.send_zdistance_setpoint(0, 0, 0, 0.8 - i * 0.025)
    time.sleep(0.1)
cf.commander.send_stop_setpoint()
cf.commander.send_notify_setpoint_stop()

# Angles and raw thrust: nothing holds the height or the position. Low, over a net
cf = scf.cf
cf.commander.send_setpoint(0, 0, 0, 0)          # thrust 0 first: until then the firmware locks the thrust
for i in range(20):                             # 2 s: thrust up from 20000 towards hover (60000 is full)
    cf.commander.send_setpoint(0, 0, 0, 20000 + i * 800)
    time.sleep(0.1)
cf.commander.send_setpoint(0, 0, 0, 0)
cf.commander.send_stop_setpoint()

import time
import cflib.crtp
from cflib.crazyflie.swarm import CachedCfFactory, Swarm
​
# four drones on one Crazyradio: one channel, one address each
URIS = [
    'radio://0/80/2M/E7E7E7E701',
    'radio://0/80/2M/E7E7E7E702',
    'radio://0/80/2M/E7E7E7E703',
    'radio://0/80/2M/E7E7E7E704',
]
# each flies the same star from its own corner of a 0.6 m square, so none sits in another one's downwash
SLOT = {URIS[0]: [0.0, 0.0], URIS[1]: [0.6, 0.0], URIS[2]: [0.0, -0.6], URIS[3]: [0.6, -0.6]}
# star points in the Lighthouse frame (m) and the LEDs at each: 138 green, 148 red, 129 blue
STAR = [(4.0, 5.1, 1.5, 138), (3.12, 2.39, 1.2, 148), (5.43, 4.06, 1.5, 138),
        (2.57, 4.06, 1.5, 148), (4.88, 2.39, 1.2, 138), (4.0, 5.1, 1.5, 129)]
LEG = 4.0                                         # s per leg: about 0.8 m/s along a 3 m side
​
​
def arm(scf):
    scf.cf.supervisor.send_arming_request(True)
    time.sleep(1.0)
​
​
def show(scf, dx, dy):
    cf = scf.cf
    hl = cf.high_level_commander
    hl.takeoff(1.0, 2.0)                          # to 1 m in 2 s, from wherever it stands
    time.sleep(2.5)
    for x, y, z, leds in STAR:
        hl.go_to(x + dx, y + dy, z, 0.0, LEG)     # yaw in radians; it flies on even if the link drops
        cf.param.set_value('led.bitmask', leds)
        time.sleep(LEG)
    cf.param.set_value('led.bitmask', 0)
    hl.go_to(2.0 + dx, 1.6 + dy, 1.0, 0.0, 5.0)   # back over its slot on the pad
    time.sleep(5.0)
    hl.land(0.0, 2.0)
    time.sleep(2.5)
    hl.stop()
​
​
if __name__ == '__main__':
    cflib.crtp.init_drivers()
    with Swarm(URIS, factory=CachedCfFactory(rw_cache='./cache')) as swarm:
        swarm.reset_estimators()                  # waits until every Kalman filter has settled
        swarm.parallel_safe(arm)
        swarm.parallel_safe(show, args_dict=SLOT)