Offbord Control not working (cpp. suspend at height 0.5m )

[yiwc]

Hello. Issue is offboard control is not working with this simple cpp. Copter fly FAR MORE higher than setpoint.

This cpp works well in Gazebo,and virtual copter keeps at 0.5m. ok. But in my real copter, Copter flies far more than 0.5m maybe 3~4m and still keep lifting , has no attemption to stop lifting. (For safty, I use 5m rope to restrict it fly away.) It supposed to stop at 0.5m. Right? I have no idea what this problem is. Thanks for your help.

Flight log: (sorry,I'm not sure which one is, these logs messed up. Maybe 1st below is the test of hovering 0.5m)

https://logs.px4.io/plot_app?log=ccf4ea7b-d356-4d52-a792-f6c70f3ef38f or

https://logs.px4.io/plot_app?log=1ddf6832-e20f-44a3-a34c-27eb77b71b76 or

https://logs.px4.io/plot_app?log=7f39c0c9-24d4-4827-aab1-e366213e0289 _system info:

Offboard: RasipBerry --UART--> PX4
Airframe: DJI Flame Wheel F450
Quadrotor x (4011)
Hardware: PX4_FMU_V3 (V2)
Software Version: 161cf7f5
branch: master
OS Version: NuttX, v7.22.0
Estimator: EKF2
Logging Duration: 0:02:47
Vehicle UUID: 0001000000003436333932355110001f0032
Max Altitude Difference: | 1 m
Max Tilt Angle: | 0.5 deg
Max Rotation Speed: | 9.2 deg/s
Average Current: | 0.3 A
Max Current: | 0.8 A_

test cpp is like this. simply changed from http://dev.px4.io/en/ros/mavros_offboard.html

`

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}

int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");

//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);

// wait for FCU connection
while(ros::ok() && current_state.connected){
ros::spinOnce();
rate.sleep();
}

geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0.5;

//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";//原程序下面会用这个更改模式

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true; //原程序下面会用这条指令解锁

ros::Time last_request = ros::Time::now();

while(ros::ok()){

ROS_INFO("CYWROS: mode=%s",&current_state.mode);
local_pos_pub.publish(pose);
ROS_INFO("Pose Published!");

if(current_state.mode == "OFFBOARD"){
ROS_INFO("_ CYWROS: Offboard Enabled");

if((ros::Time::now() - last_request > ros::Duration(5.0))){
ROS_INFO("_ _ CYWROS: Duration Finished");
last_request = ros::Time::now();
local_pos_pub.publish(pose);
ROS_INFO("_ _ CYWROS: Pose Published!");
}
else{
ROS_INFO("_ _ CYWROS: Duration Not Finished");
}
}

else if(current_state.mode != "OFFBOARD"){
ROS_INFO("_ CYWROS: Offboard Disabled");
ROS_INFO("_ _ CYWROS: Do Nothing~ (030)");
}

ros::spinOnce();
rate.sleep();
}

return 0;
}

[Jaeyoung-Lim]

@E666GT Did you check if your real copter stays in the same altitude on altitude mode / position hold mode?

[yiwc]

@Jaeyoung-Lim Hello. I did checked , position mode works fine, copter is so stable . Altitude mode not tried yet, should I have a try ? Maybe no need . Or do you have any more idea about this problem. Thanks. I looking forward for your help

[Jaeyoung-Lim]

@E666GT Looking at your logs, all the three logs show that the copter never went into offboard mode and was in manual mode.

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.