Pixhawk原生固件PX4之串口添加读取传感器实现
本博客承接前一篇,对FreeApe的串口添加超声波传感器博文后半部分进行学习。
为什么叫前奏呢,因为用了伪传感器,把单片机用串口发送的有规律的数据当作了传感器读取到的数据。但是无碍的。
开发环境:Ubuntu Firmware 1.4.1
在无人机运行时,首先是要将应用随系统启动时就启动起来的,且将获得的超声波数据不断的发布出去,从而让其他应用得以订阅使用。这里也使用Pixhawk里面的通用模式,即主线程,检测app命令输入,创建一个线程来不断的发布数据。
定义主题和发布主题
- 在
Firmware/msg
目录下新建read_uart_sensor.msg
文件。传送门
char[4] datastr
int16 data#TOPICS read_uart_sensor
- 1
- 2
- 3
- 4
- 5
并添加到CMakeLists.txt中,编译后自动生成uORB/topics/read_uart_sensor.h
头文件
串口读取例程
在Firmware/src/modules
目录下新建文件夹read_uart_sensor
- 添加文件
read_uart_sensor.c
。传送门
/** read_uart_sensor.c* * read sensor through uart*/#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 <systemlib/systemlib.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <uORB/topics/read_uart_sensor.h>__EXPORT int read_uart_sensor_main(int argc, char *argv[]);static bool thread_should_exit = false; /*Ddemon exit flag*/
static bool thread_running = false; /*Daemon status flag*/
static int daemon_task;/*** Main loop*/
int read_uart_thread_main(int argc, char *argv[]);static int uart_init(const char * uart_name);
static int set_uart_baudrate(const int fd, unsigned int baud);
static void usage(const char *reason);int set_uart_baudrate(const int fd, unsigned int baud)
{int speed;switch (baud) {case 9600: speed = B9600; break;case 19200: speed = B19200; break;case 38400: speed = B38400; break;case 57600: speed = B57600; break;case 115200: speed = B115200; break;default:warnx("ERR: baudrate: %d\n", baud);return -EINVAL;}struct termios uart_config;int termios_state;/* fill the struct for the new configuration */tcgetattr(fd, &uart_config);/* clear ONLCR flag (which appends a CR for every LF) */uart_config.c_oflag &= ~ONLCR;/* no parity, one stop bit */uart_config.c_cflag &= ~(CSTOPB | PARENB);/* set baud rate */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;}if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {warnx("ERR: %d (tcsetattr)\n", termios_state);return false;}return true;
}int uart_init(const char * uart_name)
{int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);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[])
{if (argc < 2) {usage("[Fantasy]missing command");}if (!strcmp(argv[1], "start")) {if (thread_running) {warnx("[Fantasy]already running\n");return 0;}thread_should_exit = false;daemon_task = px4_task_spawn_cmd("read_uart_sensor",SCHED_DEFAULT,SCHED_PRIORITY_MAX - 5,2000,read_uart_thread_main,(argv) ? (char * const *)&argv[2] : (char * const *)NULL);return 0;}if (!strcmp(argv[1], "stop")) {thread_should_exit = true;return 0;}if (!strcmp(argv[1], "status")) {if (thread_running) {warnx("[Fantasy]running");return 0;} else {warnx("[Fantasy]stopped");return 1;}return 0;}usage("unrecognized command");return 1;
}int read_uart_thread_main(int argc, char *argv[])
{if (argc < 2) {errx(1, "[Fantasy]need a serial port name as argument");usage("eg:");}const char *uart_name = argv[1];warnx("[Fantasy]opening port %s", uart_name);char data = '0';char buffer[4] = "";/** 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);if(false == uart_read)return -1;if(false == set_uart_baudrate(uart_read,9600)){printf("[Fantasy]set_uart_baudrate is failed\n");return -1;}printf("[Fantasy]uart init is successful\n");thread_running = true;/*初始化数据结构体 */struct read_uart_sensor_s sensordata;memset(&sensordata, 0, sizeof(sensordata));/* 公告主题 */orb_advert_t read_uart_sensor_pub = orb_advertise(ORB_ID(read_uart_sensor), &sensordata);while(!thread_should_exit){read(uart_read,&data,1);if(data == 'R'){for(int i = 0;i <4;++i){read(uart_read,&data,1);buffer[i] = data;data = '0';}strncpy(sensordata.datastr,buffer,4);// 复制字符串Buffer中前4个数字到Datastr中sensordata.data = atoi(sensordata.datastr);//将字符串转换成整形数据//printf("[Fantasy]sensor.data=%d\n",sensordata.data);orb_publish(ORB_ID(read_uart_sensor), read_uart_sensor_pub, &sensordata);}}warnx("[Fantasy]exiting");thread_running = false;close(uart_read);fflush(stdout);return 0;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 添加
CMakeLists.txt
文件
set(MODULE_CFLAGS)
px4_add_module(MODULE modules__read_uart_sensorMAIN read_uart_sensorCOMPILE_FLAGS-OsSRCSread_uart_sensor.cDEPENDSplatforms__common)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 在
Firmware/cmake/configs/nuttx/nuttx_px4fmu-v2_default.cmake
中注册该模块
测试发布的主题
- 测试可以随便一个启动的app中进行主题订阅,然后将订阅的数据打印出来,看是否是超声波的数据。这里新建一个应用px4_test进行测试。
px4_test.c
/** px4_test.c* * test the uart sensor app*/
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>#include <uORB/uORB.h>
#include <uORB/topics/read_uart_sensor.h>__EXPORT int px4_test_main(int argc, char *argv[]);int px4_test_main(int argc, char *argv[])
{PX4_INFO("Hello Sky!\n");/* subscribe to rw_uart_sensor topic */int sensor_sub_fd = orb_subscribe(ORB_ID(read_uart_sensor));/*limit the update rate to 20 Hz */orb_set_interval(sensor_sub_fd, 50);/* one could wait for multiple topics with this technique, just using one here */px4_pollfd_struct_t fds[] = {{ .fd = sensor_sub_fd, .events = POLLIN },/* there could be more file descriptors here, in the form like:* { .fd = other_sub_fd, .events = POLLIN },*/};int error_counter = 0;for (int i = 0; ; i++) { // infinite loop/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */int poll_ret = poll(fds, 1, 1000);/* handle the poll result */if (poll_ret == 0) {/* this means none of our providers is giving us data */printf("[px4_test] Got no data within a second\n");} else if (poll_ret < 0) {/* this is seriously bad - should be an emergency */if (error_counter < 10 || error_counter % 50 == 0) {/* use a counter to prevent flooding (and slowing us down) */printf("[px4_test] ERROR return value from poll(): %d\n", poll_ret);}error_counter++;} else {if (fds[0].revents & POLLIN) {/* obtained data for the first file descriptor */struct read_uart_sensor_s raw;/* copy sensors raw data into local buffer */orb_copy(ORB_ID(read_uart_sensor), sensor_sub_fd, &raw);PX4_INFO("[px4_test] sensor data:\t%s\t%d\n",raw.datastr,raw.data);}/* there could be more file descriptors here, in the form like:* if (fds[1..n].revents & POLLIN) {}*/}}PX4_INFO("exiting");return 0;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
编译并上传固件
- make px4fmu-v2_default upload
在NSH中测试
- read_uart_sensor start /dev/ttyS2
- px4_test
这个大有搞头!
添加到脚本文件
在rcS中仿照mavlink的启动方式添加了上面的应用,使得与ttyS2连接的外设默认为启动状态
现在如果ttyS2上连接了设备,就是自动启动的,在nsh中可以直接调用px4_test
应用就可以读取数据了,与使用px4_simple_app读取内部传感器的方式无异。
大致模型就是这样了,接下来就是具体的代码优化以及应用了。
By Fantasy
Pixhawk原生固件PX4之串口添加读取传感器实现相关推荐
- Windows / Ubuntu操作系统下Pixhawk原生固件PX4的编译方法
欢迎交流~ 个人 Gitter 交流平台,点击直达: 更新于2017.3.13 FAQ 本文说明针对 PX4 Firmware 1.6.0 问题 1: 找不到python jinja2模块 CMake ...
- Pixhawk原生固件PX4之常用函数解读
PX4Firmware 经常有人将Pixhawk.PX4.APM还有ArduPilot弄混.这里首先还是简要说明一下: Pixhawk是飞控硬件平台,PX4和ArduPilot都是开源的可以烧写到Pi ...
- Pixhawk原生固件PX4之添外置传感器MPU6500
欢迎交流~ 个人 Gitter 交流平台,点击直达: 成功的在Pixhawk上添加了一个自定义的传感器MPU6500. Pixhawk飞控板上空余出一个SPI4接口 提示: 多出来的GPIO_EXT引 ...
- Pixhawk原生固件PX4之HIL硬件在环仿真
欢迎交流~ 个人 Gitter 交流平台,点击直达: 硬件在环仿真可以接遥控器进行调试. 串口设置 下载minicom sudo apt-get install minicom 配置飞控的设备号(/d ...
- Pixhawk原生固件PX4之位姿控制算法解读
参考文献:Minimum Snap Trajectory Generation and Control for Quadrotors PX4中多旋翼无人机的控制分为姿态控制和位置控制两个部分. 大致解 ...
- Pixhawk原生固件PX4之顶层软件结构
下图所示为PX4的软件顶层架构,很重要. 左边是整个飞控系统的控制功能实现 用户通过地面站或者遥控器发出模式切换以及摇杆操作对飞行器进行控制,commander根据飞行器的当前状态对用户想要切换到的状 ...
- Pixhawk原生固件PX4之驱动ID
欢迎交流~ 个人 Gitter 交流平台,点击直达: 驱动ID PX4使用驱动ID将独立传感器贯穿于整个系统.这些ID存储于配置参数中,用于匹配传感器校正值,以及决定哪些传感器被记录到log中. 传感 ...
- pixhawk原生固件笔记
常用资料 名称 简介 官方文档 虽然是官方文档,但是很多地方并没有讲清楚 github issues 最全面的开发者问题集锦 被删除的官方文档(已被删除,此处为谷歌快照) 此教程详细介绍了如何自己实现 ...
- Pixhawk原生固件以往代码版本的下载
我们按照官网的下载方法只能下载到最新版本的代码,而GitHub上http://write.blog.csdn.net/postedit?type=edit包含了以往的各个版本: 若需要下载其他版本,该 ...
最新文章
- gst-crypto GStreamer插件
- Windows 7新睹为快!!
- CMSIS对异常和中断标识符、中断处理函数名以及中断向量异常号都有严格的要求。
- ajax联系人数,setInterval定时调用ajax实现在线人数统计
- Android listview addHeaderView 和 addFooterView 详解
- 不写代码,带你徒手开发一个健康打卡应用
- java按键修改_修改键位映射,打造自己的个性键盘 [转自赵翔鹏的Blog]
- 69.Daily Temperatures(日常气温)
- python核心装饰_《python核心编程》中高级闭包和装饰器理解?
- 解决两个WINDOWS10的卡死问题
- C#毕业设计——基于C#+asp.net+SQL server的房地产信息管理系统设计与实现(毕业论文+程序源码)——房地产信息管理系统
- 【转载】JPEG2000
- select()函数的作用
- 【英语语法入门】 第29讲 情态动词的否定和疑问
- 商业模式与盈利模式的区别
- 被误解的明朝——中国的文艺复兴(转)
- NLS(National Language Support)
- 同步挤压s变换matlab,同步挤压广义S变换信号时频分解与重构方法与流程
- 【M365运维】扩充OneDrive存储空间
- 金山云: mysql
热门文章
- 【Tool】Augmentor和imgaug——python图像数据增强库
- HDU1172猜数字 [模拟]
- MongoDB数据导入hbase + 代码
- 【杭州(含嘉兴,绍兴,金华,湖州,义乌)】Uber优步司机奖励政策(2月1日~2月7日)...
- 认识zookeeper
- kbengine定义实体
- 46 关于Linux的I/O重定向
- C03-Java同步实践加强班第5周上机任务
- 2020年第十一届蓝桥杯 - 国赛 - Python大学组 - G.重复字符串
- LeetCode Algorithm 811. 子域名访问计数