Hi, dear colleges,
I am controlling a drone with pixhawk 2.4.8 with firmware 1.7.3. After defining an identity matrix, I am able to control the PWM that Pixhawk sends to ECSs. The topic that I publish my message is the actuator_control.
However, the drone is able to be armed and be switched into OFFBOARD mode. However, after 5-6s, the drone loses its armed status. I can not find the reason. Is here anyone having the same problem?
`Is seems that the service arming_client.call in my codes can not arm the drone.
Here is the my codes:
void UAVROS::takeoffAndSync()
{
ROS_INFO_THROTTLE(10,"take off begings");
// this target position is just 0.5m above the home position (where the drone is)
TakeOff_position.pose.position = Home.pose.position;
TakeOff_position.pose.position.z = Home.pose.position.z+0.5;
// the target position is initialized to be the home
Target_position.pose.position = Home.pose.position;
ros::Time last_request = ros::Time::now();
ros::Rate takeoff_rate(50);
offb_set_mode.request.custom_mode = "OFFBOARD";
arm_cmd.request.value = true;
while ( !(takeoff_pre && ready_in_air) && ros::ok()) //!takeoff_pre || !ready_in_air
{
ROS_INFO_THROTTLE(10,"In the loop");
// receive new sensor data
takeoff_rate.sleep();
ros::spinOnce();
// 1. arm the drone and swtich to the OFFBOAD mode
if (!mav_state.armed) //(mav_state.mode != "OFFBOARD") || !mav_state.armed
{
ROS_INFO("To arm the drone");
if( mav_state.mode != "OFFBOARD")
{
if(set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
}
if(!mav_state.armed)
{
if(arming_client.call(arm_cmd) &&
arm_cmd.response.success)
{
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
//doAttitudeControl(F_arm_lower_limit, 0,0,0);
doMotorControl(F_arm_lower_limit, 0,0,0);
ROS_INFO("In arming process");
}
// 2. increase the thrust force from the lower limit to the take off
if ((mav_state.mode == "OFFBOARD") && mav_state.armed && !takeoff_pre)
{
ROS_INFO_THROTTLE(1,"prepare to take off");
F_takeoff_des = 6.5;
F_takeoff = F_arm_lower_limit + (F_takeoff_des - F_arm_lower_limit)/T_arm *t_takeoff;
t_takeoff++;
doMotorControl(F_takeoff, 0,0,0);
if (F_takeoff == F_takeoff_des)
{
takeoff_pre = true;
ROS_INFO_ONCE("ready to take off");
}
ROS_INFO("In prepareing process");
/* code */
}
// 3. control the drone to take off into the air (0.5m above)
if((mav_state.mode == "OFFBOARD") && mav_state.armed && takeoff_pre && !ready_in_air)
{
//
ROS_INFO("to take off in the air");
double distance = hypot(hypot(Pose.pose.position.x-TakeOff_position.pose.position.x,
Pose.pose.position.y-TakeOff_position.pose.position.y),
Pose.pose.position.z-TakeOff_position.pose.position.z);
if (distance>=0.001)// if the drone is far away from the take_off position at lease 0.2m, we increase the target.z
{
// increase the tracking hight a little by little
if (Target_position.pose.position.z <= TakeOff_position.pose.position.z)
{
Target_position.pose.position.z = Target_position.pose.position.z + 0.01;
}
// call the controller to take off in the air
doTrajectoryTracking(Target_position.pose.position.x, Target_position.pose.position.y, Target_position.pose.position.z, 0);
}
else // if the drone is reaching or be stable at the take_off station
{
ready_in_air = true;
}
}
}`