PX4编写自己的队列任务并执行

已于 2024-03-30 10:22:46 修改
阅读量100 收藏
点赞数
文章标签： 1024程序员节
于 2023-10-24 10:01:31 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_41869763/article/details/134004982
版权

参考链接：PX4 User Guide - Development - Applicaiton/Module Template。

一、基于话题订阅决定运行频率

  1. 在 Firmware/src/modules 下新建文件夹 mc_self_check，并在里面添加 MulticopterSelfCheck.cpp，MulticopterSelfCheck.hpp 和 CMakeLists.txt 三个文件。
  2. 按照官方给的模板，也参考PX4源文件 Firmware/src/modules/mc_att_control 中的文件写法，三个文件内容分别如下

MulticopterSelfCheck.hpp

#pragma once

#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <matrix/matrix/math.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>

using namespace time_literals;

class MulticopterSelfCheck :
	public ModuleBase<MulticopterSelfCheck>,
	public ModuleParams,
	public px4::WorkItem
{
public:
	MulticopterSelfCheck();
	~MulticopterSelfCheck() override;

	bool init();
	static int task_spawn(int argc, char *argv[]);
	static int custom_command(int argc, char *argv[]);
	static int print_usage(const char *reason = nullptr);
private:
	hrt_abstime	_time_stamp_last_loop{0};

	void Run() override;
	perf_counter_t _loop_perf;
	uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};
	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
};

MulticopterSelfCheck.cpp

#include "MulticopterSelfCheck.hpp"
#include <stdio.h>

MulticopterSelfCheck::MulticopterSelfCheck():
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
}

MulticopterSelfCheck::~MulticopterSelfCheck()
{
	perf_free(_loop_perf);
}

bool MulticopterSelfCheck::init()
{
	if (!_local_pos_sub.registerCallback()) {
		PX4_ERR("vehicle_local_position callback registration failed!");
		return false;
	}
	return true;
}


void MulticopterSelfCheck::Run()
{
	if (should_exit()){
		_local_pos_sub.unregisterCallback();
		exit_and_cleanup();
		return;
	}

	perf_begin(_loop_perf);

	vehicle_local_position_s local_pos;
	if (_local_pos_sub.update(&local_pos)) {
		const hrt_abstime time_stamp_now = local_pos.timestamp;
		const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
		_time_stamp_last_loop = time_stamp_now;

		printf("dt = %f\r\n", (double)dt);
	}
	perf_end(_loop_perf);
}

int MulticopterSelfCheck::task_spawn(int argc, char *argv[])
{
	MulticopterSelfCheck *instance = new MulticopterSelfCheck();

	if (instance) {
		_object.store(instance);
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
			return PX4_OK;
		}

	} else {
		PX4_ERR("alloc failed");
	}
	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
}

int MulticopterSelfCheck::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}

int MulticopterSelfCheck::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
Self check.
)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("mc_self_check", "check");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
	return 0;
}

extern "C" __EXPORT int mc_self_check_main(int argc, char *argv[])
{
	return MulticopterSelfCheck::main(argc, argv);
}

CMakeLists.txt

px4_add_module(
	MODULE modules__mc_self_check
	MAIN mc_self_check
	COMPILE_FLAGS
		${MAX_CUSTOM_OPT_LEVEL}
	SRCS
		MulticopterSelfCheck.cpp
		MulticopterSelfCheck.hpp
	DEPENDS
		px4_work_queue
	)
  1. 添加编译指令
    在 Firmware/boards/px4/sitl/default.cmake 下的 px4_add_board 函数的 MODULES 后加上 mc_self_check。

一些版本的PX4需要 Kconfig 文件，并且使用指令进行配置编译项。Kconfig例子如下

menuconfig MODULES_MC_SELF_CHECK
	bool "self_check"
	default n
	---help---
		Enable support for self_check

menuconfig USER_MC_SELF_CHECK
	bool "mc_self_check running as userspace module"
	default y
	depends on BOARD_PROTECTED && MODULES_MC_SELF_CHECK
	---help---
		Put self_check in userspace memory

指令为

make px4_sitl_default boardconfig

随后选择需要编译的文件。

  1. 添加启动命令
    在 Firmware/ROMFS/px4fmu_common/init.d/rc.mc_apps 最后加上 mc_self_check start。

  2. 编译运行
    先清除编译，否则可能会报错。依次执行以下指令

make clean
make px4_sitl_default gazebo

最终可见如下输出

在这里插入图片描述
该输出是在 Multicopter.cpp/MulticopterSelfCheck::Run() 中编写的。

二、定时执行任务

如果要定时执行，修改代码如下

MulticopterSelfCheck.hpp

/**
 * Multicopter self-check.
 */

