重建仓库

.gitee/ISSUE_TEMPLATE.zh-CN.md

@@ -0,0 +1,13 @@
+### 该问题是怎么引起的？
+
+
+
+### 重现步骤
+
+
+
+### 报错信息
+
+
+
+

.gitee/PULL_REQUEST_TEMPLATE.zh-CN.md

@@ -0,0 +1,15 @@
+### 相关的Issue
+
+
+### 原因（目的、解决的问题等）
+
+
+### 描述（做了什么，变更了什么）
+
+
+### 测试用例
+
+
+
+
+

.gitignore

@@ -0,0 +1,3 @@
+papers
+competition
+*.bag

.vscode/launch.json

@@ -0,0 +1,26 @@
+{
+    // 使用 IntelliSense 了解相关属性。 
+    // 悬停以查看现有属性的描述。
+    // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Python: 当前文件",
+            "type": "python",
+            "request": "launch",
+            "program": "${file}",
+            "console": "integratedTerminal"
+        },
+        {
+            "name": "Python: Pyramid 应用",
+            "type": "python",
+            "request": "launch",
+            "module": "pyramid.scripts.pserve",
+            "args": [
+                "${workspaceFolder}/development.ini"
+            ],
+            "pyramid": true,
+            "jinja": true
+        }
+    ]
+}

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "python.pythonPath": "/usr/bin/python"
+}

README.en.md

@@ -0,0 +1,74 @@
+# XTDrone
+
+<div id="sidebar"><a href="./README.md" target="_blank"><font color=#0000FF size=5px >[中文版]<font></center><a></div>
+
+#### Description
+
+XTdrone is a customizable Multi-Rotor UAVs simulation platform based on PX4 and ROS. Now the simulator of XTDrone is Gazebo, and the connection to Airsim is being developed.
+
+<img src="./image/architecture.png" width="640" height="480" /> 
+
+Developers can quickly verify algorithms with XTDrone, such as:
+
+1. Object Detection and Tracking
+<img src="./image/human_tracking.gif" width="640" height="368" /> 
+
+2. Visual SLAM
+<img src="./image/vslam.gif" width="640" height="368" /> 
+
+3. Laser Slam
+<img src="./image/laser_slam.gif" width="640" height="368" /> 
+
+4. VIO 
+<img src="./image/vio.gif" width="640" height="368" />  
+
+5. Motion Planning
+<img src="./image/motion_planning.gif" width="640" height="368" />  
+
+6. Formation
+<img src="./image/cooperation.gif" width="640" height="368" />  
+
+#### Software Architecture
+- Comunication: The communication between PX4 and ROS is encapsulated in the Python class, and multi-machine communication starts multiple processes
+- Control：Use the keyboard to switch drone flight modes, control unlocking, adjust speed and yaw steering
+- Perception
+  1. Object Detection and Tracking
+       - YOLO
+  2. SLAM：
+     1. VSLAM: 
+         - ORBSLAM2
+     2. Laser_SLAM:
+         - PLICP+gmapping
+     3. VIO
+         - VINS-Mono（pre-flight initialization issues need to be improved）
+  3. Ground true pose acquisition
+  4. Speech Recognition（to be developed）
+- Motion Planning(currently only supports 2D )
+  1. Global planning
+      - A*
+      - Dijkstra
+  2. Local planning
+      - DWA
+- Cooperation：Multi-UAV Formation. Supply simple 3D simulator to speed up algorithm validation.
+- Simulation configuration
+  1. PX4 configuration
+     - Can reject GPS and magnetic compass
+  2. Launch script
+  3. Gazebo models
+     - Stereo Camera、Depth Camera、LiDAR
+  4. Gazebo worlds
+     - 2 outdoor worlds
+     - 3 indoor worlds
+
+
+#### Installation
+
+View the tutorial doc [XTDrone](https://www.yuque.com/xtdrone/manual_en)
+
+#### Contribution
+
+
+1.  Fork the repository
+2.  Create Feat_xxx branch
+3.  Commit your code
+4.  Create Pull Request

README.md

@@ -0,0 +1,73 @@
+# XTDrone
+
+<div id="sidebar"><a href="./README.en.md" target="_blank"><font color=#0000FF size=5px >[ENGLISH]<font></center><a></div>
+
+#### 介绍
+这是基于PX4和ROS的无人机仿真平台(目前模拟器使用Gazebo，与Airsim的连接正在开发中)。
+
+<img src="./image/architecture.png" width="640" height="480" /> 
+
+
+在这个平台上，开发者可以快速验证算法。如：
+
+1. 目标检测与追踪
+<img src="./image/human_tracking.gif" width="640" height="368" /> 
+
+2. 视觉SLAM
+<img src="./image/vslam.gif" width="640" height="368" /> 
+
+3. 激光SLAM
+<img src="./image/laser_slam.gif" width="640" height="368" /> 
+
+4. VIO 
+<img src="./image/vio.gif" width="640" height="368" />  
+
+5. 运动规划
+<img src="./image/motion_planning.gif" width="640" height="368" />  
+  
+6. 多机协同
+<img src="./image/cooperation.gif" width="640" height="368" />  
+
+#### 软件架构
+- 通信: PX4与ROS之间的通信封装进Python类, 多机通信启动多进程
+- 控制：键盘切换无人机飞行模式，控制解锁上锁，调节速度和偏航转速
+- 感知
+  1. 目标检测与追踪
+       - YOLO
+  2. SLAM：
+     1. VSLAM: 
+         - ORBSLAM2
+     2. Laser_SLAM:
+         - PLICP+gmapping
+     3. VIO
+         - VINS-Mono（起飞前初始化问题有待完善）
+  3. 位姿真值获取
+  4. 语音识别（待开发）
+- 运动规划(目前只有二维)
+  1. 全局规划
+      - A*
+      - Dijkstra
+  2. 局部规划
+      - DWA
+- 协同：多机编队构型变换。提供简易3D仿真器，加快算法验证。
+- 仿真配置
+  1. 无人机PX4参数
+     - 可拒止GPS和磁罗盘
+  2. 启动脚本
+  3. Gazebo模型
+     - 支持双目相机、深度相机、单线雷达和多线雷达 
+  4. Gazebo世界
+     - 两个户外场景
+     - 三个室内场景
+
+
+#### 安装教程
+
+见[XTDrone使用文档](https://www.yuque.com/xtdrone/manual_cn)
+
+#### 参与贡献
+
+1.  Fork 本仓库
+2.  新建 Feat_xxx 分支
+3.  提交代码
+4.  新建 Pull Request

communication/1.9/px4_communication.py

@@ -0,0 +1,181 @@
+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()

communication/1.9/px4_multi_communication.py

@@ -0,0 +1,194 @@
+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()

communication/px4_communication.py

@@ -0,0 +1,181 @@
+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()

communication/px4_multi_communication.py

@@ -0,0 +1,194 @@
+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()

communication/px4_multi_communication_1.9.py

@@ -0,0 +1,194 @@
+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()

control/1.9/keyboard_control.py

@@ -0,0 +1,195 @@
+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)

control/1.9/keyboard_multi_control.py

@@ -0,0 +1,264 @@
+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 = 'AUTO.LOITER'
+                cmd_vel_mask = False
+                print_msg()
+                print('Loiter')
+            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)

