PX4例程学习

PX4的源代码中提供了一些学习例程，对于PX4的二次开发非常有用。现在学习下，应该有很多能够运用上的东西。例程的目录位于Firmware\src\examples目录下，每个目录是独立的应用程序，包含需要用到的源代码和CMake文件。

CMake的写法

在每个样例中有一个CMakeLists.txt文件，对于每个样例都需要这样一个文件来把程序编译进可执行程序中，如下：

px4_add_module(
	MODULE examples__px4_simple_app  //model的名称
	MAIN px4_simple_app              //可执行的命令
	STACK_MAIN 2000                  //堆栈大小
	SRCS                             //依赖的源文件
		px4_simple_app.c
	DEPENDS                          //暂时不知道是干什么的
		platforms__common
	)

源文件和CMake文件编写好之后需要在编译配置中加入我们的样例， 然后在Firmware\cmake\configs\nuttx_px4fmu-v2_default.cmake的配置文件中加上需要编译的文件夹，比如example/px4_sample_app，这里的目录是相对于Firmware/src目录的。需要注意的是，在定义的文件中一定要有一个px4_simple_app_main的函数，否则编译的时候会找不到这个函数，导致nuttx无法创建app应用程序。为了能够使得其他模块使用这个主函数，需要__EXPORT 这个函数。对于.c文件和.cpp文件分别需要加上这条语句

__EXPORT int px4_simple_app_main(int argc, char *argv[]);
extern "C" __EXPORT int px4_simple_app_main(int argc, char *argv[]);

下面一个个的分析每个样例代码。

px4_simple_app

这个样例主要是关于uorb主题订阅和发布的，关于uorb的资料网上有很多，下面介绍主题订阅和发布流程

订阅一个主题的流程为

获取主题的fd
/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

设置更新的频率
/* limit the update rate to 5 Hz */
orb_set_interval(sensor_sub_fd, 200);

把订阅主题的fd添加到检测列表中
/* one could wait for multiple topics with this technique, just using one here */
px4_pollfd_struct_t fds[] = {
    { .fd = sensor_sub_fd,   .events = POLLIN },
	/* there could be more file descriptors here, in the form like:
		* { .fd = other_sub_fd,   .events = POLLIN },
	*/
};

阻塞等待传感器信息的更新，最多等待1000，如果1s后还没有数据poll_ret返回0,1s内有数据返回1
/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
int poll_ret = px4_poll(fds, 1, 1000);

判断是否有数据推送，是的话，通过orb_copy把数据放在raw结构中
if (fds[0].revents & POLLIN) {
		/* obtained data for the first file descriptor */
		struct sensor_combined_s raw;
		/* copy sensors raw data into local buffer */
		orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);

发布一个主题的流程

先定义一个用于存放数据的结构并申请内存，根据事件的id，得到发布数据需要的fd
/* advertise attitude topic */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

把需要更新的数据放入att中，最后发布出去
/* set att and publish this information for other apps
the following does not have any meaning, it's just an example
*/
att.q[0] = raw.accelerometer_m_s2[0];
att.q[1] = raw.accelerometer_m_s2[1];
att.q[2] = raw.accelerometer_m_s2[2];

orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);

px4_mavlink_debug

好像是用debug_key_value这个uorb_id可以将需要发送的信息丢给mavlink，从而在地面站上显示出来？

publisher

这也是一个发布消息的程序，但是很奇怪，好像不是用uorb来发布，不清楚是什么功能，等下吧程序烧录进去看一下。发布的方法有点诡异。

/* Publish example message */
px4_rc_channels rc_channels_msg;
rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros();
PX4_INFO("rc: %" PRIu64, rc_channels_msg.data().timestamp_last_valid);
_rc_channels_pub->publish(rc_channels_msg);

hwtest

这是一个硬件测试的程序，程序执行后会驱动0,1通道上的设备工作起来，所以一定要把电机的螺旋桨卸掉，同时关闭姿态控制程序。就如代码中的信息说的

	warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!");
	warnx("(run ,)");
	warnx("(     and)");
	warnx("(     to do so)");
	warnx("usage: http://px4.io/dev/examples/write_output");

然后订阅驱动器的 信息，发布解锁的消息actuator_armed，先发布已经解锁的消息方便后面的输出。然后开始测试驱动器。

	struct actuator_controls_s actuators;
	memset(&actuators, 0, sizeof(actuators));
	orb_advert_t actuator_pub_ptr = orb_advertise(ORB_ID(actuator_controls_0), &actuators);

	struct actuator_armed_s arm;
	memset(&arm, 0 , sizeof(arm));

	arm.timestamp = hrt_absolute_time();
	arm.ready_to_arm = true;
	arm.armed = true;
	orb_advert_t arm_pub_ptr = orb_advertise(ORB_ID(actuator_armed), &arm);
	orb_publish(ORB_ID(actuator_armed), arm_pub_ptr, &arm);

	/* read back values to validate */
	int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
	orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm);

	if (arm.ready_to_arm && arm.armed) {
		warnx("Actuator armed");

	} else {
		errx(1, "Arming actuators failed");
	}

	hrt_abstime stime;

	int count = 0;

	while (count != 36) {
		stime = hrt_absolute_time();

		while (hrt_absolute_time() - stime < 1000000) {
			for (int i = 0; i != 2; i++) {
				if (count <= 5) {
					actuators.control[i] = -1.0f;

				} else if (count <= 10) {
					actuators.control[i] = -0.7f;

				} else if (count <= 15) {
					actuators.control[i] = -0.5f;

				} else if (count <= 20) {
					actuators.control[i] = -0.3f;

				} else if (count <= 25) {
					actuators.control[i] = 0.0f;

				} else if (count <= 30) {
					actuators.control[i] = 0.3f;

				} else {
					actuators.control[i] = 0.5f;
				}
			}

			actuators.timestamp = hrt_absolute_time();
			orb_publish(ORB_ID(actuator_controls_0), actuator_pub_ptr, &actuators);
			usleep(10000);
		}

		warnx("count %i", count);
		count++;
	}

	return OK;

 fixedwing_control

