輪子——基於ROS的匿名地面站驅動（ubuntu 20.04 & ROS Noetic）

匿名地面站方便查看動態數據，而且便於調試系統參數，多用於基於單片機的系統調試。
由於項目需要基於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 <stdint.h>
#include <string>

#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

ROS發送數據至匿名地面站

//serial_port.cpp
#include <ros/ros.h>
#include "serial/serial.h"
#include <iostream>
#include <math.h>
#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;
}

匿名地面站發送數據至ROS

刪除上一小節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 <ros/ros.h>
#include "serial/serial.h"
#include <iostream>
#include <math.h>
#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中獲取數據顯示在匿名地面站上的效果