control/1.9/leader_follower_formation10.py

@@ -0,0 +1,156 @@
+#!/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()

control/1.9/leader_follower_formation10_with_ac.py

@@ -0,0 +1,207 @@
+#!/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()

control/1.9/leader_follower_formation6_with_ac.py

@@ -0,0 +1,140 @@
+#!/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()

control/1.9/yolo_human_tracking.py

@@ -0,0 +1,40 @@
+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()

control/avoid.py

@@ -0,0 +1,49 @@
+import rospy
+from geometry_msgs.msg import Twist, Vector3, PoseStamped
+from std_msgs.msg import String 
+import time
+import math
+import numpy 
+import sys
+
+uav_num = int(sys.argv[1])
+pose = [None]*uav_num
+pose_sub = [None]*uav_num
+avoid_vel_pub = [None]*uav_num
+avoid_kp = 1
+avoid_radius = 1
+aid_vec1 = [1, 0, 0]
+aid_vec2 = [0, 1, 0]
+uavs_avoid_vel = [Vector3()] * uav_num
+
+def pose_callback(msg, id):
+    pose[id] = msg
+
+rospy.init_node('avoid')
+for i in range(uav_num):
+    pose_sub[i] = rospy.Subscriber('/uav'+str(i+1)+'/mavros/local_position/pose',PoseStamped,pose_callback,i)
+    avoid_vel_pub[i] = rospy.Publisher("/xtdrone/uav"+str(i+1)+"/avoid_vel", Vector3,queue_size=10)
+
+time.sleep(1)
+rate = rospy.Rate(50)
+while not rospy.is_shutdown():
+    for i in range(uav_num):
+        position1 = numpy.array([pose[i].pose.position.x, pose[i].pose.position.y, pose[i].pose.position.z])
+        for j in range(1, uav_num-i):
+            position2 = numpy.array([pose[i+j].pose.position.x, pose[i+j].pose.position.y, pose[i+j].pose.position.z])
+            dir_vec = position1-position2
+            if numpy.linalg.norm(dir_vec) < avoid_radius:
+                cos1 = dir_vec.dot(aid_vec1)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec1))
+                cos2 = dir_vec.dot(aid_vec2)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec2))
+                if  abs(cos1) < abs(cos2):
+                    avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1))
+                else:
+                    avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2))
+                uavs_avoid_vel[i] = Vector3(uavs_avoid_vel[i].x+avoid_vel[0],uavs_avoid_vel[i].y+avoid_vel[1],uavs_avoid_vel[i].z+avoid_vel[2])
+                uavs_avoid_vel[i+j] = Vector3(uavs_avoid_vel[i+j].x-avoid_vel[0],uavs_avoid_vel[i+j].y-avoid_vel[1],uavs_avoid_vel[i+j].z-avoid_vel[2])
+    for i in range(uav_num):
+        avoid_vel_pub[i].publish(uavs_avoid_vel[i])
+    uavs_avoid_vel = [Vector3()] * uav_num
+    rate.sleep()
+            
+            

control/follower_accel_control.py

