rpy = np.zeros(3)
sp_rpy = np.zeros(3)
cmd = np.array([1000, 1000, 1500, 1500])
throttle = 1100
tt = t = time.time()
running = True
while running:
t = time.time()
dt = t - tt
tt = t
# calculate the error from the set_point
rpy[[1, 0, 2]] = imu.read()
err_rpy = sp_rpy - rpy
# run each of the pid controllers
cmd_rpy = [pid.step(e, dt) for pid, e in zip(pids, err_rpy)]
# copy the rates into the servo array
cmd[0] = throttle #+ cmd_rpy[2]
cmd[1] = throttle #- cmd_rpy[2]
cmd[2] = 1555 + int(cmd_rpy[0])
cmd[3] = 1460 + int(cmd_rpy[1])
# and write the command to the servos
print(dt)
servos.write(cmd)
