distance_x = center[0] - target[0]
distance_y = center[1] - target[1]
distance_x /= -320.0
distance_y /= -240.0
angle = angleFromCenterAndTail(center, tail) / -180.0
# logger.info("Correcting angle: %s", angle)
# if abs(angle) > 0.05:
# # logger.info("Correcting angle: %s", angle)
# distance_x = 0.0
# distance_y = 0.0
rl = x_pid.GenOut(distance_x)
fb = y_pid.GenOut(distance_y)
aa = angle_pid.GenOut(angle)
if math.isnan(aa):
aa = 0.0
drone.at_cmd(True, rl, fb, 0, aa)
navdata = drone.get_navdata()
bat = navdata[0]['battery']
# logger.info("RL: %s FB: %s AA: %s BAT: %s", rl, fb, aa, bat)
# logger.info(navdata)
cv2.putText(image, "Bat: {}".format(bat), (20, 20),
cv2.cv.CV_FONT_HERSHEY_PLAIN, 1.0, (255, 255, 255))
class Drone(object):
def __init__(self, autoconnect=True):
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.thrust = 10000
self.bat = 0.0
self.stabilizer_roll = 0.0
self.stabilizer_pitch = 0.0
self.stabilizer_yaw = 0.0
# Angle to the Y axis
self.angle = 0.0
self.pid_roll = PID(Kp=1, Kd=0.4, Ki=0.00025)
self.pid_pitch = PID(Kp=1, Kd=0.4, Ki=0.00025)
self.pid_yaw = PID(Kp=10.0, Kd=0.0, Ki=0.0)
self.pid_thrust = PID(Kp=0.1, Kd=0.1, Ki=0.00025)
self.cf = Crazyflie(link=None, ro_cache="./cache", rw_cache="./cache")
self.cf.connectSetupFinished.add_callback(self.setup_finished)
self.init_interfaces()
self.connect()
def init_interfaces(self):
# logging.basicConfig(level=logging.DEBUG)
cflib.crtp.init_drivers()
available = cflib.crtp.scan_interfaces()
for i in available:
text = "Interface with URI [%s] found and name/comment [%s]"
print(text.format(i[0], i[1]))
if len(available) < 2:
print("No drone interface found!")
# sys.exit(0)
def connect(self, interface="radio://0/10/250K"):
self.cf.open_link(interface)
def disconnect(self):
self.stop()
time.sleep(0.1)
self.cf.close_link()
def reconnect(self, interface="radio://0/10/250K"):
self.disconnect()
self.connect(interface)
def setup_finished(self, linkURI):
accel_log_conf = LogConfig("Stabilizer", 10)
accel_log_conf.addVariable(LogVariable("stabilizer.roll", "float"))
accel_log_conf.addVariable(LogVariable("stabilizer.pitch", "float"))
accel_log_conf.addVariable(LogVariable("stabilizer.yaw", "float"))
bat_log_conf = LogConfig("Battery", 100)
bat_log_conf.addVariable(LogVariable("pm.vbat", "float"))
accel_log = self.cf.log.create_log_packet(accel_log_conf)
bat_log = self.cf.log.create_log_packet(bat_log_conf)
if accel_log is not None:
accel_log.dataReceived.add_callback(self.accel_data_callback)
accel_log.start()
else:
print("acc.x/y/z not found in TOC")
if bat_log is not None:
bat_log.dataReceived.add_callback(self.bat_data_callback)
bat_log.start()
else:
print("pm.bat not found in TOC")
def accel_data_callback(self, data):
self.stabilizer_roll = data['stabilizer.roll']
self.stabilizer_pitch = data['stabilizer.pitch']
self.stabilizer_yaw = data['stabilizer.yaw']
def bat_data_callback(self, data):
self.bat = data['pm.bat']
def set_point(self, roll, pitch, yawrate, thrust):
self.cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
def stop(self):
self.thrust = 10000
def update(self,
roll_error=None,
pitch_error=None,
yaw_error=None,
thrust_error=None):
if roll_error:
self.roll = self.pid_roll.GenOut(roll_error)
if pitch_error:
self.pitch = self.pid_pitch.GenOut(pitch_error)
if yaw_error:
self.yaw = self.pid_yaw.GenOut(yaw_error)
if thrust_error:
self.thrust = self.pid_thrust.GenOut(thrust_error)
self.set_point(self.roll, self.pitch, self.yaw, self.thrust)