@@ -0,0 +1,327 @@
+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+import rospy
+from geometry_msgs.msg import Twist, Vector3, PoseStamped, TwistStamped
+from std_msgs.msg import String 
+from pyquaternion import Quaternion
+from formation_dict import formation_dict
+import time
+import math
+import numpy 
+import sys
+import heapq
+
+class Follower:
+
+    def __init__(self, uav_id, uav_num):
+        self.hover = True
+        self.id = uav_id
+        self.uav_num = uav_num
+        self.local_pose = PoseStamped()
+        self.local_velocity = TwistStamped()
+        self.cmd_vel_enu = Twist()
+        self.cmd_accel_enu = Vector3()
+        self.avoid_vel = Vector3()
+        self.following_switch = False
+        self.following_ids = []
+        self.formation_config = 'waiting'
+        self.following_count = 0
+        self.Kp = 100
+        self.kr = (4/int((self.uav_num-1)/3))**0.5
+        self.f = 50
+        self.velxy_max = 0.5
+        self.velz_max = 1
+        self.following_local_pose = [None]*self.uav_num
+        self.following_local_pose_sub = [None]*self.uav_num
+        self.following_local_velocity = [None]*self.uav_num
+        self.following_local_velocity_sub = [None]*self.uav_num
+        self.arrive_count = 0
+        self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
+        self.local_velocity_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/velocity_local", TwistStamped , self.local_velocity_callback)
+        #self.cmd_vel_sub = rospy.Subscriber("/xtdrone/follower/cmd_vel", Twist, self.cmd_vel_callback)
+        self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
+        self.formation_switch_sub = rospy.Subscriber("/xtdrone/formation_switch",String, self.formation_switch_callback)
+        self.vel_enu_pub =  rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
+        self.info_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/info', String, queue_size=10)
+        
+    def local_pose_callback(self, msg):
+        self.local_pose = msg
+        #print(self.arrive_count)
+
+    def local_velocity_callback(self, msg):
+        self.local_velocity = msg
+
+    def following_local_pose_callback(self, msg, id):
+        self.following_local_pose[id] = msg 
+        #print('here')
+    
+    def following_local_velocity_callback(self, msg, id):
+        self.following_local_velocity[id] = msg
+
+    def cmd_vel_callback(self, msg):
+        self.cmd_vel_enu = msg
+        if msg.linear.z == 0:
+            self.hover = True
+        else:
+            self.hover = False
+
+    def formation_switch_callback(self, msg):
+        if not self.formation_config == msg.data:
+            self.following_switch = True
+        else:
+            self.following_switch = False
+        self.formation_config = msg.data
+        
+
+    def avoid_vel_callback(self, msg):
+        self.avoid_vel = msg
+
+    def loop(self):
+        rospy.init_node('follower'+str(self.id-1))
+        rate = rospy.Rate(self.f)
+        while True:
+            if self.arrive_count > 100:
+                self.info_pub.publish("Arrived")
+                #print("Arrived")
+            if self.following_switch:
+                self.following_switch = False
+                self.info_pub.publish("Received")
+                print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
+                self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config])
+                #print(self.L_matrix)
+                #self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]])
+                self.following_ids = numpy.argwhere(self.L_matrix[self.id-1,:] == 1)
+                #if self.id == 2:
+                    #print(self.following_ids)
+                self.following_count = 0
+                for i in range(self.uav_num):
+                    if not self.following_local_pose_sub[i] == None:
+                        self.following_local_pose_sub[i].unregister()
+                    if not self.following_local_velocity_sub[i] == None:
+                        self.following_local_velocity_sub[i].unregister()
+                for following_id in self.following_ids:
+                    #print('here')
+                    self.following_local_pose_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
+                    self.following_local_velocity_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/velocity_local", TwistStamped , self.following_local_velocity_callback,following_id[0])
+                    self.following_count += 1
+                    time.sleep(1)
+
+            self.cmd_accel_enu = Vector3(0, 0, 0)
+            for following_id in self.following_ids:
+                if self.following_local_pose[following_id[0]] == None and self.following_local_velocity[following_id[0]] == None:
+                    print(following_id)
+                self.cmd_accel_enu.x += self.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict[self.formation_config][0, self.id-2]
+                self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict[self.formation_config][1, self.id-2]
+                self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict[self.formation_config][2, self.id-2]
+                if not following_id[0] == 0:
+                    self.cmd_accel_enu.x -= formation_dict[self.formation_config][0, following_id[0]-1]
+                    self.cmd_accel_enu.y -= formation_dict[self.formation_config][1, following_id[0]-1]
+                    self.cmd_accel_enu.z -= formation_dict[self.formation_config][2, following_id[0]-1]
+           
+            self.cmd_vel_enu.linear.x = self.local_velocity.twist.linear.x + self.avoid_vel.x +  self.Kp * self.cmd_accel_enu.x / self.f
+            self.cmd_vel_enu.linear.y = self.local_velocity.twist.linear.y +self.avoid_vel.y +  self.Kp * self.cmd_accel_enu.y / self.f
+            self.cmd_vel_enu.linear.z = self.local_velocity.twist.linear.z +self.avoid_vel.z +  self.Kp * self.cmd_accel_enu.z / self.f
+            if self.cmd_vel_enu.linear.x > self.velxy_max:
+                self.cmd_vel_enu.linear.x = self.velxy_max
+            if self.cmd_vel_enu.linear.y > self.velxy_max:
+                self.cmd_vel_enu.linear.y = self.velxy_max
+            if self.cmd_vel_enu.linear.z > self.velz_max:
+                self.cmd_vel_enu.linear.z = self.velz_max
+
+            if (self.cmd_vel_enu.linear.x)**2+(self.cmd_vel_enu.linear.y)**2+(self.cmd_vel_enu.linear.z)**2<0.2:
+                self.arrive_count += 1
+            else:
+                self.arrive_count = 0
+            self.vel_enu_pub.publish(self.cmd_vel_enu)
+            rate.sleep()
+
+    #函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵
+    def get_L_matrix(self, rel_posi):
+
+        #假设无论多少UAV，都假设尽可能3层通信（叶子节点除外）
+        c_num=int((self.uav_num-1)/3)
+        
+        comm=[[]for i in range (self.uav_num)]
+        w=numpy.ones((self.uav_num,self.uav_num))*0 # 定义邻接矩阵
+        nodes_next=[]
+        node_flag = [self.uav_num-1]
+
+        rel_d=[0]*(self.uav_num-1)
+        # 规定每个无人机可以随机连接三台距离自己最近的无人机,且不能连接在flag中的无人机(即已经判断过连接点的无人机)。
+        # 计算第一层通信（leader）：获得离自己最近的三台无人机编号
+        
+        for i in range(0,self.uav_num-1):
+
+            rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
+
+        min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
+        min_num_index_list=list(min_num_index_list)
+
+        #leader 连接的无人机编号：
+        for j in range(0,c_num):
+
+                if min_num_index_list[j] in node_flag:
+                                
+                    nodes_next=nodes_next
+
+                else:
+                    if min_num_index_list[j] in nodes_next:
+                        nodes_next=nodes_next
+                    else:
+                        nodes_next.append(min_num_index_list[j])
+                        
+                    comm[self.uav_num-1].append(min_num_index_list[j])  
+
+        size_=len(node_flag)
+
+        while (nodes_next!=[]) and (size_<(self.uav_num-1)):
+
+            next_node= nodes_next[0]
+            nodes_next=nodes_next[1:]
+
+            rel_d=[0]*(self.uav_num-1)
+            for i in range(0,self.uav_num-1):
+                    
+                if i==next_node or i in node_flag:
+                        
+                    rel_d[i]=2000   #这个2000是根据相对位置和整个地图的大小决定的，要比最大可能相对距离大
+                else:
+
+                    rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
+
+            min_num_index_list =  map(rel_d.index, heapq.nsmallest(c_num, rel_d))
+            min_num_index_list=list(min_num_index_list)
+            node_flag.append(next_node)
+
+            size_=len(node_flag)                    
+
+            for j in range(0,c_num):
+
+                if min_num_index_list[j] in node_flag:
+                                
+                    nodes_next=nodes_next
+
+                else:
+                    if min_num_index_list[j] in nodes_next:
+                        nodes_next=nodes_next
+                    else:
+                        nodes_next.append(min_num_index_list[j])
+                        
+                    comm[next_node].append(min_num_index_list[j])
+        # comm为每个uav连接其他uav的编号，其中数组的最后一行为leader   
+        #leader是拉普拉斯矩阵的第一行，为0
+        #第0号飞机（相对位置矩阵中的第一个位置）为第二行，以此类推
+
+        for i in range (0,self.uav_num):
+            for j in range(0,self.uav_num-1):
+
+                if i==0:
+                    if j in comm[self.uav_num-1]:
+                        w[j+1][i]=1
+                    else:
+                        w[j+1][i]=0
+                else:
+                    if j in comm[i-1]:
+                        w[j+1][i]=1
+                    else:
+                        w[j+1][i]=0
+                        
+        L=w  #定义拉普拉斯矩阵
+        for i in range (0,self.uav_num):
+
+            L[i][i]=-sum(w[i])
+
+        return L 
+'''
+    def get_L_matrix(self, rel_posi):
+
+        #假设无论多少UAV，都假设尽可能3层通信（叶子节点除外）
+        c_num=int((self.uav_num-1)/3)
+        
+        comm=[[]for i in range (self.uav_num)]
+        w=numpy.ones((self.uav_num,self.uav_num))*0 # 定义邻接矩阵
+        nodes_next=[]
+        node_flag = [self.uav_num-1]
+
+        rel_d=[0]*(self.uav_num-1)
+        # 规定每个无人机可以随机连接三台距离自己最近的无人机,且不能连接在flag中的无人机(即已经判断过连接点的无人机)。
+        # 计算第一层通信（leader）：获得离自己最近的三台无人机编号
+        
+        for i in range(0,self.uav_num-1):
+
+            rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
+
+        min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
+        min_num_index_list=list(min_num_index_list)
+        #leader 连接的无人机编号：
+        comm[self.uav_num-1]=min_num_index_list
+
+        nodes_next.extend(comm[self.uav_num-1])    
+
+        size_=len(node_flag)
+
+        while (nodes_next!=[]) and (size_<(self.uav_num-1)):
+
+            next_node= nodes_next[0]
+            nodes_next=nodes_next[1:]
+
+            rel_d=[0]*(self.uav_num-1)
+            for i in range(0,self.uav_num-1):
+                    
+                if i==next_node or i in node_flag:
+                        
+                    rel_d[i]=2000   #这个2000是根据相对位置和整个地图的大小决定的，要比最大可能相对距离大
+                else:
+
+                    rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
+
+            min_num_index_list =  map(rel_d.index, heapq.nsmallest(c_num, rel_d))
+            min_num_index_list=list(min_num_index_list)
+            node_flag.append(next_node)
+
+            size_=len(node_flag)                    
+
+            for j in range(0,c_num):
+
+                if min_num_index_list[j] in node_flag:
+                                
+                    nodes_next=nodes_next
+
+                else:
+                    if min_num_index_list[j] in nodes_next:
+                        nodes_next=nodes_next
+                    else:
+                        nodes_next.append(min_num_index_list[j])
+                        
+                    comm[next_node].append(min_num_index_list[j])
+        # comm为每个uav连接其他uav的编号，其中数组的最后一行为leader   
+        #print (comm)
+        #leader是拉普拉斯矩阵的第一行，为0
+        #第0号飞机（相对位置矩阵中的第一个位置）为第二行，以此类推
+
+        for i in range (0,self.uav_num):
+            for j in range(0,self.uav_num-1):
+
+                if i==0:
+                    if j in comm[self.uav_num-1]:
+                        w[j+1][i]=1
+                    else:
+                        w[j+1][i]=0
+                else:
+                    if j in comm[i-1]:
+                        w[j+1][i]=1
+                    else:
+                        w[j+1][i]=0
+                        
+        L=w  #定义拉普拉斯矩阵
+        for i in range (0,self.uav_num):
+
+            L[i][i]=-sum(w[i])
+                    
+        #print (L)               
+        return L
+'''
+
+if __name__ == '__main__':
+    follower = Follower(int(sys.argv[1]),9)
+    follower.loop()   

