UART串口-＞PX4飞控-＞QGC显示
Posted XXX_UUU_XXX
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了UART串口-＞PX4飞控-＞QGC显示相关的知识，希望对你有一定的参考价值。
目的：通过UART串口给PX4飞控发送信息，通过Mavlink协议在QGC界面上显示出来。
- PX4版本：1.11.0
- QGC版本：3.5.6
- QT版本：5.12.6
- Ubuntu：18.04
文件：mavlink/message_definitions/v1.0/read_sensor.xml
<?xml version="1.0"?>
<mavlink>
<version>3</version>
<messages>
<message id="223" name="READ_UART_SENSOR">
<description>Test all field types</description>
<field type="char[20]" name="voltage_str">string</field>
<field type="char[20]" name="current_str">string</field>
<field type="char[20]" name="temperature_str">string</field>
<field type="float" name="voltage_sensor">float</field>
<field type="float" name="current_sensor">float</field>
<field type="float" name="temperature_sensor">float</field>
</message>
</messages>
</mavlink>
根据示例：添加与编译一个mavlink（test.xml）创建文件mavlink/read_sensor/mavlink_msg_read_uart_sensor.h，并将该文件放到Firmware/mavlink/include/mavlink/v2.0/common目录下，在common.h中添加
//第2113行
#include "./mavlink_msg_read_uart_sensor.h"
修改 Firmware/src/modules/mavlink/mavlink_messages.cpp文件，添加类
/* read_uart_sensor */
class MavlinkStreamread_uart_sensor : public MavlinkStream
{
public:
void update(orb_advert_t *mavlink_log_pub);
const char *get_name() const override
{
return MavlinkStreamread_uart_sensor::get_name_static();
}
static constexpr const char *get_name_static()
{
return "READ_UART_SENSOR";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_READ_UART_SENSOR;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamread_uart_sensor(mavlink);
}
unsigned get_size() override
{
return MAVLINK_MSG_ID_READ_UART_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
// MavlinkOrbSubscription *_read_uart_sensor_sub;
uORB::Subscription _read_uart_sensor_sub{ORB_ID(read_uart_sensor)};
// uint64_t _read_uart_sensor_time;
/* do not allow top copying this class */
MavlinkStreamread_uart_sensor(MavlinkStreamread_uart_sensor &) = delete;
MavlinkStreamread_uart_sensor &operator = (const MavlinkStreamread_uart_sensor &) = delete;
protected:
explicit MavlinkStreamread_uart_sensor(Mavlink *mavlink) : MavlinkStream(mavlink)
// ,
// _read_uart_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(read_uart_sensor))),
// _read_uart_sensor_time(0)
{}
~MavlinkStreamread_uart_sensor()
{}
bool send(const hrt_abstime t) override
{
read_uart_sensor_s orbtest = {};
// if( _read_uart_sensor_sub->update(&_read_uart_sensor_time,&orbtest)){
if(_read_uart_sensor_sub.update(&orbtest)){
__mavlink_read_uart_sensor_t msg = {};
msg.voltage_sensor = orbtest.voltage_sensor;
msg.current_sensor = orbtest.current_sensor;
msg.temperature_sensor = orbtest.temperature_sensor;
// char a1[20];
// char b1[20];
// char c1[20];
// sprintf(a1,"%s",msg.voltage_str);
// sprintf(b1,"%s",msg.current_str);
// sprintf(c1,"%s",msg.temperature_str);
mavlink_msg_read_uart_sensor_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
//第5334行
static const StreamListItem streams_list[] = {
create_stream_list_item<MavlinkStreamread_uart_sensor>(),//添加一行创建流列表修改Firmware/src/modules/mavlink/mavlink_main.cpp文件，设置频率
configure_stream("READ_UART_SENSOR", 20.0f);文件：src/modules/read_uart_sensor/read_uart_sensor.c
#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <string.h>
#include <systemlib/err.h>
#include <nuttx/config.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/read_uart_sensor.h>
#include <px4_platform_common/tasks.h>
static int daemon_task;
static bool thread_should_exit = true;
static bool thread_running = false;
__EXPORT int read_uart_sensor_main(int argc, char *argv[]);
int read_uart_thread_main(int argc, char *argv[]);
static void usage(const char *reason);
static int uart_init(char * uart_name);
static int set_uart_baudrate(const int fd, unsigned int baud);
int set_uart_baudrate(const int fd, unsigned int baud)
{
int speed;
switch (baud) {
case 9600: speed = 9600; break;
case 19200: speed = 19200; break;
case 38400: speed = 38400; break;
case 57600: speed = 57600; break;
case 115200: speed = 115200; break;
default:
warnx("ERR: baudrate: %d\\n", baud);
return -EINVAL;
}
struct termios uart_config;
/*
termios 函数族提供了一个常规的终端接口，用于控制非同步通信端口。 这个结构包含了至少下列成员：
tcflag_t c_iflag; 输入模式
tcflag_t c_oflag; 输出模式
tcflag_t c_cflag; 控制模式
tcflag_t c_lflag; 本地模式
cc_t c_cc[NCCS]; 控制字符
*/
int termios_state;
/* 以新的配置填充结构体 */
/* 设置某个选项，那么就使用"|="运算，
* 如果关闭某个选项就使用"&="和"~"运算
* */
tcgetattr(fd, &uart_config); // 获取终端参数
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;// 将NL转换成CR(回车)-NL后输出。
/* 无偶校验，一个停止位 */
uart_config.c_cflag &= ~(CSTOPB | PARENB);// CSTOPB 使用两个停止位，PARENB 表示偶校验
/* 设置波特率 */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
warnx("ERR: %d (cfsetispeed)\\n", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERR: %d (cfsetospeed)\\n", termios_state);
return false;
}
// 设置与终端相关的参数，TCSANOW 立即改变参数
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR: %d (tcsetattr)\\n", termios_state);
return false;
}
return true;
}
int uart_init(char * uart_name)
{
int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
// printf("serial_fd = %d", serial_fd);
if (serial_fd < 0)
{
err(1, "failed to open port: %s", uart_name);
return false;
}
return serial_fd;
}
static void usage(const char *reason)
{
if (reason) {
fprintf(stderr, "%s\\n", reason);
}
fprintf(stderr, "usage: read_uart_sensor {start|stop|status} [param]\\n\\n");
exit(1);
}
int read_uart_sensor_main(int argc, char *argv[])
{
// printf("argv_0 = %s,argv_1 = %s, argv_2 = %s",argv[0],argv[1], argv[2]);
if (argc < 2)
{
usage("missing command");
}
if (!strcmp(argv[1], "start")){
if (thread_running){
warnx("already running\\n");
// exit(0);
return 0;
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("read_uart_sensor",//任务接口句柄
SCHED_DEFAULT,
// SCHED_PRIORITY_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
read_uart_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
// exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")){
thread_should_exit = true;
// exit(0);
return 0;
}
if (!strcmp(argv[1], "status")){
if (thread_running)
{
warnx("running");
// return 0;//
} else
{
warnx("stopped");
// return 1;//
}
return 0;
}
usage("unrecognized command");
PX4_INFO("exiting");
return 1;
}
int read_uart_thread_main(int argc,char *argv[])
{
int index = 0;
char data = '0';
char buffer[20] = "";
const char douhao[1] = ","; //逗号
char *result = NULL;
/*
* TELEM1 : /dev/ttyS1
* TELEM2 : /dev/ttyS2
* GPS : /dev/ttyS3
* NSH : /dev/ttyS5
* SERIAL4: /dev/ttyS6
* N/A : /dev/ttyS4
* IO DEBUG (RX only):/dev/ttyS0
*/
// int uart_read = uart_init(uart_name);
int uart_read = uart_init("/dev/ttyS3");
if(false == uart_read){
printf("read uart failed\\n");
return -1;
}
if(false == set_uart_baudrate(uart_read,115200))
{
printf("set_uart_baudrate is failed\\n");
return -1;
}
printf("uart init is successful\\n");
thread_running = true;
/*初始化数据结构体 */
struct read_uart_sensor_s orbtest;
memset(&orbtest,0,sizeof(orbtest));
//公告，公告必须在发布之前，否则即使订阅也得不到任何数据
orb_advert_t pub_fd = orb_advertise(ORB_ID(read_uart_sensor),&orbtest);
while (!thread_should_exit){
read(uart_read,&data,1);
// printf("data = %s\\n", data); 不能打印data，打印会崩溃
/* 例：串口输入R14.68,10.23,25.67, 输出14.68,10.23,25.67, */
if(data == 'R'){
for(int i = 0;i <17;++i){
read(uart_read,&data,1);
buffer[i] = data;
data = '0';
}
//逗号分割,返回下一个分割后的字符串指针，如果没有可检索的字符串，则返回一个空指针。
result = strtok(buffer, douhao);
while(result != NULL) {
index++;
switch(index){
case 1:
printf("case 1\\n");
//定义的数组不够大越界操作NULL被覆盖掉，每个数组起码加大1容纳NULL结尾
// strncpy(orbtest.voltage_str, result, 5);//5+1=6
strncpy(orbtest.voltage_str, result, 6);
break;
case 2:
printf("case 2\\n");
strncpy(orbtest.current_str, result, 6);
break;
case 3:
printf("case 3\\n");
strncpy(orbtest.temperature_str, result, 6);
break;
}
printf("result = %s\\n", result);
result = strtok(NULL, douhao);
}
index = 0;
//指定长度的字符串复制到字符数组中
// strncpy(orbtest.datastr, buffer, 19);
//atoi()把字符串转换成整型数
// atof()把字符串转换成浮点数,默认为6位小数
orbtest.voltage_sensor = atof(orbtest.voltage_str);
orbtest.current_sensor = atof(orbtest.current_str);
orbtest.temperature_sensor = atof(orbtest.temperature_str);
//发布
orb_publish(ORB_ID(read_uart_sensor), pub_fd, &orbtest);
// PX4_INFO("orbtest.voltage_str = %s", orbtest.voltage_str);
// PX4_INFO("orbtest.current_str = %s", orbtest.current_str);
// PX4_INFO("orbtest.temperature_str = %s", orbtest.temperature_str);
printf("orbtest.voltage_sensor = %.2f\\r\\n", (double)orbtest.voltage_sensor);
printf("orbtest.current_sensor = %.2f\\r\\n", (double)orbtest.current_sensor);
printf("orbtest.temperature_sensor = %.2f\\r\\n", (double)orbtest.temperature_sensor);
}
}
warnx("exiting");
thread_running = false;
close(uart_read);
fflush(stdout);
return 0;
}文件：src/modules/read_uart_sensor/CMakeLists.txt
set(MODULE_CFLAGS)
px4_add_module(
MODULE modules_read_uart_sensor
MAIN read_uart_sensor
COMPILE_FLAGS
-Os
SRCS
read_uart_sensor.c
DEPENDS
)文件：Firmware/mavlink/inclde/mavlink/v2.0/standard/standard.h
至此无人机设置完毕，下面设置QGC地面站的Qt程序
将mavlink_msg_read_uart_sensor.h放置到qgroundcontrol/libs/mavlink/include/mavlink/v2.0/common/目录下
文件：qgroundcontrol/libs/mavlink/include/mavlink/v2.0/common/common.h
//第1428行
// MESSAGE DEFINITIONS
#include "./mavlink_msg_read_uart_sensor.h"
文件：qgroundcontrol/libs/mavlink/include/mavlink/v2.0/ardupilotmega/ardupilotmega.h
//第27行
#define MAVLINK_MESSAGE_CRCS //添加{223, 70, 72, 0, 0, 0},同理飞控的standard.h文件：qgroundcontrol/src/ui/toolbar/BatteryIndicator.qml
//第99行
QGCLabel {text: qsTr("BMS_voltage:") }
QGCLabel {text:(_activeVehicle && _activeVehicle.battery.BMS_voltage.value != -1) ? (_activeVehicle.battery.BMS_voltage.valueString + " " + _activeVehicle.battery.BMS_voltage.units) : "N/A" }
文件：qgroundcontrol/src/Vehicle/Vehicle.h
class VehicleBatteryFactGroup : public FactGroup
{
Q_OBJECT
public:
VehicleBatteryFactGroup(QObject* parent = nullptr);
//add BMS voltage, current, temperature
//******************************************************************
Q_PROPERTY(Fact* BMS_voltage READ BMS_voltage CONSTANT)
Q_PROPERTY(Fact* BMS_current READ BMS_current CONSTANT)
Q_PROPERTY(Fact* BMS_temperature READ BMS_temperature CONSTANT)
//******************************************************************
Q_PROPERTY(Fact* voltage READ voltage CONSTANT)
Q_PROPERTY(Fact* percentRemaining READ percentRemaining CONSTANT)
Q_PROPERTY(Fact* mahConsumed READ mahConsumed CONSTANT)
Q_PROPERTY(Fact* current READ current CONSTANT)
Q_PROPERTY(Fact* temperature READ temperature CONSTANT)
Q_PROPERTY(Fact* cellCount READ cellCount CONSTANT)
Q_PROPERTY(Fact* instantPower READ instantPower CONSTANT)
Q_PROPERTY(Fact* timeRemaining READ timeRemaining CONSTANT)
Q_PROPERTY(Fact* chargeState READ chargeState CONSTANT)
//******************************************************************
Fact* BMS_voltage (void) { return &_BMS_voltageFact; }
Fact* BMS_current (void) { return &_BMS_currentFact; }
Fact* BMS_temperature (void) { return &_BMS_temperatureFact; }
//******************************************************************
Fact* voltage (void) { return &_voltageFact; }
Fact* percentRemaining (void) { return &_percentRemainingFact; }
Fact* mahConsumed (void) { return &_mahConsumedFact; }
Fact* current (void) { return &_currentFact; }
Fact* temperature (void) { return &_temperatureFact; }
Fact* cellCount (void) { return &_cellCountFact; }
Fact* instantPower (void) { return &_instantPowerFact; }
Fact* timeRemaining (void) { return &_timeRemainingFact; }
Fact* chargeState (void) { return &_chargeStateFact; }
//******************************************************************
static const char* _BMS_voltageFactName;
static const char* _BMS_currentFactName;
static const char* _BMS_temperatureFactName;
//******************************************************************
static const char* _voltageFactName;
static const char* _percentRemainingFactName;
static const char* _mahConsumedFactName;
static const char* _currentFactName;
static const char* _temperatureFactName;
static const char* _cellCountFactName;
static const char* _instantPowerFactName;
static const char* _timeRemainingFactName;
static const char* _chargeStateFactName;
static const char* _settingsGroup;
//******************************************************************
static const double _BMS_voltageUnavailable;
static const int _BMS_currentUnavailable;
static const int _BMS_temperatureUnavailable;
//******************************************************************
static const double _voltageUnavailable;
static const int _percentRemainingUnavailable;
static const int _mahConsumedUnavailable;
static const int _currentUnavailable;
static const double _temperatureUnavailable;
static const int _cellCountUnavailable;
static const double _instantPowerUnavailable;
private:
//******************************************************************
Fact _BMS_voltageFact;
Fact _BMS_currentFact;
Fact _BMS_temperatureFact;
//******************************************************************
Fact _voltageFact;
Fact _percentRemainingFact;
Fact _mahConsumedFact;
Fact _currentFact;
Fact _temperatureFact;
Fact _cellCountFact;
Fact _instantPowerFact;
Fact _timeRemainingFact;
Fact _chargeStateFact;
};
文件：qgroundcontrol/src/Vehicle/Vehicle.cc
//第786行
case MAVLINK_MSG_ID_READ_UART_SENSOR:
_handleReadUartSensor(message);
break;//第1050行
void Vehicle::_handleReadUartSensor(mavlink_message_t& message)
{
mavlink_read_uart_sensor_t readUartSensor;
mavlink_msg_read_uart_sensor_decode(&message, &readUartSensor);
VehicleBatteryFactGroup& batteryFactGroup = _battery1FactGroup;
if (readUartSensor.voltage_sensor == -1) {
batteryFactGroup.BMS_voltage()->setRawValue(VehicleBatteryFactGroup::_BMS_voltageUnavailable);
} else {
batteryFactGroup.BMS_voltage()->setRawValue(readUartSensor.voltage_sensor);
}
qDebug() << "Receive read_uart_sensor_t" << readUartSensor.voltage_sensor;
}//第3877行
//******************************************************************
const char* VehicleBatteryFactGroup::_BMS_voltageFactName = "BMS_voltage";
const char* VehicleBatteryFactGroup::_BMS_currentFactName = "BMS_current";
const char* VehicleBatteryFactGroup::_BMS_temperatureFactName = "BMS_temperature";
//******************************************************************
const char* VehicleBatteryFactGroup::_voltageFactName = "voltage";
const char* VehicleBatteryFactGroup::_percentRemainingFactName = "percentRemaining";
const char* VehicleBatteryFactGroup::_mahConsumedFactName = "mahConsumed";
const char* VehicleBatteryFactGroup::_currentFactName = "current";
const char* VehicleBatteryFactGroup::_temperatureFactName = "temperature";
const char* VehicleBatteryFactGroup::_cellCountFactName = "cellCount";
const char* VehicleBatteryFactGroup::_instantPowerFactName = "instantPower";
const char* VehicleBatteryFactGroup::_timeRemainingFactName = "timeRemaining";
const char* VehicleBatteryFactGroup::_chargeStateFactName = "chargeState";
const char* VehicleBatteryFactGroup::_settingsGroup = "Vehicle.battery";
//******************************************************************
const double VehicleBatteryFactGroup::_BMS_voltageUnavailable = -1.0;
const int VehicleBatteryFactGroup::_BMS_currentUnavailable = -1.0;
const int VehicleBatteryFactGroup::_BMS_temperatureUnavailable = -1.0;
//******************************************************************
const double VehicleBatteryFactGroup::_voltageUnavailable = -1.0;
const int VehicleBatteryFactGroup::_percentRemainingUnavailable = -1;
const int VehicleBatteryFactGroup::_mahConsumedUnavailable = -1;
const int VehicleBatteryFactGroup::_currentUnavailable = -1;
const double VehicleBatteryFactGroup::_temperatureUnavailable = -1.0;
const int VehicleBatteryFactGroup::_cellCountUnavailable = -1.0;
const double VehicleBatteryFactGroup::_instantPowerUnavailable = -1.0;
VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent)
//******************************************************************
, _BMS_voltageFact (0, _BMS_voltageFactName, FactMetaData::valueTypeDouble)
, _BMS_currentFact (0, _BMS_currentFactName, FactMetaData::valueTypeDouble)
, _BMS_temperatureFact (0, _BMS_temperatureFactName, FactMetaData::valueTypeDouble)
//******************************************************************
, _voltageFact (0, _voltageFactName, FactMetaData::valueTypeDouble)
, _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeInt32)
, _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeInt32)
, _currentFact (0, _currentFactName, FactMetaData::valueTypeFloat)
, _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble)
, _cellCountFact (0, _cellCountFactName, FactMetaData::valueTypeInt32)
, _instantPowerFact (0, _instantPowerFactName, FactMetaData::valueTypeFloat)
, _timeRemainingFact (0, _timeRemainingFactName, FactMetaData::valueTypeInt32)
, _chargeStateFact (0, _chargeStateFactName, FactMetaData::valueTypeUint8)
{
//******************************************************************
_addFact(&_BMS_voltageFact, _BMS_voltageFactName);
_addFact(&_BMS_currentFact, _BMS_currentFactName);
_addFact(&_BMS_temperatureFact, _BMS_temperatureFactName);
//******************************************************************
_addFact(&_voltageFact, _voltageFactName);
_addFact(&_percentRemainingFact, _percentRemainingFactName);
_addFact(&_mahConsumedFact, _mahConsumedFactName);
_addFact(&_currentFact, _currentFactName);
_addFact(&_temperatureFact, _temperatureFactName);
_addFact(&_cellCountFact, _cellCountFactName);
_addFact(&_instantPowerFact, _instantPowerFactName);
_addFact(&_timeRemainingFact, _timeRemainingFactName);
_addFact(&_chargeStateFact, _chargeStateFactName);
// Start out as not available
//******************************************************************
_BMS_voltageFact.setRawValue (_BMS_voltageUnavailable);
_BMS_currentFact.setRawValue (_BMS_currentUnavailable);
_BMS_temperatureFact.setRawValue (_BMS_temperatureUnavailable);
//******************************************************************
_voltageFact.setRawValue (_voltageUnavailable);
_percentRemainingFact.setRawValue (_percentRemainingUnavailable);
_mahConsumedFact.setRawValue (_mahConsumedUnavailable);
_currentFact.setRawValue (_currentUnavailable);
_temperatureFact.setRawValue (_temperatureUnavailable);
_cellCountFact.setRawValue (_cellCountUnavailable);
_instantPowerFact.setRawValue (_instantPowerUnavailable);
}演示效果：
以上是关于UART串口-＞PX4飞控-＞QGC显示的主要内容，如果未能解决你的问题，请参考以下文章
PX4APM无人机仿真连接QGC地面站记录（udp连接更改home点等）
PX4APM无人机仿真连接QGC地面站记录（udp连接更改home点等）