PX4无人机速度控制

没想到这个这么快，早知道到和上一篇一起发了

控制代码

	float err_x = 0;     float err_x0 = 0;		float err_x_err = 0;
	float err_y = 0;	 float err_y0 = 0;		float err_y_err = 0;
	float err_z = 0;	 float err_z0 = 0;		float err_z_err = 0;
	float sum_ex=0;
	float sum_ey=0;
	float sum_ez=0;
	float vel_x = 0;
	float vel_y = 0;
	float vel_z = 0;
	float kp = 2.0;
	float kd = 0.5;  
	float ki = 0.01; 
		// -0. 2 超调量1 左右   + 0.2 超调量减小
	//  0.5-[0.3]   0.8-[??]  1.0-[比0.8 好一些]  
	//  设定控制无人机的位置
void local_pos_control(float pos_x, float pos_y, float pos_z)
{
	// 误差
	err_x = pos_x - local_pos.pose.position.x;
	err_y = pos_y - local_pos.pose.position.y;
	err_z = pos_z - local_pos.pose.position.z;
	// 误差的误差
	err_x_err = err_x - err_x0;
	err_y_err = err_y - err_y0;
	err_z_err = err_z - err_z0;
	// 保存本次误差
	err_x0 = err_x;
	err_y0 = err_y;
	err_z0 = err_z;
	sum_ex+=err_x;
	sum_ey+=err_y;
	sum_ez+=err_z;
	vel_x = kp * err_x + kd * err_x_err+ki*sum_ex;
	vel_y = kp * err_y + kd * err_y_err+ki*sum_ey;
	vel_z = kp * err_z + kd * err_y_err+ki*sum_ez;

	// 比例控制
	// vel_x = adj_kp * err_x;
	// vel_y = adj_kp * err_y;
	// vel_z = adj_kp * err_z;


	ROS_INFO("Pose-x: %f",local_pos.pose.position.x);
	ROS_INFO("Pose-y: %f",local_pos.pose.position.y);
	ROS_INFO("Pose-z: %f",local_pos.pose.position.z);
}

就是简单的PID 控制器，积分项是有必要的，参数闭眼给的经典2 ，0. 5，0.01，最后飞起来的时候震荡就很厉害，不过不重要，主要是体验下速度控制历程，后面大概率是用现控去优化，没必要浪费时间调pid

完整代码

/**
 *         功能包：  roscpp std_msgs geometry_msgs mavros_msgs
 *         程序实现内容 ：使无人机飞行一个矩形轨迹后降落 （路径点）
 *          vec_pub.publish(vector);               ——  2021/12/1    Poao
 * 
 *            话题/服务名称                                     操作                                                            消息类型															头文件
 *          mavros/cmd/arming                      服务的客户端（进入待机模式）     mavros_msgs::CommandBool			#include <mavros_msgs/CommandBool.h>	
 *          mavros/set_mode