control/follower_vel_control.py

@@ -0,0 +1,226 @@
+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+import rospy
+from geometry_msgs.msg import Twist, Vector3, PoseStamped
+from std_msgs.msg import String 
+from pyquaternion import Quaternion
+from formation_dict import formation_dict_9
+import time
+import math
+import numpy 
+import sys
+import heapq
+<<<<<<< HEAD
+import copy
+=======
+>>>>>>> 0f5c47fd97cf0ee2f414346b113bbcc958d29f96
+
+class Follower:
+
+    def __init__(self, uav_id, uav_num):
+        self.hover = True
+        self.id = uav_id
+        self.uav_num = uav_num
+        self.local_pose = PoseStamped()
+        self.cmd_vel_enu = Twist()
+        self.avoid_vel = Vector3()
+        self.following_switch = False
+        self.following_ids = []
+        self.formation_config = 'waiting'
+        self.following_count = 0
+        self.Kp = 1
+        self.vel_xy_max = 1
+        self.vel_z_max = 1
+        self.following_local_pose = [None]*self.uav_num
+        self.following_local_pose_sub = [None]*self.uav_num
+        self.arrive_count = 0
+        self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
+        #self.cmd_vel_sub = rospy.Subscriber("/xtdrone/follower/cmd_vel", Twist, self.cmd_vel_callback)
+        self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
+        self.formation_switch_sub = rospy.Subscriber("/xtdrone/formation_switch",String, self.formation_switch_callback)
+        self.vel_enu_pub =  rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
+        self.info_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/info', String, queue_size=10)
+        
+    def local_pose_callback(self, msg):
+        self.local_pose = msg
+        #print(self.arrive_count)
+
+    def following_local_pose_callback(self, msg, id):
+        self.following_local_pose[id] = msg 
+        #print('here')
+
+    def cmd_vel_callback(self, msg):
+        self.cmd_vel_enu = msg
+        if msg.linear.z == 0:
+            self.hover = True
+        else:
+            self.hover = False
+
+    def formation_switch_callback(self, msg):
+        if not self.formation_config == msg.data:
+            self.following_switch = True
+        else:
+            self.following_switch = False
+        self.formation_config = msg.data
+        
+
+    def avoid_vel_callback(self, msg):
+        self.avoid_vel = msg
+<<<<<<< HEAD
+        print('here')
+=======
+>>>>>>> 0f5c47fd97cf0ee2f414346b113bbcc958d29f96
+        #if not self.avoid_vel.x == 0:
+        #    print('follower'+str(self.id-1)+' avoid_vel: ',self.avoid_vel)
+
+    def loop(self):
+        rospy.init_node('follower'+str(self.id-1))
+        rate = rospy.Rate(50)
+        while not rospy.is_shutdown():
+            if self.arrive_count > 100:
+                self.info_pub.publish("Arrived")
+                #print("Arrived")
+            if self.following_switch:
+                self.following_switch = False
+                self.info_pub.publish("Received")
+                print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
+                #self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config])
+                #print(self.L_matrix)
+                self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]])
+                self.following_ids = numpy.argwhere(self.L_matrix[self.id-1,:] == 1)
+                #if self.id == 2:
+                    #print(self.following_ids)
+                self.following_count = 0
+                for i in range(self.uav_num):
+                    if not self.following_local_pose_sub[i] == None:
+                        self.following_local_pose_sub[i].unregister()
+                for following_id in self.following_ids:
+                    #print('here')
+                    self.following_local_pose_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
+                    self.following_count += 1
+                    time.sleep(1)
+            print(self.avoid_vel)
+            self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_vel)
+            if self.cmd_vel_enu.linear.x == 0 and self.cmd_vel_enu.linear.y == 0 and self.cmd_vel_enu.linear.z == 0:
+                for following_id in self.following_ids:
+                    self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict_9[self.formation_config][0, self.id-2]
+                    self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict_9[self.formation_config][1, self.id-2]
+                    self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z+ formation_dict_9[self.formation_config][2, self.id-2]
+                    if not following_id[0] == 0:
+                        self.cmd_vel_enu.linear.x -= formation_dict_9[self.formation_config][0, following_id[0]-1]
+                        self.cmd_vel_enu.linear.y -= formation_dict_9[self.formation_config][1, following_id[0]-1]
+                        self.cmd_vel_enu.linear.z -= formation_dict_9[self.formation_config][2, following_id[0]-1]
+                self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x
+                self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y
+                self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z
+            if self.cmd_vel_enu.linear.x > self.vel_xy_max:
+                self.cmd_vel_enu.linear.x = self.vel_xy_max
+            elif self.cmd_vel_enu.linear.x < - self.vel_xy_max:
+                self.cmd_vel_enu.linear.x = - self.vel_xy_max
+            if self.cmd_vel_enu.linear.y > self.vel_xy_max:
+                self.cmd_vel_enu.linear.y = self.vel_xy_max
+            elif self.cmd_vel_enu.linear.y < - self.vel_xy_max:
+                self.cmd_vel_enu.linear.y = - self.vel_xy_max
+            if self.cmd_vel_enu.linear.z > self.vel_z_max:
+                self.cmd_vel_enu.linear.z = self.vel_z_max
+            elif self.cmd_vel_enu.linear.z < - self.vel_z_max:
+                self.cmd_vel_enu.linear.z = - self.vel_z_max
+            if (self.cmd_vel_enu.linear.x)**2+(self.cmd_vel_enu.linear.y)**2+(self.cmd_vel_enu.linear.z)**2<0.2:
+                self.arrive_count += 1
+            else:
+                self.arrive_count = 0
+            if not self.formation_config == 'waiting':
+                self.vel_enu_pub.publish(self.cmd_vel_enu)
+            rate.sleep()
+
+    def get_L_matrix(self, rel_posi):
+
+        #假设无论多少UAV，都假设尽可能3层通信（叶子节点除外）
+        c_num=int((self.uav_num-1)/3)
+        
+        comm=[[]for i in range (self.uav_num)]
+        w=numpy.ones((self.uav_num,self.uav_num))*0 # 定义邻接矩阵
+        nodes_next=[]
+        node_flag = [self.uav_num-1]
+
+        rel_d=[0]*(self.uav_num-1)
+        # 规定每个无人机可以随机连接三台距离自己最近的无人机,且不能连接在flag中的无人机(即已经判断过连接点的无人机)。
+        # 计算第一层通信（leader）：获得离自己最近的三台无人机编号
+        
+        for i in range(0,self.uav_num-1):
+
+            rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
+
+        min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
+        min_num_index_list=list(min_num_index_list)
+        #leader 连接的无人机编号：
+        comm[self.uav_num-1]=min_num_index_list
+
+        nodes_next.extend(comm[self.uav_num-1])    
+
+        size_=len(node_flag)
+
+        while (nodes_next!=[]) and (size_<(self.uav_num-1)):
+
+            next_node= nodes_next[0]
+            nodes_next=nodes_next[1:]
+
+            rel_d=[0]*(self.uav_num-1)
+            for i in range(0,self.uav_num-1):
+                    
+                if i==next_node or i in node_flag:
+                        
+                    rel_d[i]=2000   #这个2000是根据相对位置和整个地图的大小决定的，要比最大可能相对距离大
+                else:
+
+                    rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
+
+            min_num_index_list =  map(rel_d.index, heapq.nsmallest(c_num, rel_d))
+            min_num_index_list=list(min_num_index_list)
+            node_flag.append(next_node)
+
+            size_=len(node_flag)                    
+
+            for j in range(0,c_num):
+
+                if min_num_index_list[j] in node_flag:
+                                
+                    nodes_next=nodes_next
+
+                else:
+                    if min_num_index_list[j] in nodes_next:
+                        nodes_next=nodes_next
+                    else:
+                        nodes_next.append(min_num_index_list[j])
+                        
+                    comm[next_node].append(min_num_index_list[j])
+        # comm为每个uav连接其他uav的编号，其中数组的最后一行为leader   
+        #print (comm)
+        #leader是拉普拉斯矩阵的第一行，为0
+        #第0号飞机（相对位置矩阵中的第一个位置）为第二行，以此类推
+
+        for i in range (0,self.uav_num):
+            for j in range(0,self.uav_num-1):
+
+                if i==0:
+                    if j in comm[self.uav_num-1]:
+                        w[j+1][i]=1
+                    else:
+                        w[j+1][i]=0
+                else:
+                    if j in comm[i-1]:
+                        w[j+1][i]=1
+                    else:
+                        w[j+1][i]=0
+                        
+        L=w  #定义拉普拉斯矩阵
+        for i in range (0,self.uav_num):
+
+            L[i][i]=-sum(w[i])
+                    
+        #print (L)               
+        return L
+
+if __name__ == '__main__':
+    follower = Follower(int(sys.argv[1]),int(sys.argv[2]))
+    follower.loop()   