这是固定翼姿态控制一个程序,这仅仅是一个非常简单的控制程序，采用的是比例控制器。

1、首先是设置几个参数，PX4新建参数可以参考FantasyJXF的这篇博客

首先在你的文件夹下面新建params.h和params.c文件，在.c文件中新定义几个参数，这里包括roll和pitch比例控制器的增益

/**
 *
 */
PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.1f);

/**
 *
 */
PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f);

/**
 *
 */
PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f);

然后在头文件中定义这3个参数的结构

#include 

struct params {
	float hdng_p;
	float roll_p;
	float pitch_p;
};

struct param_handles {
	param_t hdng_p;
	param_t roll_p;
	param_t pitch_p;
};

调用构造函数，获取到这个参数

int parameters_init(struct param_handles *handles)
{
	/* PID parameters */
	handles->hdng_p 	=	param_find("EXFW_HDNG_P");
	handles->roll_p 	=	param_find("EXFW_ROLL_P");
	handles->pitch_p 	=	param_find("EXFW_PITCH_P");

	return 0;
}

还可以判断参数是否被修改，修改过的话可以更新

int parameters_update(const struct param_handles *handles, struct params *parameters)
{
	param_get(handles->hdng_p, &(parameters->hdng_p));
	param_get(handles->roll_p, &(parameters->roll_p));
	param_get(handles->pitch_p, &(parameters->pitch_p));

	return 0;
}

2 定义用于守护进程运行的主函数ex_fixedwing_control_main，通过这个函数可以开始运行或者停止运行这个守护进程。如果参数是start，在操作系统上建立一个应用程序，运行对应的函数，如果参数是stop，则停止运行这个函数。

int ex_fixedwing_control_main(int argc, char *argv[])
{
	if (argc < 2) {
		usage("missing command");
	}

	if (!strcmp(argv[1], "start")) {

		if (thread_running) {
			printf("ex_fixedwing_control already running\n");
			/* this is not an error */
			exit(0);
		}

		thread_should_exit = false;
		deamon_task = px4_task_spawn_cmd("ex_fixedwing_control",
						 SCHED_DEFAULT,
						 SCHED_PRIORITY_MAX - 20,
						 2048,
						 fixedwing_control_thread_main,
						 (argv) ? (char *const *)&argv[2] : (char *const *)nullptr);
		thread_running = true;
		exit(0);
	}

	if (!strcmp(argv[1], "stop")) {
		thread_should_exit = true;
		exit(0);
	}

	if (!strcmp(argv[1], "status")) {
		if (thread_running) {
			printf("\tex_fixedwing_control is running\n");

		} else {
			printf("\tex_fixedwing_control not started\n");
		}

		exit(0);
	}

	usage("unrecognized command");
	exit(1);
}

3  接下来是线程的主函数fixedwing_control_thread_main

在主函数里面，主要有这些操作：参数的初始化、更新参数，订阅程序需要的消息。然后等待新消息的到来。加入一有新的姿态数据过来了，就调用一次姿态控制函数，姿态控制函数如下：

void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
		      struct vehicle_rates_setpoint_s *rates_sp,
		      struct actuator_controls_s *actuators)
{

	/*
	 * The PX4 architecture provides a mixer outside of the controller.
	 * The mixer is fed with a default vector of actuator controls, representing
	 * moments applied to the vehicle frame. This vector
	 * is structured as:
	 *
	 * Control Group 0 (attitude):
	 *
	 *    0  -  roll   (-1..+1)
	 *    1  -  pitch  (-1..+1)
	 *    2  -  yaw    (-1..+1)
	 *    3  -  thrust ( 0..+1)
	 *    4  -  flaps  (-1..+1)
	 *    ...
	 *
	 * Control Group 1 (payloads / special):
	 *
	 *    ...
	 */

	/*
	 * Calculate roll error and apply P gain
	 */

	matrix::Eulerf att_euler = matrix::Quatf(att->q);
	matrix::Eulerf att_sp_euler = matrix::Quatf(att_sp->q_d);

	float roll_err = att_euler.phi() - att_sp_euler.phi();
	actuators->control[0] = roll_err * p.roll_p;

	/*
	 * Calculate pitch error and apply P gain
	 */
	float pitch_err = att_euler.theta() - att_sp_euler.theta();
	actuators->control[1] = pitch_err * p.pitch_p;
}

非常简短，这里就是一个比例控制器，来控制飞机的roll和pitch.