PX4二次开发 创建进程

目录

一、创建进程

二、仿真测试

PX4官方手册：Module Template for Full Applications | PX4 User Guide

编写参照PX4源码 src/templates：PX4-Autopilot/src/templates/template_module at main · PX4/PX4-Autopilot · GitHub

一、创建进程

创建自己的进程，仿照PX4源码，在 src/modules/文件夹下，创建一个新的文件夹，名字为进程名。

比如我在这里希望创建一个进程 safe_detector ，用于实时检测无人机上搭载的下激光测距传感器的数值，如果>5m，认为高度过高，向地面站发出警告信息。于是在 src/modules/文件夹下，创建safe_detector文件夹。文件夹里新建4个文件，分别是SafeDetector.cpp SafeDetector.hpp CMakeLists.txt和Kconfig。

代码的编写参照src/modules/templates中的文件，把所有的template_module替换为自己的进程名。

PX4最新的固件（v1.13.0）需要加上Kconfig文件

menuconfig MODULES_SAFE_DETECTOR
	bool "safe_detector"
	default n
	---help---
		Enable support for safe_detector

对应功能的实现只需要修改.cpp文件中的Run()函数即可

void SafeDetector::Run()
{
	if (should_exit()) {
		ScheduleClear();
		exit_and_cleanup();
		return;
	}

	// reschedule backup

	perf_begin(_loop_perf);
	perf_count(_loop_interval_perf);

	// Check if parameters have changed
	if (_parameter_update_sub.updated()) {
		// clear update
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);
		updateParams(); // update module parameters (in DEFINE_PARAMETERS)
	}
	
	//获取无人机状态（是否解锁）
	if (_vehicle_status_sub.updated()) {
		vehicle_status_s vehicle_status;

		if (_vehicle_status_sub.copy(&vehicle_status)) {

			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);

			_armed = armed;
		}
	}

	distance_sensor_s distance_sensor;

	//获取激光传感器数值
	for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
		if (_distance_sensor_subs[i].copy(&distance_sensor)) {
			_down = distance_sensor.current_distance;
		}
	}

	//发送警告信息
	if(_armed && (_down > 5.0f)){
		PX4_INFO("高度过高！");
	}

	perf_end(_loop_perf);
}

我们可以借助PX4本身的仿真环境测试编写的代码。为了让进程默认启动，还需要修改两个文件，分别是

1、ROMFS/px4fmu_common/init.d/rc.mc_apps(以多旋翼为对象，mc即multicopter)

2、boards/px4/sitl/default.px4board

二、仿真测试

PX4官方仿真模型介绍：Gazebo Vehicles | PX4 User Guide

选择带光流与下激光测距的模型 ：

make px4_sitl gazebo_iris_opt_flow

在地面站中勾选虚拟遥控，可以手动操控无人机飞行

 拉动油门让无人机升高，可以看到在下激光超过5m后出现警告信息