进一步完善
This commit is contained in:
parent
c9b46995fa
commit
05ca368843
|
|
@ -1,9 +0,0 @@
|
|||
# number of different kinds of vehicles
|
||||
rover: 2
|
||||
plane: 2
|
||||
typhoon: 2
|
||||
solo: 3
|
||||
iris: 3
|
||||
tiltrotor: 2
|
||||
tailsitter: 2
|
||||
standard_vtol: 2
|
||||
|
|
@ -1,181 +0,0 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
|
||||
class PX4Communication:
|
||||
|
||||
def __init__(self):
|
||||
|
||||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.current_heading = None
|
||||
self.takeoff_height = 1
|
||||
|
||||
self.target_pose = PoseStamped()
|
||||
self.target_vel=TwistStamped()
|
||||
self.global_target = None
|
||||
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.flag = 0
|
||||
self.flight_mode = None
|
||||
|
||||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.pose_target_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
|
||||
self.twist_target_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode)
|
||||
|
||||
|
||||
print("PX4 Communication Initialized!")
|
||||
|
||||
|
||||
def start(self):
|
||||
rospy.init_node("px4_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
'''
|
||||
while(rospy.is_shutdown):
|
||||
if(self.flag==0):
|
||||
self.pose_target_pub.publish(self.target_pose)
|
||||
else:
|
||||
self.twist_target_pub.publish(self.target_vel)
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
|
||||
if(self.disarm()):
|
||||
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose.position.x, self.target_pose.pose.position.y = self.flu2enu(msg.position.x, msg.position.y)
|
||||
self.target_pose.pose.position.z = msg.position.z
|
||||
target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose = msg
|
||||
target_yaw = self.q2yaw(msg.orientation)
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist.linear.x, self.target_vel.twist.linear.y = self.flu2enu(msg.linear.x, msg.linear.y)
|
||||
self.target_vel.twist.linear.z = msg.linear.z
|
||||
self.target_vel.twist.angular.x = 0
|
||||
self.target_vel.twist.angular.y = 0
|
||||
self.target_vel.twist.angular.z = msg.angular.z
|
||||
|
||||
def cmd_vel_enu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist = msg
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
return
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print('Armed'+str(self.arm_state))
|
||||
elif msg.data == 'DISARM':
|
||||
disarm_state =self.disarm()
|
||||
if disarm_state:
|
||||
self.arm_state = False
|
||||
print('Armed'+str(self.arm_state))
|
||||
else:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def flu2enu(self, x_flu, y_flu):
|
||||
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
|
||||
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
|
||||
return x_enu, y_enu
|
||||
|
||||
def cmd_yaw(self, yaw):
|
||||
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
||||
self.target_pose.pose.orientation.x = quaternion[0]
|
||||
self.target_pose.pose.orientation.y = quaternion[1]
|
||||
self.target_pose.pose.orientation.z = quaternion[2]
|
||||
self.target_pose.pose.orientation.w = quaternion[3]
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print("Vehicle Arming Failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print("Vehicle Disarming Failed!")
|
||||
return False
|
||||
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print(self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print(self.flight_mode+"Failed")
|
||||
return False
|
||||
|
||||
def takeoff_detection(self):
|
||||
if self.local_pose.pose.position.z > 0.3 and self.offboard_state and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
px4_com = PX4Communication()
|
||||
px4_com.start()
|
||||
|
|
@ -1,194 +0,0 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
from multiprocessing import Process
|
||||
import sys
|
||||
|
||||
class PX4Communication:
|
||||
|
||||
def __init__(self,id):
|
||||
|
||||
self.id = id
|
||||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.current_heading = None
|
||||
self.takeoff_height = 1
|
||||
|
||||
self.target_pose = PoseStamped()
|
||||
self.target_vel=TwistStamped()
|
||||
self.global_target = None
|
||||
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.flag = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
||||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.pose_target_pub = rospy.Publisher("/uav"+str(self.id)+"/mavros/setpoint_position/local", PoseStamped, queue_size=10)
|
||||
self.twist_target_pub = rospy.Publisher("/uav"+str(self.id)+"/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/set_mode", SetMode)
|
||||
|
||||
|
||||
print("UAV"+str(self.id)+": "+"PX4 Communication Initialized!")
|
||||
|
||||
|
||||
def start(self):
|
||||
rospy.init_node("px4_uav"+str(self.id)+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
'''
|
||||
while(rospy.is_shutdown):
|
||||
if(self.flag==0):
|
||||
self.pose_target_pub.publish(self.target_pose)
|
||||
else:
|
||||
self.twist_target_pub.publish(self.target_vel)
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
|
||||
if(self.disarm()):
|
||||
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose.position.x, self.target_pose.pose.position.y = self.flu2enu(msg.position.x, msg.position.y)
|
||||
self.target_pose.pose.position.z = msg.position.z
|
||||
target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose = msg
|
||||
target_yaw = self.q2yaw(msg.orientation)
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist.linear.x, self.target_vel.twist.linear.y = self.flu2enu(msg.linear.x, msg.linear.y)
|
||||
self.target_vel.twist.linear.z = msg.linear.z
|
||||
self.target_vel.twist.angular.x = 0
|
||||
self.target_vel.twist.angular.y = 0
|
||||
self.target_vel.twist.angular.z = msg.angular.z
|
||||
|
||||
def cmd_vel_enu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist = msg
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
return
|
||||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
disarm_state =self.disarm()
|
||||
if disarm_state:
|
||||
self.arm_state = False
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
|
||||
self.mission = msg.data
|
||||
print("UAV"+str(self.id)+": "+msg.data)
|
||||
|
||||
elif not msg.data == self.flight_mode:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def flu2enu(self, x_flu, y_flu):
|
||||
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
|
||||
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
|
||||
return x_enu, y_enu
|
||||
|
||||
def cmd_yaw(self, yaw):
|
||||
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
||||
self.target_pose.pose.orientation.x = quaternion[0]
|
||||
self.target_pose.pose.orientation.y = quaternion[1]
|
||||
self.target_pose.pose.orientation.z = quaternion[2]
|
||||
self.target_pose.pose.orientation.w = quaternion[3]
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+"Vehicle Arming Failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+"Vehicle Disarming Failed!")
|
||||
return False
|
||||
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode+"Failed")
|
||||
return False
|
||||
|
||||
def takeoff_detection(self):
|
||||
if self.local_pose.pose.position.z > 0.3 and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
uav_num = int(sys.argv[1])
|
||||
for i in range(uav_num):
|
||||
px4_com = PX4Communication(i)
|
||||
p = Process(target=px4_com.start)
|
||||
p.start()
|
||||
|
|
@ -38,34 +38,34 @@ class Communication:
|
|||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback)
|
||||
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback)
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback)
|
||||
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+self.vehicle_id+'ground_truth/odom', Odometry, queue_size=10)
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+'_'+self.vehicle_id+'/ground_truth/odom', Odometry, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.gazeboModelstate = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
|
||||
|
||||
print(self.vehicle_type+self.vehicle_id+": "+"communication initialized")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+"communication initialized")
|
||||
|
||||
def start(self):
|
||||
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
|
||||
rospy.init_node(self.vehicle_type+'_'+self.vehicle_id+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
|
|
@ -170,15 +170,15 @@ class Communication:
|
|||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
dself.arm_state = not self.disarm()
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
|
||||
self.mission = msg.data
|
||||
print(self.vehicle_type+self.vehicle_id+": "+msg.data)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+msg.data)
|
||||
|
||||
elif not msg.data == self.flight_mode:
|
||||
self.flight_mode = msg.data
|
||||
|
|
@ -198,14 +198,14 @@ class Communication:
|
|||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": arming failed!")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": arming failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": disarming failed!")
|
||||
return False
|
||||
|
||||
def hover(self):
|
||||
|
|
@ -216,13 +216,13 @@ class Communication:
|
|||
if self.flight_mode == 'HOVER':
|
||||
self.hover_flag = 1
|
||||
self.hover()
|
||||
print(self.vehicle_type+self.vehicle_id+":"+self.flight_mode)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+":"+self.flight_mode)
|
||||
elif self.flightModeService(custom_mode=self.flight_mode):
|
||||
self.hover_flag = 0
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode+"failed")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode+"failed")
|
||||
return False
|
||||
|
||||
def takeoff_detection(self):
|
||||
|
|
@ -34,29 +34,29 @@ class Communication:
|
|||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
#self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
#self.imu_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+self.vehicle_id+'ground_truth/odom', Odometry, queue_size=10)
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+'_'+self.vehicle_id+'/ground_truth/odom', Odometry, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.gazeboModelstate = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
|
||||
|
||||
print("communication initialized")
|
||||
|
||||
def start(self):
|
||||
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
|
||||
rospy.init_node(self.vehicle_type+'_'+self.vehicle_id+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
|
|
@ -113,11 +113,11 @@ class Communication:
|
|||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
self.arm_state = not self.disarm()
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'takeoff' or msg.data == 'land' or msg.data == 'loiter' or msg.data == 'idle':
|
||||
self.mission = msg.data
|
||||
|
|
@ -130,21 +130,21 @@ class Communication:
|
|||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": arming failed!")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": arming failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": disarming failed!")
|
||||
return False
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode+"failed")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode+"failed")
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
|
@ -1,205 +0,0 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
|
||||
from nav_msgs.msg import Odometry
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
import sys
|
||||
|
||||
class PX4Communication:
|
||||
|
||||
def __init__(self):
|
||||
|
||||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.current_heading = None
|
||||
self.takeoff_height = 1
|
||||
|
||||
self.target_pose = PoseStamped()
|
||||
self.target_vel = TwistStamped()
|
||||
self.global_target = None
|
||||
|
||||
self.vehicle = sys.argv[1]
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.flag = 0
|
||||
self.flight_mode = None
|
||||
|
||||
|
||||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/cmd", String, self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.pose_target_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
|
||||
self.twist_target_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/ground_truth/odom', Odometry, queue_size=10)
|
||||
self.imu_groundtruth_pub = rospy.Publisher('/xtdrone/ground_truth/imu', Imu, queue_size=10)
|
||||
self.pose_groundtruth_pub = rospy.Publisher('/xtdrone/ground_truth/pose', PoseStamped, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode)
|
||||
self.gazeboModelstate = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
|
||||
|
||||
|
||||
print("PX4 Communication Initialized!")
|
||||
|
||||
|
||||
def start(self):
|
||||
rospy.init_node("px4_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
'''
|
||||
while(rospy.is_shutdown):
|
||||
if(self.flag==0):
|
||||
self.pose_target_pub.publish(self.target_pose)
|
||||
else:
|
||||
self.twist_target_pub.publish(self.target_vel)
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
|
||||
if(self.disarm()):
|
||||
|
||||
self.flight_mode = "DISARMED"
|
||||
try:
|
||||
response = self.gazeboModelstate (self.vehicle,'ground_plane')
|
||||
except rospy.ServiceException, e:
|
||||
print "Gazebo model state service call failed: %s"%e
|
||||
odom = Odometry()
|
||||
odom.header = response.header
|
||||
odom.pose.pose = response.pose
|
||||
odom.twist.twist = response.twist
|
||||
self.odom_groundtruth_pub.publish(odom)
|
||||
'''
|
||||
imu = Imu()
|
||||
imu.header = response.header
|
||||
imu.orientation = response.pose.orientation
|
||||
imu.angular_velocity = response.twist.angular
|
||||
imu.linear_acceleration =
|
||||
'''
|
||||
rate.sleep()
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose.position.x, self.target_pose.pose.position.y = self.flu2enu(msg.position.x, msg.position.y)
|
||||
self.target_pose.pose.position.z = msg.position.z
|
||||
target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose = msg
|
||||
target_yaw = self.q2yaw(msg.orientation)
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist.linear.x, self.target_vel.twist.linear.y = self.flu2enu(msg.linear.x, msg.linear.y)
|
||||
self.target_vel.twist.linear.z = msg.linear.z
|
||||
self.target_vel.twist.angular.x = 0
|
||||
self.target_vel.twist.angular.y = 0
|
||||
self.target_vel.twist.angular.z = msg.angular.z
|
||||
|
||||
def cmd_vel_enu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist = msg
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
return
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print('Armed'+str(self.arm_state))
|
||||
elif msg.data == 'DISARM':
|
||||
disarm_state =self.disarm()
|
||||
if disarm_state:
|
||||
self.arm_state = False
|
||||
print('Armed'+str(self.arm_state))
|
||||
else:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def flu2enu(self, x_flu, y_flu):
|
||||
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
|
||||
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
|
||||
return x_enu, y_enu
|
||||
|
||||
def cmd_yaw(self, yaw):
|
||||
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
||||
self.target_pose.pose.orientation.x = quaternion[0]
|
||||
self.target_pose.pose.orientation.y = quaternion[1]
|
||||
self.target_pose.pose.orientation.z = quaternion[2]
|
||||
self.target_pose.pose.orientation.w = quaternion[3]
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print("Vehicle Arming Failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print("Vehicle Disarming Failed!")
|
||||
return False
|
||||
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print(self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print(self.flight_mode+"Failed")
|
||||
return False
|
||||
|
||||
def takeoff_detection(self):
|
||||
if self.local_pose.pose.position.z > 0.3 and self.offboard_state and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
px4_com = PX4Communication()
|
||||
px4_com.start()
|
||||
|
|
@ -1,194 +0,0 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
from multiprocessing import Process
|
||||
import sys
|
||||
|
||||
class PX4Communication:
|
||||
|
||||
def __init__(self,id):
|
||||
|
||||
self.id = id
|
||||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.current_heading = None
|
||||
self.takeoff_height = 1
|
||||
|
||||
self.target_pose = PoseStamped()
|
||||
self.target_vel=TwistStamped()
|
||||
self.global_target = None
|
||||
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.flag = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
||||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.pose_target_pub = rospy.Publisher("/uav"+str(self.id)+"/mavros/setpoint_position/local", PoseStamped, queue_size=10)
|
||||
self.twist_target_pub = rospy.Publisher("/uav"+str(self.id)+"/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/set_mode", SetMode)
|
||||
|
||||
|
||||
print("UAV"+str(self.id)+": "+"PX4 Communication Initialized!")
|
||||
|
||||
|
||||
def start(self):
|
||||
rospy.init_node("px4_uav"+str(self.id)+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
'''
|
||||
while(rospy.is_shutdown):
|
||||
if(self.flag==0):
|
||||
self.pose_target_pub.publish(self.target_pose)
|
||||
else:
|
||||
self.twist_target_pub.publish(self.target_vel)
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
|
||||
if(self.disarm()):
|
||||
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose.position.x, self.target_pose.pose.position.y = self.flu2enu(msg.position.x, msg.position.y)
|
||||
self.target_pose.pose.position.z = msg.position.z
|
||||
target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose = msg
|
||||
target_yaw = self.q2yaw(msg.orientation)
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist.linear.x, self.target_vel.twist.linear.y = self.flu2enu(msg.linear.x, msg.linear.y)
|
||||
self.target_vel.twist.linear.z = msg.linear.z
|
||||
self.target_vel.twist.angular.x = 0
|
||||
self.target_vel.twist.angular.y = 0
|
||||
self.target_vel.twist.angular.z = msg.angular.z
|
||||
|
||||
def cmd_vel_enu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist = msg
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
return
|
||||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
disarm_state =self.disarm()
|
||||
if disarm_state:
|
||||
self.arm_state = False
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
|
||||
self.mission = msg.data
|
||||
print("UAV"+str(self.id)+": "+msg.data)
|
||||
|
||||
elif not msg.data == self.flight_mode:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def flu2enu(self, x_flu, y_flu):
|
||||
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
|
||||
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
|
||||
return x_enu, y_enu
|
||||
|
||||
def cmd_yaw(self, yaw):
|
||||
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
||||
self.target_pose.pose.orientation.x = quaternion[0]
|
||||
self.target_pose.pose.orientation.y = quaternion[1]
|
||||
self.target_pose.pose.orientation.z = quaternion[2]
|
||||
self.target_pose.pose.orientation.w = quaternion[3]
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+"Vehicle Arming Failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+"Vehicle Disarming Failed!")
|
||||
return False
|
||||
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode+"Failed")
|
||||
return False
|
||||
|
||||
def takeoff_detection(self):
|
||||
if self.local_pose.pose.position.z > 0.3 and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
uav_num = int(sys.argv[1])
|
||||
for i in range(uav_num):
|
||||
px4_com = PX4Communication(i+1)
|
||||
p = Process(target=px4_com.start)
|
||||
p.start()
|
||||
|
|
@ -1,194 +0,0 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
from multiprocessing import Process
|
||||
import sys
|
||||
|
||||
class PX4Communication:
|
||||
|
||||
def __init__(self,id):
|
||||
|
||||
self.id = id
|
||||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.current_heading = None
|
||||
self.takeoff_height = 1
|
||||
|
||||
self.target_pose = PoseStamped()
|
||||
self.target_vel=TwistStamped()
|
||||
self.global_target = None
|
||||
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.flag = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
||||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.pose_target_pub = rospy.Publisher("/uav"+str(self.id)+"/mavros/setpoint_position/local", PoseStamped, queue_size=10)
|
||||
self.twist_target_pub = rospy.Publisher("/uav"+str(self.id)+"/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/set_mode", SetMode)
|
||||
|
||||
|
||||
print("UAV"+str(self.id)+": "+"PX4 Communication Initialized!")
|
||||
|
||||
|
||||
def start(self):
|
||||
rospy.init_node("px4_uav"+str(self.id)+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
'''
|
||||
while(rospy.is_shutdown):
|
||||
if(self.flag==0):
|
||||
self.pose_target_pub.publish(self.target_pose)
|
||||
else:
|
||||
self.twist_target_pub.publish(self.target_vel)
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
|
||||
if(self.disarm()):
|
||||
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose.position.x, self.target_pose.pose.position.y = self.flu2enu(msg.position.x, msg.position.y)
|
||||
self.target_pose.pose.position.z = msg.position.z
|
||||
target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.flag = 0
|
||||
self.target_pose.pose = msg
|
||||
target_yaw = self.q2yaw(msg.orientation)
|
||||
self.cmd_yaw(target_yaw)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist.linear.x, self.target_vel.twist.linear.y = self.flu2enu(msg.linear.x, msg.linear.y)
|
||||
self.target_vel.twist.linear.z = msg.linear.z
|
||||
self.target_vel.twist.angular.x = 0
|
||||
self.target_vel.twist.angular.y = 0
|
||||
self.target_vel.twist.angular.z = msg.angular.z
|
||||
|
||||
def cmd_vel_enu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_vel.twist = msg
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
return
|
||||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
disarm_state =self.disarm()
|
||||
if disarm_state:
|
||||
self.arm_state = False
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
|
||||
self.mission = msg.data
|
||||
print("UAV"+str(self.id)+": "+msg.data)
|
||||
|
||||
elif not msg.data == self.flight_mode:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def flu2enu(self, x_flu, y_flu):
|
||||
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
|
||||
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
|
||||
return x_enu, y_enu
|
||||
|
||||
def cmd_yaw(self, yaw):
|
||||
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
||||
self.target_pose.pose.orientation.x = quaternion[0]
|
||||
self.target_pose.pose.orientation.y = quaternion[1]
|
||||
self.target_pose.pose.orientation.z = quaternion[2]
|
||||
self.target_pose.pose.orientation.w = quaternion[3]
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+"Vehicle Arming Failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+"Vehicle Disarming Failed!")
|
||||
return False
|
||||
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode+"Failed")
|
||||
return False
|
||||
|
||||
def takeoff_detection(self):
|
||||
if self.local_pose.pose.position.z > 0.3 and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
uav_num = int(sys.argv[1])
|
||||
for i in range(uav_num):
|
||||
px4_com = PX4Communication(i)
|
||||
p = Process(target=px4_com.start)
|
||||
p.start()
|
||||
|
|
@ -1,246 +0,0 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, Pose, TwistStamped, Twist, Vector3Stamped, Vector3
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
from multiprocessing import Process
|
||||
import sys
|
||||
|
||||
class PX4Communication:
|
||||
|
||||
def __init__(self,id):
|
||||
|
||||
self.id = id
|
||||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.current_heading = None
|
||||
self.takeoff_height = 1
|
||||
self.hover_flag = 0
|
||||
|
||||
self.target_motion = PositionTarget()
|
||||
self.global_target = None
|
||||
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.flag = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
||||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_accel_flu", Vector3, self.cmd_accel_flu_callback)
|
||||
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_accel_enu", Vector3, self.cmd_accel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.target_motion_pub = rospy.Publisher("/uav"+str(self.id)+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/set_mode", SetMode)
|
||||
|
||||
|
||||
print("UAV"+str(self.id)+": "+"PX4 Communication Initialized!")
|
||||
|
||||
|
||||
def start(self):
|
||||
rospy.init_node("px4_uav"+str(self.id)+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
'''
|
||||
while(rospy.is_shutdown):
|
||||
self.target_motion_pub.publish(self.target_motion)
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
|
||||
if(self.disarm()):
|
||||
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0):
|
||||
target_raw_pose = PositionTarget()
|
||||
target_raw_pose.coordinate_frame = 1
|
||||
|
||||
target_raw_pose.position.x = x
|
||||
target_raw_pose.position.y = y
|
||||
target_raw_pose.position.z = z
|
||||
|
||||
target_raw_pose.velocity.x = vx
|
||||
target_raw_pose.velocity.y = vy
|
||||
target_raw_pose.velocity.z = vz
|
||||
|
||||
target_raw_pose.acceleration_or_force.x = afx
|
||||
target_raw_pose.acceleration_or_force.y = afy
|
||||
target_raw_pose.acceleration_or_force.z = afz
|
||||
|
||||
target_raw_pose.yaw = yaw
|
||||
target_raw_pose.yaw_rate = yaw_rate
|
||||
|
||||
if(self.flag == 0):
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
|
||||
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
|
||||
+ PositionTarget.IGNORE_YAW
|
||||
if(self.flag == 1):
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
|
||||
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
|
||||
+ PositionTarget.IGNORE_YAW
|
||||
if(self.flag == 2):
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
|
||||
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
|
||||
+ PositionTarget.IGNORE_YAW
|
||||
|
||||
return target_raw_pose
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.flag = 0
|
||||
target_x_enu, target_y_enu = self.flu2enu(msg.position.x, msg.position.y)
|
||||
target_z_enu = msg.position.z
|
||||
#target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.target_motion = self.construct_target(x=target_x_enu,y=target_y_enu,z=target_z_enu)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.flag = 0
|
||||
#target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
if self.hover_flag == 0:
|
||||
self.flag = 1
|
||||
target_vx_enu, target_vy_enu = self.flu2enu(msg.linear.x, msg.linear.y)
|
||||
target_vz_enu = msg.linear.z
|
||||
target_yaw_rate = msg.angular.z
|
||||
self.target_motion = self.construct_target(vx=target_vx_enu,vy=target_vy_enu,vz=target_vz_enu,yaw_rate=target_yaw_rate)
|
||||
|
||||
def cmd_vel_enu_callback(self, msg):
|
||||
if self.hover_flag == 0:
|
||||
self.flag = 1
|
||||
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)
|
||||
|
||||
def cmd_accel_flu_callback(self, msg):
|
||||
if self.hover_flag == 0:
|
||||
self.flag = 2
|
||||
target_afx_enu, target_afy_enu = self.flu2enu(msg.x, msg.y)
|
||||
target_afz_enu = msg.z
|
||||
self.target_motion = self.construct_target(afx=target_afx_enu,afy=target_afy_enu,afz=target_afz_enu)
|
||||
def cmd_accel_enu_callback(self, msg):
|
||||
if self.hover_flag == 0:
|
||||
self.flag = 2
|
||||
self.target_motion = self.construct_target(afx=msg.x,afy=msg.y,afz=msg.z)
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
return
|
||||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
disarm_state =self.disarm()
|
||||
if disarm_state:
|
||||
self.arm_state = False
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
|
||||
self.mission = msg.data
|
||||
print("UAV"+str(self.id)+": "+msg.data)
|
||||
|
||||
elif not msg.data == self.flight_mode:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def flu2enu(self, x_flu, y_flu):
|
||||
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
|
||||
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
|
||||
return x_enu, y_enu
|
||||
|
||||
def cmd_yaw(self, yaw):
|
||||
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
||||
self.target_pose.pose.orientation.x = quaternion[0]
|
||||
self.target_pose.pose.orientation.y = quaternion[1]
|
||||
self.target_pose.pose.orientation.z = quaternion[2]
|
||||
self.target_pose.pose.orientation.w = quaternion[3]
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": Vehicle Arming Failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": Vehicle Disarming Failed!")
|
||||
return False
|
||||
|
||||
def hover(self):
|
||||
self.flag = 0
|
||||
self.target_motion = self.construct_target(x=self.local_pose.pose.position.x,y=self.local_pose.pose.position.y,z=self.local_pose.pose.position.z)
|
||||
|
||||
def flight_mode_switch(self):
|
||||
if self.flight_mode == 'HOVER':
|
||||
self.hover_flag = 1
|
||||
self.hover()
|
||||
print("UAV"+str(self.id)+": Hover")
|
||||
elif self.flightModeService(custom_mode=self.flight_mode):
|
||||
self.hover_flag = 0
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode+"Failed")
|
||||
return False
|
||||
|
||||
def takeoff_detection(self):
|
||||
if self.local_pose.pose.position.z > 0.3 and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
uav_num = int(sys.argv[1])
|
||||
for i in range(uav_num):
|
||||
px4_com = PX4Communication(i+1)
|
||||
p = Process(target=px4_com.start)
|
||||
p.start()
|
||||
|
|
@ -35,32 +35,32 @@ class Communication:
|
|||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+self.vehicle_id+'ground_truth/odom', Odometry, queue_size=10)
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+'_'+self.vehicle_id+'/ground_truth/odom', Odometry, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.gazeboModelstate = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
|
||||
|
||||
print(self.vehicle_type+self.vehicle_id+": "+"communication initialized")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+"communication initialized")
|
||||
|
||||
def start(self):
|
||||
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
|
||||
rospy.init_node(self.vehicle_type+'_'+self.vehicle_id+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
|
|
@ -138,15 +138,15 @@ class Communication:
|
|||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
self.arm_state =not self.disarm()
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
|
||||
self.mission = msg.data
|
||||
print(self.vehicle_type+self.vehicle_id+": "+msg.data)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+msg.data)
|
||||
|
||||
elif not msg.data == self.flight_mode:
|
||||
self.flight_mode = msg.data
|
||||
|
|
@ -156,22 +156,22 @@ class Communication:
|
|||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": arming failed!")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": arming failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": disarming failed!")
|
||||
return False
|
||||
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode+"failed")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode+"failed")
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
|
@ -37,29 +37,29 @@ class Communication:
|
|||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback)
|
||||
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.imu_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback)
|
||||
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+self.vehicle_id+'ground_truth/odom', Odometry, queue_size=10)
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+'_'+self.vehicle_id+'/ground_truth/odom', Odometry, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.gazeboModelstate = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
|
||||
self.transition = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+'/mavros/cmd/vtol_transition', CommandVtolTransition)
|
||||
self.transition = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+'/mavros/cmd/vtol_transition', CommandVtolTransition)
|
||||
self.transition_state = 'multirotor'
|
||||
print(self.transition_state)
|
||||
print(self.transition(state=3))
|
||||
|
|
@ -67,7 +67,7 @@ class Communication:
|
|||
print("communication initialized")
|
||||
|
||||
def start(self):
|
||||
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
|
||||
rospy.init_node(self.vehicle_type+'_'+self.vehicle_id+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
|
|
@ -182,25 +182,25 @@ class Communication:
|
|||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print(self.vehicle_type+self.vehicle_id+": armed"+str(self.arm_state))
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": armed"+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
self.arm_state = not self.disarm()
|
||||
print(self.vehicle_type+self.vehicle_id+": armed"+str(self.arm_state))
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": armed"+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'multirotor':
|
||||
self.transition_state = msg.data
|
||||
print(self.vehicle_type+self.vehicle_id+':'+msg.data)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+':'+msg.data)
|
||||
print(self.transition(state=3))
|
||||
|
||||
elif msg.data == 'plane':
|
||||
self.transition_state = msg.data
|
||||
print(self.vehicle_type+self.vehicle_id+':'+msg.data)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+':'+msg.data)
|
||||
print(self.transition(state=4))
|
||||
|
||||
elif msg.data == 'loiter' or msg.data == 'idle':
|
||||
self.plane_mission = msg.data
|
||||
print(self.vehicle_type+self.vehicle_id+':'+self.plane_mission)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+':'+self.plane_mission)
|
||||
|
||||
else:
|
||||
self.flight_mode = msg.data
|
||||
|
|
@ -213,14 +213,14 @@ class Communication:
|
|||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": arming failed!")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": arming failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": disarming failed!")
|
||||
return False
|
||||
|
||||
def hover(self):
|
||||
|
|
@ -232,13 +232,13 @@ class Communication:
|
|||
if self.flight_mode == 'HOVER':
|
||||
self.hover_flag = 1
|
||||
self.hover()
|
||||
print(self.vehicle_type+self.vehicle_id+": Hover")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": Hover")
|
||||
elif self.flightModeService(custom_mode=self.flight_mode):
|
||||
self.hover_flag = 0
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode+"Failed")
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode+"Failed")
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandVtolTransition
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
|
||||
from nav_msgs.msg import Odometry
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
import sys
|
||||
|
||||
|
||||
trans = rospy.ServiceProxy('/mavros/cmd/vtol_transition', CommandVtolTransition)
|
||||
print(trans(state = 3))
|
||||
|
|
@ -1,22 +0,0 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
|
||||
from nav_msgs.msg import Odometry
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
import sys
|
||||
from px4_communication import PX4Communication
|
||||
px4_com = PX4Communication()
|
||||
rospy.init_node("px4_communication")
|
||||
rate = rospy.Rate(100)
|
||||
cnt = 0
|
||||
while(rospy.is_shutdown):
|
||||
px4_com.pose_target_pub.publish(px4_com.target_pose)
|
||||
if cnt == 1000:
|
||||
px4_com.target_pose.pose.position.x = 10
|
||||
|
|
@ -1,195 +0,0 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
MAX_LIN_VEL = 1
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.02
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
msg = """
|
||||
Control Your XTDrone!
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s or k : hover
|
||||
0~9 : extendable mission
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rospy.init_node('keyboard_control')
|
||||
cmd_vel_flu_pub = rospy.Publisher('/xtdrone/cmd_vel_flu', Twist, queue_size=10)
|
||||
cmd_pub = rospy.Publisher('/xtdrone/cmd',String,queue_size=10)
|
||||
cmd = String()
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print(msg)
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print(msg)
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print(msg)
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print(msg)
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
#cmd = 'AUTO.TAKEOFF'
|
||||
cmd = ''
|
||||
print(msg)
|
||||
print('Takeoff mode is disenabled now')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print(msg)
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print(msg)
|
||||
print('Landing')
|
||||
elif key == 's' or key == 'k' :
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
else:
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
twist = Twist()
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
cmd_vel_flu_pub.publish(twist)
|
||||
cmd_pub.publish(cmd)
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist = Twist()
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
cmd_vel_flu_pub.publish(twist)
|
||||
cmd_pub.publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -1,264 +0,0 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LIN_VEL = 1
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.02
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
cmd_vel_mask = False
|
||||
ctrl_leader = False
|
||||
|
||||
msg2all = """
|
||||
Control Your XTDrone!
|
||||
To all drones (press g to control the leader)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff/land
|
||||
b : offboard
|
||||
s : hover(offboard mode) and remove the mask of keyboard control
|
||||
k : hover(hover mode) and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control the leader
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
msg2leader = """
|
||||
Control Your XTDrone!
|
||||
To the leader (press g to control all drones)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s or k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control all drones
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def print_msg():
|
||||
if ctrl_leader:
|
||||
print(msg2leader)
|
||||
else:
|
||||
print(msg2all)
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
uav_num = int(sys.argv[1])
|
||||
rospy.init_node('keyboard_control')
|
||||
multi_cmd_vel_flu_pub = [None]*uav_num
|
||||
multi_cmd_pub = [None]*uav_num
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_vel", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print_msg()
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print_msg()
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print_msg()
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
#cmd = 'AUTO.TAKEOFF'
|
||||
cmd = ''
|
||||
print(msg)
|
||||
print('Takeoff mode is disenabled now')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print_msg()
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print_msg()
|
||||
print('Landing')
|
||||
elif key == 'g':
|
||||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k':
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
else:
|
||||
for i in range(10):
|
||||
if key == str(i):
|
||||
cmd = 'mission'+key
|
||||
print_msg()
|
||||
print(cmd)
|
||||
cmd_vel_mask = True
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
for i in range(uav_num):
|
||||
if ctrl_leader:
|
||||
leader_cmd_vel_pub.publish(twist)
|
||||
leader_cmd_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
cmd = ''
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -1,156 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped,Point
|
||||
from std_msgs.msg import String
|
||||
import sys
|
||||
Kp = 0.15
|
||||
uav_num = int(sys.argv[1])
|
||||
leader_id = 0
|
||||
vision_pose = [None]*(uav_num+1)
|
||||
relative_pose = [None]*(uav_num+1)
|
||||
follower_vel_enu_pub = [None]*(uav_num+1)
|
||||
relative_pose_pub = [None]*(uav_num+1)
|
||||
follower_cmd_vel = [None]*(uav_num+1)
|
||||
leader_cmd_vel = Twist()
|
||||
|
||||
for i in range(uav_num):
|
||||
vision_pose[i]=PoseStamped()
|
||||
relative_pose[i]=PoseStamped()
|
||||
follower_cmd_vel[i]=Twist()
|
||||
|
||||
# uav_5 is the leader in the formation mission
|
||||
formation=[None]*10
|
||||
|
||||
|
||||
'''2x5 formation 0
|
||||
fflff
|
||||
fffff
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
for i in range(uav_num):
|
||||
if i < (uav_num/2):
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i] = Point(0,i,0)
|
||||
else:
|
||||
formation_temp[i] = Point(0,-i-1,0)
|
||||
else:
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i] = Point( -2, 1+i-(uav_num/2),0 )
|
||||
else:
|
||||
formation_temp[i] = Point( -2, (uav_num/2)-i ,0)
|
||||
formation[0] = formation_temp
|
||||
|
||||
''' 'L' formation 1
|
||||
lffff
|
||||
f
|
||||
f
|
||||
f
|
||||
f f
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
for i in range(uav_num):
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i] = Point(0,i,0)
|
||||
elif i+1 != uav_num:
|
||||
formation_temp[i] = Point(-i-1,0,0)
|
||||
else:
|
||||
formation_temp[i] = Point(-4,4,0)
|
||||
formation[1] = formation_temp
|
||||
|
||||
''' '△' formation 2
|
||||
l
|
||||
f f
|
||||
f f f
|
||||
f f f f
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
formation_temp[0]=Point(0,0,0)
|
||||
formation_temp[1]=Point(-2,-2,0);formation_temp[2]=Point(-2,2,0)
|
||||
formation_temp[3]=Point(-4,-4,0);formation_temp[4]=Point(-4,0,0);formation_temp[6]=Point(-4,4,0)
|
||||
formation_temp[9]=Point(-6,-6,0);formation_temp[7]=Point(-6,-2,0);formation_temp[5]=Point(-6,2,0);formation_temp[8]=Point(-6,6,0)
|
||||
formation[2] = formation_temp
|
||||
|
||||
|
||||
|
||||
|
||||
def leader_cmd_vel_callback(msg):
|
||||
global leader_cmd_vel
|
||||
leader_cmd_vel = msg
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
global vision_pose
|
||||
relative_pose[uav_id].pose.position.x = vision_pose[uav_id].pose.pose.position.x - vision_pose[leader_id].pose.pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = vision_pose[uav_id].pose.pose.position.y - vision_pose[leader_id].pose.pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = vision_pose[uav_id].pose.pose.position.z - vision_pose[leader_id].pose.pose.position.z
|
||||
|
||||
|
||||
formation_id = 0
|
||||
|
||||
'''
|
||||
formation.append( [[-1,-1],[0,-1],[1,-1],[-1,0],[0,0],[0,1]] ) #2x3 formation
|
||||
# f l f
|
||||
# f f f
|
||||
formation.append( [[-2,-2],[0,-2],[2,-2],[-1,-1],[0,0],[1,-1]] )#Trianglar formation
|
||||
# l
|
||||
# f f
|
||||
# f f f
|
||||
formation.append( [[0,-4],[0,-2],[0,-6],[-2,0],[0,0],[2,0]] ) #'T' formation
|
||||
# f l f
|
||||
# f
|
||||
# f
|
||||
# f
|
||||
formation_id = int(sys.argv[2])
|
||||
'''
|
||||
|
||||
def leader_cmd_vel_callback(msg):
|
||||
global leader_cmd_vel
|
||||
leader_cmd_vel = msg
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
relative_pose[uav_id].pose.position.x = vision_pose[uav_id].pose.position.x - vision_pose[leader_id].pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = vision_pose[uav_id].pose.position.y - vision_pose[leader_id].pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = vision_pose[uav_id].pose.position.z - vision_pose[leader_id].pose.position.z
|
||||
|
||||
vision_pose_callback = [None]*(uav_num+1)
|
||||
|
||||
rospy.init_node('formation_control')
|
||||
|
||||
def vision_pose_callback(msg,id):
|
||||
global vision_pose
|
||||
vision_pose[id] = msg
|
||||
calculate_relative_pose(id)
|
||||
|
||||
def cmd_callback(msg):
|
||||
global formation_id
|
||||
if not msg.data == '':
|
||||
formation_id = int(msg.data[-1])
|
||||
print("Switch to Formation"+str(formation_id))
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
rospy.Subscriber("/uav"+str(uav_id)+"/mavros/vision_pose/pose", PoseStamped , vision_pose_callback,uav_id)
|
||||
|
||||
leader_cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, leader_cmd_vel_callback)
|
||||
formation_switch_sub = rospy.Subscriber("/xtdrone/leader_cmd",String, cmd_callback)
|
||||
leader_vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(leader_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
if uav_id != leader_id:
|
||||
follower_vel_enu_pub[i] = rospy.Publisher(
|
||||
'/xtdrone/uav'+str(i)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
|
||||
rate = rospy.Rate(100)
|
||||
while(1):
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
if uav_id != leader_id:
|
||||
follower_cmd_vel[uav_id].linear.x = (leader_cmd_vel.linear.x+Kp*(formation[formation_id][uav_id].x- relative_pose[uav_id].pose.position.x) )
|
||||
follower_cmd_vel[uav_id].linear.y = (leader_cmd_vel.linear.y+Kp*(formation[formation_id][uav_id].y- relative_pose[uav_id].pose.position.y) )
|
||||
follower_cmd_vel[uav_id].linear.z = leader_cmd_vel.linear.z
|
||||
follower_cmd_vel[uav_id].angular.x = 0.0; follower_cmd_vel[uav_id].angular.y = 0.0; follower_cmd_vel[uav_id].angular.z = 0.0
|
||||
follower_vel_enu_pub[uav_id].publish(follower_cmd_vel[uav_id])
|
||||
leader_vel_enu_pub.publish(leader_cmd_vel)
|
||||
rate.sleep()
|
||||
|
|
@ -1,207 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped,Point
|
||||
from std_msgs.msg import String
|
||||
import sys
|
||||
from pyquaternion import Quaternion
|
||||
import time
|
||||
KP_xy = 1.5
|
||||
KP_yaw = 2.8
|
||||
KP_z = 1
|
||||
max_vel_xy = 1
|
||||
max_vel_z = 0.6
|
||||
uav_num = int(sys.argv[1])
|
||||
leader_id = 0
|
||||
local_pose = [None]*(uav_num+1)
|
||||
relative_pose = [None]*(uav_num+1)
|
||||
follower_vel_enu_pub = [None]*(uav_num+1)
|
||||
relative_pose_pub = [None]*(uav_num+1)
|
||||
follower_cmd_vel = [None]*(uav_num+1)
|
||||
leader_cmd_vel = Twist()
|
||||
avoid_vel_z = [0]*(uav_num+1)
|
||||
avoid_pos_z = 0.3
|
||||
leader_height = 0
|
||||
hover = True
|
||||
avoid = False
|
||||
|
||||
for i in range(uav_num):
|
||||
local_pose[i]=PoseStamped()
|
||||
relative_pose[i]=PoseStamped()
|
||||
follower_cmd_vel[i]=Twist()
|
||||
|
||||
# uav_5 is the leader in the formation mission
|
||||
formation=[None]*10
|
||||
|
||||
|
||||
'''2x5 formation 0
|
||||
fflff
|
||||
fffff
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
for i in range(uav_num):
|
||||
if i+1 <= (uav_num/2):
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i] = Point(0,i,0)
|
||||
else:
|
||||
formation_temp[i] = Point(0,-i-1,0)
|
||||
else:
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i] = Point( -2, 1+i-(uav_num/2),0 )
|
||||
else:
|
||||
formation_temp[i] = Point( -2, (uav_num/2)-i ,0)
|
||||
formation[0] = formation_temp
|
||||
|
||||
''' 'L' formation 1
|
||||
lffff
|
||||
f
|
||||
f
|
||||
f
|
||||
f f
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
for i in range(uav_num):
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i] = Point(0,i,0)
|
||||
elif i+1 != uav_num:
|
||||
formation_temp[i] = Point(-i-1,0,0)
|
||||
else:
|
||||
formation_temp[i] = Point(-4,4,0)
|
||||
formation[1] = formation_temp
|
||||
|
||||
''' '△' formation 2
|
||||
l
|
||||
f f
|
||||
f f f
|
||||
f f f f
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
formation_temp[0]=Point(0,0,0)
|
||||
formation_temp[1]=Point(-2,-2,0);formation_temp[2]=Point(-2,2,0)
|
||||
formation_temp[3]=Point(-4,-4,0);formation_temp[4]=Point(-4,0,0);formation_temp[6]=Point(-4,4,0)
|
||||
formation_temp[9]=Point(-6,-6,0);formation_temp[7]=Point(-6,-2,0);formation_temp[5]=Point(-6,2,0);formation_temp[8]=Point(-6,6,0)
|
||||
formation[2] = formation_temp
|
||||
|
||||
|
||||
|
||||
|
||||
def leader_cmd_vel_callback(msg):
|
||||
global leader_cmd_vel, hover
|
||||
leader_cmd_vel = msg
|
||||
#print(hover)
|
||||
if msg.linear.z == 0:
|
||||
hover = True
|
||||
else:
|
||||
hover = False
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
global local_pose
|
||||
relative_pose[uav_id].pose.position.x = local_pose[uav_id].pose.pose.position.x - local_pose[leader_id].pose.pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = local_pose[uav_id].pose.pose.position.y - local_pose[leader_id].pose.pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = local_pose[uav_id].pose.pose.position.z - local_pose[leader_id].pose.pose.position.z
|
||||
|
||||
def delta_vel(target_pos, current_pos, KP, vel_max):
|
||||
delta_vel = KP*(target_pos-current_pos)
|
||||
if delta_vel > vel_max:
|
||||
delta_vel = vel_max
|
||||
return delta_vel
|
||||
|
||||
formation_id = 0
|
||||
|
||||
'''
|
||||
formation.append( [[-1,-1],[0,-1],[1,-1],[-1,0],[0,0],[0,1]] ) #2x3 formation
|
||||
# f l f
|
||||
# f f f
|
||||
formation.append( [[-2,-2],[0,-2],[2,-2],[-1,-1],[0,0],[1,-1]] )#Trianglar formation
|
||||
# l
|
||||
# f f
|
||||
# f f f
|
||||
formation.append( [[0,-4],[0,-2],[0,-6],[-2,0],[0,0],[2,0]] ) #'T' formation
|
||||
# f l f
|
||||
# f
|
||||
# f
|
||||
# f
|
||||
formation_id = int(sys.argv[2])
|
||||
'''
|
||||
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
relative_pose[uav_id].pose.position.x = local_pose[uav_id].pose.position.x - local_pose[leader_id].pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = local_pose[uav_id].pose.position.y - local_pose[leader_id].pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = local_pose[uav_id].pose.position.z - local_pose[leader_id].pose.position.z
|
||||
|
||||
local_pose_callback = [None]*(uav_num+1)
|
||||
|
||||
rospy.init_node('formation_control')
|
||||
|
||||
def local_pose_callback(msg,id):
|
||||
global local_pose
|
||||
local_pose[id] = msg
|
||||
calculate_relative_pose(id)
|
||||
|
||||
def cmd_callback(msg):
|
||||
global formation_id
|
||||
if not msg.data == '':
|
||||
formation_id = int(msg.data[-1])
|
||||
print("Switch to Formation"+str(formation_id))
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
rospy.Subscriber("/uav"+str(uav_id)+"/mavros/local_position/pose", PoseStamped , local_pose_callback,uav_id)
|
||||
|
||||
leader_cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, leader_cmd_vel_callback)
|
||||
formation_switch_sub = rospy.Subscriber("/xtdrone/leader_cmd",String, cmd_callback)
|
||||
leader_vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(leader_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
if uav_id != leader_id:
|
||||
follower_vel_enu_pub[uav_id] = rospy.Publisher(
|
||||
'/xtdrone/uav'+str(uav_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
for i in range(10):
|
||||
leader_height = local_pose[leader_id].pose.position.z
|
||||
time.sleep(0.1)
|
||||
rate = rospy.Rate(100)
|
||||
|
||||
while(1):
|
||||
|
||||
leader_orientation = Quaternion(local_pose[leader_id].pose.orientation.w,local_pose[leader_id].pose.orientation.x,local_pose[leader_id].pose.orientation.y,local_pose[leader_id].pose.orientation.z)
|
||||
leader_yaw = leader_orientation.yaw_pitch_roll[0]
|
||||
# Avoid collision with other drones
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
for j in range(1,uav_num-i):
|
||||
if pow(local_pose[uav_id].pose.position.x-local_pose[uav_id+j].pose.position.x,2)\
|
||||
+pow(local_pose[uav_id].pose.position.y-local_pose[uav_id+j].pose.position.y,2)\
|
||||
+pow(local_pose[uav_id].pose.position.z-local_pose[uav_id+j].pose.position.z,2) < 1:
|
||||
avoid = True
|
||||
avoid_vel_z[uav_id] = KP_z*avoid_pos_z
|
||||
avoid_vel_z[uav_id+j] = -KP_z*avoid_pos_z
|
||||
else:
|
||||
avoid_vel_z[uav_id] = 0
|
||||
avoid_vel_z[uav_id+j] = 0
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
if uav_id != leader_id:
|
||||
follower_cmd_vel[uav_id].linear.x = leader_cmd_vel.linear.x+delta_vel(formation[formation_id][uav_id].x,relative_pose[uav_id].pose.position.x,KP_xy, max_vel_xy)
|
||||
follower_cmd_vel[uav_id].linear.y = leader_cmd_vel.linear.y+delta_vel(formation[formation_id][uav_id].y,relative_pose[uav_id].pose.position.y, KP_xy, max_vel_xy)
|
||||
follower_cmd_vel[uav_id].linear.z = leader_cmd_vel.linear.z + delta_vel(leader_height,local_pose[uav_id].pose.position.z, KP_z, max_vel_z) + avoid_vel_z[uav_id] - avoid_vel_z[leader_id]
|
||||
|
||||
orientation = Quaternion(local_pose[uav_id].pose.orientation.w,local_pose[uav_id].pose.orientation.x,local_pose[uav_id].pose.orientation.y,local_pose[uav_id].pose.orientation.z)
|
||||
yaw = orientation.yaw_pitch_roll[0]
|
||||
follower_cmd_vel[uav_id].angular.x = 0.0; follower_cmd_vel[uav_id].angular.y = 0.0; follower_cmd_vel[uav_id].angular.z = KP_yaw*(leader_yaw - yaw)
|
||||
|
||||
follower_vel_enu_pub[uav_id].publish(follower_cmd_vel[uav_id])
|
||||
|
||||
if hover:
|
||||
leader_cmd_vel.linear.z = delta_vel(leader_height,local_pose[leader_id].pose.position.z, KP_z, max_vel_z) + avoid_vel_z[leader_id]
|
||||
else:
|
||||
leader_cmd_vel.linear.z = leader_cmd_vel.linear.z + avoid_vel_z[leader_id]
|
||||
if not avoid:
|
||||
leader_height = local_pose[leader_id].pose.position.z
|
||||
leader_vel_enu_pub.publish(leader_cmd_vel)
|
||||
|
||||
print(leader_height)
|
||||
|
||||
rate.sleep()
|
||||
|
|
@ -1,140 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped
|
||||
from std_msgs.msg import String
|
||||
import sys
|
||||
from pyquaternion import Quaternion
|
||||
import time
|
||||
KP_xy = 1
|
||||
KP_z = 1.5
|
||||
KP_yaw = 2.8
|
||||
max_vel_xy = 1
|
||||
max_vel_z = 0.6
|
||||
uav_num = int(sys.argv[1])
|
||||
leader_id = 4
|
||||
local_pose = [None]*(uav_num+1)
|
||||
relative_pose = [None]*(uav_num+1)
|
||||
follower_vel_enu_pub = [None]*(uav_num+1)
|
||||
relative_pose_pub = [None]*(uav_num+1)
|
||||
follower_cmd_vel = [None]*(uav_num+1)
|
||||
leader_cmd_vel = Twist()
|
||||
avoid_vel_z = [0]*(uav_num+1)
|
||||
avoid_pos_z = 0.3
|
||||
leader_height = 0
|
||||
hover = True
|
||||
avoid = False
|
||||
|
||||
for i in range(uav_num):
|
||||
local_pose[i]=PoseStamped()
|
||||
relative_pose[i]=PoseStamped()
|
||||
follower_cmd_vel[i]=Twist()
|
||||
|
||||
formation=[]
|
||||
formation.append( [[-1,-1],[0,-1],[1,-1],[-1,0],[0,0],[1,0]] ) #2x3 formation
|
||||
# i i i
|
||||
# i i i
|
||||
formation.append( [[-2,-2],[0,-2],[2,-2],[-1,-1],[0,0],[1,-1]] )#Trianglar formation
|
||||
# i
|
||||
# i i
|
||||
# i i i
|
||||
formation.append( [[0,-4],[0,-2],[0,-6],[-2,0],[0,0],[2,0]] ) #'T' formation
|
||||
# i i i
|
||||
# i
|
||||
# i
|
||||
# i
|
||||
formation_id = 0
|
||||
|
||||
|
||||
def leader_cmd_vel_callback(msg):
|
||||
global leader_cmd_vel, hover
|
||||
leader_cmd_vel = msg
|
||||
if msg.linear.z == 0:
|
||||
hover = True
|
||||
else:
|
||||
hover = False
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
relative_pose[uav_id].pose.position.x = local_pose[uav_id].pose.position.x - local_pose[leader_id].pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = local_pose[uav_id].pose.position.y - local_pose[leader_id].pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = local_pose[uav_id].pose.position.z - local_pose[leader_id].pose.position.z
|
||||
|
||||
def delta_vel(target_pos, current_pos, KP, vel_max):
|
||||
delta_vel = KP*(target_pos-current_pos)
|
||||
if delta_vel > vel_max:
|
||||
delta_vel = vel_max
|
||||
return delta_vel
|
||||
|
||||
local_pose_callback = [None]*(uav_num+1)
|
||||
|
||||
rospy.init_node('formation_control')
|
||||
|
||||
|
||||
def local_pose_callback(msg,id):
|
||||
global local_pose
|
||||
local_pose[id] = msg
|
||||
calculate_relative_pose(id)
|
||||
|
||||
def cmd_callback(msg):
|
||||
global formation_id
|
||||
if not msg.data == '':
|
||||
formation_id = int(msg.data[-1])
|
||||
print("Switch to Formation"+str(formation_id))
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
rospy.Subscriber("/uav"+str(uav_id)+"/mavros/local_position/pose", PoseStamped , local_pose_callback,uav_id)
|
||||
|
||||
leader_cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, leader_cmd_vel_callback)
|
||||
formation_switch_sub = rospy.Subscriber("/xtdrone/leader_cmd",String, cmd_callback)
|
||||
leader_vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(leader_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
if uav_id != leader_id:
|
||||
follower_vel_enu_pub[uav_id] = rospy.Publisher(
|
||||
'/xtdrone/uav'+str(uav_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
|
||||
for i in range(10):
|
||||
leader_height = local_pose[leader_id].pose.position.z
|
||||
time.sleep(0.1)
|
||||
rate = rospy.Rate(100)
|
||||
while(1):
|
||||
leader_orientation = Quaternion(local_pose[leader_id].pose.orientation.w,local_pose[leader_id].pose.orientation.x,local_pose[leader_id].pose.orientation.y,local_pose[leader_id].pose.orientation.z)
|
||||
leader_yaw = leader_orientation.yaw_pitch_roll[0]
|
||||
# Avoid collision with other drones
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
for j in range(1,uav_num-i):
|
||||
if pow(local_pose[uav_id].pose.position.x-local_pose[uav_id+j].pose.position.x,2)\
|
||||
+pow(local_pose[uav_id].pose.position.y-local_pose[uav_id+j].pose.position.y,2)\
|
||||
+pow(local_pose[uav_id].pose.position.z-local_pose[uav_id+j].pose.position.z,2) < 0.6:
|
||||
avoid = True
|
||||
avoid_vel_z[uav_id] = KP_z*avoid_pos_z
|
||||
avoid_vel_z[uav_id+j] = -KP_z*avoid_pos_z
|
||||
else:
|
||||
avoid_vel_z[uav_id] = 0
|
||||
avoid_vel_z[uav_id+j] = 0
|
||||
avoid = False
|
||||
for i in range(uav_num):
|
||||
uav_id = i
|
||||
if uav_id != leader_id:
|
||||
follower_cmd_vel[uav_id].linear.x = leader_cmd_vel.linear.x+delta_vel(formation[formation_id][i][0],relative_pose[uav_id].pose.position.x,KP_xy, max_vel_xy)
|
||||
follower_cmd_vel[uav_id].linear.y = leader_cmd_vel.linear.y+delta_vel(formation[formation_id][i][1],relative_pose[uav_id].pose.position.y, KP_xy, max_vel_xy)
|
||||
follower_cmd_vel[uav_id].linear.z = leader_cmd_vel.linear.z + delta_vel(leader_height,local_pose[uav_id].pose.position.z, KP_z, max_vel_z) + avoid_vel_z[uav_id] - avoid_vel_z[leader_id]
|
||||
|
||||
orientation = Quaternion(local_pose[uav_id].pose.orientation.w,local_pose[uav_id].pose.orientation.x,local_pose[uav_id].pose.orientation.y,local_pose[uav_id].pose.orientation.z)
|
||||
yaw = orientation.yaw_pitch_roll[0]
|
||||
follower_cmd_vel[uav_id].angular.x = 0.0; follower_cmd_vel[uav_id].angular.y = 0.0; follower_cmd_vel[uav_id].angular.z = KP_yaw*(leader_yaw - yaw)
|
||||
|
||||
follower_vel_enu_pub[uav_id].publish(follower_cmd_vel[uav_id])
|
||||
|
||||
if hover:
|
||||
leader_cmd_vel.linear.z = delta_vel(leader_height,local_pose[leader_id].pose.position.z, KP_z, max_vel_z) + avoid_vel_z[leader_id]
|
||||
else:
|
||||
leader_cmd_vel.linear.z = leader_cmd_vel.linear.z + avoid_vel_z[leader_id]
|
||||
if not avoid:
|
||||
leader_height = local_pose[leader_id].pose.position.z
|
||||
leader_vel_enu_pub.publish(leader_cmd_vel)
|
||||
|
||||
rate.sleep()
|
||||
|
|
@ -1,40 +0,0 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys
|
||||
sys.path.append('/home/robin/catkin_ws/devel/lib/python2.7/dist-packages')
|
||||
from darknet_ros_msgs.msg import BoundingBoxes
|
||||
import time
|
||||
import math
|
||||
twist = Twist()
|
||||
def darknet_callback(data):
|
||||
for target in data.bounding_boxes:
|
||||
print(target.id)
|
||||
if(target.id==0):
|
||||
x_error=y_center-(target.ymax+target.ymin)/2
|
||||
y_error=x_center-(target.xmax+target.xmin)/2
|
||||
twist.linear.x = Kp_linear*x_error
|
||||
twist.linear.y = 0.0
|
||||
twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0
|
||||
twist.angular.y = 0.0
|
||||
twist.angular.z = Kp_angular*math.atan(y_error/x_error)
|
||||
pub.publish(twist)
|
||||
else:
|
||||
twist.linear.x = 0.0
|
||||
twist.linear.y = 0.0
|
||||
twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0
|
||||
twist.angular.y = 0.0
|
||||
twist.angular.z = 0.0
|
||||
pub.publish(twist)
|
||||
|
||||
Kp_linear=0.05
|
||||
Kp_angular=0.2/math.pi
|
||||
x_center=752/2
|
||||
y_center=480/2
|
||||
rospy.init_node('yolo_human_tracking')
|
||||
rospy.Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes, darknet_callback)
|
||||
pub = rospy.Publisher('/xtdrone/cmd_vel_flu', Twist, queue_size=10)
|
||||
rate = rospy.Rate(60)
|
||||
while(True):
|
||||
rate.sleep()
|
||||
|
|
@ -1,195 +0,0 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
MAX_LIN_VEL = 5
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.02
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
msg = """
|
||||
Control Your XTDrone!
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v : mission
|
||||
n : land
|
||||
b : offboard
|
||||
s or k : hover
|
||||
0~9 : extendable mission
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rospy.init_node('keyboard_control')
|
||||
cmd_vel_flu_pub = rospy.Publisher('/xtdrone/cmd_vel_flu', Twist, queue_size=10)
|
||||
cmd_pub = rospy.Publisher('/xtdrone/cmd',String,queue_size=10)
|
||||
cmd = String()
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print(msg)
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print(msg)
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print(msg)
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print(msg)
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
cmd = 'AUTO.MiSSION'
|
||||
print_msg()
|
||||
print('Mission')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print(msg)
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print(msg)
|
||||
print('Landing')
|
||||
elif key == 's' or key == 'k' :
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
print(msg)
|
||||
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
else:
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
twist = Twist()
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
cmd_vel_flu_pub.publish(twist)
|
||||
cmd_pub.publish(cmd)
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist = Twist()
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
cmd_vel_flu_pub.publish(twist)
|
||||
cmd_pub.publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -1,195 +0,0 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
MAX_LIN_accel = 1
|
||||
MAX_ANG_accel = 0.1
|
||||
LIN_accel_STEP_SIZE = 0.02
|
||||
ANG_accel_STEP_SIZE = 0.01
|
||||
|
||||
msg = """
|
||||
Control Your XTDrone!
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward accelocity (-1~1)
|
||||
a/d : increase/decrease leftward accelocity (-1~1)
|
||||
i/, : increase/decrease upward accelocity (-1~1)
|
||||
j/l : increase/decrease angular accelocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s or k : hover
|
||||
0~9 : extendable mission
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def accels(target_forward_accel, target_leftward_accel, target_upward_accel,target_angular_accel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_accel, target_leftward_accel, target_upward_accel, target_angular_accel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitaccelocity(accel):
|
||||
accel = constrain(accel, -MAX_LIN_accel, MAX_LIN_accel)
|
||||
return accel
|
||||
|
||||
def checkAngularLimitaccelocity(accel):
|
||||
accel = constrain(accel, -MAX_ANG_accel, MAX_ANG_accel)
|
||||
return accel
|
||||
|
||||
if __name__=="__main__":
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rospy.init_node('keyboard_control')
|
||||
cmd_accel_flu_pub = rospy.Publisher('/xtdrone/cmd_accel_flu', Twist, queue_size=10)
|
||||
cmd_pub = rospy.Publisher('/xtdrone/cmd',String,queue_size=10)
|
||||
cmd = String()
|
||||
|
||||
target_forward_accel = 0.0
|
||||
target_leftward_accel = 0.0
|
||||
target_upward_accel = 0.0
|
||||
target_angular_accel = 0.0
|
||||
control_forward_accel = 0.0
|
||||
control_leftward_accel = 0.0
|
||||
control_upward_accel = 0.0
|
||||
control_angular_accel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print(msg)
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_accel = checkLinearLimitaccelocity(target_forward_accel + LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'x' :
|
||||
target_forward_accel = checkLinearLimitaccelocity(target_forward_accel - LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'a' :
|
||||
target_leftward_accel = checkLinearLimitaccelocity(target_leftward_accel + LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'd' :
|
||||
target_leftward_accel = checkLinearLimitaccelocity(target_leftward_accel - LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'i' :
|
||||
target_upward_accel = checkLinearLimitaccelocity(target_upward_accel + LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == ',' :
|
||||
target_upward_accel = checkLinearLimitaccelocity(target_upward_accel - LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'j':
|
||||
target_angular_accel = checkAngularLimitaccelocity(target_angular_accel + ANG_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'l':
|
||||
target_angular_accel = checkAngularLimitaccelocity(target_angular_accel - ANG_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print(msg)
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print(msg)
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print(msg)
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
#cmd = 'AUTO.TAKEOFF'
|
||||
cmd = ''
|
||||
print(msg)
|
||||
print('Takeoff mode is disenabled now')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print(msg)
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print(msg)
|
||||
print('Landing')
|
||||
elif key == 's' or key == 'k' :
|
||||
target_forward_accel = 0.0
|
||||
target_leftward_accel = 0.0
|
||||
target_upward_accel = 0.0
|
||||
target_angular_accel = 0.0
|
||||
control_forward_accel = 0.0
|
||||
control_leftward_accel = 0.0
|
||||
control_upward_accel = 0.0
|
||||
control_angular_accel = 0.0
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,-target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
else:
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
twist = Twist()
|
||||
|
||||
control_forward_accel = makeSimpleProfile(control_forward_accel, target_forward_accel, (LIN_accel_STEP_SIZE/2.0))
|
||||
control_leftward_accel = makeSimpleProfile(control_leftward_accel, target_leftward_accel, (LIN_accel_STEP_SIZE/2.0))
|
||||
control_upward_accel = makeSimpleProfile(control_upward_accel, target_upward_accel, (LIN_accel_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_accel; twist.linear.y = control_leftward_accel; twist.linear.z = control_upward_accel
|
||||
|
||||
control_angular_accel = makeSimpleProfile(control_angular_accel, target_angular_accel, (ANG_accel_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_accel
|
||||
|
||||
cmd_accel_flu_pub.publish(twist)
|
||||
cmd_pub.publish(cmd)
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist = Twist()
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
cmd_accel_flu_pub.publish(twist)
|
||||
cmd_pub.publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -85,32 +85,6 @@ def print_msg():
|
|||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
|
|
@ -129,8 +103,8 @@ if __name__=="__main__":
|
|||
multi_cmd_vel_flu_pub = [None]*multirotor_num
|
||||
multi_cmd_pub = [None]*multirotor_num
|
||||
for i in range(multirotor_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+str(i)+'/cmd',String,queue_size=10)
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
|
||||
|
|
@ -138,55 +112,58 @@ if __name__=="__main__":
|
|||
multi_cmd_accel_flu_pub = [None]*multirotor_num
|
||||
multi_cmd_pub = [None]*multirotor_num
|
||||
for i in range(multirotor_num):
|
||||
multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+str(i)+'/cmd_accel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+str(i)+'/cmd',String,queue_size=10)
|
||||
multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
||||
upward = 0.0
|
||||
angular = 0.0
|
||||
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
forward = forward + LIN_VEL_STEP_SIZE
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
if control_type = 'vel':
|
||||
print("currently:\t forward vel %f\t leftward vel %f\t upward vel %f\t angular %f " % (forward, leftward, upward, angular))
|
||||
else:
|
||||
print("currently:\t forward vel %f\t leftward vel %f\t upward vel %f\t angular %f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
forward = forward + LIN_VEL_STEP_SIZE
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
if control_type = 'vel':
|
||||
print("currently:\t forward vel %f\t leftward vel %f\t upward vel %f\t angular %f " % (forward, leftward, upward, angular))
|
||||
else:
|
||||
print("currently:\t forward vel %f\t leftward vel %f\t upward vel %f\t angular %f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print_msg()
|
||||
|
|
@ -243,8 +220,8 @@ if __name__=="__main__":
|
|||
break
|
||||
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_forward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = -makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
|
|
@ -261,7 +238,8 @@ if __name__=="__main__":
|
|||
else:
|
||||
if not cmd_vel_mask:
|
||||
if control_type == 'vel':
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
print(twist)
|
||||
else:
|
||||
multi_cmd_accel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
|
@ -120,8 +120,8 @@ if __name__=="__main__":
|
|||
multi_cmd_vel_flu_pub = [None]*plane_num
|
||||
multi_cmd_pub = [None]*plane_num
|
||||
for i in range(plane_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/plane'+str(i)+'/cmd_pose_enu', Pose, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/plane'+str(i)+'/cmd',String,queue_size=10)
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/plane_'+str(i)+'/cmd_pose_enu', Pose, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/plane_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_pose", Pose, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
cmd= String()
|
||||
|
|
@ -121,8 +121,8 @@ if __name__=="__main__":
|
|||
multi_cmd_vel_flu_pub = [None]*rover_num
|
||||
multi_cmd_pub = [None]*rover_num
|
||||
for i in range(rover_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/rover'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/rover'+str(i)+'/cmd',String,queue_size=10)
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/rover_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/rover_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_vel", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
cmd= String()
|
||||
|
|
@ -125,12 +125,12 @@ if __name__=="__main__":
|
|||
multi_cmd_vel_flu_pub = [None]*vehicle_num
|
||||
multi_cmd_pub = [None]*vehicle_num
|
||||
for i in range(vehicle_num):
|
||||
multi_cmd_pose_enu_pub[i] = rospy.Publisher('/xtdrone/'+vehicle_type+str(i)+'/cmd_pose_enu', Pose, queue_size=10)
|
||||
multi_cmd_pose_enu_pub[i] = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+str(i)+'/cmd_pose_enu', Pose, queue_size=10)
|
||||
if control_type == 'vel':
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+vehicle_type+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
else:
|
||||
multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+str(i)+'/cmd_accel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+vehicle_type+str(i)+'/cmd',String,queue_size=10)
|
||||
multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_pose_enu_pub = rospy.Publisher("/xtdrone/leader/cmd_pose_enu", Pose, queue_size=10)
|
||||
if control_type == 'vel':
|
||||
leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 1
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24560
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34571 -r 4000000
|
||||
mavlink start -x -u 34572 -r 4000000 -m onboard -o 14540
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34571
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34571
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34571
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34571
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34571
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34571
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34571
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34571
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34571
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34571
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 10
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24578
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34589 -r 4000000
|
||||
mavlink start -x -u 34590 -r 4000000 -m onboard -o 14549
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34589
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34589
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34589
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34589
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34589
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34589
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34589
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34589
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34589
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34589
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 11
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24580
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34591 -r 4000000
|
||||
mavlink start -x -u 34592 -r 4000000 -m onboard -o 14550
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34591
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34591
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34591
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34591
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34591
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34591
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34591
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34591
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34591
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34591
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 12
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24582
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34593 -r 4000000
|
||||
mavlink start -x -u 34594 -r 4000000 -m onboard -o 14551
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34593
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34593
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34593
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34593
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34593
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34593
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34593
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34593
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34593
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34593
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 13
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24584
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34595 -r 4000000
|
||||
mavlink start -x -u 34596 -r 4000000 -m onboard -o 14552
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34595
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34595
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34595
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34595
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34595
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34595
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34595
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34595
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34595
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34595
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 14
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24586
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34597 -r 4000000
|
||||
mavlink start -x -u 34598 -r 4000000 -m onboard -o 14553
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34597
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34597
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34597
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34597
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34597
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34597
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34597
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34597
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34597
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34597
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 15
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24588
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34599 -r 4000000
|
||||
mavlink start -x -u 34600 -r 4000000 -m onboard -o 14554
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34599
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34599
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34599
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34599
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34599
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34599
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34599
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34599
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34599
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34599
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 16
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24590
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34601 -r 4000000
|
||||
mavlink start -x -u 34602 -r 4000000 -m onboard -o 14555
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34601
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34601
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34601
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34601
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34601
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34601
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34601
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34601
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34601
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34601
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 17
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24592
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34603 -r 4000000
|
||||
mavlink start -x -u 34604 -r 4000000 -m onboard -o 14556
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34603
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34603
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34603
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34603
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34603
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34603
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34603
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34603
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34603
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34603
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 18
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24594
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34605 -r 4000000
|
||||
mavlink start -x -u 34606 -r 4000000 -m onboard -o 14557
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34605
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34605
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34605
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34605
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34605
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34605
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34605
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34605
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34605
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34605
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 19
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24596
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34607 -r 4000000
|
||||
mavlink start -x -u 34608 -r 4000000 -m onboard -o 14558
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34607
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34607
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34607
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34607
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34607
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34607
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34607
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34607
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34607
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34607
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 2
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24562
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34573 -r 4000000
|
||||
mavlink start -x -u 34574 -r 4000000 -m onboard -o 14541
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34573
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34573
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34573
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34573
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34573
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34573
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34573
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34573
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34573
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34573
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 20
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24598
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34609 -r 4000000
|
||||
mavlink start -x -u 34610 -r 4000000 -m onboard -o 14559
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34609
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34609
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34609
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34609
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34609
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34609
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34609
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34609
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34609
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34609
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 21
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24600
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34611 -r 4000000
|
||||
mavlink start -x -u 34612 -r 4000000 -m onboard -o 14560
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34611
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34611
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34611
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34611
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34611
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34611
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34611
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34611
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34611
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34611
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 22
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24602
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34613 -r 4000000
|
||||
mavlink start -x -u 34614 -r 4000000 -m onboard -o 14561
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34613
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34613
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34613
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34613
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34613
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34613
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34613
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34613
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34613
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34613
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 23
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24604
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34615 -r 4000000
|
||||
mavlink start -x -u 34616 -r 4000000 -m onboard -o 14562
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34615
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34615
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34615
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34615
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34615
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34615
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34615
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34615
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34615
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34615
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 24
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24606
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34617 -r 4000000
|
||||
mavlink start -x -u 34618 -r 4000000 -m onboard -o 14563
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34617
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34617
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34617
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34617
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34617
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34617
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34617
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34617
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34617
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34617
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 25
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24608
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34619 -r 4000000
|
||||
mavlink start -x -u 34620 -r 4000000 -m onboard -o 14564
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34619
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34619
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34619
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34619
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34619
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34619
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34619
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34619
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34619
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34619
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 26
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24610
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34621 -r 4000000
|
||||
mavlink start -x -u 34622 -r 4000000 -m onboard -o 14565
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34621
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34621
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34621
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34621
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34621
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34621
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34621
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34621
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34621
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34621
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 27
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24612
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34623 -r 4000000
|
||||
mavlink start -x -u 34624 -r 4000000 -m onboard -o 14566
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34623
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34623
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34623
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34623
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34623
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34623
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34623
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34623
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34623
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34623
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 28
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24614
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34625 -r 4000000
|
||||
mavlink start -x -u 34626 -r 4000000 -m onboard -o 14567
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34625
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34625
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34625
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34625
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34625
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34625
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34625
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34625
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34625
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34625
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 29
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24616
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34627 -r 4000000
|
||||
mavlink start -x -u 34628 -r 4000000 -m onboard -o 14568
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34627
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34627
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34627
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34627
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34627
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34627
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34627
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34627
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34627
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34627
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 3
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24564
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34575 -r 4000000
|
||||
mavlink start -x -u 34576 -r 4000000 -m onboard -o 14542
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34575
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34575
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34575
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34575
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34575
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34575
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34575
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34575
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34575
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34575
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 30
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24618
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34629 -r 4000000
|
||||
mavlink start -x -u 34630 -r 4000000 -m onboard -o 14569
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34629
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34629
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34629
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34629
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34629
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34629
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34629
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34629
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34629
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34629
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 31
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24620
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34631 -r 4000000
|
||||
mavlink start -x -u 34632 -r 4000000 -m onboard -o 14570
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34631
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34631
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34631
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34631
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34631
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34631
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34631
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34631
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34631
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34631
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 32
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24622
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34633 -r 4000000
|
||||
mavlink start -x -u 34634 -r 4000000 -m onboard -o 14571
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34633
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34633
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34633
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34633
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34633
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34633
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34633
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34633
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34633
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34633
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 33
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24624
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34635 -r 4000000
|
||||
mavlink start -x -u 34636 -r 4000000 -m onboard -o 14572
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34635
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34635
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34635
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34635
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34635
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34635
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34635
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34635
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34635
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34635
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 34
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24626
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34637 -r 4000000
|
||||
mavlink start -x -u 34638 -r 4000000 -m onboard -o 14573
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34637
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34637
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34637
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34637
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34637
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34637
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34637
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34637
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34637
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34637
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 35
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24628
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34639 -r 4000000
|
||||
mavlink start -x -u 34640 -r 4000000 -m onboard -o 14574
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34639
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34639
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34639
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34639
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34639
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34639
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34639
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34639
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34639
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34639
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 36
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24630
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34641 -r 4000000
|
||||
mavlink start -x -u 34642 -r 4000000 -m onboard -o 14575
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34641
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34641
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34641
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34641
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34641
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34641
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34641
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34641
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34641
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34641
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 37
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24632
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34643 -r 4000000
|
||||
mavlink start -x -u 34644 -r 4000000 -m onboard -o 14576
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34643
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34643
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34643
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34643
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34643
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34643
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34643
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34643
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34643
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34643
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 38
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24634
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34645 -r 4000000
|
||||
mavlink start -x -u 34646 -r 4000000 -m onboard -o 14577
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34645
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34645
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34645
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34645
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34645
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34645
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34645
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34645
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34645
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34645
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 39
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24636
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34647 -r 4000000
|
||||
mavlink start -x -u 34648 -r 4000000 -m onboard -o 14578
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34647
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34647
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34647
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34647
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34647
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34647
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34647
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34647
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34647
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34647
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 4
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24566
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34577 -r 4000000
|
||||
mavlink start -x -u 34578 -r 4000000 -m onboard -o 14543
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34577
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34577
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34577
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34577
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34577
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34577
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34577
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34577
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34577
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34577
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 40
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24638
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34649 -r 4000000
|
||||
mavlink start -x -u 34650 -r 4000000 -m onboard -o 14579
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34649
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34649
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34649
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34649
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34649
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34649
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34649
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34649
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34649
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34649
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 41
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24640
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34651 -r 4000000
|
||||
mavlink start -x -u 34652 -r 4000000 -m onboard -o 14580
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34651
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34651
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34651
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34651
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34651
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34651
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34651
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34651
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34651
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34651
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 42
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24642
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34653 -r 4000000
|
||||
mavlink start -x -u 34654 -r 4000000 -m onboard -o 14581
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34653
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34653
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34653
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34653
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34653
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34653
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34653
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34653
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34653
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34653
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 43
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24644
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34655 -r 4000000
|
||||
mavlink start -x -u 34656 -r 4000000 -m onboard -o 14582
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34655
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34655
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34655
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34655
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34655
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34655
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34655
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34655
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34655
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34655
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 44
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24646
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34657 -r 4000000
|
||||
mavlink start -x -u 34658 -r 4000000 -m onboard -o 14583
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34657
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34657
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34657
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34657
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34657
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34657
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34657
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34657
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34657
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34657
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 45
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24648
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34659 -r 4000000
|
||||
mavlink start -x -u 34660 -r 4000000 -m onboard -o 14584
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34659
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34659
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34659
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34659
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34659
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34659
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34659
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34659
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34659
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34659
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 46
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24650
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34661 -r 4000000
|
||||
mavlink start -x -u 34662 -r 4000000 -m onboard -o 14585
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34661
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34661
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34661
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34661
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34661
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34661
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34661
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34661
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34661
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34661
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 47
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24652
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34663 -r 4000000
|
||||
mavlink start -x -u 34664 -r 4000000 -m onboard -o 14586
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34663
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34663
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34663
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34663
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34663
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34663
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34663
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34663
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34663
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34663
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 48
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24654
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34665 -r 4000000
|
||||
mavlink start -x -u 34666 -r 4000000 -m onboard -o 14587
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34665
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34665
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34665
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34665
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34665
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34665
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34665
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34665
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34665
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34665
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 49
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24656
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34667 -r 4000000
|
||||
mavlink start -x -u 34668 -r 4000000 -m onboard -o 14588
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34667
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34667
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34667
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34667
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34667
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34667
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34667
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34667
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34667
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34667
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 5
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24568
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34579 -r 4000000
|
||||
mavlink start -x -u 34580 -r 4000000 -m onboard -o 14544
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34579
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34579
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34579
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34579
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34579
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34579
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34579
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34579
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34579
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34579
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 50
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24658
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34669 -r 4000000
|
||||
mavlink start -x -u 34670 -r 4000000 -m onboard -o 14589
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34669
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34669
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34669
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34669
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34669
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34669
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34669
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34669
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34669
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34669
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 6
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24570
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34581 -r 4000000
|
||||
mavlink start -x -u 34582 -r 4000000 -m onboard -o 14545
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34581
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34581
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34581
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34581
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34581
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34581
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34581
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34581
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34581
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34581
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 7
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24572
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34583 -r 4000000
|
||||
mavlink start -x -u 34584 -r 4000000 -m onboard -o 14546
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34583
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34583
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34583
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34583
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34583
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34583
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34583
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34583
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34583
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34583
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 8
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24574
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34585 -r 4000000
|
||||
mavlink start -x -u 34586 -r 4000000 -m onboard -o 14547
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34585
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34585
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34585
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34585
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34585
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34585
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34585
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34585
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34585
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34585
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 9
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 24576
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 34587 -r 4000000
|
||||
mavlink start -x -u 34588 -r 4000000 -m onboard -o 14548
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 34587
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 34587
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 34587
|
||||
mavlink stream -r 50 -s ATTITUDE -u 34587
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 34587
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 34587
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 34587
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 34587
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 34587
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 34587
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
uorb start
|
||||
param load
|
||||
dataman start
|
||||
param set MAV_SYS_ID 3
|
||||
param set BAT_N_CELLS 3
|
||||
param set CAL_ACC0_ID 1376264
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_ID 1310728
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_GYRO0_ID 2293768
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_MAG0_ID 196616
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set COM_DISARM_LAND 3
|
||||
param set COM_OBL_ACT 2
|
||||
param set COM_OBL_RC_ACT 0
|
||||
param set COM_OF_LOSS_T 5
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set EKF2_AID_MASK 24
|
||||
param set EKF2_ANGERR_INIT 0.01
|
||||
param set EKF2_GBIAS_INIT 0.01
|
||||
param set EKF2_HGT_MODE 3
|
||||
param set EKF2_MAG_TYPE 5
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCH_P 6
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_ROLL_P 6
|
||||
param set MC_ROLLRATE_P 0.2
|
||||
param set MIS_TAKEOFF_ALT 2.5
|
||||
param set MPC_HOLD_MAX_Z 2.0
|
||||
param set MPC_Z_VEL_I 0.15
|
||||
param set MPC_Z_VEL_P 0.6
|
||||
param set NAV_ACC_RAD 2.0
|
||||
param set NAV_DLL_ACT 2
|
||||
param set RTL_DESCEND_ALT 5.0
|
||||
param set RTL_LAND_DELAY 5
|
||||
param set RTL_RETURN_ALT 30.0
|
||||
param set SDLOG_DIRS_MAX 7
|
||||
param set SENS_BOARD_ROT 0
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SITL_UDP_PRT 14564
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_MC_EST_GROUP 2
|
||||
param set SYS_RESTART_TYPE 2
|
||||
replay tryapplyparams
|
||||
simulator start -s
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim start
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
|
||||
mavlink start -x -u 14572 -r 4000000
|
||||
mavlink start -x -u 14573 -r 4000000 -m onboard -o 14542
|
||||
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14572
|
||||
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14572
|
||||
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14572
|
||||
mavlink stream -r 50 -s ATTITUDE -u 14572
|
||||
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14572
|
||||
mavlink stream -r 50 -s ATTITUDE_TARGET -u 14572
|
||||
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14572
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 14572
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 14572
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14572
|
||||
logger start -e -t
|
||||
mavlink boot_complete
|
||||
replay trystart
|
||||
|
|
@ -1,264 +0,0 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LIN_VEL = 1
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.02
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
cmd_vel_mask = False
|
||||
ctrl_leader = False
|
||||
|
||||
msg2all = """
|
||||
Control Your XTDrone!
|
||||
To all drones (press g to control the leader)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff/land
|
||||
b : offboard
|
||||
s : hover(offboard mode) and remove the mask of keyboard control
|
||||
k : hover(hover mode) and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control the leader
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
msg2leader = """
|
||||
Control Your XTDrone!
|
||||
To the leader (press g to control all drones)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s or k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control all drones
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def print_msg():
|
||||
if ctrl_leader:
|
||||
print(msg2leader)
|
||||
else:
|
||||
print(msg2all)
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
uav_num = int(sys.argv[1])
|
||||
rospy.init_node('keyboard_control')
|
||||
multi_cmd_vel_flu_pub = [None]*uav_num
|
||||
multi_cmd_pub = [None]*uav_num
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_vel", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print_msg()
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print_msg()
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print_msg()
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
#cmd = 'AUTO.TAKEOFF'
|
||||
cmd = ''
|
||||
print(msg)
|
||||
print('Takeoff mode is disenabled now')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print_msg()
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print_msg()
|
||||
print('Landing')
|
||||
elif key == 'g':
|
||||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k':
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
else:
|
||||
for i in range(10):
|
||||
if key == str(i):
|
||||
cmd = 'mission'+key
|
||||
print_msg()
|
||||
print(cmd)
|
||||
cmd_vel_mask = True
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
for i in range(uav_num):
|
||||
if ctrl_leader:
|
||||
leader_cmd_vel_pub.publish(twist)
|
||||
leader_cmd_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
cmd = ''
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -1,273 +0,0 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist, Vector3
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LIN_VEL = 1
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.02
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
cmd_vel_mask = False
|
||||
ctrl_leader = False
|
||||
uav_num = int(sys.argv[1])
|
||||
if uav_num == 9:
|
||||
formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
if uav_num == 6:
|
||||
formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
|
||||
msg2all = """
|
||||
Control Your XTDrone!
|
||||
To all drones (press g to control the leader)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff/land
|
||||
b : offboard
|
||||
s : hover(offboard mode) and remove the mask of keyboard control
|
||||
k : hover(hover mode) and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control the leader
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
msg2leader = """
|
||||
Control Your XTDrone!
|
||||
To the leader (press g to control all drones)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s or k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control all drones
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def print_msg():
|
||||
if ctrl_leader:
|
||||
print(msg2leader)
|
||||
else:
|
||||
print(msg2all)
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rospy.init_node('keyboard_control')
|
||||
multi_cmd_vel_flu_pub = [None]*uav_num
|
||||
multi_cmd_pub = [None]*uav_num
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd_accel_enu', Vector3, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_accel", Vector3, queue_size=10)
|
||||
formation_switch_pub = rospy.Publisher("/gcs/formation_switch", String, queue_size=10)
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
accel = Vector3()
|
||||
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print_msg()
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print_msg()
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print_msg()
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
#cmd = 'AUTO.TAKEOFF'
|
||||
cmd = ''
|
||||
print(msg)
|
||||
print('Takeoff mode is disenabled now')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print_msg()
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print_msg()
|
||||
print('Landing')
|
||||
elif key == 'g':
|
||||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k':
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
else:
|
||||
for i in range(10):
|
||||
if key == str(i):
|
||||
cmd = formation_configs[i]
|
||||
print_msg()
|
||||
print(cmd)
|
||||
cmd_vel_mask = True
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
accel = twist.linear
|
||||
|
||||
for i in range(uav_num):
|
||||
if ctrl_leader:
|
||||
leader_cmd_vel_pub.publish(accel)
|
||||
if not cmd == '':
|
||||
formation_switch_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
multi_cmd_vel_flu_pub[i].publish(accel)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
accel.x = 0.0; accel.y = 0.0; accel.z = 0.0
|
||||
cmd = ''
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i].publish(accel)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -1,273 +0,0 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LIN_VEL = 1
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.02
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
cmd_vel_mask = False
|
||||
ctrl_leader = False
|
||||
uav_num = int(sys.argv[1])
|
||||
|
||||
if uav_num == 18:
|
||||
formation_configs = ['waiting','cube', 'sphere', 'diamond']
|
||||
if uav_num == 9:
|
||||
formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
if uav_num == 6:
|
||||
formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
|
||||
msg2all = """
|
||||
Control Your XTDrone!
|
||||
To all drones (press g to control the leader)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v : mission
|
||||
n : land
|
||||
b : offboard
|
||||
s : hover(offboard mode) and remove the mask of keyboard control
|
||||
k : hover(hover mode) and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control the leader
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
msg2leader = """
|
||||
Control Your XTDrone!
|
||||
To the leader (press g to control all drones)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v : mission
|
||||
n : land
|
||||
b : offboard
|
||||
s or k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control all drones
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def print_msg():
|
||||
if ctrl_leader:
|
||||
print(msg2leader)
|
||||
else:
|
||||
print(msg2all)
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rospy.init_node('keyboard_control')
|
||||
multi_cmd_vel_flu_pub = [None]*uav_num
|
||||
multi_cmd_pub = [None]*uav_num
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_vel", Twist, queue_size=10)
|
||||
formation_switch_pub = rospy.Publisher("/gcs/formation_switch", String, queue_size=10)
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print_msg()
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print_msg()
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print_msg()
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
cmd = 'AUTO.MiSSION'
|
||||
print_msg()
|
||||
print('Mission')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print_msg()
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print_msg()
|
||||
print('Landing')
|
||||
elif key == 'g':
|
||||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k':
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Hover')
|
||||
elif key == 's':
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
else:
|
||||
for i in range(10):
|
||||
if key == str(i):
|
||||
cmd = formation_configs[i]
|
||||
print_msg()
|
||||
print(cmd)
|
||||
cmd_vel_mask = True
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
for i in range(uav_num):
|
||||
if ctrl_leader:
|
||||
leader_cmd_vel_pub.publish(twist)
|
||||
if not cmd == '':
|
||||
formation_switch_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
cmd = ''
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -78,10 +78,10 @@ with open('launch_temp_1.11','r') as f:
|
|||
launch_lines.append(line)
|
||||
|
||||
|
||||
with open('./launch_xtd/multi_vehicle.launch','w') as f:
|
||||
with open('multi_vehicle.launch','w') as f:
|
||||
f.write(launch_head)
|
||||
id_in_all = 0
|
||||
row_num = 0
|
||||
id_in_all = 0
|
||||
for type_id in range(8):
|
||||
type_num = num_of_type[type_id]
|
||||
|
||||
|
|
@ -97,9 +97,7 @@ with open('./launch_xtd/multi_vehicle.launch','w') as f:
|
|||
|
||||
if type_num > 0:
|
||||
row_num +=1
|
||||
for id_in_type in range(1,type_num+1):
|
||||
|
||||
id_in_all+=1
|
||||
for id_in_type in range(0,type_num):
|
||||
|
||||
mavlink_1=34570-1+id_in_all*2
|
||||
mavlink_2=mavlink_1+1
|
||||
|
|
@ -113,6 +111,8 @@ with open('./launch_xtd/multi_vehicle.launch','w') as f:
|
|||
f.write(''' <group ns="%s_%d">\n''' %(type_name,id_in_type) )
|
||||
elif '''<arg name="ID" value="0"/>''' in line:
|
||||
f.write(''' <arg name="ID" value="%d"/>\n''' %id_in_all)
|
||||
elif '''<arg name="ID_in_group" value="0"/>''' in line:
|
||||
f.write(''' <arg name="ID_in_group" value="%d"/>\n''' %id_in_type)
|
||||
elif "udp://:" in line:
|
||||
f.write(''' <arg name="fcu_url" default="udp://:%d@127.0.0.1:%d"/>\n''' %(onboard,mavlink_2))
|
||||
elif "mavlink_udp_port" in line:
|
||||
|
|
@ -128,12 +128,15 @@ with open('./launch_xtd/multi_vehicle.launch','w') as f:
|
|||
elif '''name="y"''' in line:
|
||||
f.write(''' <arg name="y" value="%d"/>\n''' %(id_in_type*3) )
|
||||
else:
|
||||
f.write('%s' %line)
|
||||
f.write('%s' %line)
|
||||
f.write("\n")
|
||||
id_in_all+=1
|
||||
|
||||
|
||||
|
||||
f.write('</launch>\n')
|
||||
f.write('<!--the launch file is generated by XTDrone multi-vehicle generator.py -->')
|
||||
print ("all down!")
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -2,6 +2,7 @@
|
|||
<group ns="uav0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="0"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:14540@localhost:14580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
|
|
@ -16,6 +17,7 @@
|
|||
<arg name="mavlink_udp_port" value="14560"/>
|
||||
<arg name="mavlink_tcp_port" value="4560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
|
|
@ -18,23 +18,84 @@
|
|||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
</include>
|
||||
<!-- plane1 -->
|
||||
<!-- rover_0 -->
|
||||
<group ns="rover_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="0"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:14540@127.0.0.1:34570"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="0"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="rover"/>
|
||||
<arg name="sdf" value="rover"/>
|
||||
<arg name="mavlink_udp_port" value="24560"/>
|
||||
<arg name="mavlink_tcp_port" value="4560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- plane_0 -->
|
||||
<group ns="plane_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:14541@127.0.0.1:34572"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="3"/>
|
||||
<arg name="y" value="0"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="plane"/>
|
||||
<arg name="sdf" value="plane"/>
|
||||
<arg name="mavlink_udp_port" value="24562"/>
|
||||
<arg name="mavlink_tcp_port" value="4561"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- plane_1 -->
|
||||
<group ns="plane_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:14541@127.0.0. 1:34572"/>
|
||||
<arg name="ID" value="2"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34574"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
|
||||
<arg name="x" value="4"/>
|
||||
<arg name="y" value="4"/>
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="3"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="plane"/>
|
||||
<arg name="mavlink_udp_port" value="24562"/ >
|
||||
<arg name="mavlink_tcp_port" value="4561"/ >
|
||||
<arg name="sdf" value="plane"/>
|
||||
<arg name="mavlink_udp_port" value="24564"/>
|
||||
<arg name="mavlink_tcp_port" value="4562"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
|
|
@ -44,23 +105,26 @@
|
|||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- plane2 -->
|
||||
<group ns="plane_2">
|
||||
<!-- solo_0 -->
|
||||
<group ns="solo_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:14542@127.0.0. 1:34574"/>
|
||||
<arg name="ID" value="3"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34576"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
|
||||
<arg name="x" value="4"/>
|
||||
<arg name="y" value="8"/>
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="6"/>
|
||||
<arg name="y" value="0"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="plane"/>
|
||||
<arg name="mavlink_udp_port" value="24564"/ >
|
||||
<arg name="mavlink_tcp_port" value="4562"/ >
|
||||
<arg name="vehicle" value="solo"/>
|
||||
<arg name="sdf" value="solo_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/>
|
||||
<arg name="mavlink_tcp_port" value="4563"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
|
|
@ -70,101 +134,26 @@
|
|||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- plane3 -->
|
||||
<group ns="plane_3">
|
||||
<!-- solo_1 -->
|
||||
<group ns="solo_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<arg name="fcu_url" default="udp://:14543@127.0.0. 1:34576"/>
|
||||
<arg name="ID" value="4"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34578"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
|
||||
<arg name="x" value="4"/>
|
||||
<arg name="y" value="12"/>
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="6"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="plane"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/ >
|
||||
<arg name="mavlink_tcp_port" value="4563"/ >
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h4804 -->
|
||||
<group ns="typhoon_h480_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<arg name="fcu_url" default="udp://:14544@127.0.0. 1:34578"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
|
||||
<arg name="x" value="8"/>
|
||||
<arg name="y" value="4"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="typhoon_h480"/>
|
||||
<arg name="mavlink_udp_port" value="24568"/ >
|
||||
<arg name="mavlink_tcp_port" value="4564"/ >
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h4805 -->
|
||||
<group ns="typhoon_h480_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="5"/>
|
||||
<arg name="fcu_url" default="udp://:14545@127.0.0. 1:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
|
||||
<arg name="x" value="8"/>
|
||||
<arg name="y" value="8"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="typhoon_h480"/>
|
||||
<arg name="mavlink_udp_port" value="24570"/ >
|
||||
<arg name="mavlink_tcp_port" value="4565"/ >
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h4806 -->
|
||||
<group ns="typhoon_h480_3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<arg name="fcu_url" default="udp://:14546@127.0.0. 1:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
|
||||
<arg name="x" value="8"/>
|
||||
<arg name="y" value="12"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="typhoon_h480"/>
|
||||
<arg name="mavlink_udp_port" value="24572"/ >
|
||||
<arg name="mavlink_tcp_port" value="4566"/ >
|
||||
<arg name="vehicle" value="solo"/>
|
||||
<arg name="sdf" value="solo_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24568"/>
|
||||
<arg name="mavlink_tcp_port" value="4564"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
|
|
@ -1,292 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="iris"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<!-- Gazebo sim -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="world_name" value="$(arg world)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
</include>
|
||||
<!-- UAV0 -->
|
||||
<group ns="uav0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:14540@localhost:34571"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-1"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24560"/>
|
||||
<arg name="mavlink_tcp_port" value="4560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV1-->
|
||||
<group ns="uav1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:14541@127.0.0.1:34572"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="1"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24562"/>
|
||||
<arg name="mavlink_tcp_port" value="4561"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV2-->
|
||||
<group ns="uav2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34574"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-2"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24564"/>
|
||||
<arg name="mavlink_tcp_port" value="4562"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV3-->
|
||||
<group ns="uav3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34576"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/>
|
||||
<arg name="mavlink_tcp_port" value="4563"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV4-->
|
||||
<group ns="uav4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34578"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-4"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24568"/>
|
||||
<arg name="mavlink_tcp_port" value="4564"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV5-->
|
||||
<group ns="uav5">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="5"/>
|
||||
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="5"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24570"/>
|
||||
<arg name="mavlink_tcp_port" value="4565"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV6-->
|
||||
<group ns="uav6">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24572"/>
|
||||
<arg name="mavlink_tcp_port" value="4566"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV7-->
|
||||
<group ns="uav7">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="7"/>
|
||||
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34584"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="7"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24574"/>
|
||||
<arg name="mavlink_tcp_port" value="4567"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV8-->
|
||||
<group ns="uav8">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="8"/>
|
||||
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34586"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-8"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24576"/>
|
||||
<arg name="mavlink_tcp_port" value="4568"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV9-->
|
||||
<group ns="uav9">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="9"/>
|
||||
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34588"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="mavlink_udp_port" value="24578"/>
|
||||
<arg name="mavlink_tcp_port" value="4569"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1+arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue