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