def ControlLoop(self):
# Set the rate
rate = rospy.Rate(50)
# Keep track how many loops have happend
loop_counter = 0
# While running
while not rospy.is_shutdown():
# Create the message we are going to send
msg = RateThrust()
msg.header.stamp = rospy.get_rostime()
# If the drone has not been armed
if self.armed == False:
# Arm the drone
msg.thrust = Vector3(0, 0, 10)
msg.angular_rates = Vector3(0, 0, 0)
self.armed = True
# Behave normally
else:
# Use a PID to calculate the rates you want to publish to maintain an angle
roll_output = self.rollPID.get_output(self.roll_setpoint,
self.roll_reading)
pitch_output = self.pitchPID.get_output(
self.pitch_setpoint, self.pitch_reading)
if self.no_yaw:
yaw_output = self.yaw_output
#rospy.loginfo("my output: " + str(yaw_output) + " old yaw:" + str(self.yawPID.get_output(self.yaw_setpoint, self.yaw_reading)))
else:
yaw_output = self.yawPID.get_output(
self.yaw_setpoint, self.yaw_reading)
# Create the message
msg.thrust = Vector3(0, 0, self.thrust_setpoint)
msg.angular_rates = Vector3(roll_output, pitch_output,
yaw_output)
# Publish the message
self.rate_pub.publish(msg)
# Sleep any excess time
rate.sleep()
def ControlLoop(self):
# Set the rate
rate = rospy.Rate(50)
# Keep track how many loops have happend
loop_counter = 0
# While running
while not rospy.is_shutdown():
# Create the message we are going to send
msg = RateThrust()
msg.header.stamp = rospy.get_rostime()
# If the drone has not been armed
if self.armed == False:
# Arm the drone
msg.thrust = Vector3(0, 0, 10)
msg.angular_rates = Vector3(0, 0, 0)
# Behave normally (Drone is armed)
else:
# Use a PID to calculate the rates you want to publish to maintain an angle
output = [
pids.get_output(setp, read) for pids, setp, read in zip(
self.rpy_PIDS, self.rpy_setpoints, self.rpy_readings)
]
# Create the message
msg.thrust = Vector3(0, 0, self.thrust_setpoint)
msg.angular_rates = Vector3(output[0], output[1], output[2])
# Publish the message
self.rate_pub.publish(msg)
# Sleep any excess time
rate.sleep()
def clean(control):
cleaned=RateThrust()
cleaned.head=control.head
cleaned.angular_rates=control.bodyrates
cleaned.thrust=control.collective_thrust
return cleaned