control/formation_dict.py

@@ -0,0 +1,11 @@
+import numpy as np
+
+formation_dict_6 = {"T":np.array([[2,0,2],[-2,0,2],[0,0,2],[0,0,-2],[0,0,-4]]) , "diamond": np.array([[2,2,-2],[2,-2,-2],[-2,-2,-2],[-2,2,-2],[0,0,-4]]), "triangle": np.array([[-3,0,0],[3,0,0],[-1.5,0,1.5],[1.5,0,1.5],[0,0,3]]),"waiting":np.zeros([3,5])}
+formation_dict_6["T"] = np.transpose(formation_dict_6["T"])
+formation_dict_6["diamond"] = np.transpose(formation_dict_6["diamond"])
+formation_dict_6["triangle"] = np.transpose(formation_dict_6["triangle"])
+
+formation_dict_9 = {"cube": np.array([[2,2,2],[-2,2,2],[-2,-2,2],[2,-2,2],[2,2,-2],[-2,2,-2],[-2,-2,-2],[2,-2,-2]]), "pyramid": np.array([[2,2,-2],[-2,2,-2],[-2,-2,-2],[2,-2,-2],[4,4,-4],[-4,4,-4],[-4,-4,-4],[4,-4,-4]]), "triangle": np.array([[0,4,-4],[0,0,-2],[0,0,-4],[0,-2,-2],[0,2,-4],[0,2,-2],[0,-4,-4],[0,-2,-4]]),"waiting":np.zeros([3,8])}
+formation_dict_9["cube"] = np.transpose(formation_dict_9["cube"])
+formation_dict_9["pyramid"] = np.transpose(formation_dict_9["pyramid"])
+formation_dict_9["triangle"] = np.transpose(formation_dict_9["triangle"])

control/keyboard_control.py

@@ -0,0 +1,195 @@
+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)

control/keyboard_multi_control.py

@@ -0,0 +1,264 @@
+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 = 'AUTO.LOITER'
+                cmd_vel_mask = False
+                print_msg()
+                print('Loiter')
+            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)

control/leader.py

@@ -0,0 +1,86 @@
+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+import rospy
+from geometry_msgs.msg import Twist, Vector3, PoseStamped
+from std_msgs.msg import String 
+from pyquaternion import Quaternion
+import time
+import math
+import numpy 
+import sys
+
+class Leader:
+
+    def __init__(self, leader_id, uav_num):
+        self.hover = True
+        self.id = leader_id
+        self.local_pose = PoseStamped()
+        self.cmd_vel_enu = Twist()
+        self.follower_num = uav_num - 1
+        self.followers_info = ["Moving"]*self.follower_num
+        self.follower_arrived_num = 0
+        self.follower_all_arrived = True
+        self.avoid_vel = Vector3(0,0,0)
+        self.formation_config = 'waiting'
+        self.target_height_recorded = False
+        self.Kz = 0.5
+        self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
+        self.cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, self.cmd_vel_callback)
+        self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
+        self.formation_switch_sub = rospy.Subscriber("/gcs/formation_switch",String, self.cmd_callback)
+        for i in range(self.follower_num):
+            rospy.Subscriber('/xtdrone/uav'+str(i+1)+'/info',String,self.followers_info_callback,i)
+        self.local_pose_pub = rospy.Publisher("/xtdrone/leader/pose", PoseStamped , queue_size=10)
+        self.formation_switch_pub = rospy.Publisher("/xtdrone/formation_switch",String, queue_size=10)
+        self.vel_enu_pub =  rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
+
+    def local_pose_callback(self, msg):
+        self.local_pose = msg
+
+    def cmd_vel_callback(self, msg):
+        self.cmd_vel_enu = msg
+        if msg.linear.z == 0:
+            self.hover = True
+        else:
+            self.hover = False
+
+    def cmd_callback(self, msg):
+        if not msg.data == '': 
+            self.formation_config = msg.data
+            #print("Switch to Formation"+self.formation_config)
+
+    def avoid_vel_callback(self, msg):
+        self.avoid_vel = msg
+        #print('leader: ', self.avoid_vel)
+    
+    def followers_info_callback(self, msg, id):
+        self.followers_info[id] = msg.data
+        #print("follower"+str(id)+":"+ msg.data) 
+
+    def loop(self):
+        rospy.init_node('leader')
+        rate = rospy.Rate(50)
+        while True:
+            self.cmd_vel_enu = Twist()
+            for follower_info in self.followers_info:
+                if follower_info == "Arrived":
+                    self.follower_arrived_num += 1
+            if self.follower_arrived_num > self.follower_num - 1:
+                self.follower_all_arrived = True
+            if self.follower_all_arrived:
+                self.formation_switch_pub.publish(self.formation_config)
+            if self.formation_config == 'pyramid':
+                if not self.target_height_recorded:
+                    target_height =  self.local_pose.pose.position.z + 2
+                    self.target_height_recorded = True
+                self.cmd_vel_enu.linear.z = self.Kz * (target_height - self.local_pose.pose.position.z)
+            self.cmd_vel_enu.linear.x += self.avoid_vel.x
+            self.cmd_vel_enu.linear.y += self.avoid_vel.y
+            self.cmd_vel_enu.linear.z += self.avoid_vel.z
+            self.vel_enu_pub.publish(self.cmd_vel_enu)
+            self.local_pose_pub.publish(self.local_pose)
+            rate.sleep()
+
+if __name__ == '__main__':
+    leader = Leader(1,int(sys.argv[1]))
+    leader.loop()   