#pragma once

#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>

#include <matrix/matrix/math.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>

using namespace time_literals;

class MulticopterSelfCheck :
	public ModuleBase<MulticopterSelfCheck>,
	public ModuleParams,
	//public px4::WorkItem
	public px4::ScheduledWorkItem  // different
{
public:
	MulticopterSelfCheck();
	~MulticopterSelfCheck() override;

	bool init();
	static int task_spawn(int argc, char *argv[]);
	static int custom_command(int argc, char *argv[]);
	static int print_usage(const char *reason = nullptr);

private:
	hrt_abstime	_time_stamp_last_loop{0};

	void Run() override;
	perf_counter_t _loop_perf;

	uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};
	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
};

MulticopterSelfCheck.cpp

#include "MulticopterSelfCheck.hpp"


MulticopterSelfCheck::MulticopterSelfCheck():
	ModuleParams(nullptr),
	//WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), // different
	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{

}

MulticopterSelfCheck::~MulticopterSelfCheck()
{
	perf_free(_loop_perf);
}


bool MulticopterSelfCheck::init()
{
	// if (!_local_pos_sub.registerCallback()) {
	// 	PX4_ERR("vehicle_local_position callback registration failed!");
	// 	return false;
	// }
	ScheduleOnInterval(50000_us); // different
	return true;
}


void MulticopterSelfCheck::Run()
{
	if (should_exit()){
		_local_pos_sub.unregisterCallback();
		exit_and_cleanup();
		return;
	}

	perf_begin(_loop_perf);

	const hrt_abstime time_stamp_now = hrt_absolute_time();
	const float dt = (time_stamp_now - _time_stamp_last_loop) * 1e-6f;
	_time_stamp_last_loop = time_stamp_now;

	printf("dt = %f\r\n", (double)dt);

	perf_end(_loop_perf);
}

int MulticopterSelfCheck::task_spawn(int argc, char *argv[])
{
	MulticopterSelfCheck *instance = new MulticopterSelfCheck();

	if (instance) {
		_object.store(instance);
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
			return PX4_OK;
		}

	} else {
		PX4_ERR("alloc failed");
	}
	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
}

int MulticopterSelfCheck::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}

int MulticopterSelfCheck::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
Self check.
)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("mc_self_check", "check");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
	return 0;
}

extern "C" __EXPORT int mc_self_check_main(int argc, char *argv[])
{
	return MulticopterSelfCheck::main(argc, argv);
}

在 init() 中决定运行频率，此时是50000us=50ms，运行结果如下

在这里插入图片描述
并不是精确度50ms=0.05s，这主要是由于该循环也有一定调用周期，好像是250Hz=4ms，因此很难刚好总在0.05s。

