def __init__(self):
rp.init_node("magdrone_node")
# Create PID Controller
self.pid_z = PIDcontroller(0.1, 0.0, 0.0, 1)
# Create log file
self.log_file = open("log.txt", 'a')
# Set up Subscribers
self.pose_sub = rp.Subscriber("/aruco_single/pose",
PoseStamped,
self.pose_callback,
queue_size=1)
# Set up Publishers
# Connect to the Vehicle
self.printAndLog('Connecting to Vehicle')
self.vehicle = connect('/dev/serial0', wait_ready=True, baud=57600)
# Variables
self.cmds = None
self.land = 0
self.dsrm = 0
self.arm = 0
self.exit = 0
# Create thread for publisher
self.rate = 20
t = threading.Thread(target=self.send_commands)
t.start()
rp.spin()
def __init__(self):
rp.init_node("magdrone_node")
# Create PID Controller
self.pid_z = PIDcontroller(0.1, 0.0, 0.01, 1)
# Create log file
self.log_book = LogBook("test_flight")
# Set up Subscribers
self.pose_sub = rp.Subscriber("/aruco_single/pose", PoseStamped, self.pose_callback, queue_size=1)
self.joy_sub = rp.Subscriber("/joy", Joy, self.joy_callback, queue_size=1)
# Labeling the log file by controller
self.log_book.printAndLog('Running aruco_joy_controller.py')
# Connect to the Vehicle
#self.log_book.printAndLog('Connecting to Vehicle')
print('Connecting to Vehicle')
self.vehicle = connect('/dev/serial0', wait_ready=True, baud=57600)
# Variables
self.cmds = None
self.linear_z_cmd = 0.0
self.land = 0
self.dsrm = 0
self.arm = 0
self.exit = 0
# Create thread for publisher
self.rate = 30
t = threading.Thread(target=self.send_commands)
t.start()
rp.spin()
def send_commands(self):
print("Accepting Commands")
r = rp.Rate(self.rate)
while not rp.is_shutdown():
if self.cmds is not None:
# PID update error
self.pid_z.updateError(self.z_error)
self.pid_y.updateError(self.y_error)
self.pid_x.updateError(self.x_error)
# Generate commands
self.linear_z_cmd = self.clipCommand(
self.pid_z.getCommand() + 0.5, 0.65, 0.35)
self.linear_y_cmd = self.clipCommand(self.pid_y.getCommand(),
7.5, -7.5)
self.linear_x_cmd = self.clipCommand(self.pid_x.getCommand(),
7.5, -7.5)
self.set_attitude(roll_angle=self.linear_y_cmd,
pitch_angle=-self.linear_x_cmd,
yaw_angle=None,
yaw_rate=-self.cmds.angular.z,
use_yaw_rate=True,
thrust=self.linear_z_cmd,
duration=1.0 / self.rate)
#msg = "thrust: " + str(self.linear_z_cmd) + " roll angle: " + str(self.cmds.linear.x) + " pitch angle: " + str(self.cmds.linear.y)
#self.log_book.justLog(msg)
if self.arm > 0:
self.log_book.printAndLog("Arming...")
self.arm_and_takeoff_nogps()
if self.arm > 0:
self.log_book.printAndLog("Arming...")
self.arm_and_takeoff_nogps()
if self.exit == 0:
msg = str(self.z_error) + "\t" + str(
self.linear_z_cmd) + "\t" + str(
self.y_error) + "\t" + str(
self.linear_y_cmd) + "\t" + str(
self.x_error) + "\t" + str(
self.linear_x_cmd)
self.log_book.justLog(msg)
if self.z_error == 0.0:
print("Changing thrust PID")
self.pid_z = PIDcontroller(1.0, 0.0, 17.5, 3)
if self.y_error == 0.0:
print("Changing roll PID")
self.pid_y = PIDcontroller(9.5, 0.03, 200.0, 30)
if self.x_error == 0.0:
print("Changing pitch PID")
self.pid_x = PIDcontroller(9.5, 0.03, 200.0, 30)
r.sleep()
def __init__(self):
rp.init_node("magdrone_aruco")
# Connect to the Vehicle
rp.loginfo('Connecting to Vehicle')
self.vehicle = connect('/dev/serial0', wait_ready=True, baud=57600)
# Set up tf listener
self.tf_listener = tf.TransformListener()
# Set up Subscribers
self.joy_sub = rp.Subscriber("/joy",
Joy,
self.joy_callback,
queue_size=1)
# Set up Publishers
self.setpoint_pub = rp.Publisher("/setpoint",
Vector3Stamped,
queue_size=1)
self.command_pub = rp.Publisher("/commands",
TwistStamped,
queue_size=1)
# Create PID Controller
self.pid_z = PIDcontroller(1.0, 0.0, 17.5, 3)
self.pid_y = PIDcontroller(9.5, 0.0, 200.0, 30)
self.pid_x = PIDcontroller(9.5, 0.0, 200.0, 30)
self.pid_w = PIDcontroller(0.15, 0.0, 0.0, 30)
# Variables
self.arm = 0
self.engage_controller = False
# Create thread for publisher
self.rate = 30
t = threading.Thread(target=self.send_commands)
t.start()
rp.spin()
def __init__(self):
rp.init_node("magdrone_node")
# Create PID Controller
self.pid_z = PIDcontroller(1.0, 0.0, 17.5, 3)
self.pid_y = PIDcontroller(9.5, 0.0, 200.0, 30)
self.pid_x = PIDcontroller(9.5, 0.0, 200.0, 30)
# Create log file
self.log_book = LogBook("test_flight")
# Set up Subscribers
self.pose_sub = rp.Subscriber("/vrpn_client_node/UAV/pose",
PoseStamped,
self.pose_callback,
queue_size=1)
self.joy_sub = rp.Subscriber("/joy",
Joy,
self.joy_callback,
queue_size=1)
# Labeling the log file by controller
self.log_book.printAndLog('Running optitrack_controller.py')
# Connect to the Vehicle
#self.log_book.printAndLog('Connecting to Vehicle')
print('Connecting to Vehicle')
self.vehicle = connect('/dev/serial0', wait_ready=True, baud=57600)
# Variables
self.cmds = None
self.linear_z_cmd = 0.0
self.linear_y_cmd = 0.0
self.land = 0
self.change_PIDs = 0
self.arm = 0
self.exit = 0
self.z_error = 0.0
self.y_error = 0.0
self.x_error = 0.0
# Create thread for publisher
self.rate = 30
t = threading.Thread(target=self.send_commands)
t.start()
rp.spin()
class magdroneControlNode():
def __init__(self):
rp.init_node("magdrone_node")
# Create PID Controller
self.pid_z = PIDcontroller(0.1, 0.0, 0.0, 1)
# Create log file
self.log_file = open("log.txt", 'a')
# Set up Subscribers
self.pose_sub = rp.Subscriber("/aruco_single/pose",
PoseStamped,
self.pose_callback,
queue_size=1)
# Set up Publishers
# Connect to the Vehicle
self.printAndLog('Connecting to Vehicle')
self.vehicle = connect('/dev/serial0', wait_ready=True, baud=57600)
# Variables
self.cmds = None
self.land = 0
self.dsrm = 0
self.arm = 0
self.exit = 0
# Create thread for publisher
self.rate = 20
t = threading.Thread(target=self.send_commands)
t.start()
rp.spin()
def printAndLog(self, msg):
print(msg)
self.log_file.write(msg)
self.log_file.write("\n")
def arm_and_takeoff_nogps(self, aTargetAltitude=-1.0):
"""
Arms vehicle and fly to aTargetAltitude without GPS data.
"""
##### CONSTANTS #####
DEFAULT_TAKEOFF_THRUST = 0.55
SMOOTH_TAKEOFF_THRUST = 0.52
self.printAndLog("Basic pre-arm checks")
# Don't let the user try to arm until autopilot is ready
# If you need to disable the arming check,
# just comment it with your own responsibility.
# while not self.vehicle.is_armable:
# print(" Waiting for vehicle to initialise...")
# time.sleep(1)
self.printAndLog("Arming motors")
# GUIDED_NOGPS mode is recommended by DroneKit
self.vehicle.mode = VehicleMode("GUIDED_NOGPS")
self.vehicle.armed = True
while not self.vehicle.armed:
self.printAndLog(" Waiting for arming...")
self.vehicle.armed = True
time.sleep(1)
self.printAndLog("Armed!")
if aTargetAltitude > 0:
print("Taking off!")
thrust = DEFAULT_TAKEOFF_THRUST
while True:
current_altitude = self.vehicle.location.global_relative_frame.alt
print(" Altitude: %f Desired: %f" %
(current_altitude, aTargetAltitude))
# Trigger just below target alt.
if current_altitude >= aTargetAltitude * 0.95:
print("Reached target altitude")
break
elif current_altitude >= aTargetAltitude * 0.6:
thrust = SMOOTH_TAKEOFF_THRUST
self.set_attitude(thrust=thrust)
time.sleep(0.2)
def send_attitude_target(self,
roll_angle=0.0,
pitch_angle=0.0,
yaw_angle=None,
yaw_rate=0.0,
use_yaw_rate=False,
thrust=0.5):
"""
use_yaw_rate: the yaw can be controlled using yaw_angle OR yaw_rate.
When one is used, the other is ignored by Ardupilot.
thrust: 0 <= thrust <= 1, as a fraction of maximum vertical thrust.
Note that as of Copter 3.5, thrust = 0.5 triggers a special case in
the code for maintaining current altitude.
"""
if yaw_angle is None:
# this value may be unused by the vehicle, depending on use_yaw_rate
yaw_angle = self.vehicle.attitude.yaw
# Thrust > 0.5: Ascend
# Thrust == 0.5: Hold the altitude
# Thrust < 0.5: Descend
msg = self.vehicle.message_factory.set_attitude_target_encode(
0, # time_boot_ms
1, # Target system
1, # Target component
0b00000000 if use_yaw_rate else 0b00000100,
to_quaternion(roll_angle, pitch_angle, yaw_angle), # Quaternion
0, # Body roll rate in radian
0, # Body pitch rate in radian
math.radians(yaw_rate), # Body yaw rate in radian/second
thrust # Thrust
)
self.vehicle.send_mavlink(msg)
def set_attitude(self,
roll_angle=0.0,
pitch_angle=0.0,
yaw_angle=None,
yaw_rate=0.0,
use_yaw_rate=False,
thrust=0,
duration=0.05):
"""
Note that from AC3.3 the message should be re-sent more often than every
second, as an ATTITUDE_TARGET order has a timeout of 1s.
In AC3.2.1 and earlier the specified attitude persists until it is canceled.
The code below should work on either version.
Sending the message multiple times is the recommended way.
"""
self.send_attitude_target(roll_angle, pitch_angle, yaw_angle, yaw_rate,
use_yaw_rate, thrust)
start = time.time()
while time.time() - start < duration:
self.send_attitude_target(roll_angle, pitch_angle, yaw_angle,
yaw_rate, use_yaw_rate, thrust)
time.sleep(0.1)
# Reset attitude, or it will persist for 1s more due to the timeout
self.send_attitude_target(0, 0, 0, 0, True, thrust)
def pose_callback(self, data):
self.cmds = Twist()
# Create Empty Commands
self.cmds.linear.x = 0 # roll
self.cmds.linear.y = 0 # pitch
self.cmds.linear.z = 0 # thrust
self.cmds.angular.z = 0 # yaw
# Defining the desired positions
self.z_des = 0 #thrust
self.y_des = 0 #roll
self.x_des = 1 #pitch
"""
+ z error = + thrust
- z error = - thrust
+ y error = - roll
- y error = + roll
+ x error = + pitch
- x error = - pitch
"""
# Position conversions where the reported position is in terms of the camera frame
# z-error = x-tag - z_des = y-camera
# y-error = y-tag - y_des = x-camera
# x-error = z-tag - x_des = z-camera
self.z_error = data.pose.position.y + self.z_des
self.y_error = data.pose.position.x + self.y_des
self.x_error = data.pose.position.z + self.x_des
# PID update error
self.pid_z.updateError(self.z_error)
# generate thrust command
self.cmds.linear.z = self.pid_z.getCommand() + 0.5
if self.cmds.linear.z > 0.55:
self.cmds.linear.z = 0.55
if self.cmds.linear.z < 0.45:
self.cmds.linear.z = 0.45
msg = "error: " + str(self.z_error) + " read position: " + str(
data.pose.position.y) + " thrust: " + str(self.cmds.linear.z)
self.printAndLog(msg)
def send_commands(self):
self.printAndLog("Accepting Commands")
self.printAndLog("Initiating Arming")
self.arm_and_takeoff_nogps()
r = rp.Rate(self.rate)
while not rp.is_shutdown():
# print(self.vehicle.attitude.yaw)
if self.cmds is not None and self.vehicle.armed:
self.set_attitude(roll_angle=0,
pitch_angle=0,
yaw_angle=None,
yaw_rate=0,
use_yaw_rate=True,
thrust=self.cmds.linear.z)
msg = "thrust: " + str(self.cmds.linear.z)
self.printAndLog(msg)
if self.dsrm > 0:
self.printAndLog("Disarming")
self.set_attitude(thrust=0, duration=8)
self.printAndLog("Disarm complete")
if self.arm > 0:
self.printAndLog("Arming...")
self.arm_and_takeoff_nogps()
if self.exit > 0:
self.printAndLog("Switched to manual controls")
r.sleep()
class magdroneControlNode():
def __init__(self):
rp.init_node("magdrone_node")
# Create PID Controller
self.pid_z = PIDcontroller(1.0, 0.0, 17.5, 3)
self.pid_y = PIDcontroller(9.5, 0.0, 200.0, 30)
self.pid_x = PIDcontroller(9.5, 0.0, 200.0, 30)
# Create log file
self.log_book = LogBook("test_flight")
# Set up Subscribers
self.pose_sub = rp.Subscriber("/vrpn_client_node/UAV/pose",
PoseStamped,
self.pose_callback,
queue_size=1)
self.joy_sub = rp.Subscriber("/joy",
Joy,
self.joy_callback,
queue_size=1)
# Labeling the log file by controller
self.log_book.printAndLog('Running optitrack_controller.py')
# Connect to the Vehicle
#self.log_book.printAndLog('Connecting to Vehicle')
print('Connecting to Vehicle')
self.vehicle = connect('/dev/serial0', wait_ready=True, baud=57600)
# Variables
self.cmds = None
self.linear_z_cmd = 0.0
self.linear_y_cmd = 0.0
self.land = 0
self.change_PIDs = 0
self.arm = 0
self.exit = 0
self.z_error = 0.0
self.y_error = 0.0
self.x_error = 0.0
# Create thread for publisher
self.rate = 30
t = threading.Thread(target=self.send_commands)
t.start()
rp.spin()
def clipCommand(self, cmd, upperBound, lowerBound):
if cmd < lowerBound:
cmd = lowerBound
elif cmd > upperBound:
cmd = upperBound
return cmd
def arm_and_takeoff_nogps(self, aTargetAltitude=-1.0):
"""
Arms vehicle and fly to aTargetAltitude without GPS data.
"""
##### CONSTANTS #####
DEFAULT_TAKEOFF_THRUST = 0.55
SMOOTH_TAKEOFF_THRUST = 0.52
#self.log_book.printAndLog("Basic pre-arm checks")
# Don't let the user try to arm until autopilot is ready
# If you need to disable the arming check,
# just comment it with your own responsibility.
# while not self.vehicle.is_armable:
# print(" Waiting for vehicle to initialise...")
# time.sleep(1)
#self.log_book.printAndLog("Arming motors")
# GUIDED_NOGPS mode is recommended by DroneKit
self.vehicle.mode = VehicleMode("GUIDED_NOGPS")
self.vehicle.armed = True
while not self.vehicle.armed:
#self.log_book.printAndLog(" Waiting for arming...")
self.vehicle.armed = True
time.sleep(1)
#self.log_book.printAndLog("Armed!")
print('Armed')
if aTargetAltitude > 0:
print("Taking off!")
thrust = DEFAULT_TAKEOFF_THRUST
while True:
current_altitude = self.vehicle.location.global_relative_frame.alt
print(" Altitude: %f Desired: %f" %
(current_altitude, aTargetAltitude))
# Trigger just below target alt.
if current_altitude >= aTargetAltitude * 0.95:
print("Reached target altitude")
break
elif current_altitude >= aTargetAltitude * 0.6:
thrust = SMOOTH_TAKEOFF_THRUST
self.set_attitude(thrust=thrust)
time.sleep(0.2)
def send_attitude_target(self,
roll_angle=0.0,
pitch_angle=0.0,
yaw_angle=None,
yaw_rate=0.0,
use_yaw_rate=False,
thrust=0.5):
"""
use_yaw_rate: the yaw can be controlled using yaw_angle OR yaw_rate.
When one is used, the other is ignored by Ardupilot.
thrust: 0 <= thrust <= 1, as a fraction of maximum vertical thrust.
Note that as of Copter 3.5, thrust = 0.5 triggers a special case in
the code for maintaining current altitude.
"""
if yaw_angle is None:
# this value may be unused by the vehicle, depending on use_yaw_rate
yaw_angle = self.vehicle.attitude.yaw
# Thrust > 0.5: Ascend
# Thrust == 0.5: Hold the altitude
# Thrust < 0.5: Descend
msg = self.vehicle.message_factory.set_attitude_target_encode(
0, # time_boot_ms
1, # Target system
1, # Target component
0b00000000 if use_yaw_rate else 0b00000100,
to_quaternion(roll_angle, pitch_angle, yaw_angle), # Quaternion
0, # Body roll rate in radian
0, # Body pitch rate in radian
math.radians(yaw_rate), # Body yaw rate in radian/second
thrust # Thrust
)
self.vehicle.send_mavlink(msg)
def set_attitude(self,
roll_angle=0.0,
pitch_angle=0.0,
yaw_angle=None,
yaw_rate=0.0,
use_yaw_rate=False,
thrust=0,
duration=0.05):
"""
Note that from AC3.3 the message should be re-sent more often than every
second, as an ATTITUDE_TARGET order has a timeout of 1s.
In AC3.2.1 and earlier the specified attitude persists until it is canceled.
The code below should work on either version.
Sending the message multiple times is the recommended way.
"""
self.send_attitude_target(roll_angle, pitch_angle, yaw_angle, yaw_rate,
use_yaw_rate, thrust)
start = time.time()
while time.time() - start < duration:
self.send_attitude_target(roll_angle, pitch_angle, yaw_angle,
yaw_rate, use_yaw_rate, thrust)
time.sleep(duration)
# Reset attitude, or it will persist for 1s more due to the timeout
self.send_attitude_target(0, 0, 0, 0, True, thrust)
def pose_callback(self, data):
# Empty Commands
# self.cmds.linear.x = 0 # roll
# self.cmds.linear.y = 0 # pitch
# self.cmds.angular.z = 0 # yaw
# self.linear_z_cmd = 0 # thrust
"""
+ z error = + thrust
- z error = - thrust
+ y error = - roll
- y error = + roll
+ x error = + pitch
- x error = - pitch
"""
# Define the desired position
self.z_des = 1.0 # thrust
self.y_des = 0.0 # roll
self.x_des = 0.0 # pitch
# Position conversions where the reported position is in terms of the camera frame
# z-error = x-tag - z_des = y-camera
# y-error = y-tag - y_des = x-camera
# x-error = z-tag - x_des = z-camera
self.z_error = self.z_des - data.pose.position.z
self.y_error = self.y_des - data.pose.position.y
self.x_error = data.pose.position.x - self.x_des
def joy_callback(self, data):
# Empty Command
self.cmds = Twist()
# Joystick Controls
self.cmds.linear.x = data.axes[2] * 10 # roll
self.cmds.linear.y = data.axes[3] * 10 # pitch
self.cmds.angular.z = data.axes[0] * 10 # yaw
# Button Controls
self.change_PIDs = data.buttons[0]
self.land = data.buttons[1]
self.arm = data.buttons[9]
self.mag = data.axes[5]
self.exit = data.buttons[2]
def send_commands(self):
print("Accepting Commands")
r = rp.Rate(self.rate)
while not rp.is_shutdown():
if self.cmds is not None:
# PID update error
self.pid_z.updateError(self.z_error)
self.pid_y.updateError(self.y_error)
self.pid_x.updateError(self.x_error)
# Generate commands
self.linear_z_cmd = self.clipCommand(
self.pid_z.getCommand() + 0.5, 0.65, 0.35)
self.linear_y_cmd = self.clipCommand(self.pid_y.getCommand(),
7.5, -7.5)
self.linear_x_cmd = self.clipCommand(self.pid_x.getCommand(),
7.5, -7.5)
self.set_attitude(roll_angle=self.linear_y_cmd,
pitch_angle=-self.linear_x_cmd,
yaw_angle=None,
yaw_rate=-self.cmds.angular.z,
use_yaw_rate=True,
thrust=self.linear_z_cmd,
duration=1.0 / self.rate)
#msg = "thrust: " + str(self.linear_z_cmd) + " roll angle: " + str(self.cmds.linear.x) + " pitch angle: " + str(self.cmds.linear.y)
#self.log_book.justLog(msg)
if self.arm > 0:
self.log_book.printAndLog("Arming...")
self.arm_and_takeoff_nogps()
if self.arm > 0:
self.log_book.printAndLog("Arming...")
self.arm_and_takeoff_nogps()
if self.exit == 0:
msg = str(self.z_error) + "\t" + str(
self.linear_z_cmd) + "\t" + str(
self.y_error) + "\t" + str(
self.linear_y_cmd) + "\t" + str(
self.x_error) + "\t" + str(
self.linear_x_cmd)
self.log_book.justLog(msg)
if self.z_error == 0.0:
print("Changing thrust PID")
self.pid_z = PIDcontroller(1.0, 0.0, 17.5, 3)
if self.y_error == 0.0:
print("Changing roll PID")
self.pid_y = PIDcontroller(9.5, 0.03, 200.0, 30)
if self.x_error == 0.0:
print("Changing pitch PID")
self.pid_x = PIDcontroller(9.5, 0.03, 200.0, 30)
r.sleep()
class magdroneControlNode():
def __init__(self):
rp.init_node("magdrone_aruco")
# Connect to the Vehicle
rp.loginfo('Connecting to Vehicle')
self.vehicle = connect('/dev/serial0', wait_ready=True, baud=57600)
# Set up tf listener
self.tf_listener = tf.TransformListener()
# Set up Subscribers
self.joy_sub = rp.Subscriber(
"/joy", Joy, self.joy_callback, queue_size=1)
# Set up Publishers
self.setpoint_pub = rp.Publisher(
"/setpoint", Vector3Stamped, queue_size=1)
self.command_pub = rp.Publisher(
"/commands", TwistStamped, queue_size=1)
# Create PID Controller
self.pid_z = PIDcontroller(1.0, 0.0, 17.5, 3)
self.pid_y = PIDcontroller(9.5, 0.0, 200.0, 30)
self.pid_x = PIDcontroller(9.5, 0.0, 200.0, 30)
self.pid_w = PIDcontroller(0.15, 0.0, 0.0, 30)
# Variables
self.arm = 0
self.engage_controller = False
# Create thread for publisher
self.rate = 30
t = threading.Thread(target=self.send_commands)
t.start()
rp.spin()
'''
DroneKit Functions
'''
def arm_and_takeoff_nogps(self, aTargetAltitude=-1.0):
"""
Arms vehicle and fly to aTargetAltitude without GPS data.
"""
##### CONSTANTS #####
DEFAULT_TAKEOFF_THRUST = 0.55
SMOOTH_TAKEOFF_THRUST = 0.52
rp.loginfo("Basic pre-arm checks")
# Don't let the user try to arm until autopilot is ready
# If you need to disable the arming check,
# just comment it with your own responsibility.
while not self.vehicle.is_armable:
rp.loginfo(" Waiting for vehicle to initialise...")
time.sleep(1)
rp.loginfo("Arming motors")
# GUIDED_NOGPS mode is recommended by DroneKit
self.vehicle.mode = VehicleMode("GUIDED_NOGPS")
self.vehicle.armed = True
while not self.vehicle.armed:
rp.loginfo(" Waiting for arming...")
self.vehicle.armed = True
time.sleep(1)
rp.loginfo('Armed')
if aTargetAltitude > 0:
rp.loginfo("Taking off!")
thrust = DEFAULT_TAKEOFF_THRUST
while True:
current_altitude = self.vehicle.location.global_relative_frame.alt
rp.loginfo(" Altitude: %f Desired: %f" %
(current_altitude, aTargetAltitude))
# Trigger just below target alt.
if current_altitude >= aTargetAltitude*0.95:
rp.loginfo("Reached target altitude")
break
elif current_altitude >= aTargetAltitude*0.6:
thrust = SMOOTH_TAKEOFF_THRUST
self.set_attitude(thrust=thrust)
time.sleep(0.2)
def send_attitude_target(self, roll_angle=0.0, pitch_angle=0.0,
yaw_angle=None, yaw_rate=0.0, use_yaw_rate=False,
thrust=0.5):
"""
use_yaw_rate: the yaw can be controlled using yaw_angle OR yaw_rate.
When one is used, the other is ignored by Ardupilot.
thrust: 0 <= thrust <= 1, as a fraction of maximum vertical thrust.
Note that as of Copter 3.5, thrust = 0.5 triggers a special case in
the code for maintaining current altitude.
"""
if yaw_angle is None:
# this value may be unused by the vehicle, depending on use_yaw_rate
yaw_angle = self.vehicle.attitude.yaw
# Thrust > 0.5: Ascend
# Thrust == 0.5: Hold the altitude
# Thrust < 0.5: Descend
msg = self.vehicle.message_factory.set_attitude_target_encode(
0, # time_boot_ms
1, # Target system
1, # Target component
0b00000000 if use_yaw_rate else 0b00000100,
to_quaternion(roll_angle, pitch_angle, yaw_angle), # Quaternion
0, # Body roll rate in radian
0, # Body pitch rate in radian
math.radians(yaw_rate), # Body yaw rate in radian/second
thrust # Thrust
)
self.vehicle.send_mavlink(msg)
def set_attitude(self, roll_angle=0.0, pitch_angle=0.0,
yaw_angle=None, yaw_rate=0.0, use_yaw_rate=False,
thrust=0, duration=0.05):
"""
Note that from AC3.3 the message should be re-sent more often than every
second, as an ATTITUDE_TARGET order has a timeout of 1s.
In AC3.2.1 and earlier the specified attitude persists until it is canceled.
The code below should work on either version.
Sending the message multiple times is the recommended way.
"""
self.send_attitude_target(roll_angle, pitch_angle,
yaw_angle, yaw_rate, use_yaw_rate,
thrust)
start = time.time()
while time.time() - start < duration:
self.send_attitude_target(roll_angle, pitch_angle,
yaw_angle, yaw_rate, use_yaw_rate,
thrust)
time.sleep(duration)
# Reset attitude, or it will persist for 1s more due to the timeout
self.send_attitude_target(0, 0, 0, 0, True, thrust)
'''
Callbacks
'''
def joy_callback(self, data):
# Empty Command
self.cmds = Twist()
# Joystick Controls
self.cmds.linear.x = data.axes[2] * 10.0 # roll
self.cmds.linear.y = data.axes[3] * 10.0 # pitch
self.cmds.angular.z = data.axes[0] * 10.0 # yaw
self.cmds.linear.z = 0.25 * (data.axes[1] / 2.0) + 0.5 # thrust
# Button Controls
self.arm = data.buttons[9]
if data.buttons[4] == 1.0:
if not self.engage_controller:
rp.loginfo("Controller Engaged")
else:
rp.loginfo("Controller Disengaged")
self.engage_controller = not self.engage_controller
def clipCommand(self, cmd, upperBound, lowerBound):
if cmd < lowerBound:
cmd = lowerBound
elif cmd > upperBound:
cmd = upperBound
return cmd
'''
Main Loop
'''
def send_commands(self):
rp.loginfo("Accepting Commands")
r = rp.Rate(self.rate)
while not rp.is_shutdown():
# Arm motors
if self.arm > 0:
rp.loginfo("Arming...")
self.arm_and_takeoff_nogps()
if self.engage_controller:
# Get latest transform
try:
(T, R) = self.tf_listener.lookupTransform('UAV', 'bundle', rp.Time())
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
lastTFtime = self.tf_listener.getLatestCommonTime('raspicam', 'bundle')
# Get current pose wrt Tag
qx = R[0]
qy = R[1]
qz = R[2]
qw = R[3]
orientation = to_rpy(qw, qx, qy, qz)
# Drone body system is Front-Left-Down
w_drone = orientation[2]
x_drone = T[0]
y_drone = T[1]
z_drone = T[2]
# Set desired position
des_position = [0, 0, -0.5] # x, y, and z
# Publish setpoint
setpoint = Vector3Stamped()
setpoint.header.stamp = rp.Time.now()
setpoint.vector.x = des_position[0]
setpoint.vector.y = des_position[1]
setpoint.vector.z = des_position[2]
self.setpoint_pub.publish(setpoint)
"""
+ z error = + thrust
- z error = - thrust
+ y error = - roll
- y error = + roll
+ x error = + pitch
- x error = - pitch
"""
# Position conversions where the reported position is in terms of the camera frame
# z-error = x-tag - z_des = y-camera
# y-error = y-tag - y_des = x-camera
# x-error = z-tag - x_des = z-camera
# Calculate error
x_error = des_position[0] - x_drone
y_error = des_position[1] - y_drone
z_error = des_position[2] - z_drone
dw = w_drone
if dw > 180.0:
w_error = dw - 360.0
else:
w_error = dw
# Update errors
self.pid_w.updateError(w_error)
self.pid_x.updateError(x_error)
self.pid_y.updateError(y_error)
self.pid_z.updateError(z_error)
# Get commands
dt_ros = rp.get_rostime() - lastTFtime
dt = dt_ros.secs + dt_ros.nsecs * 1e-9
if dt < 2.0:
angular_z_cmd = self.clipCommand(self.pid_w.getCommand(), 5, -5)
linear_z_cmd = self.clipCommand(self.pid_z.getCommand() + 0.5, 0.65, 0.35)
linear_y_cmd = self.clipCommand(self.pid_y.getCommand() - 1.3, 7.5, -7.5)
linear_x_cmd = self.clipCommand(self.pid_x.getCommand() + 0.45, 7.5, -7.5)
else:
angular_z_cmd = 0.0
linear_z_cmd = 0.5
linear_y_cmd = -1.3
linear_x_cmd = 0.45
rp.logwarn("Marker lost for more than 2 seconds!!! Hovering")
# Apply commands
self.set_attitude(roll_angle=-linear_y_cmd,
pitch_angle=-linear_x_cmd,
yaw_angle=None,
yaw_rate=angular_z_cmd, use_yaw_rate=True,
thrust=linear_z_cmd,
duration=1.0/self.rate)
# Publish commands for logging
cmd = TwistStamped()
cmd.header.stamp = rp.Time.now()
cmd.twist.angular.x = -linear_y_cmd
cmd.twist.angular.y = -linear_x_cmd
cmd.twist.angular.z = angular_z_cmd
cmd.twist.linear.z = linear_z_cmd
self.command_pub.publish(cmd)
r.sleep()