control/leader_follower_formation10.py

@@ -0,0 +1,156 @@
+#!/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 = 1
+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+1]=PoseStamped()
+    relative_pose[i+1]=PoseStamped()
+    follower_cmd_vel[i+1]=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+1] = Point(0,i,0)
+        else:
+            formation_temp[i+1] = Point(0,-i-1,0)    
+    else:                                         
+        if (i+1)%2 == 1:                          
+            formation_temp[i+1] = Point( -2, 1+i-(uav_num/2),0 )
+        else:
+            formation_temp[i+1] = 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+1] = Point(0,i,0)    
+    elif i+1 != uav_num:                      
+        formation_temp[i+1] = Point(-i-1,0,0)   
+    else:                                   
+        formation_temp[i+1] = 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[1]=Point(0,0,0)
+formation_temp[2]=Point(-2,-2,0);formation_temp[3]=Point(-2,2,0)
+formation_temp[4]=Point(-4,-4,0);formation_temp[5]=Point(-4,0,0);formation_temp[7]=Point(-4,4,0)
+formation_temp[10]=Point(-6,-6,0);formation_temp[8]=Point(-6,-2,0);formation_temp[6]=Point(-6,2,0);formation_temp[9]=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+1
+    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+1
+    if uav_id != leader_id:
+        follower_vel_enu_pub[i+1] = rospy.Publisher(
+         '/xtdrone/uav'+str(i+1)+'/cmd_vel_enu', Twist, queue_size=10)
+
+rate = rospy.Rate(100)
+while(1):
+    for i in range(uav_num):
+        uav_id = i+1
+        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()

control/leader_follower_formation10_with_ac.py

@@ -0,0 +1,207 @@
+#!/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 = 1
+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+1]=PoseStamped()
+    relative_pose[i+1]=PoseStamped()
+    follower_cmd_vel[i+1]=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+1] = Point(0,i,0)
+        else:
+            formation_temp[i+1] = Point(0,-i-1,0)    
+    else:                                         
+        if (i+1)%2 == 1:                          
+            formation_temp[i+1] = Point( -2, 1+i-(uav_num/2),0 )
+        else:
+            formation_temp[i+1] = 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+1] = Point(0,i,0)    
+    elif i+1 != uav_num:                      
+        formation_temp[i+1] = Point(-i-1,0,0)   
+    else:                                   
+        formation_temp[i+1] = 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[1]=Point(0,0,0)
+formation_temp[2]=Point(-2,-2,0);formation_temp[3]=Point(-2,2,0)
+formation_temp[4]=Point(-4,-4,0);formation_temp[5]=Point(-4,0,0);formation_temp[7]=Point(-4,4,0)
+formation_temp[10]=Point(-6,-6,0);formation_temp[8]=Point(-6,-2,0);formation_temp[6]=Point(-6,2,0);formation_temp[9]=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
+
+
+
+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+1
+    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+1
+    if uav_id != leader_id:
+        follower_vel_enu_pub[i+1] = rospy.Publisher(
+         '/xtdrone/uav'+str(i+1)+'/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+1      
+        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+1
+        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()

control/leader_follower_formation6_with_ac.py

@@ -0,0 +1,141 @@
+#!/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 = 5
+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+1]=PoseStamped()
+    relative_pose[i+1]=PoseStamped()
+    follower_cmd_vel[i+1]=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+1
+    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+1
+    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+1      
+        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+1
+        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()

control/run.sh

@@ -0,0 +1,9 @@
+#!/bin/bash
+python leader.py $1 &
+uav_num=2
+while(( $uav_num<= $1 )) 
+do
+    python follower_vel_control.py $uav_num $1&
+    #echo $uav_num
+    let "uav_num++"
+done

control/simple_3d_simulator_accel.py

@@ -0,0 +1,84 @@
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D   
+import matplotlib.animation as animation              
+import numpy as np 
+import rospy
+from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped
+from gazebo_msgs.srv import GetModelState
+import sys
+
+use_1_8 = 1
+
+
+uav_num = int(sys.argv[1])
+
+step_time=0.001
+
+pose_puber=[None]*uav_num
+vel_puber=[None]*uav_num
+
+plot_x=[0]*(uav_num)
+plot_y=[0]*(uav_num)
+plot_z=[0]*(uav_num)
+
+for i in range(uav_num):
+    uav_id=i+use_1_8
+    plot_x[i]= i//3
+    plot_y[i]= i%3
+    pose_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/pose', PoseStamped, queue_size=10)
+    vel_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/velocity_local', TwistStamped, queue_size=10)
+
+fig = plt.figure()
+plt.ion()
+ax = Axes3D(fig)
+label_lim = 20
+
+def scroll_call_back(event):
+    global label_lim
+    if event.button == 'up':
+        label_lim+=2
+        #print('up')
+    elif event.button == 'down':
+        label_lim=label_lim-2 if label_lim>1 else 1
+        #print('down')
+
+
+fig.canvas.mpl_connect('scroll_event', scroll_call_back)
+
+def init():
+    ax.set_xlim3d(-label_lim, label_lim)
+    ax.set_ylim3d(-label_lim, label_lim)
+    ax.set_zlim3d(-label_lim, label_lim)
+
+
+#def cmd_accel_callback(msg,id):
+    accel=msg
+    plot_x[id]+=step_time*accel.linear.x
+    plot_y[id]+=step_time*accel.linear.y
+    plot_z[id]+=step_time*accel.linear.z
+
+
+rospy.init_node('simple_3d_simulator')
+rate = rospy.Rate(1000)
+
+for i in range(uav_num):
+    uav_id=i+use_1_8
+    rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_accel_flu', Twist, cmd_accel_callback,i) 
+    rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_accel_enu', Twist, cmd_accel_callback,i)    
+
+try:
+    while not rospy.is_shutdown():
+        for i in range(uav_num):
+            local_pose=PoseStamped()
+            local_pose.pose.position.x=plot_x[i]
+            local_pose.pose.position.y=plot_y[i]
+            local_pose.pose.position.z=plot_z[i]    
+            pose_puber[i].publish(local_pose)
+            pose_puber[i].publish(local_pose)
+        init()
+        ax.scatter(plot_x,plot_y,plot_z,marker="x")
+        plt.pause(step_time)
+        ax.cla()
+        rate.sleep()
+except KeyboardInterrupt:
+        plt.ioff()

control/simple_3d_simulator_vel.py

