I am trying to control two drones using the OFFBOARD mode by following the tutorial given on the px4 website
I am able to get it working for the first drone, but when I try to do it for the second drone, I get the following error.
[ERROR] : MODE: Unsupported FCU
The following is the code that I use
mavros_msgs::SetMode offb_set_mode_1;
offb_set_mode_1.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd_1;
arm_cmd_1.request.value = true;
mavros_msgs::SetMode offb_set_mode_2;
offb_set_mode_2.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd_2;
arm_cmd_2.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if( current_state_1.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client_1.call(offb_set_mode_1) &&
offb_set_mode_1.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state_1.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client_1.call(arm_cmd_1) &&
arm_cmd_1.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
if( current_state_2.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
printf("I am here\n");
if( set_mode_client_2.call(offb_set_mode_2) &&
offb_set_mode_2.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state_2.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client_2.call(arm_cmd_2) &&
arm_cmd_2.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
pose_1.pose.position.x = x_traj_1[pos_counter];
pose_1.pose.position.y = y_traj_1[pos_counter];
pose_1.pose.position.z = z_traj_1[pos_counter];
pose_2.pose.position.x = x_traj_1[pos_counter];
pose_2.pose.position.y = y_traj_1[pos_counter];
pose_2.pose.position.z = z_traj_2[pos_counter];
local_pos_pub_1.publish(pose_1);
local_pos_pub_2.publish(pose_2);
// put a better criteria to know whether the destination has reached
//&& round(current_pose_2.pose.position.x)==pose_2.pose.position.x && round(current_pose_2.pose.position.y)==pose_2.pose.position.y && round(current_pose_2.pose.position.z)==pose_2.pose.position.z
if (round(current_pose_1.pose.position.x)==pose_1.pose.position.x && round(current_pose_1.pose.position.y)==pose_1.pose.position.y && round(current_pose_1.pose.position.z)==pose_1.pose.position.z)
{
// printf("%f\n",current_pose_1.pose.position.x);
pos_counter++;
pos_counter =pos_counter % 4;
}
ros::spinOnce();
rate.sleep();
}
I ran the rostopic echo -n2 /diagnostics and go the following output
header:
seq: 1435
stamp:
secs: 1476
nsecs: 22000000
frame_id: ''
status:
-
level: 0
name: iris_1: FCU connection
message: connected
hardware_id: udp://:14540@127.0.0.1:14557
values:
-
key: Received packets:
value: 5784
-
key: Dropped packets:
value: 0
-
key: Buffer overruns:
value: 0
-
key: Parse errors:
value: 0
-
key: Rx sequence number:
value: 154
-
key: Tx sequence number:
value: 228
-
key: Rx total bytes:
value: 24673844
-
key: Tx total bytes:
value: 2442242
-
key: Rx speed:
value: 16778.000000
-
key: Tx speed:
value: 1737.000000
-
.
.
.
.
.
.
.
.
header:
seq: 1418
stamp:
secs: 1476
nsecs: 264000000
frame_id: ''
status:
-
level: 1
name: iris_2: FCU connection
message: not connected
hardware_id: udp://:14541@127.0.0.1:14559
values:
-
key: Received packets:
value: 2619
-
key: Dropped packets:
value: 0
-
key: Buffer overruns:
value: 0
-
key: Parse errors:
value: 0
-
key: Rx sequence number:
value: 60
-
key: Tx sequence number:
value: 24
-
key: Rx total bytes:
value: 24549441
-
key: Tx total bytes:
value: 2441039
-
key: Rx speed:
value: 24247.000000
-
key: Tx speed:
value: 2600.000000