大强强小强强
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
PX4姿态误差计算过程分析
03-04
PX4姿态误差计算分析
px4 firmware1.11.0.zip
08-07
px4固件v1.11.0，我已下载更新好所有子模块，免去了翻墙改文件等一堆麻烦，下载即用
PX4-6-串口设备驱动
AcmeUav的博客
10-12 785
上一篇我们讲了PX4的SPI/IIC设备驱动，现在讲一下PX4的串口设备。 PX4的串口设备驱动框架比SPI/IIC设备简单不少，使用了两种底层实现方式： 一种是系统自带的标准字符设备接口，一种是直接使用mcu的底层资源通过自行配置中断和DMA的方式实现的串口收发。 使用系统接口 PX4使用Nuttx的标准的字符设备接口，通过标准接口函数open、close、read、write、ioctl等函数操作。 熟悉linux的同学对这个应该不陌生，通过看linux的驱动编程书籍对Nuttx的串口编程也同
PX4-2-系统架构浅析
AcmeUav的博客
09-06 730
PX4软件架构图是PX4官方开发者网站中的一张总体框架图，相信很多同学都看过，我们今天围绕这张图作一下简单的分析。 如果希望仔细研究PX4源码的同学应该仔细阅读这一张图，它包含了所有的核心模块，以及它们的关系。 整个PX4源码分为几个部分 消息总线 uORB PX4软件架构的核心思想就是去耦合，所以消息中间件（uORB）处于整个框架图的中心位置。这样的设计是基本所有的大型软件系统的通用设计，比如ROS、ROS2。关于uORB详细分析，将在下一讲中详细描述。 基础服务 任
4.4WorkQueue
Peng的专栏
08-10 542
前言 之前在写单片机的程序时候，在通信接口的驱动编写的时候遇到过缓存的思想，在后端代码编写上也是顺序读取并执行相关代码；说到底是对这一系列的操作没有一个完整可描述的处理框架，现在接触到WorkQueue，大致搜罗一些资料看了下，感性上觉得是一种处理这种前后台的框架，下面以nuttx为基础进行详细的分析； 中断 中断服务程序一般都是在中断请求关闭的条件下执行的,以避免嵌套而使中断控制复杂化。但是，中断
PX4项目学习::（四）中间件::任务管理与调度
one__leaf的博客
02-02 711
Work queue tasks: 该模块在共享工作队列上运行，与队列中的其他模块共享相同的堆栈和工作队列线程优先级。Tasks: 模块在自己的任务中运行，具有自己的堆栈和进程优先级。
PX4飞控--从多旋翼姿态控制模块看WorkQueue工作队列
最新发布
qq_42985705的博客
12-28 1676
终于算是写完了，虽然时间花也没花太多，不过第一次动笔写完，也算有些意义。写的过程中发现还是有很多没有完全搞明白的地方，px4对现在的我来说确实是算庞大的系统了，还得从小地方入手一点点来吧。不过能进一步就是一步，人生不过三万天，努力一天算一天，就这样，接下来去码头整点薯条吧，respect。
PX4/Pixhawk 教程 - 任务线程 - workqueue 和 task
linxiaobo110的专栏
07-18 1800
介绍 一个完整的px4的应用程序（或者叫任务）分为前台部分和后台部分，前台部分是跑在shell任务中的，比如helloworld那个程序就只有前台部分，敲入指令即可在ssh中运行，前台部分一般是用来测试或者启动后台任务。 px4的后台任务主要油两种，一种叫任务（Tasks），一种叫工作队列（Work queue tasks）。两者的主要区别是任务拥有独立的栈空间和进程优先级，而工作队列是跑在一个统一的进程里的多个线程，共享优先级、栈空间。 优缺点对比 任务：优点是可以保证优先级的切换；可以进行阻塞操作； 工
PX4模块设计之十三：WorkQueue设计
lida2003的专栏
07-27 1390
工作队列，其实并不负责。而PX4的工作队列为什么看起来复杂，主要是工作队列和实际的业务耦合。这里我们还没有将uORB的订阅内容放到里面，如果结合这部分，再加上多个继承业务的相互切换等内容，就看似更加复杂了。所以我们尤其强调设计需要松耦合，尽量模块化，明确接口设计，明确框架设计。......
PX4的workqueue
weixin_30493401的博客
07-23 463
Workqueue相当于是中断子程序，然后在queue的cycle里面要注意，不能在cycle函数里面用printf打印，在cycle里面printf函数是打印不出来的。 也不能在cycle里面用while(1)，就是不能让程序一直在queue里面执行，要想让cycle执行，那么就要在cycle函数的末尾用一个workqueue把这个函数再次放入工作队列当中。 转载于:https://www.c...
PX4-Autopilot.zip
09-22
git clone https://github.com/PX4/PX4-Autopilot.git --recursive 由于外网原因克隆PX4源码不完整（各子项目库下载不齐全）的可下载此PX4源码完整版
px4.zip_pixhawk_px4
09-20
对pixhawk采集的加速度值做傅里叶变换
PX4 旋翼控制四元数仿真模型
12-09
根据px4旋翼姿态控制搭建了simulink仿真模型，是使用四元数搭建的，里面的解释可以参看我的博客https://blog.csdn.net/zhao23333/article/details/110918950，通过改变当前姿态和目标姿态，可以看到是如何通过四元数获得角度差最终得到角速度
px4官方bootloader固件
03-27
本固件基于px4官方bootloader源码编译，未经过任何修改，可以直接使用
PixHawk开发PX4drivers.rar
07-21
PX4官方文档中PX4 USB Drivers，原链接已失效，现在传上来跟大家共享一下。
PX4QG自动飞行航线任务操作流程
08-09
PX4QG自动飞行航线任务操作流程
PX４QGC地面站航线导航任务指令说明
08-09
PX４QGC地面站航线导航任务指令说明 里边包含QGC地面站软件航线航点中每个任务指令的说明 需要的可自行下载 别忘了点点关注
PX4固件源码，亲测可编译
07-25
PX4固件源码，亲测可编译
编译好的PX4 1.11.3固件.zip
01-14
编译好的PX4 1.11.3固件，包括从v2到v5所有版本，可以直接下载到飞控
外边框最少10px怎么编写
03-26
可以使用CSS中的box-sizing属性，将其设为border-box，然后为外边框添加10px的内边距即可。 示例代码： ```css .box { box-sizing: border-box; padding: 10px; border: 1px solid #ccc; } ``` HTML代码： ```html <div class="box"> <!-- 内容 --> </div> ``` 这样，box元素的外边框最少为10px，而不会影响到其它内容的布局。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

大强强小强强

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值