@@ -0,0 +1,85 @@
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D   
+import matplotlib.animation as animation              
+import numpy as np 
+import rospy
+from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped
+from gazebo_msgs.srv import GetModelState
+import sys
+
+use_1_8 = 1
+
+
+uav_num = int(sys.argv[1])
+
+step_time=0.01
+
+pose_puber=[None]*uav_num
+vel_puber=[None]*uav_num
+
+plot_x=[0]*(uav_num)
+plot_y=[0]*(uav_num)
+plot_z=[0]*(uav_num)
+local_vel = [TwistStamped()]*(uav_num)
+
+for i in range(uav_num):
+    uav_id=i+use_1_8
+    plot_x[i]= i//3
+    plot_y[i]= i%3
+    pose_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/pose', PoseStamped, queue_size=10)
+    vel_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/velocity_local', TwistStamped, queue_size=10)
+
+fig = plt.figure()
+plt.ion()
+ax = Axes3D(fig)
+label_lim = 20
+
+def scroll_call_back(event):
+    global label_lim
+    if event.button == 'up':
+        label_lim+=2
+        #print('up')
+    elif event.button == 'down':
+        label_lim=label_lim-2 if label_lim>1 else 1
+        #print('down')
+
+
+fig.canvas.mpl_connect('scroll_event', scroll_call_back)
+
+def init():
+    ax.set_xlim3d(-label_lim, label_lim)
+    ax.set_ylim3d(-label_lim, label_lim)
+    ax.set_zlim3d(-label_lim, label_lim)
+
+
+def cmd_vel_callback(msg,id):
+    local_vel[id].twist=msg
+    plot_x[id]+=step_time*local_vel[id].twist.linear.x
+    plot_y[id]+=step_time*local_vel[id].twist.linear.y
+    plot_z[id]+=step_time*local_vel[id].twist.linear.z
+
+
+rospy.init_node('simple_3d_simulator')
+rate = rospy.Rate(1/step_time)
+
+for i in range(uav_num):
+    uav_id=i+use_1_8
+    rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_vel_flu', Twist, cmd_vel_callback,i) 
+    rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_vel_enu', Twist, cmd_vel_callback,i)    
+
+try:
+    while not rospy.is_shutdown():
+        for i in range(uav_num):
+            local_pose=PoseStamped()
+            local_pose.pose.position.x=plot_x[i]
+            local_pose.pose.position.y=plot_y[i]
+            local_pose.pose.position.z=plot_z[i]    
+            pose_puber[i].publish(local_pose)
+            vel_puber[i].publish(local_vel[i])
+        init()
+        ax.scatter(plot_x,plot_y,plot_z,marker="x")
+        plt.pause(step_time)
+        ax.cla()
+        rate.sleep()
+except KeyboardInterrupt:
+        plt.ioff()

control/stop.sh

