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 <commander stop>,)");
warnx("( <mc_att_control stop> and)");
warnx("( <fw_att_control stop> 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 <parameters/param.h>
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.
9519
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
