基于PX4 1.13固件的自定义UAVCAN消息定义与使用-优快云博客

文章目录

前言
一、执行器通信协议
二、UAVCAN消息定义
三、UAVCAN消息使用
总结

前言

本文针对飞控需要增加新的CAN驱动执行器，如电机等定义UAVCAN节点，并进行使用。

只进行了前期的开发，完成了.uavcan 消息定义和简单的发送。

作者接触can和uavcan 的时间很短，内容如有错误，请在评论区指出，多交流。

一、执行器通信协议

执行器为市面上某款电机，通信协议为can，不支持uavcan 协议，STM32通信协议如下：

angle_to_can_offset(target_angle1*scale, tx_buf1);
//		MyCAN_Transmit(0x01, 5,  tx_buf1);

void angle_to_can_offset(float value, uint8_t tx_buf[5]) 
{
    // 将float强制转换为int32_t
    int32_t int_value = (int32_t)value;
    
   
    // 将int32_t转换为4个uint8_t字节（大端序）
	tx_buf[0] = 0x1E; 
    tx_buf[1] = int_value & 0xFF;           // 最低位字节
    tx_buf[2] = (int_value >> 8) & 0xFF;    // 次低位字节
    tx_buf[3] = (int_value >> 16) & 0xFF;   // 次高位字节
    tx_buf[4] = (int_value >> 24) & 0xFF;   // 最高位字节

如果飞控无其他uavcan协议传感器和执行器，可直接关掉uavcan接口，使用系统原有can接口控制电机。

后续有时间总结如何直接使用can接口输出控制数据。（并不是所有飞控板都可以实现，且uavcan将无法使用，目前还没有实现一个can接口uavcan，一个can接口普通can）

二、UAVCAN消息定义

在…\Firmware\src\drivers\uavcan\libuavcan\dsdl 中可以看到定义了许多uavcan消息，其命名有以下相似

命名规范：ID.MessageName.uavcan

uavcan消息定义
结合电机的通信协议，增加 uavcan/libuavcan/dsdl/uavcan/equipment/actuator/1200.HpjmTiltMotor.uavcan，写入以下代码

uint8 motor_id        # Motor ID
uint8 header = 0x1E   # Fixed packet header
int32 angle_data      # Angle data

在 …\Firmware\src\drivers\uavcan\CMakeLists.txt 中，有以下代码，即dsdl内的.uavcan文件均被编译，生成在...\include\dsdlc_generated中。

# generated DSDL
set(DSDLC_DIR "${CMAKE_CURRENT_SOURCE_DIR}/dsdl")
set(DSDLC_INPUTS
	"${LIBUAVCAN_DIR}/dsdl/ardupilot"
	"${LIBUAVCAN_DIR}/dsdl/com"
	"${LIBUAVCAN_DIR}/dsdl/cuav"
	"${LIBUAVCAN_DIR}/dsdl/dronecan"
	"${LIBUAVCAN_DIR}/dsdl/uavcan"
)
set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated")

因此，通过编译飞控，可以在下边文件夹获得 HpjmTiltMotor.hpp
…\Firmware\build\cuav_x7pro_default\src\drivers\uavcan\include\dsdlc_generated\uavcan\equipment\actuator\HpjmTiltMotor.hpp

三、UAVCAN消息使用

结合前边生成的HpjmTiltMotor.hpp，发送电机控制信号。

1. 初始化函数

结合…\Firmware\src\drivers\uavcan\actuators 中文件，新建 tiltmotor.hpp 和 tiltmotor. cpp文件：

tiltmotor.hpp

#pragma once
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/actuator/HpjmTiltMotor.hpp>

class TiltMotorController
{
public:
    TiltMotorController(uavcan::INode &node);

    int init();

    void send_fixed_command(uint8_t motor_id, int32_t angle);

private:
    uavcan::Publisher<uavcan::equipment::actuator::HpjmTiltMotor> _pub;
};

tiltmotor. cpp

#include "tiltmotor.hpp"

TiltMotorController::TiltMotorController(uavcan::INode &node) :
    _pub(node)
{
}

int TiltMotorController::init()
{
    // Publisher 不需要 start()，直接返回 0
    return 0;
}

void TiltMotorController::send_fixed_command(uint8_t motor_id, int32_t angle)
{
    uavcan::equipment::actuator::HpjmTiltMotor msg;
    msg.motor_id = motor_id;
    msg.angle_data = angle;

    _pub.broadcast(msg);    // 直接广播
}

在…\Firmware\src\drivers\uavcan\CMakeLists.txt 增加编译，

		# Actuators
		actuators/esc.cpp
		actuators/hardpoint.cpp
		actuators/servo.cpp
		actuators/tiltmotor.cpp   # <-- 新加的
		actuators/tiltmotor.hpp   # 新加的

2. 使用实例

在 uavcan_main.cpp 中添加测试代码，位于：…\Firmware\src\drivers\uavcan\uavcan_main.cpp

包含头文件

#include "tiltmotor.hpp"

测试例程

void example_send_tilt()
{
    if (!UavcanNode::instance()) {
        PX4_ERR("UAVCAN node not started");
        return;  // 直接退出函数
    }

    uavcan::INode &node = UavcanNode::instance()->get_node();
    TiltMotorController tilt(node);

    tilt.init();

    // 循环发送几次
    for (int i = 0; i < 5; i++) {
        tilt.send_fixed_command(1, 1000); // motor_id=1, angle_data=1000
        // tilt.send_fixed_command(2, 2000); // motor_id=2, angle_data=2000
        PX4_INFO("TiltMotor commands sent %d/5", i+1);
        px4_usleep(1000000); // 1秒
    }
}

使用方法

extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
{
	if (argc < 2) {
		print_usage();
		::exit(1);
	}

	if (!std::strcmp(argv[1], "start")) {
		if (UavcanNode::instance()) {
			// Already running, no error
			PX4_INFO("already started");
			::exit(0);
		}

		// Node ID
		int32_t node_id = 1;
		(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);

		if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
			PX4_ERR("Invalid Node ID %" PRId32, node_id);
			::exit(1);
		}

		// CAN bitrate
		int32_t bitrate = 1000000;
		(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);

		// Start
		PX4_INFO("Node ID %" PRIu32 ", bitrate %" PRIu32, node_id, bitrate);
		// 启动 UAVCAN
		int start_res = UavcanNode::start(node_id, bitrate);

		if (start_res == 0) {
		// 启动成功，再调用 TiltMotor 测试
		PX4_INFO("Starting TiltMotor test...");
		example_send_tilt();   // 调用测试函数
		}

		return start_res;
	}

3. 接线与测试

在uavcan_main.cpp中，有以下代码，其中can = new CanInitHelper(board_get_can_interfaces());，其会将系统中所有可用的can接口绑定到同一个 UavcanNode 上，使得两路 CAN 输出的数据完全一致。

UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
	if (_instance != nullptr) {
		PX4_WARN("Already started");
		return -1;
	}

	/*
	 * CAN driver init
	 * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
	 * shipped with libuavcan does not support deinitialization.
	 */
	static CanInitHelper *can = nullptr;

	if (can == nullptr) {

		can = new CanInitHelper(board_get_can_interfaces());

		if (can == nullptr) {                    // We don't have exceptions so bad_alloc cannot be thrown
			PX4_ERR("Out of memory");
			return -1;
		}
		
	...

因此，usb-can模块可接到任意can接口上。
在这里插入图片描述
通过Mavlink 控制台，当 uavcan start，can 接口每隔一段时间发送tilt.send_fixed_command(1, 1000); // motor_id=1, angle_data=1000，共发送5次。

uavcan start
uavcan stop
uavcan status

由于uavcan开机自启动，因此，需要先停止，再重新打开
在这里插入图片描述

使用CAN上位机，可以拿到数据，但是由于代码的通信协议是CAN，我强制使用UAVCAN输出，因此无法直接控制电机。

总结

本文只是简单在1.13固件上完成了新建一个uavcan消息，并且使用这个消息发送数据的测试例程，无法完成CAN电机的UAVCAN驱动。

目前并未找到很好的在飞控中使用can接口驱动传感器和执行器的方法，已经试过的方案

关掉 uavcan ，使用can驱动，但不是所有飞控均可以，板子有已经写好的驱动才可以，如 …\Firmware\boards\px4\fmu-v5x\src\can.c
一个接口uavcan，另一个接口 can，修改can = new CanInitHelper(board_get_can_interfaces()); 为can = new CanInitHelper(1); // 固定使用 CAN1;，确实可以分开can1 和can 2 口。但是 can2 的初始化无法完成（代码水平有限）。