@@ -0,0 +1,2 @@
+kill -9 $(ps -ef|grep follower_vel_control.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
+kill -9 $(ps -ef|grep leader.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

control/yolo_human_tracking.py

@@ -0,0 +1,40 @@
+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()

cooperation/ekf2_config/iris_1

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_10

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_11

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_12

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_13

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_14

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_15

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_16

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_17

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_18

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_19

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_2

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_20

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_21

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_22

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_23

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_24

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_25

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_26

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_27

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_28

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_29

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_3

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_30

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_31

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_32

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_33

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_34

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_35

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_36

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_37

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_38

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_39

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_4

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_40

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_41

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_42

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_43

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_44

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_45

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_46

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_47

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_48

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_49

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_5

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_50

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_6

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_7

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_8

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_config/iris_9

@@ -0,0 +1,81 @@
+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

cooperation/ekf2_temp

@@ -0,0 +1,81 @@
+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

cooperation/launch/multi_uav.launch

@@ -0,0 +1,199 @@
+<?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>
+    <!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/xacro.rviz" required="true" />-->
+
+
+     <!-- UAV1-->
+     <group ns="uav1">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="1"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14540@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="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="$(arg vehicle)"/>
+            <arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
+            <arg name="mavlink_udp_port" value="24560"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV2-->
+     <group ns="uav2">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="2"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14541@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_2"/>
+            <arg name="mavlink_udp_port" value="24562"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV3-->
+     <group ns="uav3">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="3"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14542@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="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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_3"/>
+            <arg name="mavlink_udp_port" value="24564"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV4-->
+     <group ns="uav4">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="4"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14543@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_4"/>
+            <arg name="mavlink_udp_port" value="24566"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV5-->
+     <group ns="uav5">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="5"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14544@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="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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_5"/>
+            <arg name="mavlink_udp_port" value="24568"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV6-->
+     <group ns="uav6">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="6"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14545@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_6"/>
+            <arg name="mavlink_udp_port" value="24570"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+</launch>

cooperation/launch/multi_uav10.launch

@@ -0,0 +1,315 @@
+<?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>
+    <!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/xacro.rviz" required="true" />-->
+
+
+     <!-- UAV1-->
+     <group ns="uav1">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="1"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14540@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="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="$(arg vehicle)"/>
+            <arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
+            <arg name="mavlink_udp_port" value="24560"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV2-->
+     <group ns="uav2">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="2"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14541@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_2"/>
+            <arg name="mavlink_udp_port" value="24562"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV3-->
+     <group ns="uav3">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="3"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14542@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="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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_3"/>
+            <arg name="mavlink_udp_port" value="24564"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV4-->
+     <group ns="uav4">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="4"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14543@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_4"/>
+            <arg name="mavlink_udp_port" value="24566"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV5-->
+     <group ns="uav5">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="5"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14544@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="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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_5"/>
+            <arg name="mavlink_udp_port" value="24568"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV6-->
+     <group ns="uav6">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="6"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14545@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_6"/>
+            <arg name="mavlink_udp_port" value="24570"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV7-->
+     <group ns="uav7">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="7"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14546@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="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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_7"/>
+            <arg name="mavlink_udp_port" value="24572"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV8-->
+     <group ns="uav8">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="8"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14547@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_8"/>
+            <arg name="mavlink_udp_port" value="24574"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV9-->
+     <group ns="uav9">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="9"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14548@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="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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_9"/>
+            <arg name="mavlink_udp_port" value="24576"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV10-->
+     <group ns="uav10">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="10"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14549@127.0.0.1:34590"/>
+        <!-- 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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_10"/>
+            <arg name="mavlink_udp_port" value="24578"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+</launch>

cooperation/launch/multi_uav_6.launch

@@ -0,0 +1,199 @@
+<?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>
+    <!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/xacro.rviz" required="true" />-->
+
+
+     <!-- UAV1-->
+     <group ns="uav1">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="1"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14540@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="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="$(arg vehicle)"/>
+            <arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
+            <arg name="mavlink_udp_port" value="24560"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV2-->
+     <group ns="uav2">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="2"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14541@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_2"/>
+            <arg name="mavlink_udp_port" value="24562"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV3-->
+     <group ns="uav3">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="3"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14542@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="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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_3"/>
+            <arg name="mavlink_udp_port" value="24564"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV4-->
+     <group ns="uav4">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="4"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14543@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_4"/>
+            <arg name="mavlink_udp_port" value="24566"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV5-->
+     <group ns="uav5">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="5"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14544@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="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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_5"/>
+            <arg name="mavlink_udp_port" value="24568"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+     <!-- UAV6-->
+     <group ns="uav6">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="6"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14545@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="-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="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_6"/>
+            <arg name="mavlink_udp_port" value="24570"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+</launch>

cooperation/launch_head

@@ -0,0 +1,24 @@
+<?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>
+    <!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/xacro.rviz" required="true" />-->
+
+

cooperation/launch_head_1.9

@@ -0,0 +1,48 @@
+<?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>
+

cooperation/launch_temp

@@ -0,0 +1,28 @@
+ <!-- UAV1 -->
+    <group ns="uav1">
+        <!-- MAVROS and vehicle configs -->        
+        <arg name="ID" value="1"/>
+        <!--default="14540@localhost:14557"/>-->
+        
+        <arg name="fcu_url" default="udp://:14540@127.0.0.1:14557"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
+            <arg name="x" value="1"/>
+            <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="$(arg vehicle)"/>
+            <arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
+            <arg name="mavlink_udp_port" value="14560"/>
+            <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="$(arg ID)"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>

cooperation/launch_temp_1.9

@@ -0,0 +1,26 @@
+ <!-- UAV1 -->
+    <group ns="uav1">
+        <!-- MAVROS and vehicle configs -->
+        <arg name="ID" value="1"/>
+        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
+            <arg name="x" value="1"/>
+            <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="$(arg vehicle)"/>
+            <arg name="mavlink_udp_port" value="14561"/>
+            <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>

cooperation/sitl_config_generate.py

@@ -0,0 +1,135 @@
+#The number of drones
+import sys
+uav_num=int(sys.argv[1])
+ekf_dir="./ekf2_config/"
+launch_dir="./launch/"
+with open('launch_head','r') as f:
+    launch_head=[]
+    launch_head=f.read()
+with open('launch_temp','r') as f:
+    launch_lines=[]
+    for line in f.readlines():
+         launch_lines.append(line)
+with open('launch_head_1.9','r') as f:
+    launch_head_1_9=[]
+    launch_head_1_9=f.read()
+with open('launch_temp_1.9','r') as f:
+    launch_lines_1_9=[]
+    for line in f.readlines():
+         launch_lines_1_9.append(line)
+with open('ekf2_temp','r') as f:
+    ekf2lines=[]
+    for line in f.readlines():
+        if line!='\n':
+         ekf2lines.append(line)
+
+
+for num in range(1,uav_num+1):
+    iris_name="iris_"+str(num)
+    mavlink_1=34570-1+num*2
+    mavlink_2=mavlink_1+1
+    onboard=14540+num-1
+    SITL=24560+(num-1)*2
+    with open(ekf_dir+iris_name,'w') as f:
+        for line in ekf2lines:
+            if "MAV_SYS_ID" in line:
+                line ="param set MAV_SYS_ID "+str(num)             
+                f.write('%s\n' %line)
+            elif "SITL_UDP_PRT" in line:
+                line ="param set SITL_UDP_PRT "+str(SITL)
+                f.write('%s\n' %line)
+            elif "mavlink" in line:
+                continue
+            elif "replay trystart" in line:
+                continue
+            elif "logger start" in line:
+                continue
+            else:
+                f.write('%s' %line)
+        f.write("mavlink start -x -u %d -r 4000000\n" %mavlink_1)
+        f.write("mavlink start -x -u %d -r 4000000 -m onboard -o %d\n" %(mavlink_2,onboard))
+        f.write("mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u %d\n" %mavlink_1)
+        f.write("mavlink stream -r 50 -s LOCAL_POSITION_NED -u %d\n" %mavlink_1)
+        f.write("mavlink stream -r 50 -s GLOBAL_POSITION_INT -u %d\n" %mavlink_1)
+        f.write("mavlink stream -r 50 -s ATTITUDE -u %d\n" %mavlink_1)
+        f.write("mavlink stream -r 50 -s ATTITUDE_QUATERNION -u %d\n" %mavlink_1)
+        f.write("mavlink stream -r 50 -s ATTITUDE_TARGET -u %d\n" %mavlink_1)    
+        f.write("mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u %d\n" %mavlink_1)    
+        f.write("mavlink stream -r 20 -s RC_CHANNELS -u %d\n" %mavlink_1)    
+        f.write("mavlink stream -r 250 -s HIGHRES_IMU -u %d\n" %mavlink_1)    
+        f.write("mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u %d\n" %mavlink_1)    
+        f.write("logger start -e -t\n") 
+        f.write("mavlink boot_complete\n") 
+        f.write("replay trystart\n")
+        print iris_name," down"
+
+with open('./launch/multi_uav.launch','w') as f:
+    f.write(launch_head)
+    for num in range(1,uav_num+1):
+        mavlink_1=34570-1+num*2
+        mavlink_2=mavlink_1+1
+        onboard=14540+num-1
+        SITL=24560+(num-1)*2
+        for line in launch_lines:
+            if "<!-- UAV" in line:
+                f.write("     <!-- UAV%d-->\n" %num)
+            elif "<group ns" in line:
+                f.write('''     <group ns="uav%d">\n''' %num)
+            elif '''<arg name="ID" value="1"/>''' in line:
+                f.write('''        <arg name="ID" value="%d"/>\n''' %num)
+            elif "udp://:" in line:
+                f.write('''        <arg name="fcu_url" default="udp://:%d@127.0.0.1:%d"/>\n''' %(onboard,mavlink_2))
+            elif "rcS" in line:
+                f.write('''            <arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_%d"/>\n''' %num)
+            elif "mavlink_udp_port" in line:
+                f.write('''            <arg name="mavlink_udp_port" value="%d"/>\n'''%SITL)
+            elif '''name="x"''' in line:
+                f.write('''            <arg name="x" value="0"/>\n''')
+            elif '''name="y"''' in line:
+                f.write('''            <arg name="y" value="%d"/>\n''' %( (2*(num%2)-1 )*(num-1) ) )
+            else:
+                f.write('%s' %line)
+        f.write("\n")
+            
+    f.write('</launch>')
+    print ".launch for 1.8  down"
+
+
+with open('./launch_1.9/multi_uav.launch','w') as f:
+    f.write(launch_head_1_9)
+    for num in range(1,uav_num):
+        mavlink_1=34570-1+num*2
+        mavlink_2=mavlink_1+1
+        onboard=14540+num
+        SITL=24560+(num)*2
+        TCP=4560+num
+        for line in launch_lines_1_9:
+            if "<!-- UAV" in line:
+                f.write("     <!-- UAV%d-->\n" %num)
+            elif "<group ns" in line:
+                f.write('''     <group ns="uav%d">\n''' %num)
+            elif '''<arg name="ID" value="1"/>''' in line:
+                f.write('''        <arg name="ID" value="%d"/>\n''' %num)
+            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:
+                f.write('''            <arg name="mavlink_udp_port" value="%d"/>\n'''%SITL)
+            elif "mavlink_tcp_port" in line:
+                f.write('''            <arg name="mavlink_tcp_port" value="%d"/>\n'''%TCP)
+            elif '''name="x"''' in line:
+                f.write('''            <arg name="x" value="0"/>\n''')
+            elif '''name="y"''' in line:
+                f.write('''            <arg name="y" value="%d"/>\n''' %( (2*(num%2)-1 )*(num) ) )
+            else:
+                f.write('%s' %line)
+        f.write("\n")
+            
+    f.write('</launch>')
+    print ".launch for 1.9 down"      
+
+
+
+
+       
+    
+