写在前面
因为项目原因，需要无人机在事先指定的飞行任务（Mission）中加入特定的指令，控制外部设备完成一些工作。
最初是想在地面站和飞控之间打一条通道，这样地面站就可以控制外部设备了，详见我之前的博客。但是很难做到事先规划，在指定航点（WAYPOINT）上做计划的操作，当然手动改控制是一点问题没有，就是有点考验用户的耐心和注意力，而且我们也不能指望用户有多高的水平（只有甲方爸爸挑你），即使用户水平再高，当一件事变成工作也就没意思了。
最后就想能不能事先在任务中规划好指令，到一个航点就自动完成工作呢，这样一旦任务做好，就可以一键起飞，完成指定工作后自动返回，把用户解放出来。
反复查看QGC和PX4代码这个功能在最近终于基本完成了，特地写下来给大家分享，就当给自己做个备忘录吧。
本来想直接粘代码的但是有些给客户做的东西不能往外放，重写个demo有点费事，好在PX4，QGC有现成的功能实现，我也是照着思路写出来的，大家可以照猫画虎。
不卖关子了，这个功能就是相机控制（Camera），下面就以Camera为例，一步一步讲讲具体实现。
1.自定义MavLink命令
1.1 MavLink 消息和命令
大家可以在Mavlink的官网上找到它们的定义。简单的说MavLink协议上传输的都是各种消息(Message)，消息其实就是一种数据包格式，是为了通信而准备的，消息的格式多种多样。而命令(Command)是一种数据，格式完全统一，通过命令号来解释不同的意义，这也就方便在飞行任务中定义、存储、解释、执行。
1.2 MavLink命令（MAV_CMD）的格式
命令由命令号和七个参数组成加上其他我们不需要关心控制的字段，下面贴出来的是MavLink传输命令的消息所定义的结构体。
//mavlink_msg_command_long.h
#define MAVLINK_MSG_ID_COMMAND_LONG 76
MAVPACKED(
typedef struct __mavlink_command_long_t {
float param1; /*< Parameter 1 (for the specific command).*/
float param2; /*< Parameter 2 (for the specific command).*/
float param3; /*< Parameter 3 (for the specific command).*/
float param4; /*< Parameter 4 (for the specific command).*/
float param5; /*< Parameter 5 (for the specific command).*/
float param6; /*< Parameter 6 (for the specific command).*/
float param7; /*< Parameter 7 (for the specific command).*/
uint16_t command; /*< Command ID (of command to send).*/
uint8_t target_system; /*< System which should execute the command*/
uint8_t target_component; /*< Component which should execute the command, 0 for all components*/
uint8_t confirmation; /*< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)*/
}) mavlink_command_long_t;
1.3 自定义消息号
可以使用Mavlink官方提供的生成器，也可以直接在头文件中直接加上，其实就是个16位以内不与已有重复的数字。
1.3.1 安装Mavlink生成器
#下载源码
git clone https://github.com/mavlink/mavlink.git
#进入代码目录并更新子模块
cd mavlink
git submodule update --init --recursive
#可能报错需要安装，如果以前没有安装的话，目前python3的脚本还不怎么好使
sudo apt install python-tk
pip install future
#运行python脚本
python -m mavgenerate
1.3.2 定义命令
- 创建一个XML，如xxx.xml,cmd号随便定义，16位无符号整型范围内（ < 65536）,不和已有的冲突即可,已有的在QGC那个
ardupilotmega.h定义得比较全。
<mavlink>
<enums>
<enum name="MAV_CMD">
<description>Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data.</description>
<entry value="56302" name="MAV_CMD_XXXX_AMP_SINGLE">
<description>single XXXX power control</description>
<param index="1" label="index">XXXX index</param>
<param index="2" label="power">if 1 poweron the XXXX,0 poweroff XXXX</param>
<param index="3" label="amp">XXXX power amp,0~1,0</param>
<param index="4">Reserved (all remaining params)</param>
</entry>
<entry value="56340" name="MAV_CMD_XXXX_PARAM_MODULATE">
<description>MODULATE parameters</description>
<param index="1" label="index">XXXX index</param>
<param index="2" label="modulateType"></param>
<param index="3" label="modulateBandWidth"></param>
<param index="4" label="modulateMode"></param>
<param index="5" label="modulatePulseNum"></param>
<param index="6" label="modulateSubcodeWidth"></param>
<param index="7" label="modulateCodeLen"></param>
</entry>
<!-- ... 可以加很多 -->
</enum>
</enums>
</mavlink>
- 生成
python mavgenerate.py
#进入界面后选择xml位置，生成位置
#语言选C
#Protocol选2.0
然后就可以在你生成的位置找到xxx.h文件了
typedef enum MAV_CMD
{
MAV_CMD_XXXX_AMP_SINGLE=56302, /* single XXXX power control |XXXX index| if 1 poweron the XXXX,0 poweroff XXXX| XXXX power amp,0~1,0| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */
MAV_CMD_XXXX_PARAM_MODULATE=56340, /* MODULATE parameters |XXXX index| | | | | | | */
//...
MAV_CMD_ENUM_END=56362, /* | */
}
呵呵，搞了半天就是个数字，所以直接在用的头文件里直接加就好了，写个xml可能更好在团队中沟通吧。
1.4 头文件中添加命令
QGC 在libs/mavlink/include/mavlink/v2.0/ardupilotmega/ardupilotmega.h
PX4 在mavlink/include/mavlink/v2.0/common/common.h
//...
typedef enum MAV_CMD
{
//...
//MAV_CMD_ENUM_END=42651, /* | */ 将以前尾部注释掉
MAV_CMD_XXXX_AMP_SINGLE=56302, /* single XXXX power control |XXXX index| if 1 poweron the XXXX,0 poweroff XXXX| XXXX power amp,0~1,0| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */
MAV_CMD_XXXX_PARAM_MODULATE=56340, /* MODULATE parameters |XXXX index| | | | | | | */
//...
MAV_CMD_ENUM_END=56362, /* | */ //加上我们定义最大的号+1
}
//...
2 QGC中相机控制任务功能实现分析
2.1 任务命令的传输存储和处理在数据结构上的区别
2.1.1 任务命令的传输和存储的数据结构
就和所有通信协议一样，数据都是串行的，任务将所有命令挨个存放，是一个线性列表，命令与命令之间只有先后关系没有从属关系。你在QGC的任务界面下随便编辑一个任务，加上几个航点，在航点上对相机进行操作，然后导出任务，其实就是一个json文件了，用文本编辑器看看就知道了。但是奇怪的是相机操作明明就是依附在航点上的呀，直觉上应该是个树状关系，后面在处理环节上说。
这些命令被 MAVLINK_MSG_ID_COMMAND_LONG 消息发送到PX4上存到SD卡上，即使飞控关机也不会丢失。飞控与QGC连接后，也是通过刚才那个消息，GQC会从飞控上下载任务，显示到飞行界面上。飞控接到任务模式开始指令之后，读取任务并执行，执行过程在后面PX4修改中详解。
2.1.2 任务命令在处理过程中的数据结构
通过下面的说明可以看出这个数据结构是以基本航点为线性列表，其他命令存储在航点下的section中就是树的关系
PlanMasterController
|__ GeoFenceController //围栏
|__ RallyPointController //集结
|__ MissionController //任务控制器
|__ List<VisualMissionItem*>* visualItems; //这就是任务处理过程中的任务的储存容器,下面两个类都是VisualMissionItem的子类
|__ ComplexMissionItem //复杂任务吧，没怎么研究它
|__ SimpleMissionItem //这就是我们任务处理中航点的记录对象
|__ MissionItem missionItem //就是CMD的的表达，即命令号加7个参数，正常情况下存储任务的经纬度高度，可以是WAYPOINT或者其他的飞行命令
|__ SpeedSection* speedSection //航速控制节 继承Section，实现方法和Camera相同，可作为对照，相对简单
|__ CameraSection* cameraSection //相机控制节 继承Section
|__ YourSection* yourSection //如果要添加功能就要自己实现Section，来存储自定义命令，在原版中是没有的
2.2 数据处理过程
2.2.1 从文件或飞控中读取任务
前面说了任务是线性列表结构，读取的过程就是就是要转换树状结构，简单说一下这个过程,代码大家自行在项目中搜索。
- 将所有任务都存到到 MissionController.visualItems 中，只不过这个时候没有处理，里面全部都是SimpleMissionItem，任务数据对应的是
SimpleMissionItem.missionItem, section中没有数据 - MissionController._scanForAdditionalSettings()顺序遍历 MissionController.visualItems ，如果是SimpleMissionItem,调用SimpleMissionItem.scanForMissionSettings(),调用成员对象speedSection.scanForSection(visualItems, scanIndex),cameraSection.scanForSection(visualItems, scanIndex)，你也要修改SimpleMissionItem调用你yourSection.scanForSection(visualItems, scanIndex)
- 在Section.scanForSection(visualItems, scanIndex),查找scanIndex对应的SimpleMissionItem.missionItem.command是否是自己需要的，如果是想办法存起来，删除当前item，返回true，如果不是返回false。注意一个section可以有多个命令共同组成。
- 这样就将线性结构变成了树状结构。这一段要仔细看码才能读懂。
还是用代看代码比较清晰
void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle)
{
int scanIndex = 0;
while (scanIndex < visualItems->count()) {
VisualMissionItem* visualItem = visualItems->value<VisualMissionItem*>(scanIndex);
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
if (simpleItem) {
scanIndex++;
simpleItem->scanForSections(visualItems, scanIndex, vehicle);
} else {
// Complex item, can't have sections
scanIndex++;
}
}
}
bool SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle)
{
bool sectionFound = false;
if (_cameraSection->available()) {
sectionFound |= _cameraSection->scanForSection(visualItems, scanIndex);
}
if (_speedSection->available()) {
sectionFound |= _speedSection->scanForSection(visualItems, scanIndex);
}
return sectionFound;
}
//如果时多个命令，一定在这将下面连续几个属于自己的命令都找到并删除掉，应为命令都是连续的，而且本次不读完，就会混到航点中去，污染结构
bool SpeedSection::scanForSection(QmlObjectListModel* visualItems, int scanIndex)
{
SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(scanIndex);
MissionItem& missionItem = item->missionItem();
// See SpeedSection::appendMissionItems for specs on what consitutes a known speed setting
if (missionItem.command() == MAV_CMD_DO_CHANGE_SPEED && missionItem.param3() == -1 && ...) {
visualItems->removeAt(scanIndex)->deleteLater();
_flightSpeedFact.setRawValue(missionItem.param2());
setSpecifyFlightSpeed(true);
return true;
}
return false;
}
//CammeraSection 不贴太长了,道理一样
2.2.2 转换成任务文件或发送给飞控
相比起前面的识别这部分相对简单,同样代码大家自行在项目中搜索。
- MissionController._convertToMissionItems()遍历visualItems，如果是SimpleMissionItem，调用SimpleMissionItem.appendMissionItems (QList<MissionItem>& items, QObject missionItemParent)
- SimpleMissionItem 将自己的missionItem，append到items中。并且调用所有section->appendSectionItems(items, missionItemParent, seqNum);
- Section根据自己的实际情况在appendSectionItems(QList<MissionItem>& items, QObject missionItemParent, int& nextSequenceNumber)函数中将任务转换为一个或多个命令，append到list中。
上码
bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent)
{
bool endActionSet = false;
int lastSeqNum = 0;
for (int i=0; i<visualMissionItems->count(); i++) {
VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(visualMissionItems->get(i));
lastSeqNum = visualItem->lastSequenceNumber();
visualItem->appendMissionItems(rgMissionItems, missionItemParent);
}
// Mission settings has a special case for end mission action
MissionSettingsItem* settingsItem = visualMissionItems->value<MissionSettingsItem*>(0);
if (settingsItem) {
endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent);
}
return endActionSet;
}
void SimpleMissionItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
int seqNum = sequenceNumber();
items.append(new MissionItem(missionItem(), missionItemParent));
seqNum++;
_cameraSection->appendSectionItems(items, missionItemParent, seqNum);
_speedSection->appendSectionItems(items, missionItemParent, seqNum);
}
//自己定义时可以有多个，因为复杂命令一个7个参数可能不够用
void SpeedSection::appendSectionItems(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum)
{
if (_specifyFlightSpeed) {
MissionItem* item =
new MissionItem(seqNum++,
MAV_CMD_DO_CHANGE_SPEED, //这是命令号
MAV_FRAME_MISSION, //这个地方一定写成这个值，表示随航点一同执行，不然飞控不会执行
_vehicle->multiRotor() ? 1 /* groundspeed */ : 0 /* airspeed */, // p1 Change airspeed or groundspeed
_flightSpeedFact.rawValue().toDouble(), // p2
-1, // p3 No throttle change
0, // p4 Absolute speed change
0, 0, 0, // param 5-7 not used
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
//CammeraSection 不贴太长了,道理一样
2.2.3 添加自己的Section
当然是要创建一个类实现 Section 了，最重要的是实现解析函数 scanForSection 和生成函数 appendSectionItems ，当然也要有自己的数据内容，这是我们加这个东西的核心目的。
还有要注意的是一旦在界面上修改核心数据时一定要 emit Section 定义的一些信号，以便更新这个任务链的序号以及更新前台显示的序号。
最后手动搜索 cammeraSection或 speedSection ,在它们出现的地方，加上我们定义的section对应操作。当然要区分是不是它们自己专有的，标准就是看Section中有没有定义，没有就是专有的。
还是有必要把Section.h拉出来看看
class Section : public QObject
{
Q_OBJECT
public:
Section(Vehicle* vehicle, QObject* parent = NULL)
: QObject(parent)
, _vehicle(vehicle)
{
}
Q_PROPERTY(bool available READ available WRITE setAvailable NOTIFY availableChanged)
Q_PROPERTY(bool settingsSpecified READ settingsSpecified NOTIFY settingsSpecifiedChanged)
Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY availableChanged)
virtual bool available (void) const = 0;
virtual bool settingsSpecified (void) const = 0;
virtual bool dirty (void) const = 0;
virtual void setAvailable (bool available) = 0;
virtual void setDirty (bool dirty) = 0;
/// Scans the loaded items for the section items
/// @param visualItems Item list
/// @param scanIndex Index to start scanning from
/// @return true: section found, items added, scanIndex updated
virtual bool scanForSection(QmlObjectListModel* visualItems, int scanIndex) = 0;
/// Appends the mission items associated with this section
/// @param items List to append to
/// @param missionItemParent QObject parent for created MissionItems
/// @param nextSequenceNumber[in,out] Sequence number for first item, updated as items are added
virtual void appendSectionItems(QList<MissionItem*>& items, QObject* missionItemParent, int& nextSequenceNumber) = 0;
/// Returns the number of mission items represented by this section.
/// Signals: itemCountChanged
virtual int itemCount(void) const = 0; //没数据返回0，需要多少命令就返回多少以appendSectionItems()产生的为准
signals:
void availableChanged (bool available);
void settingsSpecifiedChanged (bool settingsSpecified);
void dirtyChanged (bool dirty); //好像没啥用
void itemCountChanged (int itemCount); //这是我觉得最重要的信号量，一旦改数据一般都要emit它
protected:
Vehicle* _vehicle;
};
2.3 修改前台
Talk is cheap， show me your code.
//planView.qml ====================================================================================
QGCView {
//...
// Mission Item Editor
Item {
id: missionItemEditor
anchors.left: parent.left
anchors.right: parent.right
anchors.top: rightControls.bottom
anchors.topMargin: ScreenTools.defaultFontPixelHeight * 0.5
anchors.bottom: parent.bottom
anchors.bottomMargin: ScreenTools.defaultFontPixelHeight * 0.25
visible: _editingLayer == _layerMission && !planControlColapsed
QGCListView {
id: missionItemEditorListView
anchors.fill: parent
spacing: ScreenTools.defaultFontPixelHeight / 4
orientation: ListView.Vertical
model: _missionController.visualItems //所有的航点数据
cacheBuffer: Math.max(height * 2, 0)
clip: true
currentIndex: _missionController.currentPlanViewIndex
highlightMoveDuration: 250
visible: _editingLayer == _layerMission && !planControlColapsed
//-- List Elements
delegate: MissionItemEditor { //所有航点的编辑界面，不仅仅是WAYPOINT
map: editorMap
masterController: _planMasterController
missionItem: object
width: parent.width
readOnly: false
rootQgcView: _qgcView
onClicked: {
_missionController.setCurrentPlanViewIndex(object.sequenceNumber, false);
}
onRemove: {
var removeIndex = index
_missionController.removeMissionItem(removeIndex)
if (removeIndex >= _missionController.visualItems.count) {
removeIndex--
}
_missionController.setCurrentPlanViewIndex(removeIndex, true)
}
onInsertWaypoint: insertSimpleMissionItem(editorMap.center, index)
onInsertComplexItem: insertComplexMissionItem(complexItemName, editorMap.center, index)
}
}
}
//...
}
//MissionItemEditor.qml ==================================================================================
Rectangle {
property var missionItem
// ...
//这个地方就是加载具体编辑器的地方
Loader {
id: editorLoader
anchors.leftMargin: _margin
anchors.topMargin: _margin
anchors.left: parent.left
anchors.top: commandPicker.bottom
source: missionItem.editorQml //在scanForSections中定义的是qrc:/qml/SimpleItemEditor.qml
visible: _currentItem
property var masterController: _masterController
property real availableWidth: _root.width - (_margin * 2) ///< How wide the editor should be
property var editorRoot: _root
}
}
//SimpleItemEditor.qml ================================================================================
Rectangle {
// ...
CameraSection {
checked: missionItem.cameraSection.settingsSpecified
visible: missionItem.cameraSection.available
}
// ...
//如果自定义section就要自定一个QML来编辑，可以是使用Loader
//或许定义单独的组件就像CameraSection，需要在src/QmlControls/QGroundControl.Controls.qmldir中添加
//具体怎么添加，项目搜索关键字CameraSection就知道了
}
//CameraSection.qml =================================================================================
Column {
property var _camera: missionItem.cameraSection //竟然可以直接读取包含它框架的属性，见识了
// ... 对代码做了些简化，看得清楚
SectionHeader {
id: cameraSectionHeader
text: qsTr("Camera")
checked: false
}
FactComboBox {
id: cameraActionCombo
anchors.left: parent.left
anchors.right: parent.right
fact: _camera.cameraAction
indexModel: false
}
FactTextField {
fact: _camera.cameraPhotoIntervalTime
Layout.preferredWidth: _fieldWidth
}
FactTextField {
fact: _camera.cameraPhotoIntervalDistance
Layout.preferredWidth: _fieldWidth
}
// ...
// 都是一些属性数据的绑定，绑定也有技巧，一般是属性绑定到编辑控件的值上，初始化的时候写到控件上
// QGC专门定制了一套FactXXX控件如果可以用的话用起来也挺好
}
- 关于界面，如果是命令与前序点的命令有关联，可能需要做很多事，需要遍历前序节点；
- 如果控制参数比较复杂，就像我这个项目有几百个参数就需要在PlanView写专用界面来控制，Section中直接就是存的就是missionItem列表，至于怎么生成它们，怎么将他们变成我要的参数，都在这个控制器中。参数也是增量生成的，只发送和前序节点不一样的数据。这样据就需要知道MissionController.visualItems,和当前的航点序号。而且每当 当前航点序号发生该改变时就要遍历前序节点，把前序节点的的状态保存再来为当前节点的改变做对比，每当用户改变参数时都要把missionitem重新生成，防止用户点下一个航点丢失数据。
3 PX4中相机控制任务功能实现分析
3.1 上传/下载命令
当任务从QGC上传到PX4上，判断是否支持当前命令，然后存储到SD卡中，这步最重要的时要放行我们自定义的命令，下载同理。
src/modules/mavlink/mavlink_mission.cpp
//上传解析
int
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item,
struct mission_item_s *mission_item)
{
//...
if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
switch (mavlink_mission_item->command) {
//...
case MAV_CMD_IMAGE_START_CAPTURE:
case MAV_CMD_IMAGE_STOP_CAPTURE:
case MAV_CMD_VIDEO_START_CAPTURE:
case MAV_CMD_VIDEO_STOP_CAPTURE:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case MAV_CMD_SET_CAMERA_MODE:
//...
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
break;
//..
}
}
}
//下载编码
int
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item,
mavlink_mission_item_t *mavlink_mission_item){
//...
if (mission_item->frame == MAV_FRAME_MISSION) {
switch (mavlink_mission_item->command) {
//...
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
//...
break;
}
}
}
3.2 在navigator模块中放行
- 添加 NAV_CMD,在
src/modules/navigator/navigation.h中添加命令，注意把'MAV'改为'NAV'开头
/* compatible to mavlink MAV_CMD */
enum NAV_CMD {
//...
NAV_CMD_DO_MOUNT_CONFIGURE = 204,
NAV_CMD_DO_MOUNT_CONTROL = 205,
NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214,
NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
NAV_CMD_SET_CAMERA_MODE = 530,
NAV_CMD_IMAGE_START_CAPTURE = 2000,
NAV_CMD_IMAGE_STOP_CAPTURE = 2001,
//...
NAV_CMD_XXXX_AMP_SINGLE=56302, /* single XXXX power control |XXXX index| if 1 poweron the XXXX,0 poweroff XXXX| XXXX power amp,0~1,0| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */
NAV_CMD_XXXX_PARAM_MODULATE=56340, /* MODULATE parameters |XXXX index| | | | | | | */
//...
NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
};
- 在导航模块中放行命令，
src/modules/navigator/mission_feasibility_checker.cpp
bool
MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
{
bool
MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
{
// check if we find unsupported items and reject mission if so
if (missionitem.nav_cmd != NAV_CMD_IDLE &&
missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
//...
missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
//..
){
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1),
(int)missionitem.nav_cmd);
return false;
}
//...
}
bool
MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt)
{
if (takeoff_index != -1) {
// checks if all the mission items before the first takeoff waypoint
// are not waypoints or position-related items;
// this means that, before a takeoff waypoint, one can set
// one of the bellow mission items
for (size_t i = 0; i < (size_t)takeoff_index; i++) {
if (missionitem.nav_cmd != NAV_CMD_IDLE &&
//...
missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
//...
) {
takeoff_first = false;
} else {
takeoff_first = true;
}
}
}
}
3.3 到达航点发送任务
- 判断是否达到目标位置，表示任务可以执行了，全部都可以发送了
src/modules/navigator/mission_block.cpp
bool
MissionBlock::is_mission_item_reached()
{
/* handle non-navigation or indefinite waypoints */
switch (_mission_item.nav_cmd) {
//...
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
return true;
//...
}
}
- 定义一个模块号,不和以往冲突就行
mavlink/include/mavlink/v2.0/common/common.h
typedef enum MAV_COMPONENT
{
//...
MAV_COMP_ID_CAMERA=100, /* Camera #1. | */
MAV_COMP_ID_CAMERA2=101, /* Camera #2. | */
MAV_COMP_ID_CAMERA3=102, /* Camera #3. | */
MAV_COMP_ID_CAMERA4=103, /* Camera #4. | */
MAV_COMP_ID_CAMERA5=104, /* Camera #5. | */
MAV_COMP_ID_CAMERA6=105, /* Camera #6. | */
//...
} MAV_COMPONENT;
- 发布uORB消息
src/modules/navigator/navigator_main.cpp
void
Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
{
switch (vcmd->command) {
//...
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
vcmd->target_component = 100; // Camera #1，接受时需要通过这个值来判断自己是不是需要处理
break;
//...
}
//发布消息
if (_vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd);
} else {
_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
}
3.4 命令的最终执行模块/驱动
-
建立目录
src/drivers/camera_capture -
创建头h和cpp文件，这个文件我做了一些简化
src/drivers/camera_capture/camera_capture.cpp
namespace camera_capture
{
CameraCapture *g_camera_capture;
}
CameraCapture::CameraCapture() :
{
}
CameraCapture::~CameraCapture()
{
camera_capture::g_camera_capture = nullptr;
}
void
CameraCapture::cycle_trampoline(void *arg)
{
CameraCapture *dev = reinterpret_cast<CameraCapture *>(arg);
dev->cycle();
}
void
CameraCapture::cycle()
{
//订阅命令消息，处理命令
if (_command_sub < 0) {
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
}
bool updated = false;
orb_check(_command_sub, &updated);
// Command handling
if (updated) {
vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _command_sub, &cmd);
// TODO : this should eventuallly be a capture control command
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
// Enable/disable signal capture
if (commandParamToInt(cmd.param1) == 1) {
set_capture_control(true);
} else if (commandParamToInt(cmd.param1) == 0) {
set_capture_control(false);
}
// Reset capture sequence
if (commandParamToInt(cmd.param2) == 1) {
reset_statistics(true);
}
}
}
//低速循环队列，比开进程好多了
work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, camera_capture::g_camera_capture,
USEC2TICK(100000)); // 100ms
}
int
CameraCapture::start()
{
/* allocate basic report buffers */
_trig_buffer = new ringbuffer::RingBuffer(2, sizeof(_trig_s));
if (_trig_buffer == nullptr) {
return PX4_ERROR;
}
// start to monitor at low rates for capture control commands
work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this,
USEC2TICK(1));
return PX4_OK;
}
void
CameraCapture::stop()
{
work_cancel(LPWORK, &_work);
work_cancel(HPWORK, &_work_publisher);
if (camera_capture::g_camera_capture != nullptr) {
delete (camera_capture::g_camera_capture);
}
}
extern "C" __EXPORT int camera_capture_main(int argc, char *argv[]);
int camera_capture_main(int argc, char *argv[])
{
if (argc < 2) {
return usage();
}
if (!strcmp(argv[1], "start")) {
camera_capture::g_camera_capture = new CameraCapture();
return 0;
} else if (!strcmp(argv[1], "stop")) {
camera_capture::g_camera_capture->stop();
} else {
return usage();
}
return 0;
}
- 创建编写CMakeList.txt
px4_add_module(
MODULE drivers__camera_capture
MAIN camera_capture
COMPILE_FLAGS
SRCS
camera_capture.cpp
)
- 在要生成的bord上加上这个驱动
boards/px4/fmu-vX/default.cmake
DRIVERS
//...
camera_capture
//...
- 如果要自启动需要在
ROMFS/px4fmu_common/init.d/rcS上添加启动命令
4 在飞行模式中的改进
前面任务都完成了，但是用户可能想知道任务执行过程中设备的状态，这时就需要在QGC上修改，下面只有这些状态的数据来源，怎样处理仁者见仁了。
基本都在下面QML上下功夫
/home/ted/src/qgroundcontrol/src/FlightDisplay/FlightDisplayView.qml
QGCView {
property bool vehicleArmed: _activeVehicle ? _activeVehicle.armed : true // true here prevents pop up from showing during shutdown
property bool vehicleWasArmed: false
property bool vehicleInMissionFlightMode: _activeVehicle ? (_activeVehicle.flightMode === _activeVehicle.missionFlightMode) : false
property bool promptForMissionRemove: false
//里面有任务数据
property var _missionController: _planMasterController.missionController
//当前连接的飞机，非常有用，可以收发mavlink消息
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
//通过它可以知道当前飞到哪个航点
property int currentMissionIndex : flightMissionController?(flightMissionController.currentMissionIndex):0
}
-
设备反馈模式，设备由串口通过PX4向QGC发送当前的状态，这个比较靠谱，方法请参见之前博文。
-
用户还能可能需要手动操作，这就要定义新的MavLink 消息了，个人觉得，还是以直接发送MAV_CMD同样的数据比较好，QGC端命令生成不用变，PX4只需要在mavlink模块上加上接受这个自定义消息的处理，并像navigator模块一样发布UORB消息，执行也完全不用动。
传送门: PX4 QGC透明串口转发
1. PX4串口读写
2.自定义Mavlink消息实现QGC和PX4 mavlink守护进程通信
3.自定义uORB消息实现，实现PX4模块间数据传递
最后再吐槽下CSDN，这篇文章在CSDN上的MarkDown编辑器下死活格式不对，呵呵，搞副业的老板
网友评论