文章目录
- 前言
- 一、概述
- 二、二次开发基础（自定义工作队列，自定义uorb）
- 三、自定义串口驱动（添加一个毫米波雷达并定高）
- 四、自定义I2C驱动（驱动一个oled显示屏）
- 五、自定义参数
- 六、自定义日志记录
- 七、自定义机型
- 八、自定义mavlink消息和QGC通信
- 九、自定义mavlink消息和MAVROS通信
- 十、OFFBAORD控制接口（状态控制 pwm输出）
- 十一、基于模型开发（替换控制律/基于模型的编队）
前言
交流学习可加文章底部作者微信
本教程的适用对象：
1、零基础或者有较少基础（尚未入门）
2、希望在短时间里（一到两个月）学会对PX4进行二次开发的开源爱好者或工程师
硬件：
pixhawk飞控
超维M系列无人机
软件：
PX4 1.13.3
QGC4.2.4
课程源码：https://gitee.com/Mbot_admin/px4-1.13.3-cwkj
B站视频：
PX4二次开发快速入门（一）：概述
PX4二次开发快速入门2.1：自定义工作队列
一、概述
二、二次开发基础（自定义工作队列，自定义uorb）
自定义工作队列
在modules文件夹下添加work_item文件夹
添加下面四个文件
CMakeLists.txt
px4_add_module(
MODULE modules__work_item
MAIN work_item_test
COMPILE_FLAGS
#-DDEBUG_BUILD # uncomment for PX4_DEBUG output
#-O0 # uncomment when debugging
SRCS
WorkItemTest.cpp
WorkItemTest.hpp
DEPENDS
px4_work_queue
)
Kconfig
menuconfig MODULES_WORK_ITEM
bool "work_item"
default n
---help---
Enable support for work_item
WorkItemTest.cpp
#include "WorkItemTest.hpp"
WorkItemTest::WorkItemTest() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
{
}
WorkItemTest::~WorkItemTest()
{
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
}
bool WorkItemTest::init()
{
ScheduleOnInterval(100000_us);
return true;
}
void WorkItemTest::Run()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
return;
}
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(¶m_update);
updateParams(); // update module parameters (in DEFINE_PARAMETERS)
}
PX4_INFO("test workitem");
perf_end(_loop_perf);
}
int WorkItemTest::task_spawn(int argc, char *argv[])
{
WorkItemTest *instance = new WorkItemTest();
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 WorkItemTest::print_status()
{
perf_print_counter(_loop_perf);
perf_print_counter(_loop_interval_perf);
return 0;
}
int WorkItemTest::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int WorkItemTest::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Example of a simple module running out of a work queue.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("work_item_example", "template");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int work_item_test_main(int argc, char *argv[])
{
return WorkItemTest::main(argc, argv);
}
WorkItemTest.hpp
#pragma once
#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 <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/orb_test.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/vehicle_status.h>
using namespace time_literals;
class WorkItemTest : public ModuleBase<WorkItemTest>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
WorkItemTest();
~WorkItemTest() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
int print_status() override;
private:
void Run() override;
// Publications
uORB::Publication<orb_test_s> _orb_test_pub{ORB_ID(orb_test)};
// Subscriptions
uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)}; // subscription that schedules WorkItemExample when updated
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // regular subscription for additional data
// Performance (perf) counters
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig /**< another parameter */
)
bool _armed{false};
};
修改仿真的编译脚本
添加如下：
CONFIG_MODULES_WORK_ITEM=y
然后执行
make px4_sitl_default gazbo
启动仿真后执行：
work_item_test start
正常的话会打印下面的信息
自定义uorb消息
三、自定义串口驱动（添加一个毫米波雷达并定高）
四、自定义I2C驱动（驱动一个oled显示屏）
五、自定义参数
在第一节的基础上进一步修改
添加params.c
params.c内容如下：
PARAM_DEFINE_FLOAT(PARAM_TEST1, 0.2f);
PARAM_DEFINE_INT32(PARAM_TEST2, 0);
CMakeLists.txt
px4_add_module(
MODULE modules__work_item
MAIN work_item_test
COMPILE_FLAGS
#-DDEBUG_BUILD # uncomment for PX4_DEBUG output
#-O0 # uncomment when debugging
SRCS
WorkItemTest.cpp
WorkItemTest.hpp
DEPENDS
px4_work_queue
)
Kconfig
menuconfig MODULES_WORK_ITEM
bool "work_item"
default n
---help---
Enable support for work_item
WorkItemTest.cpp
#include "WorkItemTest.hpp"
WorkItemTest::WorkItemTest() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
{
}
WorkItemTest::~WorkItemTest()
{
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
}
bool WorkItemTest::init()
{
ScheduleOnInterval(100000_us);
return true;
}
void WorkItemTest::Run()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
return;
}
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(¶m_update);
updateParams(); // update module parameters (in DEFINE_PARAMETERS)
}
double param_test1=_param_test1.get();
int param_test2=_param_test2.get();
PX4_INFO("param_test1=%lf param_test2=%d\n",param_test1,param_test2);
PX4_INFO("test workitem");
perf_end(_loop_perf);
}
int WorkItemTest::task_spawn(int argc, char *argv[])
{
WorkItemTest *instance = new WorkItemTest();
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 WorkItemTest::print_status()
{
perf_print_counter(_loop_perf);
perf_print_counter(_loop_interval_perf);
return 0;
}
int WorkItemTest::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int WorkItemTest::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Example of a simple module running out of a work queue.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("work_item_example", "template");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int work_item_test_main(int argc, char *argv[])
{
return WorkItemTest::main(argc, argv);
}
WorkItemTest.hpp
#pragma once
#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 <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/orb_test.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/vehicle_status.h>
using namespace time_literals;
class WorkItemTest : public ModuleBase<WorkItemTest>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
WorkItemTest();
~WorkItemTest() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
int print_status() override;
private:
void Run() override;
// Publications
uORB::Publication<orb_test_s> _orb_test_pub{ORB_ID(orb_test)};
// Subscriptions
uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)}; // subscription that schedules WorkItemExample when updated
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // regular subscription for additional data
// Performance (perf) counters
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig, /**< another parameter */
(ParamFloat<px4::params::PARAM_TEST1>) _param_test1, /**< example parameter */
(ParamInt<px4::params::PARAM_TEST2>) _param_test2 /**< another parameter */
)
bool _armed{false};
};
修改仿真的编译脚本
添加如下：
CONFIG_MODULES_WORK_ITEM=y
然后先执行（不执行这一步后面会报错）
make distclean
再执行
make parameters_metadata
最后执行
make px4_sitl_default gazebo
启动仿真后执行（注意要启动work_item_test ，不然地面站搜不到参数）：
work_item_test start
如果在启动work_item_test 之前已经打开了地面站，还需要再刷新一下参数
就可以搜到添加的参数
六、自定义日志记录
首先把需要记录的数据定义成uorb消息
我这里添加了log_test.msg
内容如下：
uint64 timestamp # time since system start (microseconds)
uint32 seq
然后在CMakeLists.txt中添加文件名
然后编译即可生成自定义uorb消息头文件
然后在之前添加的工作队列里发布这个自定义的消息
修改：WorkItemTest.cpp
#include "WorkItemTest.hpp"
WorkItemTest::WorkItemTest() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
{
}
WorkItemTest::~WorkItemTest()
{
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
}
bool WorkItemTest::init()
{
ScheduleOnInterval(100000_us);
return true;
}
void WorkItemTest::Run()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
return;
}
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(¶m_update);
updateParams(); // update module parameters (in DEFINE_PARAMETERS)
}
double param_test1=_param_test1.get();
int param_test2=_param_test2.get();
PX4_INFO("param_test1=%lf param_test2=%d\n",param_test1,param_test2);
PX4_INFO("test workitem");
_log_test.timestamp=hrt_absolute_time();
_log_test.seq++;
PX4_INFO("_log_test.seq=%d\n",_log_test.seq);
_log_test_pub.publish(_log_test);
perf_end(_loop_perf);
}
int WorkItemTest::task_spawn(int argc, char *argv[])
{
WorkItemTest *instance = new WorkItemTest();
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 WorkItemTest::print_status()
{
perf_print_counter(_loop_perf);
perf_print_counter(_loop_interval_perf);
return 0;
}
int WorkItemTest::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int WorkItemTest::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Example of a simple module running out of a work queue.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("work_item_example", "template");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int work_item_test_main(int argc, char *argv[])
{
return WorkItemTest::main(argc, argv);
}
修改：WorkItemTest.hpp
#pragma once
#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 <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/orb_test.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/log_test.h>
using namespace time_literals;
class WorkItemTest : public ModuleBase<WorkItemTest>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
WorkItemTest();
~WorkItemTest() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
int print_status() override;
private:
void Run() override;
// Publications
uORB::Publication<orb_test_s> _orb_test_pub{ORB_ID(orb_test)};
uORB::Publication<log_test_s> _log_test_pub{ORB_ID(log_test)};
// Subscriptions
uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)}; // subscription that schedules WorkItemExample when updated
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // regular subscription for additional data
// Performance (perf) counters
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig, /**< another parameter */
(ParamFloat<px4::params::PARAM_TEST1>) _param_test1, /**< example parameter */
(ParamInt<px4::params::PARAM_TEST2>) _param_test2 /**< another parameter */
)
bool _armed{false};
log_test_s _log_test{};
};
然后在logged_topics.cpp中下面的位置添加：
add_topic("log_test");
然后编译启动仿真：
make px4_sitl_default gazebo
启动仿真后执行（注意要启动work_item_test ，不然的uorb话题没有更新，日志不会记录）：
work_item_test start
终端打印如下：
然后下载飞控的日志，用plotjuggler打开
可以看到添加进去的日志数据，值也是随之时间递增的
七、自定义机型
一、定义机型文件
PX4的机型文件存放在Firmware/ROMFS/px4fmu_common/init.d/airframes目录下，每个机型文件开头的数字代表是该机型的ID，即参数SYS_AUTOSTART，设置不同的SYS_AUTOSTART参数即可设置不同的机型，每个机型的SYS_AUTOSTART都不一样，本文自定义了一个机型文件80003_testframs，该机型文件对应的SYS_AUTOSTART即是80003。
添加完机型文件后，还需要修改该目录下的CMakeLists.txt文件，
在文件的最后添加上自定义机型的名字
# [80000, 81000] (Unmanned) TEST
80003_testframs
本例中定义的机型文件如下:注意name、type、class要自己定义，其他的部分我是参考的其他的机型写的。
#!/bin/sh
#
# @name Test
#
# @url https://www.baidu.com
#
# @type Test
# @class Test
#
# @output MAIN0 Speed of left wheels
# @output MAIN1 Speed of right wheels
#
# @maintainer Xujun
#
# @board px4_fmu-v2 exclude
# @board bitcraze_crazyflie exclude
#
. ${R}etc/init.d/rc.rover_defaults
param set-default BAT1_N_CELLS 4
param set-default EKF2_GBIAS_INIT 0.01
param set-default EKF2_ANGERR_INIT 0.01
param set-default EKF2_MAG_TYPE 1
param set-default FW_AIRSPD_MIN 0
param set-default FW_AIRSPD_TRIM 1
param set-default FW_AIRSPD_MAX 3
param set-default GND_SP_CTRL_MODE 1
param set-default GND_L1_DIST 5
param set-default GND_L1_PERIOD 3
param set-default GND_THR_CRUISE 0.7
param set-default GND_THR_MAX 0.5
# Because this is differential drive, it can make a turn with radius 0.
# This corresponds to a turn angle of pi radians.
# If a special case is made for differential-drive, this will need to change.
param set-default GND_MAX_ANG 3.142
param set-default GND_WHEEL_BASE 0.3
# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
# to support negative throttle.
param set-default GND_THR_MIN 0
param set-default GND_SPEED_P 0.25
param set-default GND_SPEED_I 3
param set-default GND_SPEED_D 0.001
param set-default GND_SPEED_IMAX 0.125
param set-default GND_SPEED_THR_SC 1
param set-default MIS_LTRMIN_ALT 0.01
param set-default MIS_TAKEOFF_ALT 0.01
param set-default NAV_ACC_RAD 0.5
# Provide ESC a constant 1500 us pulse, which corresponds to
# idle on the Roboclaw motor controller on the Aion R1
param set-default PWM_MAIN_DISARM 1500
param set-default PWM_MAIN_DIS0 1500
param set-default PWM_MAIN_DIS1 1500
param set-default PWM_MAIN_MAX 2000
param set-default PWM_MAIN_MIN 1000
# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
param set-default CBRK_AIRSPD_CHK 162128
# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
param set-default GND_MAX_ANG 3.1415
param set-default RBCLW_BAUD 8
param set-default RBCLW_COUNTS_REV 1200
param set-default RBCLW_ADDRESS 128
# 104 corresponds to Telem 4
param set-default RBCLW_SER_CFG 104
# Start this driver after setting parameters, because the driver uses some of those parameters.
# roboclaw start /dev/ttyS3
# Set mixer
set MIXER generic_diff_rover
set PWM_MAIN_REV2 1
type对应地面站上的机架类型，name对应地面站上的载具。
二、修改srcparser.py
修改下图的srcparser.py文件
在下图位置加入一行：
elif (self.name == "Test"):
return "Test"
注意这里的elif和return的缩进，elif不能超过上面的elif，return不能超过最下面的那个return，例如：
否则后面make airframe_metadata编译会报下面的错：
File “/home/amov/6/PX4-Autopilot/Tools/px4airframes/srcparser.py”,
line 103
elif (self.name == “Test”):
^ IndentationError: unindent does not match any outer indentation level FAILED:
CMakeFiles/metadata_airframes
修改后的文件如下：
import sys
import re
import os
class ParameterGroup(object):
"""
Single parameter group
"""
def __init__(self, name, af_class):
self.name = name
self.af_class = af_class
self.params = []
def AddParameter(self, param):
"""
Add parameter to the group
"""
self.params.append(param)
def GetName(self):
"""
Get parameter group name
"""
return self.name
def GetClass(self):
"""
Get parameter group vehicle type.
"""
return self.af_class
def GetImageName(self):
"""
Get parameter group image base name (w/o extension)
"""
if (self.name == "Standard Plane"):
return "Plane"
elif (self.name == "Flying Wing"):
return "FlyingWing"
elif (self.name == "Quadrotor x"):
return "QuadRotorX"
elif (self.name == "Quadrotor +"):
return "QuadRotorPlus"
elif (self.name == "Hexarotor x"):
return "HexaRotorX"
elif (self.name == "Hexarotor +"):
return "HexaRotorPlus"
elif (self.name == "Octorotor +"):
return "OctoRotorPlus"
elif (self.name == "Octorotor x"):
return "OctoRotorX"
elif (self.name == "Octorotor Coaxial"):
return "OctoRotorXCoaxial"
elif (self.name == "Octo Coax Wide"):
return "OctoRotorXCoaxial"
elif (self.name == "Quadrotor Wide"):
return "QuadRotorWide"
elif (self.name == "Quadrotor H"):
return "QuadRotorH"
elif (self.name == "Dodecarotor cox"):
return "DodecaRotorXCoaxial"
elif (self.name == "Simulation"):
return "AirframeSimulation"
elif (self.name == "Plane A-Tail"):
return "PlaneATail"
elif (self.name == "Plane V-Tail"):
return "PlaneVTail"
elif (self.name == "VTOL Duo Tailsitter"):
return "VTOLDuoRotorTailSitter"
elif (self.name == "Standard VTOL"):
return "VTOLPlane"
elif (self.name == "VTOL Quad Tailsitter"):
return "VTOLQuadRotorTailSitter"
elif (self.name == "VTOL Tiltrotor"):
return "VTOLTiltRotor"
elif (self.name == "VTOL Octoplane"):
return "VTOLPlaneOcto"
elif (self.name == "Coaxial Helicopter"):
return "HelicopterCoaxial"
elif (self.name == "Helicopter"):
return "Helicopter"
elif (self.name == "Hexarotor Coaxial"):
return "Y6B"
elif (self.name == "Y6A"):
return "Y6A"
elif (self.name == "Tricopter Y-"):
return "YMinus"
elif (self.name == "Tricopter Y+"):
return "YPlus"
elif (self.name == "Autogyro"):
return "Autogyro"
elif (self.name == "Airship"):
return "Airship"
elif (self.name == "Rover"):
return "Rover"
elif (self.name == "Boat"):
return "Boat"
elif (self.name == "Balloon"):
return "Balloon"
elif (self.name == "Vectored 6 DOF UUV"):
return "Vectored6DofUUV"
elif (self.name == "Test"):
return "Test"
return "AirframeUnknown"
def GetParams(self):
"""
Returns the parsed list of parameters. Every parameter is a Parameter
object. Note that returned object is not a copy. Modifications affect
state of the parser.
"""
return sorted(self.params, key=lambda x: x.GetId())
class Parameter(object):
"""
Single parameter
"""
# Define sorting order of the fields
priority = {
"board": 9,
"short_desc": 8,
"long_desc": 7,
"min": 5,
"max": 4,
"unit": 3,
"AUX1": -10,
"AUX2": -10,
"AUX3": -10,
"AUX4": -10,
"AUX5": -10,
"AUX6": -10,
"AUX7": -10,
"AUX8": -10,
# all others == 0 (sorted alphabetically)
}
def __init__(self, path, post_path, name, airframe_type, airframe_class, airframe_id, maintainer):
self.fields = {}
self.outputs = {}
self.archs = {}
self.path = path
self.post_path = post_path
self.name = name
self.type = airframe_type
self.af_class = airframe_class
self.id = airframe_id
self.maintainer = maintainer
def GetPath(self):
"""
Get path to airframe startup script
"""
return self.path
def GetPostPath(self):
"""
Get path to airframe post startup script
"""
return self.post_path
def GetName(self):
"""
Get airframe name
"""
return self.name
def GetType(self):
"""
Get airframe type
"""
return self.type
def GetClass(self):
"""
Get airframe class
"""
return self.af_class
def GetId(self):
"""
Get airframe id
"""
return self.id
def GetMaintainer(self):
"""
Get airframe maintainer
"""
return self.maintainer
def SetField(self, code, value):
"""
Set named field value
"""
self.fields[code] = value
def SetOutput(self, code, value):
"""
Set named output value
"""
self.outputs[code] = value
def SetArch(self, code, value):
"""
Set named arch value
"""
self.archs[code] = value
def GetFieldCodes(self):
"""
Return list of existing field codes in convenient order
"""
keys = self.fields.keys()
keys = sorted(keys)
keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
return keys
def GetFieldValue(self, code):
"""
Return value of the given field code or None if not found.
"""
fv = self.fields.get(code)
if not fv:
# required because python 3 sorted does not accept None
return ""
return self.fields.get(code)
def GetOutputCodes(self):
"""
Return list of existing output codes in convenient order
"""
keys = self.outputs.keys()
keys = sorted(keys)
keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
return keys
def GetOutputValue(self, code):
"""
Return value of the given output code or None if not found.
"""
fv = self.outputs.get(code)
if not fv:
# required because python 3 sorted does not accept None
return ""
return self.outputs.get(code)
def GetArchCodes(self):
"""
Return list of existing arch codes in convenient order
"""
keys = self.archs.keys()
keys = sorted(keys)
keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
return keys
def GetArchValue(self, code):
"""
Return value of the given arch code or None if not found.
"""
fv = self.archs.get(code)
if not fv:
# required because python 3 sorted does not accept None
return ""
return self.archs.get(code)
class SourceParser(object):
"""
Parses provided data and stores all found parameters internally.
"""
re_split_lines = re.compile(r'[\r\n]+')
re_comment_start = re.compile(r'^\#\s')
re_comment_content = re.compile(r'^\#\s*(.*)')
re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
re_comment_end = re.compile(r'(.*?)\s*\#\n/')
re_cut_type_specifier = re.compile(r'[a-z]+$')
re_is_a_number = re.compile(r'^-?[0-9\.]')
re_remove_dots = re.compile(r'\.+$')
re_remove_carriage_return = re.compile('\n+')
valid_tags = set(["url", "maintainer", "output", "arch", "name", "type", "desc"])
# Order of parameter groups
priority = {
# All other groups = 0 (sort alphabetically)
"Miscellaneous": -10
}
def __init__(self):
self.param_groups = {}
def GetSupportedExtensions(self):
"""
Returns list of supported file extensions that can be parsed by this
parser. The parser uses any extension.
"""
return ["", ".hil"]
def Parse(self, path, contents):
"""
Incrementally parse program contents and append all found airframes
to the list.
"""
airframe_id = None
airframe_id = os.path.split(path)[1].split('_',1)[0]
# Skip if not numeric
if (not self.IsNumber(airframe_id)):
return True
# This code is essentially a comment-parsing grammar. "state"
# represents parser state. It contains human-readable state
# names.
state = None
tags = {}
outputs = {}
archs = {}
for line in self.re_split_lines.split(contents):
line = line.strip()
# Ignore empty lines
if line == "":
continue
if state is None and self.re_comment_start.match(line):
state = "wait-short"
short_desc = None
long_desc = None
if state is not None and state != "comment-processed":
m = self.re_comment_end.search(line)
if m:
line = m.group(1)
last_comment_line = True
else:
last_comment_line = False
m = self.re_comment_content.match(line)
if m:
comment_content = m.group(1)
if comment_content == "":
# When short comment ends with empty comment line,
# start waiting for the next part - long comment.
if state == "wait-short-end":
state = "wait-long"
else:
m = self.re_comment_tag.match(comment_content)
if m:
tag, desc = m.group(1, 2)
if (tag == "output"):
key, text = desc.split(' ', 1)
outputs[key] = text;
elif (tag == "board"):
key, text = desc.split(' ', 1)
archs[key] = text;
else:
tags[tag] = desc
current_tag = tag
state = "wait-tag-end"
elif state == "wait-short":
# Store first line of the short description
short_desc = comment_content
state = "wait-short-end"
elif state == "wait-short-end":
# Append comment line to the short description
short_desc += "\n" + comment_content
elif state == "wait-long":
# Store first line of the long description
long_desc = comment_content
state = "wait-long-end"
elif state == "wait-long-end":
# Append comment line to the long description
long_desc += "\n" + comment_content
elif state == "wait-tag-end":
# Append comment line to the tag text
tags[current_tag] += "\n" + comment_content
else:
raise AssertionError(
"Invalid parser state: %s" % state)
elif not last_comment_line:
# Invalid comment line (inside comment, but not starting with
# "*" or "*/". Reset parsed content.
state = None
if last_comment_line:
state = "comment-processed"
else:
state = None
# Process parsed content
airframe_type = None
maintainer = "John Doe <john@example.com>"
airframe_name = None
airframe_class = None
# Done with file, store
for tag in tags:
if tag == "maintainer":
maintainer = tags[tag]
elif tag == "type":
airframe_type = tags[tag]
elif tag == "class":
airframe_class = tags[tag]
elif tag == "name":
airframe_name = tags[tag]
elif tag == "desc":
# General documentation - not a parameter to be saved.
pass
elif tag not in self.valid_tags:
sys.stderr.write("Aborting due to invalid documentation tag: '%s'\n" % tag)
return False
# Sanity check
if airframe_type == None:
sys.stderr.write("Aborting due to missing @type tag in file: '%s'\n" % path)
return False
if airframe_class == None:
sys.stderr.write("Aborting due to missing @class tag in file: '%s'\n" % path)
return False
if airframe_name == None:
sys.stderr.write("Aborting due to missing @name tag in file: '%s'\n" % path)
return False
# Check if a .post script exists
if os.path.isfile(path + '.post'):
post_path = path + '.post'
else:
post_path = None
# We already know this is an airframe config, so add it
param = Parameter(path, post_path, airframe_name, airframe_type, airframe_class, airframe_id, maintainer)
# Done with file, store
for tag in tags:
if tag == "maintainer":
maintainer = tags[tag]
if tag == "type":
airframe_type = tags[tag]
if tag == "class":
airframe_class = tags[tag]
if tag == "name":
airframe_name = tags[tag]
else:
param.SetField(tag, tags[tag])
# Store outputs
for output in outputs:
param.SetOutput(output, outputs[output])
# Store outputs
for arch in archs:
param.SetArch(arch, archs[arch])
# Store the parameter
# Create a class-specific airframe group. This is needed to catch cases where an airframe type might cross classes (e.g. simulation)
class_group_identifier=airframe_type+airframe_class
if class_group_identifier not in self.param_groups:
#self.param_groups[airframe_type] = ParameterGroup(airframe_type) #HW TEST REMOVE
self.param_groups[class_group_identifier] = ParameterGroup(airframe_type, airframe_class)
self.param_groups[class_group_identifier].AddParameter(param)
return True
def IsNumber(self, numberString):
try:
float(numberString)
return True
except ValueError:
return False
def Validate(self):
"""
Validates the airframe meta data.
"""
seenParamNames = []
for group in self.GetParamGroups():
for param in group.GetParams():
name = param.GetName()
board = param.GetFieldValue("board")
# Check for duplicates
name_plus_board = name + "+" + board
for seenParamName in seenParamNames:
if seenParamName == name_plus_board:
sys.stderr.write("Duplicate parameter definition: {0}\n".format(name_plus_board))
return False
seenParamNames.append(name_plus_board)
return True
def GetParamGroups(self):
"""
Returns the parsed list of parameters. Every parameter is a Parameter
object. Note that returned object is not a copy. Modifications affect
state of the parser.
"""
groups = self.param_groups.values()
groups = sorted(groups, key=lambda x: x.GetName())
groups = sorted(groups, key=lambda x: x.GetClass())
groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)
#Rename duplicate groups to include the class (creating unique headings in page TOC)
duplicate_test=set()
duplicate_set=set()
for group in groups:
if group.GetName() in duplicate_test:
duplicate_set.add(group.GetName())
else:
duplicate_test.add(group.GetName() )
for group in groups:
if group.GetName() in duplicate_set:
group.name=group.GetName()+' (%s)' % group.GetClass()
return groups
三、编译并下载固件
先在Firmware路径下执行
make distclean
然后执行
make airframe_metadata
然后会在下面的目录生成airframes.xml文件
可以在里面找到生成了之前定义的80003这个机型
然后执行下面命令编译固件
make px4_fmu-v5_default
注意在编译固件时不要使用upload上传代码，必须用地面站进行上传
打开下图这个页面，然后插上飞控
选择自定义固件
选择刚才编译生成的.px4固件，双击上传即可
四、修改QGC
将第三步生成的airframes.xml中所有内容复制到QGC的AirframeFactMetaData.xml文件中，替换掉原有的内容
将自定义机型的svg图标放在qgroundcontrol/src/AutoPilotPlugins/PX4/Images目录下，命名为Test.svg：
在 qgcimages.qrc中点击添加-》添加文件，然后选择上一步的Test.svg，并将别名命名为Airframe/+下图image后面的名字，我这里是Test，所以命名为Airframe/Test
如下：
然后重新编译地面站
然后将飞控连到新编译的地面站上，就可以在机架那一栏设置新添加的机型
设置之后如下：
机型对应的SYS_AUTOSTART参数为80003
八、自定义mavlink消息和QGC通信
一、PX4生成自定义mavlink消息文件
PX4 1.13.0以及更新版本的固件生成自定义mavlink消息文件的方法老版本固件不同，老版本固件生成自定义MAVLINk消息可以参考：
https://mbot1.blog.csdn.net/article/details/108802979
PX4 1.13.0以前版本的固件生成自定义mavlink消息文件的方法可以参考本文。
修改下图路径中的common.xml文件
在文件的下图位置加上
<message id="12921" name="TEST_MAVLINK">
<description>Test Mavlink.</description>
<field type="uint8_t" name="test1"> </field>
<field type="int16_t" name="test2"> </field>
<field type="float" name="test3"> </field>
</message>
注意id不能与其他的消息相同，我这里取的是12921。
然后编译
先
make clean
再
make cuav_x7pro_default
编译完成后会在下图的路径中生成mavlink_msg_test_mavlink.h
二、PX4创建自定义进程和uorb消息
假设PX4源码的路径为~/PX4-Autopilot
2.1创建uorb消息
定义两个消息
gedit ~/PX4-Autopilot/msg/test_mavlink_rx.msg
gedit ~/PX4-Autopilot/msg/test_mavlink_tx.msg
都填入下面的内容
uint64 timestamp # time since system start (microseconds)
uint8 test1
int16 test2
float32 test3
修改CMakeLists.txt
gedit ~/PX4-Autopilot/msg/CMakeLists.txt
加入两行
test_mavlink_tx.msg
test_mavlink_rx.msg
如下图
然后编译固件生成消息头文件
2.2创建自定义进程
在PX4-Autopilot/src/modules下创建test_mavlink文件夹，
在该文件夹中创建CMakeLists.txt，Kconfig，test_mavlink.cpp，test_mavlink.h四个文件
编辑CMakeLists，填入以下内容后保存退出
px4_add_module(
MODULE modules__test_mavlink
MAIN test_mavlink
COMPILE_FLAGS
#-DDEBUG_BUILD # uncomment for PX4_DEBUG output
#-O0 # uncomment when debugging
SRCS
test_mavlink.cpp
DEPENDS
px4_work_queue
)
编辑Kconfig文件
menuconfig MODULES_TEST_MAVLINK
bool "test_mavlink"
default n
---help---
Enable support for test_mavlink
编辑test_mavlink.cpp文件
填写内容如下：
#include "test_mavlink.h"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
int test_mavlink::print_status()
{
PX4_INFO("Running");
// TODO: print additional runtime information about the state of the module
return 0;
}
int test_mavlink::custom_command(int argc, char *argv[])
{
/*
if (!is_running()) {
print_usage("not running");
return 1;
}
// additional custom commands can be handled like this:
if (!strcmp(argv[0], "do-something")) {
get_instance()->do_something();
return 0;
}
*/
return print_usage("unknown command");
}
int test_mavlink::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("module",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1024,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0)
{
_task_id = -1;
return -errno;
}
return 0;
}
test_mavlink *test_mavlink::instantiate(int argc, char *argv[])
{
int example_param = 0;
bool example_flag = false;
bool error_flag = false;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
// parse CLI arguments
while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF)
{
switch (ch)
{
case 'p':
example_param = (int)strtol(myoptarg, nullptr, 10);
break;
case 'f':
example_flag = true;
break;
case '?':
error_flag = true;
break;
default:
PX4_WARN("unrecognized flag");
error_flag = true;
break;
}
}
if (error_flag)
{
return nullptr;
}
test_mavlink *instance = new test_mavlink(example_param, example_flag);
if (instance == nullptr)
{
PX4_ERR("alloc failed");
}
return instance;
}
test_mavlink::test_mavlink(int example_param, bool example_flag)
: ModuleParams(nullptr)
{
}
void test_mavlink::run()
{
// initialize parameters
parameters_update(true);
while (!should_exit())
{
// PX4_INFO("run\n");
if(test_mavlink_rx_sub.updated())
{
PX4_INFO("updated\n");
test_mavlink_rx_sub.copy(&_test_mavlink_rx);
// PX4_INFO("_test_mavlink_rx.test1=%c\n",_test_mavlink_rx.test1);
// PX4_INFO("_test_mavlink_rx.test2=%d\n",_test_mavlink_rx.test2);
// PX4_INFO("_test_mavlink_rx.test3=%f\n",(double)_test_mavlink_rx.test3);
_test_mavlink_tx.test1=_test_mavlink_rx.test1+1;
_test_mavlink_tx.test2=_test_mavlink_rx.test2+1;
_test_mavlink_tx.test3=_test_mavlink_rx.test3+1;
test_mavlink_tx_pub.publish(_test_mavlink_tx);
}
PX4_INFO("_test_mavlink_rx.test1=%d\n",_test_mavlink_rx.test1);
PX4_INFO("_test_mavlink_rx.test2=%d\n",_test_mavlink_rx.test2);
PX4_INFO("_test_mavlink_rx.test3=%f\n",(double)_test_mavlink_rx.test3);
px4_usleep(50000);
parameters_update();
}
}
void test_mavlink::parameters_update(bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force)
{
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
// update parameters from storage
updateParams();
}
}
int test_mavlink::print_usage(const char *reason)
{
if (reason)
{
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Section that describes the provided module functionality.
This is a template for a module running as a task in the background with start/stop/status functionality.
### Implementation
Section describing the high-level implementation of this module.
### Examples
CLI usage example:
$ module start -f -p 42
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("module", "template");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int test_mavlink_main(int argc, char *argv[])
{
return test_mavlink::main(argc, argv);
}
编辑test_mavlink.h文件
填写内容如下：
#pragma once
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/test_mavlink_tx.h>
#include <uORB/topics/test_mavlink_rx.h>
using namespace time_literals;
extern "C" __EXPORT int test_mavlink_main(int argc, char *argv[]);
class test_mavlink : public ModuleBase<test_mavlink>, public ModuleParams
{
public:
test_mavlink(int example_param, bool example_flag);
virtual ~test_mavlink() = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static test_mavlink *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
private:
/**
* Check for parameter changes and update them if needed.
* @param parameter_update_sub uorb subscription to parameter_update
* @param force for a parameter update
*/
void parameters_update(bool force = false);
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig /**< another parameter */
)
test_mavlink_tx_s _test_mavlink_tx;
test_mavlink_rx_s _test_mavlink_rx;
// Subscriptions
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription test_mavlink_rx_sub{ORB_ID(test_mavlink_rx)};
uORB::Publication<test_mavlink_tx_s> test_mavlink_tx_pub{ORB_ID(test_mavlink_tx)};
};
编辑下图编译脚本
加入一行
CONFIG_MODULES_TEST_MAVLINK=y
如下：
三、PX4接收QGC消息
修改mavlink_receiver.h
添加自定义消息头文件：
#include <uORB/topics/test_mavlink_rx.h>
添加自定义消息处理函数
void handle_message_test_mavlink_rx(mavlink_message_t *msg);
添加话题发布
uORB::Publication<test_mavlink_rx_s> _test_mavlink_rx_pub{ORB_ID(test_mavlink_rx)};
修改mavlink_receiver.cpp
调用消息处理函数：
case MAVLINK_MSG_ID_TEST_MAVLINK:
handle_message_test_mavlink_rx(msg);
break;
实现消息处理函数，主要是解析mavlink消息并通过uorb发布：
void
MavlinkReceiver::handle_message_test_mavlink_rx(mavlink_message_t *msg)
{
mavlink_test_mavlink_t mavlink_test_msg;
mavlink_msg_test_mavlink_decode(msg, &mavlink_test_msg);
test_mavlink_rx_s __test_mavlink_rx;
__test_mavlink_rx.test1=mavlink_test_msg.test1;
__test_mavlink_rx.test2=mavlink_test_msg.test2;
__test_mavlink_rx.test3=mavlink_test_msg.test3;
_test_mavlink_rx_pub.publish(__test_mavlink_rx);
}
四、PX4发送消息给QGC
在src/modules/mavlink/streams下新建MAVLINK_TEST.hpp
在MAVLINK_TEST.hpp中填入下面内容，主要是订阅test_mavlink_tx消息然后将消息内容打包为MAVLINK数据帧并发送：
#ifndef MAVLINK_TEST_HPP
#define MAVLINK_TEST_HPP
#include <uORB/topics/test_mavlink_tx.h>
class MavlinkStreamMavlinktest : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamMavlinktest(mavlink); }
static constexpr const char *get_name_static() { return "TEST_MAVLINK"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TEST_MAVLINK; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return test_mavlink_tx_sub.advertised() ? MAVLINK_MSG_ID_TEST_MAVLINK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamMavlinktest(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription test_mavlink_tx_sub{ORB_ID(test_mavlink_tx)};
bool send() override
{
test_mavlink_tx_s test_mavlink_tx;
if (test_mavlink_tx_sub.update(&test_mavlink_tx)) {
mavlink_test_mavlink_t msg{};
msg.test1=test_mavlink_tx.test1;
msg.test2=test_mavlink_tx.test2;
msg.test3=test_mavlink_tx.test3;
mavlink_msg_test_mavlink_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif // MOUNT_ORIENTATION
修改mavlink_messages.cpp
添加hpp头文件
#include "streams/MAVLINK_TEST.hpp"
添加接口
#if defined(MAVLINK_TEST_HPP)
create_stream_list_item<MavlinkStreamMavlinktest>(),
#endif // MAVLINK_TEST_HPP
修改mavlink_main.cpp
配置MAVLINK发送速率，注意这里要加在switch (_mode)上面
configure_stream_local("TEST_MAVLINK", unlimited_rate);
注意双引号里面的是消息的名称，在前面的MAVLINK_TEST.hpp中定义：
五、在QGC中添加自定义mavlink消息
QGC中的MAVLINK库和PX4中的不一样，使用的方言也不一样，因此两者的库不能直接复制粘贴用，在QGC中也需要进行定义
安装MAVLINK生成器mavgenerate
sudo apt-get install python3-pip
pip3 install --user future
sudo apt-get install python3-tk
git clone https://github.com/mavlink/mavlink.git --recursive
下载后进入到工程文件夹
进入这个文件夹路径下执行
python3 -m mavgenerate
会出现下面这个窗口
修改XML文件并生成MAVLINK库文件
QGC采用的是ardupilotmega方言
修改QGC下图目录的ardupilotmega.xml
在最后加上
<message id="12921" name="TEST_MAVLINK">
<description>Test Mavlink.</description>
<field type="uint8_t" name="test1"> </field>
<field type="int16_t" name="test2"> </field>
<field type="float" name="test3"> </field>
</message>
打开上一节安装的mavgenerate
XML选择修改后的ardupilotmega.xml，Out选择库文件的输出目录，Language选择C，Protocol选择2.0。最后点Generate
然后会在指定的输出目录下生成消息库文件
将生成的这些文件全部复制并覆盖到qgroundcontrol/libs/mavlink/include/mavlink/v2.0目录下
六、QGC消息处理
设计界面
参考src/AnalyzeView/MavlinkConsolePage.qml 创建src/AnalyzeView/MavlinkTestPage.qml 。
MavlinkTestPage.qml内容如下：
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtQuick.Dialogs 1.2
import QtQuick.Layouts 1.2
import QGroundControl 1.0
import QGroundControl.Palette 1.0
import QGroundControl.FactSystem 1.0
import QGroundControl.FactControls 1.0
import QGroundControl.Controls 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Controllers 1.0
AnalyzePage {
id: mavlinkConsolePage
pageComponent: pageComponent
pageName: qsTr("Mavlink Test")
Mavlinktest {
id: test_mavlink
}
TextEdit {
id: textEdit
x: 285
y: 189
width: 81
height: 25
color: "#f5f3f3"
text: qsTr("TEST1")
font.family: "Times New Roman"
font.pixelSize: 12
}
Button {
id: button
x: 372
y: 189
text: qsTr("发送")
onClicked: {
test_mavlink._sendcom(textEdit.text,textEdit1.text,textEdit2.text)
}
}
TextEdit {
id: textEdit1
x: 285
y: 220
width: 81
height: 25
color: "#f5f3f3"
text: qsTr("TEST2")
font.pixelSize: 12
}
TextEdit {
id: textEdit2
x: 285
y: 251
width: 81
height: 25
color: "#f5f3f3"
text: qsTr("TEST3")
font.pixelSize: 12
}
Label {
id: label
x: 285
y: 317
width: 30
height: 19
text: qsTr("接收")
}
TextEdit {
id: textEdit3
x: 286
y: 354
width: 80
height: 19
color: "#f5f3f3"
text: test_mavlink.test1
font.pixelSize: 12
}
TextEdit {
id: textEdit4
x: 286
y: 391
width: 80
height: 19
color: "#f5f3f3"
text: test_mavlink.test2
font.pixelSize: 12
}
TextEdit {
id: textEdit5
x: 286
y: 424
width: 80
height: 19
color: "#f5f3f3"
text:test_mavlink.test3
font.pixelSize: 12
}
// Component
} // AnalyzePage
在qgroundcontrol.qrc中将上面创建的QML资源添加进去，取别名叫MavlinkTestPage.qml。注意添加完后一定不要忘了把下面别名那个输入框填上别名，注意别名不要写错，不要忘了加上后缀.qml。否则如果后面QGCCorePlugin.cc引用的时候和别名不一样，会导致MavlinkTestPage页面里是空的，不是MavlinkTestPage.qml设计的页面
然后修改QGCCorePlugin.cc文件
添加一行
_p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Mavlinktest"), QUrl::fromUserInput("qrc:/qml/MavlinkTestPage.qml"), QUrl::fromUserInput("qrc:/qmlimages/MavlinkConsoleIcon"))));
消息处理
参考src/AnalyzeView/MavlinkConsoleController.h
创建src/AnalyzeView/Mavlinktest.h
参考src/AnalyzeView/MavlinkConsoleController.cc
创建src/AnalyzeView/Mavlinktest.cpp
Mavlinktest.cpp内容如下：
#include "Mavlinktest.h"
#include "QGCApplication.h"
#include "UAS.h"
#include "MAVLinkInspectorController.h"
#include "MultiVehicleManager.h"
#include <QtCharts/QLineSeries>
Mavlinktest::Mavlinktest()
: QStringListModel(),
_cursor_home_pos{-1},
_cursor{0},
_vehicle{nullptr}
{
auto *manager = qgcApp()->toolbox()->multiVehicleManager();
connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &Mavlinktest::_setActiveVehicle);
_setActiveVehicle(manager->activeVehicle());
MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &Mavlinktest::_receiveMessage);
}
Mavlinktest::~Mavlinktest()
{
if (_vehicle)
{
QByteArray msg;
_sendSerialData(msg, true);
}
}
void
Mavlinktest::sendCommand(QString command)
{
_history.append(command);
command.append("\n");
_sendSerialData(qPrintable(command));
_cursor_home_pos = -1;
_cursor = rowCount();
}
QString
Mavlinktest::historyUp(const QString& current)
{
return _history.up(current);
}
QString
Mavlinktest::historyDown(const QString& current)
{
return _history.down(current);
}
void
Mavlinktest::_setActiveVehicle(Vehicle* vehicle)
{
for (auto &con : _uas_connections)
{
disconnect(con);
}
_uas_connections.clear();
_vehicle = vehicle;
if (_vehicle)
{
_incoming_buffer.clear();
// Reset the model
setStringList(QStringList());
_cursor = 0;
_cursor_home_pos = -1;
_uas_connections << connect(_vehicle, &Vehicle::mavlinkSerialControl, this, &Mavlinktest::_receiveData);
}
}
void
Mavlinktest::_receiveData(uint8_t device, uint8_t, uint16_t, uint32_t, QByteArray data)
{
if (device != SERIAL_CONTROL_DEV_SHELL)
{
return;
}
// auto idx = index(_cursor);
//setData(idx, QString("%1 ttyS6 -> * [%2]").arg(QTime::currentTime().toString("HH:mm:ss.zzz")).arg(12));
// Append incoming data and parse for ANSI codes
_incoming_buffer.append(data);
while(!_incoming_buffer.isEmpty())
{
bool newline = false;
int idx = _incoming_buffer.indexOf('\n');
if (idx == -1)
{
// Read the whole incoming buffer
idx = _incoming_buffer.size();
}
else
{
newline = true;
}
QByteArray fragment = _incoming_buffer.mid(0, idx);
if (_processANSItext(fragment))
{
writeLine(_cursor, fragment);
if (newline)
{
_cursor++;
}
_incoming_buffer.remove(0, idx + (newline ? 1 : 0));
}
else
{
// ANSI processing failed, need more data
return;
}
}
}
void
Mavlinktest::_receiveMessage(LinkInterface*, mavlink_message_t message)
{
if(message.msgid==MAVLINK_MSG_ID_TEST_MAVLINK)
{
qDebug()<<"receive MAVLINK_MSG_ID_TEST_MAVLINK";
mavlink_test_mavlink_t mavlink_test;
mavlink_msg_test_mavlink_decode(&message, &mavlink_test);
//id=pwm.base_mode;
//_name=QString::number(heartbeat.base_mode);
_test1=QString::number(mavlink_test.test1);
_test2=QString::number(mavlink_test.test2);
_test3=QString::number(mavlink_test.test3);
emit valueChanged();
qDebug()<<_test1<<_test2<<_test3;
// qDebug()<<"从模式"<<heartbeat.custom_mode;
}
}
void Mavlinktest::_sendcom(QString test1,QString test2,QString test3)
{
if (!_vehicle)
{
qWarning() << "Internal error";
return;
}
WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();
if (!weakLink.expired()) {
SharedLinkInterfacePtr sharedLink = weakLink.lock();
if (!sharedLink) {
qCDebug(VehicleLog) << "_handlePing: primary link gone!";
return;
}
// auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
auto priority_link =sharedLink;
uint8_t send_test1=test1.toUInt();
int16_t send_test2=test2.toShort();
float send_test3=test3.toFloat();
mavlink_message_t msg;
mavlink_msg_test_mavlink_pack_chan(_vehicle->id(),
1,
priority_link->mavlinkChannel(),
&msg,
send_test1,
send_test2,
send_test3);
_vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
qDebug()<<"send"<<send_test1<<send_test2<<send_test3;
}
}
void
Mavlinktest::_sendSerialData(QByteArray data, bool close)
{
if (!_vehicle)
{
qWarning() << "Internal error";
return;
}
WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();
if (!weakLink.expired()) {
SharedLinkInterfacePtr sharedLink = weakLink.lock();
if (!sharedLink) {
qCDebug(VehicleLog) << "_handlePing: primary link gone!";
return;
}
// Send maximum sized chunks until the complete buffer is transmitted
// while(data.size())
// {
// QByteArray chunk{data.left(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN)};
// uint8_t flags = SERIAL_CONTROL_FLAG_EXCLUSIVE | SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
// if (close)
// {
// flags = 0;
// }
// auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
// auto priority_link =sharedLink;
// mavlink_message_t msg;
// mavlink_msg_serial_control_pack_chan(
// protocol->getSystemId(),
// protocol->getComponentId(),
// priority_link->mavlinkChannel(),
// &msg,
// SERIAL_CONTROL_DEV_SHELL,
// flags,
// 0,
// 0,
// chunk.size(),
// reinterpret_cast<uint8_t*>(chunk.data()));
// _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
// data.remove(0, chunk.size());
// }
}
}
bool
Mavlinktest::_processANSItext(QByteArray &line)
{
// Iterate over the incoming buffer to parse off known ANSI control codes
for (int i = 0; i < line.size(); i++)
{
if (line.at(i) == '\x1B')
{
// For ANSI codes we expect at least 3 incoming chars
if (i < line.size() - 2 && line.at(i+1) == '[')
{
// Parse ANSI code
switch(line.at(i+2))
{
default:
continue;
case 'H':
if (_cursor_home_pos == -1)
{
// Assign new home position if home is unset
_cursor_home_pos = _cursor;
}
else
{
// Rewind write cursor position to home
_cursor = _cursor_home_pos;
}
break;
case 'K':
// Erase the current line to the end
if (_cursor < rowCount())
{
setData(index(_cursor), "");
}
break;
case '2':
// Check for sufficient buffer size
if ( i >= line.size() - 3)
{
return false;
}
if (line.at(i+3) == 'J' && _cursor_home_pos != -1)
{
// Erase everything and rewind to home
bool blocked = blockSignals(true);
for (int j = _cursor_home_pos; j < rowCount(); j++)
{
setData(index(j), "");
}
blockSignals(blocked);
QVector<int> roles;
roles.reserve(2);
roles.append(Qt::DisplayRole);
roles.append(Qt::EditRole);
emit dataChanged(index(_cursor), index(rowCount()), roles);
}
// Even if we didn't understand this ANSI code, remove the 4th char
line.remove(i+3,1);
break;
}
// Remove the parsed ANSI code and decrement the bufferpos
line.remove(i, 3);
i--;
}
else
{
// We can reasonably expect a control code was fragemented
// Stop parsing here and wait for it to come in
return false;
}
}
}
return true;
}
void
Mavlinktest::writeLine(int line, const QByteArray &text)
{
auto rc = rowCount();
if (line >= rc)
{
insertRows(rc, 1 + line - rc);
}
auto idx = index(line);
setData(idx, data(idx, Qt::DisplayRole).toString() + text);
}
void Mavlinktest::CommandHistory::append(const QString& command)
{
if (command.length() > 0)
{
// do not append duplicates
if (_history.length() == 0 || _history.last() != command)
{
if (_history.length() >= maxHistoryLength)
{
_history.removeFirst();
}
_history.append(command);
}
}
_index = _history.length();
}
QString Mavlinktest::CommandHistory::up(const QString& current)
{
if (_index <= 0)
{
return current;
}
--_index;
if (_index < _history.length())
{
return _history[_index];
}
return "";
}
QString Mavlinktest::CommandHistory::down(const QString& current)
{
if (_index >= _history.length())
{
return current;
}
++_index;
if (_index < _history.length())
{
return _history[_index];
}
return "";
}
Mavlinktest.h内容如下：
#pragma once
#include "MAVLinkProtocol.h"
#include "Vehicle.h"
#include "QmlObjectListModel.h"
#include "Fact.h"
#include "FactMetaData.h"
#include <QObject>
#include <QString>
#include <QMetaObject>
#include <QStringListModel>
// Fordward decls
class Vehicle;
/// Controller for MavlinkConsole.qml.
class Mavlinktest : public QStringListModel
{
Q_OBJECT
public:
Mavlinktest();
virtual ~Mavlinktest();
Q_INVOKABLE void sendCommand(QString command);
Q_INVOKABLE void _sendcom(QString test1,QString test2,QString test3);
Q_INVOKABLE QString historyUp(const QString& current);
Q_INVOKABLE QString historyDown(const QString& current);
Q_PROPERTY(QString test1 READ test1 NOTIFY valueChanged)
QString test1 ()
{
return _test1;
}
Q_PROPERTY(QString test2 READ test2 NOTIFY valueChanged)
QString test2 ()
{
return _test2;
}
Q_PROPERTY(QString test3 READ test3 NOTIFY valueChanged)
QString test3 ()
{
return _test3;
}
private slots:
void _setActiveVehicle (Vehicle* vehicle);
void _receiveData(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);
void _receiveMessage (LinkInterface* link, mavlink_message_t message);
signals:
void valueChanged ();
void selectedChanged ();
private:
bool _processANSItext(QByteArray &line);
void _sendSerialData(QByteArray, bool close = false);
void writeLine(int line, const QByteArray &text);
class CommandHistory
{
public:
void append(const QString& command);
QString up(const QString& current);
QString down(const QString& current);
private:
static constexpr int maxHistoryLength = 100;
QList<QString> _history;
int _index = 0;
};
QString _test1,_test2,_test3;
int _cursor_home_pos;
int _cursor;
QByteArray _incoming_buffer;
Vehicle* _vehicle;
QList<QMetaObject::Connection> _uas_connections;
CommandHistory _history;
};
将.h和.cpp源文件加入工程（如果QT Creator 没有自动添加的话），修改qgroundcontrol.pro
src/AnalyzeView/Mavlinktest.h\
src/AnalyzeView/Mavlinktest.cpp\
分别添加到下图的位置：
在src/QGCApplication.cc注册QML
先添加头文件
#include "Mavlinktest.h"
然后添加
qmlRegisterType<Mavlinktest> (kQGCControllers, 1, 0, "Mavlinktest");
经过上面注册后就可以在ＱＭＬ中用双引号里的类名进行直接构造，通过id来访问属性和方法，例如：
Mavlinktest { id: testsa }
七、测试
将编辑好的PX4固件编译烧入飞控，编译生成QGC地面站
编译QGC后，界面如下，上面有三个输入框，输入需要发送的数据，点击发送即可通过自定义MAVLINK消息发送给飞控，下面有显示接收数据的地方，会将飞控飞控QGC的自定义MAVLIK消息解析后在下面显示。
将飞控通过USB连接地面站。先在MAVLINK Console里面输入
test_mavlink start
然后打开Mavlinktest那一栏，在“发送”按钮左边的三个输入框输入发送的数据，正常的话飞控会通过自定义MAVLINK消息接收到这些数据然后加1发送给地面站，并在地面站上显示。
九、自定义mavlink消息和MAVROS通信
十、OFFBAORD控制接口（状态控制 pwm输出）
一、添加自定义模块
在PX4-Autopilot/src/modules目录下新建control_node文件夹
在control_node文件夹下新建下面四个文件
新建CMakeLists.txt，复制下面内容进去：
px4_add_module(
MODULE modules__control_node
MAIN control_node
SRCS
control_node.cpp
)
新建Kconfig，复制下面内容进去：
menuconfig MODULES_CONTROL_NODE
bool "control_node"
default n
---help---
Enable support for control_node
新建control_node.cpp，复制下面内容进去：
#include "control_node.h"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>
#include <uORB/topics/parameter_update.h>
int ControlNode::print_status()
{
PX4_INFO("Running");
// TODO: print additional runtime information about the state of the module
return 0;
}
int ControlNode::custom_command(int argc, char *argv[])
{
/*
if (!is_running()) {
print_usage("not running");
return 1;
}
// additional custom commands can be handled like this:
if (!strcmp(argv[0], "do-something")) {
get_instance()->do_something();
return 0;
}
*/
return print_usage("unknown command");
}
int ControlNode::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("module",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1024,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0)
{
_task_id = -1;
return -errno;
}
return 0;
}
ControlNode *ControlNode::instantiate(int argc, char *argv[])
{
int example_param = 0;
bool example_flag = false;
bool error_flag = false;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
// parse CLI arguments
while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF)
{
switch (ch)
{
case 'p':
example_param = (int)strtol(myoptarg, nullptr, 10);
break;
case 'f':
example_flag = true;
break;
case '?':
error_flag = true;
break;
default:
PX4_WARN("unrecognized flag");
error_flag = true;
break;
}
}
if (error_flag)
{
return nullptr;
}
ControlNode *instance = new ControlNode(example_param, example_flag);
if (instance == nullptr)
{
PX4_ERR("alloc failed");
}
return instance;
}
ControlNode::ControlNode(int example_param, bool example_flag)
: ModuleParams(nullptr)
{
}
void ControlNode::run()
{
//记录起始点位置
float begin_x;
float begin_y;
float begin_z;
float_t xy_rad=1;
float_t z_rad=1;
float_t yaw_rad=0.1;
//进入offboard模式并解锁
while(!should_exit())
{
_vehicle_status_sub.copy(&_status);
if((_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD)&&(_status.arming_state==vehicle_status_s::ARMING_STATE_ARMED))
{
PX4_INFO("in offboard and armed");
break;
}
_command.target_system = _status.system_id;
_command.target_component = _status.component_id;
ocm.timestamp = hrt_absolute_time();
_vehicle_local_position_sub.copy(&_vehicle_local_position);
sp_local.x=_vehicle_local_position.x;
sp_local.y=_vehicle_local_position.y;
sp_local.z=_vehicle_local_position.z-5;
sp_local.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(sp_local);
ocm.position=true;
ocm.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(ocm);
_command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
_command.param1=1.0f;
_command.param2=PX4_CUSTOM_MAIN_MODE_OFFBOARD;
_command.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(_command);
usleep(10000);
_command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
_command.param1 = 1.0f;
_command.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(_command);
}
//记录当前点的位置
_vehicle_local_position_sub.copy(&_vehicle_local_position);
begin_x=_vehicle_local_position.x;
begin_y=_vehicle_local_position.y;
begin_z=_vehicle_local_position.z;
time_tick=hrt_absolute_time();
//业务逻辑
while (!should_exit())
{
_vehicle_local_position_sub.copy(&_vehicle_local_position);
if(flag==0)//升至5米
{
flag++;
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
sp_local.x=begin_x;
sp_local.y=begin_y;
sp_local.z=begin_z-5;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
PX4_INFO("pos 005");
}
if((flag==1)&&(abs(_vehicle_local_position.x-sp_local.x)<xy_rad)&&(abs(_vehicle_local_position.y-sp_local.y)<xy_rad)&&(abs(_vehicle_local_position.z-sp_local.z)<z_rad))//升至5米
{
if(flag2==0)
{
flag2++;
time_tick=hrt_absolute_time();
}
if((flag2==1)&&(hrt_absolute_time()-time_tick)>1000000)
{
flag++;
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
sp_local.x=begin_x+5;
sp_local.y=begin_y;
sp_local.z=begin_z-5;
sp_local.vx=(float)NAN;
sp_local.vy=(float)NAN;
sp_local.vz=(float)NAN;
sp_local.acceleration[0]=(float)NAN;
sp_local.acceleration[1]=(float)NAN;
sp_local.acceleration[2]=(float)NAN;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
time_tick=hrt_absolute_time();
PX4_INFO("pos 505");
}
}
if((flag==2)&&(abs(_vehicle_local_position.x-sp_local.x)<xy_rad)&&(abs(_vehicle_local_position.y-sp_local.y)<xy_rad)&&(abs(_vehicle_local_position.z-sp_local.z)<z_rad))//升至5米
{
if(flag2==1)
{
flag2++;
time_tick=hrt_absolute_time();
}
if((flag2==2)&&(hrt_absolute_time()-time_tick)>1000000)
{
flag++;
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
sp_local.x=begin_x+5;
sp_local.y=begin_y+5;
sp_local.z=begin_z-5;
sp_local.vx=(float)NAN;
sp_local.vy=(float)NAN;
sp_local.vz=(float)NAN;
sp_local.acceleration[0]=(float)NAN;
sp_local.acceleration[1]=(float)NAN;
sp_local.acceleration[2]=(float)NAN;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
time_tick=hrt_absolute_time();
PX4_INFO("pos 555");
}
}
if(flag==3&&(abs(_vehicle_local_position.x-sp_local.x)<xy_rad)&&(abs(_vehicle_local_position.y-sp_local.y)<xy_rad)&&(abs(_vehicle_local_position.z-sp_local.z)<z_rad))
{
if(flag2==2)
{
flag2++;
time_tick=hrt_absolute_time();
}
if((flag2==3)&&(hrt_absolute_time()-time_tick)>1000000)
{
flag++;
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
sp_local.x=begin_x+5;
sp_local.y=begin_y+5;
sp_local.z=begin_z-5;
sp_local.vx=(float)NAN;
sp_local.vy=(float)NAN;
sp_local.vz=(float)NAN;
sp_local.acceleration[0]=(float)NAN;
sp_local.acceleration[1]=(float)NAN;
sp_local.acceleration[2]=(float)NAN;
sp_local.yaw=1;
sp_local.yawspeed=(float)NAN;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
PX4_INFO("yaw");
}
}
if(flag==4&&(abs(_vehicle_local_position.heading-sp_local.yaw)<yaw_rad))
{
if(flag2==3)
{
flag2++;
time_tick=hrt_absolute_time();
}
if((flag2==4)&&(hrt_absolute_time()-time_tick)>1000000)
{
flag++;
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
sp_local.x=begin_x+5;
sp_local.y=begin_y+5;
sp_local.z=begin_z-5;
sp_local.vx=(float)NAN;
sp_local.vy=(float)NAN;
sp_local.vz=(float)NAN;
sp_local.acceleration[0]=(float)NAN;
sp_local.acceleration[1]=(float)NAN;
sp_local.acceleration[2]=(float)NAN;
sp_local.yaw=(float)NAN;
sp_local.yawspeed=1;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
PX4_INFO("yawspeed");
}
}
if(flag==5)
{
if(flag2==4)
{
flag2++;
time_tick=hrt_absolute_time();
}
if((flag2==5)&&(hrt_absolute_time()-time_tick)>5000000)
{
flag++;
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
sp_local.x=(float)NAN;
sp_local.y=(float)NAN;
sp_local.z=begin_z-5;
sp_local.vx=1;
sp_local.vy=0;
sp_local.vz=(float)NAN;
sp_local.acceleration[0]=(float)NAN;
sp_local.acceleration[1]=(float)NAN;
sp_local.acceleration[2]=(float)NAN;
sp_local.yaw=(float)NAN;
sp_local.yawspeed=(float)NAN;
ocm.position=true;
ocm.velocity=true;
ocm.acceleration=false;
PX4_INFO("alt and velocity");
}
}
if(flag==6)
{
if(flag2==5)
{
flag2++;
time_tick=hrt_absolute_time();
}
if((flag2==6)&&(hrt_absolute_time()-time_tick)>5000000)
{
flag++;
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
sp_local.x=(float)NAN;
sp_local.y=(float)NAN;
sp_local.z=begin_z-5;
sp_local.vx=(float)NAN;
sp_local.vy=(float)NAN;
sp_local.vz=(float)NAN;
sp_local.acceleration[0]=0;
sp_local.acceleration[1]=1;
sp_local.acceleration[2]=(float)NAN;
sp_local.yaw=(float)NAN;
sp_local.yawspeed=(float)NAN;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=true;
PX4_INFO("alt and acceleration");
}
}
if(flag==7)
{
if(flag2==6)
{
flag2++;
time_tick=hrt_absolute_time();
}
if((flag2==7)&&(hrt_absolute_time()-time_tick)>5000000)
{
flag++;
_command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
_command.param1=1.0f;
_command.param2=PX4_CUSTOM_MAIN_MODE_AUTO;
_command.param3=PX4_CUSTOM_SUB_MODE_AUTO_RTL;
_command.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(_command);
PX4_INFO("return");
}
}
if(ocm.position||ocm.velocity||ocm.acceleration)
{
ocm.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(ocm);
_vehicle_status_sub.copy(&_status);
if(_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD)
{
sp_local.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(sp_local);
// PX4_INFO("sp_local.x=%lf\n",(double)sp_local.x);
// PX4_INFO("sp_local.y=%lf\n",(double)sp_local.y);
// PX4_INFO("sp_local.z=%lf\n",(double)sp_local.z);
// PX4_INFO("sp_local.vx=%lf\n",(double)sp_local.vx);
// PX4_INFO("sp_local.vy=%lf\n",(double)sp_local.vy);
// PX4_INFO("sp_local.vz=%lf\n",(double)sp_local.vz);
}
}
usleep(100000);
parameters_update();
}
}
void ControlNode::parameters_update(bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force)
{
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
// update parameters from storage
updateParams();
}
}
int ControlNode::print_usage(const char *reason)
{
if (reason)
{
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Section that describes the provided module functionality.
This is a template for a module running as a task in the background with start/stop/status functionality.
### Implementation
Section describing the high-level implementation of this module.
### Examples
CLI usage example:
$ module start -f -p 42
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("module", "template");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int control_node_main(int argc, char *argv[])
{
return ControlNode::main(argc, argv);
}
新建control_node.h，复制下面内容进去：
#pragma once
#include "commander/px4_custom_mode.h"
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
using namespace time_literals;
extern "C" __EXPORT int control_node_main(int argc, char *argv[]);
class ControlNode : public ModuleBase<ControlNode>, public ModuleParams
{
public:
ControlNode(int example_param, bool example_flag);
virtual ~ControlNode() = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static ControlNode *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
private:
/**
* Check for parameter changes and update them if needed.
* @param parameter_update_sub uorb subscription to parameter_update
* @param force for a parameter update
*/
void parameters_update(bool force = false);
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig /**< another parameter */
)
uint64_t time_tick=hrt_absolute_time();
int flag=0,flag2=0;
vehicle_local_position_s _vehicle_local_position;
vehicle_status_s _status;
vehicle_command_s _command = {};
offboard_control_mode_s ocm{};
position_setpoint_triplet_s _pos_sp_triplet{};
vehicle_command_ack_s _ack{};
vehicle_local_position_setpoint_s sp_local{};
vehicle_attitude_setpoint_s attitude_setpoint{};
// Subscriptions
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
// Publications
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<position_setpoint_triplet_s> _position_setpoint_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
};
添加到编译脚本
打开下图文件
添加一行
CONFIG_MODULES_CONTROL_NODE=y
二、测试
执行：
make px4_sitl_default gazebo
打开仿真后执行
control_node start
正常的话无人机会进入Offboard，并起飞到5米高然后按程序设定自主运动
扫一扫
498
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
