匿名地面站方便查看动态数据,而且便于调试系统参数,多用于基于单片机的系统调试。
由于项目需要基于ROS开发,考虑到系统参数调试工作量不小,便写了一个基于ROS的匿名地面站驱动。
之前使用ubuntu 16.04的时候,可以直接使用
$ sudo apt-get install ros-<版本号>-serial的命令安装基于ROS的串口工具包,如
sudo apt-get install ros-kinetic-serial
但是ubuntu 20.04和ros noetic似乎暂时没有将其集成,所以需要在工作空间内手动源码配置。
工作空间文件夹为 catkin_ws,在工作空间文件夹下的src内,放入serial的源码,命令如下
cd ~
cd catkin_ws/src
git clone https://github.com/wjwwood/serial.git
进入克隆的serial文件夹内,执行编译安装
cd serial
make
make install
其他功能包引用serial的头文件时,需要进行如下配置
(1)将serial文件夹下的CMakeLists.txt中的
## Include headers
include_directories(include ${catkin_INCLUDE_DIRS})
修改为
## Include headers
include_directories(include ${catkin_INCLUDE_DIRS})
(2)在其他功能包中的package.xml中添加
<build_depend>serial</build_depend>
<build_export_depend>serial</build_export_depend>
(3)在其他功能包中的CMakeLists.txt中的find语句里,添加serial的依赖,如
find_package(catkin REQUIRED COMPONENTS
roscpp
serial
# 。。。
)
并添加头文件路径
## Include headers
include_directories(include ${catkin_INCLUDE_DIRS})
#ifndef _APP_ANO_H_
#define _APP_ANO_H_
#include "serial/serial.h"
#include
#include
#define BYTE0(dwTemp) ( *( (uint8_t *)(&dwTemp) + 0) )
#define BYTE1(dwTemp) ( *( (uint8_t *)(&dwTemp) + 1) )
#define BYTE2(dwTemp) ( *( (uint8_t *)(&dwTemp) + 2) )
#define BYTE3(dwTemp) ( *( (uint8_t *)(&dwTemp) + 3) )
class SDK_ANO_TC_V4
{
public:
SDK_ANO_TC_V4()
{
state = 0;
_data_len = 0;
_data_cnt = 0;
send_pid_para = 0;
}
~SDK_ANO_TC_V4(){}
std::string m_port_name;
//std::string m_receive_from_ano_tc;
uint32_t state;
unsigned long m_baudrate;
uint8_t ano_data_to_send[100];
uint8_t ano_data_rec[100];
uint8_t _data_len, _data_cnt;
uint8_t send_pid_para;
struct ANO_CACHE{
float pid1_p, pid1_i, pid1_d;
float pid2_p, pid2_i, pid2_d;
float pid3_p, pid3_i, pid3_d;
float pid4_p, pid4_i, pid4_d;
float pid5_p, pid5_i, pid5_d;
float pid6_p, pid6_i, pid6_d;
float pid7_p, pid7_i, pid7_d;
float pid8_p, pid8_i, pid8_d;
float pid9_p, pid9_i, pid9_d;
float pid10_p, pid10_i, pid10_d;
float pid11_p, pid11_i, pid11_d;
float pid12_p, pid12_i, pid12_d;
float pid13_p, pid13_i, pid13_d;
float pid14_p, pid14_i, pid14_d;
float pid15_p, pid15_i, pid15_d;
float pid16_p, pid16_i, pid16_d;
float pid17_p, pid17_i, pid17_d;
float pid18_p, pid18_i, pid18_d;
};
struct ANO_CACHE ano_cache;
int8_t set_dev(serial::Serial* sp, const std::string& port_name, unsigned long baudrate)
{
serial::Timeout to = serial::Timeout::simpleTimeout(100);
sp->setPort(port_name);
sp->setBaudrate(baudrate);
sp->setTimeout(to);
m_port_name = port_name;
m_baudrate = baudrate;
return 0;
}
int8_t open_dev(serial::Serial* sp)
{
try
{
sp->open();
}
catch(serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port: " + m_port_name);
return -1;
}
if(sp->isOpen())
{
ROS_INFO_STREAM(m_port_name + " is opened.");
}
else
{
return -1;
}
}
int8_t close_dev(serial::Serial* sp)
{
sp->close();
ROS_INFO_STREAM(m_port_name + " is closed.");
return 0;
}
void send_15_data( //int16_t : -32768~+32767
serial::Serial* sp,
int16_t a_x, int16_t a_y, int16_t a_z,
int16_t g_x, int16_t g_y, int16_t g_z,
int16_t m_x, int16_t m_y, int16_t m_z,
float angle_rol, float angle_pit, float angle_yaw,
int32_t alt, uint8_t fly_model, uint8_t armed)
{
uint8_t _cnt = 0;
int16_t _temp;
ano_data_to_send[_cnt++] = 0xAA;
ano_data_to_send[_cnt++] = 0xAA;
ano_data_to_send[_cnt++] = 0x02;
ano_data_to_send[_cnt++] = 0;
_temp = a_x;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = a_y;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = a_z;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = g_x;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = g_y;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = g_z;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = m_x;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = m_y;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = m_z;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
/////////////////////////////////////////
_temp = 0;
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
ano_data_to_send[3] = _cnt - 4;
uint8_t sum = 0;
for (uint8_t i = 0; i < _cnt; i++)
sum += ano_data_to_send[i];
ano_data_to_send[_cnt++] = sum;
uint8_t _cnt2 = 0;
uint8_t _cnt3 = 0;
int32_t _temp2 = alt;
_cnt3 = _cnt;
ano_data_to_send[_cnt++] = 0xAA;
ano_data_to_send[_cnt++] = 0xAA;
ano_data_to_send[_cnt++] = 0x01;
_cnt2 = _cnt;
ano_data_to_send[_cnt++] = 0;
_temp = (int)(angle_rol * 100);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (int)(angle_pit * 100);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (int)(angle_yaw * 100);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
ano_data_to_send[_cnt++] = BYTE3(_temp2);
ano_data_to_send[_cnt++] = BYTE2(_temp2);
ano_data_to_send[_cnt++] = BYTE1(_temp2);
ano_data_to_send[_cnt++] = BYTE0(_temp2);
ano_data_to_send[_cnt++] = fly_model;
ano_data_to_send[_cnt++] = armed;
ano_data_to_send[_cnt2] = _cnt - _cnt2 - 1;
sum = 0;
for (uint8_t i = _cnt3; i < _cnt; i++)
sum += ano_data_to_send[i];
ano_data_to_send[_cnt++] = sum;
ano_data_to_send[_cnt++] = 0x0D;
ano_data_to_send[_cnt++] = 0x0A;
sp->write(ano_data_to_send, _cnt);
}
void receive_info_from_ano_tc(serial::Serial* sp)
{
uint8_t data;
if(sp->available()!=0)
{
sp->read(&data,1);
//ROS_INFO("REC: %x state: %d",data, state);
}
// Header 0xAA 0xAF
if(state == 0)
{
if(data == 0xAA)
{
state = 1;
ano_data_rec[0] = data;
//ROS_INFO("%x ",data);
//ROS_INFO("state : %d ", state);
}
else
{
state = 0;
}
return;
}
if(state == 1)
{
//ROS_INFO("HAHHAHA");
//ROS_INFO("REC: %x state: %d",data, state);
if(data == 0xAF)
{
state = 2;
ano_data_rec[1] = data;
//ROS_INFO("%x ",data);
//ROS_INFO("state : %d ", state);
}
else
{
state = 0;
}
return;
}
// ID
if(state == 2)
{
state = 3;
ano_data_rec[2] = data;
return;
}
// LENGTH
if(state == 3)
{
if(data<50)
{
state = 4;
ano_data_rec[3] = data;
_data_len = data;
_data_cnt = 0;
}
else
{
state = 0;
}
return;
}
//
if(state == 4 && _data_len>0)
{
_data_len--;
ano_data_rec[4+_data_cnt++] = data;
if(_data_len==0)
{
state = 5;
}
return;
}
if(state == 5)
{
state = 0;
ano_data_rec[4+_data_cnt] = data;
receive_dataparser(sp, ano_data_rec, _data_cnt + 5);
return;
}
}
void receive_dataparser(serial::Serial* sp, uint8_t* data_buf, uint8_t num)
{
uint8_t sum = 0;
for (uint8_t i = 0; i<(num - 1); i++)
sum += *(data_buf + i);
if (!(sum == *(data_buf + num - 1)))
{
return;
}
if (!(*(data_buf) == 0xAA && *(data_buf + 1) == 0xAF))
{
return;
}
if (*(data_buf + 2) == 0X01)
{
if (*(data_buf + 4) == 0X01)
{
//button for accelerometer calibration
ROS_INFO("button for accelerometer calibration");
}
if (*(data_buf + 4) == 0X02)
{
//button for gyroscope calibration
ROS_INFO("button for gyroscope calibration");
}
if (*(data_buf + 4) == 0X05)
{
//button for barometer calibration
ROS_INFO("button for barometer calibration");
}
if (*(data_buf + 4) == 0X04)
{
//button for magnetometer calibration
ROS_INFO("button for magnetometer calibration");
}
}
if (*(data_buf + 2) == 0X02)
{
if (*(data_buf + 4) == 0X01)
{
//button for ReadFromFlyBoard
if(send_pid_para == 0)
{
send_pid_para = 1;
}
send_pid_to_ano_tc(sp);
ROS_INFO("button for ReadFromFlyBoard");
}
if (*(data_buf + 4) == 0XA1)
{
//button for RestoreDefaults
ROS_INFO("button for RestoreDefaults");
}
}
//PID1
if (*(data_buf + 2) == 0X10)
{
ano_cache.pid1_p = 0.001f*((int)(*(data_buf + 4) << 8) | *(data_buf + 5));
ano_cache.pid1_i = 0.001f*((int)(*(data_buf + 6) << 8) | *(data_buf + 7));
ano_cache.pid1_d = 0.001f*((int)(*(data_buf + 8) << 8) | *(data_buf + 9));
ano_cache.pid2_p = 0.001f*((int)(*(data_buf + 10) << 8) | *(data_buf + 11));
ano_cache.pid2_i = 0.001f*((int)(*(data_buf + 12) << 8) | *(data_buf + 13));
ano_cache.pid2_d = 0.001f*((int)(*(data_buf + 14) << 8) | *(data_buf + 15));
ano_cache.pid3_p = 0.001f*((int)(*(data_buf + 16) << 8) | *(data_buf + 17));
ano_cache.pid3_i = 0.001f*((int)(*(data_buf + 18) << 8) | *(data_buf + 19));
ano_cache.pid3_d = 0.001f*((int)(*(data_buf + 20) << 8) | *(data_buf + 21));
ano_dt_send_check(sp, *(data_buf + 2), sum);
ROS_INFO("PID1 update");
}
//PID2
if (*(data_buf + 2) == 0X11)
{
ano_cache.pid4_p = 0.001f*((int)(*(data_buf + 4) << 8) | *(data_buf + 5));
ano_cache.pid4_i = 0.001f*((int)(*(data_buf + 6) << 8) | *(data_buf + 7));
ano_cache.pid4_d = 0.001f*((int)(*(data_buf + 8) << 8) | *(data_buf + 9));
ano_cache.pid5_p = 0.001f*((int)(*(data_buf + 10) << 8) | *(data_buf + 11));
ano_cache.pid5_i = 0.001f*((int)(*(data_buf + 12) << 8) | *(data_buf + 13));
ano_cache.pid5_d = 0.001f*((int)(*(data_buf + 14) << 8) | *(data_buf + 15));
ano_cache.pid6_p = 0.001f*((int)(*(data_buf + 16) << 8) | *(data_buf + 17));
ano_cache.pid6_i = 0.001f*((int)(*(data_buf + 18) << 8) | *(data_buf + 19));
ano_cache.pid6_d = 0.001f*((int)(*(data_buf + 20) << 8) | *(data_buf + 21));
ano_dt_send_check(sp, *(data_buf + 2), sum);
}
//PID3
if (*(data_buf + 2) == 0X12)
{
ano_cache.pid7_p = 0.001f*((int)(*(data_buf + 4) << 8) | *(data_buf + 5));
ano_cache.pid7_i = 0.001f*((int)(*(data_buf + 6) << 8) | *(data_buf + 7));
ano_cache.pid7_d = 0.001f*((int)(*(data_buf + 8) << 8) | *(data_buf + 9));
ano_cache.pid8_p = 0.001f*((int)(*(data_buf + 10) << 8) | *(data_buf + 11));
ano_cache.pid8_i = 0.001f*((int)(*(data_buf + 12) << 8) | *(data_buf + 13));
ano_cache.pid8_d = 0.001f*((int)(*(data_buf + 14) << 8) | *(data_buf + 15));
ano_cache.pid9_p = 0.001f*((int)(*(data_buf + 16) << 8) | *(data_buf + 17));
ano_cache.pid9_i = 0.001f*((int)(*(data_buf + 18) << 8) | *(data_buf + 19));
ano_cache.pid9_d = 0.001f*((int)(*(data_buf + 20) << 8) | *(data_buf + 21));
ano_dt_send_check(sp, *(data_buf + 2), sum);
}
//PID4
if (*(data_buf + 2) == 0X13)
{
ano_cache.pid10_p = 0.001f*((int)(*(data_buf + 4) << 8) | *(data_buf + 5));
ano_cache.pid10_i = 0.001f*((int)(*(data_buf + 6) << 8) | *(data_buf + 7));
ano_cache.pid10_d = 0.001f*((int)(*(data_buf + 8) << 8) | *(data_buf + 9));
ano_cache.pid11_p = 0.001f*((int)(*(data_buf + 10) << 8) | *(data_buf + 11));
ano_cache.pid11_i = 0.001f*((int)(*(data_buf + 12) << 8) | *(data_buf + 13));
ano_cache.pid11_d = 0.001f*((int)(*(data_buf + 14) << 8) | *(data_buf + 15));
ano_cache.pid12_p = 0.001f*((int)(*(data_buf + 16) << 8) | *(data_buf + 17));
ano_cache.pid12_i = 0.001f*((int)(*(data_buf + 18) << 8) | *(data_buf + 19));
ano_cache.pid12_d = 0.001f*((int)(*(data_buf + 20) << 8) | *(data_buf + 21));
ano_dt_send_check(sp, *(data_buf + 2), sum);
}
//PID5
if (*(data_buf + 2) == 0X14)
{
ano_cache.pid13_p = 0.001f*((int)(*(data_buf + 4) << 8) | *(data_buf + 5));
ano_cache.pid13_i = 0.001f*((int)(*(data_buf + 6) << 8) | *(data_buf + 7));
ano_cache.pid13_d = 0.001f*((int)(*(data_buf + 8) << 8) | *(data_buf + 9));
ano_cache.pid14_p = 0.001f*((int)(*(data_buf + 10) << 8) | *(data_buf + 11));
ano_cache.pid14_i = 0.001f*((int)(*(data_buf + 12) << 8) | *(data_buf + 13));
ano_cache.pid14_d = 0.001f*((int)(*(data_buf + 14) << 8) | *(data_buf + 15));
ano_cache.pid15_p = 0.001f*((int)(*(data_buf + 16) << 8) | *(data_buf + 17));
ano_cache.pid15_i = 0.001f*((int)(*(data_buf + 18) << 8) | *(data_buf + 19));
ano_cache.pid15_d = 0.001f*((int)(*(data_buf + 20) << 8) | *(data_buf + 21));
ano_dt_send_check(sp, *(data_buf + 2), sum);
}
//PID6
if (*(data_buf + 2) == 0X15)
{
ano_cache.pid16_p = 0.001f*((int)(*(data_buf + 4) << 8) | *(data_buf + 5));
ano_cache.pid16_i = 0.001f*((int)(*(data_buf + 6) << 8) | *(data_buf + 7));
ano_cache.pid16_d = 0.001f*((int)(*(data_buf + 8) << 8) | *(data_buf + 9));
ano_cache.pid17_p = 0.001f*((int)(*(data_buf + 10) << 8) | *(data_buf + 11));
ano_cache.pid17_i = 0.001f*((int)(*(data_buf + 12) << 8) | *(data_buf + 13));
ano_cache.pid17_d = 0.001f*((int)(*(data_buf + 14) << 8) | *(data_buf + 15));
ano_cache.pid18_p = 0.001f*((int)(*(data_buf + 16) << 8) | *(data_buf + 17));
ano_cache.pid18_i = 0.001f*((int)(*(data_buf + 18) << 8) | *(data_buf + 19));
ano_cache.pid18_d = 0.001f*((int)(*(data_buf + 20) << 8) | *(data_buf + 21));
ano_dt_send_check(sp, *(data_buf + 2), sum);
}
}
void ano_dt_send_check(serial::Serial* sp, uint8_t head, uint8_t check_sum)
{
ano_data_to_send[0] = 0xAA;
ano_data_to_send[1] = 0xAA;
ano_data_to_send[2] = 0xEF;
ano_data_to_send[3] = 2;
ano_data_to_send[4] = head;
ano_data_to_send[5] = check_sum;
uint8_t sum = 0;
for (uint8_t i = 0; i<6; i++)
sum += ano_data_to_send[i];
ano_data_to_send[6] = sum;
//ano_usart_send_data(ano_data_to_send, 7);
sp->write(ano_data_to_send, 7);
}
void send_pid_to_ano_tc(serial::Serial* sp)
{
if (send_pid_para)
{
if (send_pid_para == 1)
{
ano_dt_send_pid(sp, 1, ano_cache.pid1_p, ano_cache.pid1_i, ano_cache.pid1_d , ano_cache.pid2_p, ano_cache.pid2_i, ano_cache.pid2_d, ano_cache.pid3_p, ano_cache.pid3_i, ano_cache.pid3_d);
send_pid_para++;
//return;
}
if (send_pid_para == 2)
{
ano_dt_send_pid(sp, 2, ano_cache.pid4_p, ano_cache.pid4_i, ano_cache.pid4_d, ano_cache.pid5_p, ano_cache.pid5_i, ano_cache.pid5_d, ano_cache.pid6_p, ano_cache.pid6_i, ano_cache.pid6_d);
send_pid_para++;
//return;
}
if (send_pid_para == 3)
{
ano_dt_send_pid(sp, 3, ano_cache.pid7_p, ano_cache.pid7_i, ano_cache.pid7_d, ano_cache.pid8_p, ano_cache.pid8_i, ano_cache.pid8_d, ano_cache.pid9_p, ano_cache.pid9_i, ano_cache.pid9_d);
send_pid_para++;
//return;
}
if (send_pid_para == 4)
{
ano_dt_send_pid(sp, 4, ano_cache.pid10_p, ano_cache.pid10_i, ano_cache.pid10_d, ano_cache.pid11_p, ano_cache.pid11_i, ano_cache.pid11_d, ano_cache.pid12_p, ano_cache.pid12_i, ano_cache.pid12_d);
send_pid_para++;
//return;
}
if (send_pid_para == 5)
{
ano_dt_send_pid(sp, 5, ano_cache.pid13_p, ano_cache.pid13_i, ano_cache.pid13_d, ano_cache.pid14_p, ano_cache.pid14_i, ano_cache.pid14_d, ano_cache.pid15_p, ano_cache.pid15_i, ano_cache.pid15_d);
send_pid_para++;
//return;
}
if (send_pid_para == 6)
{
ano_dt_send_pid(sp, 6, ano_cache.pid16_p, ano_cache.pid16_i, ano_cache.pid16_d, ano_cache.pid17_p, ano_cache.pid17_i, ano_cache.pid17_d, ano_cache.pid18_p, ano_cache.pid18_i, ano_cache.pid18_d);
send_pid_para = 0;
//return;
}
//send_pid_para++;
return;
}
}
void ano_dt_send_pid(serial::Serial* sp, uint8_t group, float p1_p, float p1_i, float p1_d, float p2_p, float p2_i, float p2_d, float p3_p, float p3_i, float p3_d)
{
uint8_t _cnt = 0;
int16_t _temp;
ano_data_to_send[_cnt++] = 0xAA;
ano_data_to_send[_cnt++] = 0xAA;
ano_data_to_send[_cnt++] = 0x10 + group - 1;
ano_data_to_send[_cnt++] = 0;
_temp = (short)(p1_p * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (short)(p1_i * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (short)(p1_d * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (short)(p2_p * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (short)(p2_i * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (short)(p2_d * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (short)(p3_p * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (short)(p3_i * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
_temp = (short)(p3_d * 1000);
ano_data_to_send[_cnt++] = BYTE1(_temp);
ano_data_to_send[_cnt++] = BYTE0(_temp);
ano_data_to_send[3] = _cnt - 4;
uint8_t sum = 0;
for (uint8_t i = 0; i<_cnt; i++)
sum += ano_data_to_send[i];
ano_data_to_send[_cnt++] = sum;
//ano_usart_send_data(ano_data_to_send, _cnt);
sp->write(ano_data_to_send, _cnt);
}
};
#endif
//serial_port.cpp
#include
#include "serial/serial.h"
#include
#include
#include "ano_tc_v4/sdk_ano_tc_v4.hpp"
SDK_ANO_TC_V4 ano_tc;
serial::Serial sp;
int main(int argc, char** argv)
{
ros::init(argc, argv, "serial_port");
ros::NodeHandle nh;
//
ano_tc.set_dev(&sp,"/dev/ttyUSB1",57600);
ano_tc.open_dev(&sp);
// simulate sin data
double t = 0.0f;
ros::Rate loop_rate(20); // 测试发送功能时,过高的频率会造成ubuntu系统异常
while (ros::ok())
{
ano_tc.receive_info_from_ano_tc(&sp);
t = t + 0.001;
int16_t sin_1 = 1000.0f*sin(1*t);
int16_t sin_2 = 1000.0f*sin(2*t);
int16_t sin_3 = 1000.0f*sin(3*t);
int16_t sin_4 = 1000.0f*sin(4*t);
int16_t sin_5 = 1000.0f*sin(5*t);
int16_t sin_6 = 1000.0f*sin(6*t);
int16_t sin_7 = 1000.0f*sin(7*t);
int16_t sin_8 = 1000.0f*sin(8*t);
int16_t sin_9 = 1000.0f*sin(9*t);
ano_tc.send_15_data(
&sp,
sin_1,sin_2,sin_3,
sin_4,sin_5,sin_6,
sin_7,sin_8,sin_9,
10.0,11.0,12.0,
0,0,0);
ros::spinOnce();
loop_rate.sleep();
}
ano_tc.close_dev(&sp);
return 0;
}
删除上一小节main里while中的
t = t + 0.001;
int16_t sin_1 = 1000.0f*sin(1*t);
int16_t sin_2 = 1000.0f*sin(2*t);
int16_t sin_3 = 1000.0f*sin(3*t);
int16_t sin_4 = 1000.0f*sin(4*t);
int16_t sin_5 = 1000.0f*sin(5*t);
int16_t sin_6 = 1000.0f*sin(6*t);
int16_t sin_7 = 1000.0f*sin(7*t);
int16_t sin_8 = 1000.0f*sin(8*t);
int16_t sin_9 = 1000.0f*sin(9*t);
ano_tc.send_15_data(
&sp,
sin_1,sin_2,sin_3,
sin_4,sin_5,sin_6,
sin_7,sin_8,sin_9,
10.0,11.0,12.0,
0,0,0);
仅保留
ano_tc.receive_info_from_ano_tc(&sp);
最后,即正式使用的程序为
//serial_port.cpp
#include
#include "serial/serial.h"
#include
#include
#include "ano_tc_v4/sdk_ano_tc_v4.hpp"
SDK_ANO_TC_V4 ano_tc;
serial::Serial sp;
int main(int argc, char** argv)
{
ros::init(argc, argv, "serial_port");
ros::NodeHandle nh;
//
ano_tc.set_dev(&sp,"/dev/ttyUSB1",57600);
ano_tc.open_dev(&sp);
// simulate sin data
ros::Rate loop_rate(5760); //
while (ros::ok())
{
ano_tc.receive_info_from_ano_tc(&sp);
ros::spinOnce();
loop_rate.sleep();
}
ano_tc.close_dev(&sp);
return 0;
}
此处仅演示从ROS中获取数据显示在匿名地面站上的效果