传感器数据解析，通过飞控封装成Mavlink消息，经过数据链路传回地面站显示（二）

 <message id="12933" name="SPORTINFO">
    <description>SPORTINFO.</description>
     <field type="uint64_t" name="timestamp">Timestamp in milliseconds since system boot</field>
      <field type="uint8_t" name="head">Head</field>
      <field type="float" name="voltage">Voltage</field>
      <field type="float" name="current">Current</field>
      <field type="float" name="temperature1">Temperature1</field>
      <field type="float" name="temperature2">Temperature2</field>
      <field type="float" name="speed">Speed</field>
      <field type="float" name="rpm">Rotation Speed</field>
      <field type="float" name="height">Height</field>
      <field type="float" name="yawangularspeed">YawAngularSpeed</field>
      <field type="float" name="pitchangularspeed">PitchAngularSpeed</field>
      <field type="float" name="rollangularspeed">RollAngularSpeed</field>
      <field type="float" name="position1">position1</field>
      <field type="float" name="position2">position2</field>
      <field type="uint8_t" name="tail">Tail</field>
    </message>

在定义消息时，message id 具有唯一性，不能与其他的值重复，第一个字段必须为时间戳，必不可少，即 <field type="uint64_t" name="timestamp">Timestamp in milliseconds since system boot</field>，其余字段根据实际情况进行定义和命名。

2、编译

进行编译

（1）make clean

make clean

(2) make px4_fmu-v6c_default

make px4_fmu-v6c_default

注意该步骤的命令取决于所用飞控版本，我的飞控板是 pixhawk 6c 的，如果是雷迅的飞控板 X7 PRO，则用make cuav_x7pro_default ，其他版本参考说明书。

`3、查看编译文件`

/build/px4_fmu-v6c_default/mavlink/common ，在该目录下会生成 mavlink_msg_sportinfo.h 文件，该文件名中的sportinfo就是刚定义msg是的name。

该文件的作用是对于刚才定义的msg消息，基于mavlink协议进行结构声明、打包、发送、解析等函数的生成，后续进行直接调用，开发者不需要了解mavlink协议的具体内容，即可直接调用对用的方法。

二、定义msg消息和对mavlink协议的解析

1、在msg目录下新建sportinfo.msg ,注意文件名最好保持一致。第一行必须有timestamp,内容与common.xml中的字段一致。

uint64 timestamp		# time since system start (microseconds)
uint8 head 
float32 voltage
float32 current
float32 temperature1
float32 temperature2
float32 speed
float32 rpm
float32 height
float32 yawangularspeed
float32 pitchangularspeed
float32 rollangularspeed
float32 position1
float32 position2
uint8 tail
#TOPICS sportinfo

2、在msg 文件夹的CMakeLists.txt 中的set(msg_files 后添加

sportinfo.msg

3、编译生成消息头文件

4、在src/drivers中新建 sportinfo 文件夹，在该文件夹下新建四个文件，CMakeLists.txt 、Kconfig 、sportinfo.cpp 、sportinfo.hpp 、sportinfo_main.cpp

（1）CMakeLists.txt

px4_add_module(
	MODULE drivers__sportinfo
	MAIN sportinfo
	COMPILE_FLAGS
		#-DDEBUG_BUILD
		#-O0
	SRCS
		sportinfo.hpp
		sportinfo.cpp
		sportinfo_main.cpp
	DEPENDS
		px4_work_queue
)

注意：drivers__sportinfo 中间有两个下划线。

（2） Kconfig

menuconfig DRIVERS_SPORTINFO
	bool "sportinfo"
	default n
	---help---
		Enable support for sportinfo unit

（3） sportinfo.cpp

#include "sportinfo.hpp"
#include <uORB/topics/sportinfo.h>


SPORTINFO::SPORTINFO(const char *port): ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port))
{
	// Store the port name.
	strncpy(_port, port, sizeof(_port) - 1);

	// Enforce null termination.
	_port[sizeof(_port) - 1] = '\0';

}

SPORTINFO::~SPORTINFO()
{
	stop();
	perf_free(_sample_perf);
	perf_free(_comms_errors);
}

uint8_t SPORTINFO::crc8_calc(const unsigned char *data_frame, uint8_t crc_length)
{

	// Make sure the check start and end bytes are correct
	uint8_t sum = 0;

	for (int i = HEAD_POS; i < crc_length; ++i) {
		sum += data_frame[i];
	}

	return sum;
}

int SPORTINFO::send_data()
{
/*
	_packet[2] = ((_thr_cmd * CYL_2_T_LSB_POS) >> 8) & 0xFF;
	_packet[3] = (_thr_cmd * CYL_2_T_LSB_POS) & 0xFF;
	_packet[8] = _frame_counter;

	uint8_t checksum = 0;

	for (uint8_t i = FUELCELL_ALA_STAT_POS; i < CYL_2_T_MSB_POS; i++) {
		checksum += _packet[i];
	}

	_packet[CYL_2_T_MSB_POS] = checksum;
	int bytes_s = ::write(_file_descriptor, &_packet[0], sizeof(_packet));
	::fsync(_file_descriptor);
	return bytes_s == sizeof(_packet);
*/
	return 1;

}

int SPORTINFO::collect()
{
	perf_begin(_sample_perf);

	//bool crc_valid = false;


	// minimum bytes that we want to read
	const unsigned sleeptime = CHARACTER_COUNT * 1000000 / (115200 / 10);

	int err = 0;
	int bytes_available = 0;

	err = ::ioctl(_file_descriptor, FIONREAD, (unsigned long)&bytes_available);

	if (err != 0 || bytes_available < (int)CHARACTER_COUNT) {
		px4_usleep(sleeptime);
	}

	// Read from the FUELCELL UART buffer.
	// const hrt_abstime timestamp_sample = hrt_absolute_time();
	do {
		int bytes_read = ::read(_file_descriptor, &_linebuf[0], sizeof(_linebuf));
	//	PX4_INFO("the length of bytes_read %d\n", bytes_read);

		if (bytes_read < 0) {
			PX4_ERR("read 996  err: %d", bytes_read);
			perf_count(_comms_errors);
			perf_end(_sample_perf);
			return -EAGAIN;
		}

		for (int i = 0; i < bytes_read; i++) {
		//	PX4_INFO("bytes_read  %d",i+1);
		//	PX4_INFO("bytes %x",_linebuf[i]);

			if (data_parser(_linebuf[i], _frame_data, _parse_state, _crc8) == PX4_OK) {
		//		crc_valid = true;


			}

		}
		bytes_available -= bytes_read;

	} while (bytes_available > 0);

	//if (!crc_valid) {
	//	return -EAGAIN;
	//}

	perf_end(_sample_perf);

	return PX4_OK;

}

int SPORTINFO::data_parser(const uint8_t check_byte, uint8_t parserbuf[RX_PARSE_LENGTH], SPORTINFO_PARSE_STATE &state,
		     uint8_t &crc8)
{
	switch (state) {
	case SPORTINFO_PARSE_STATE::WAITING_FRAME:
		if (check_byte == HEAD) {
			state = SPORTINFO_PARSE_STATE::DATA;
			parserbuf[0] =  HEAD;
			data_index = 1 ;
		}else
		{
			state = SPORTINFO_PARSE_STATE::WAITING_FRAME;
		}


		break;

	case SPORTINFO_PARSE_STATE::DATA:
		/*parserbuf[data_index++] =  check_byte;
		PX4_INFO("DATA INDEX %d",data_index);
		// Determine whether all data segments have been received
		if (data_index == (FUELCELL_TBD4_LSB_POS + 1)) {

			PX4_INFO("data check 777777777777777777777777777777777777777777777777777777");
			state = FUELCELL_PARSE_STATE::CHECKSUM;

		}
		*/
		parserbuf[data_index++] =  check_byte;
		if(data_index == RX_PARSE_LENGTH-1)
		{
			state = SPORTINFO_PARSE_STATE::CHECKSUM;
		}

		break;

	case SPORTINFO_PARSE_STATE::CHECKSUM:
		// Here, reset state to `NOT-STARTED` no matter crc ok or not
		state = SPORTINFO_PARSE_STATE::WAITING_FRAME;
	//	PX4_INFO("tail %d",check_byte);
		if(check_byte == TAIL){

		//	PX4_INFO("Checksum verified 66666666666666666666666666666666666\n");
			data_process(parserbuf);
			return PX4_OK;
		}

		break;
	}

	return PX4_ERROR;
}

void SPORTINFO::data_process(uint8_t parserbuf[RX_PARSE_LENGTH])
{

	memset(&sensordata,0,sizeof(sensordata));
	orb_advert_t sportinfo_pub = orb_advertise(ORB_ID(sportinfo),&sensordata);

	sport_data.voltage =(double)(parserbuf[VOLTAGE_MSB_POS]*100+parserbuf[VOLTAGE_LSB_POS] )/100.00 ;
	sport_data.current =(double)( parserbuf[CURRENT_MSB_POS]*100+parserbuf[CURRENT_LSB_POS])/10.0;
	sport_data.temperature1 =(double) (parserbuf[TEMPERATURE1_MSB_POS]*100+parserbuf[TEMPERATURE1_LSB_POS]);
	sport_data.temperature2 =(double) (parserbuf[TEMPERATURE2_MSB_POS]*100+parserbuf[TEMPERATURE2_LSB_POS]);

	sport_data.speed =(double) (parserbuf[SPEED_MSB_POS]*100+parserbuf[SPEED_LSB_POS])/10;
	sport_data.rpm = (double) (parserbuf[RPM_MSB_POS]*100+parserbuf[RPM_LSB_POS]);

	sport_data.height = (double)(parserbuf[HEIGHT_MSB_POS]*100+parserbuf[HEIGHT_LSB_POS]);
	sport_data.yawangularspeed = (double)(parserbuf[YAWANGULARSPEED_MSB_POS]*100+parserbuf[YAWANGULARSPEED_LSB_POS]);
	sport_data.pitchangularspeed = (double)(parserbuf[PITCHANGULARSPEED_MSB_POS]*100+parserbuf[PITCHANGULARSPEED_LSB_POS]);
	sport_data.rollangularspeed = (double)(parserbuf[ROLLANGULARSPEED_MSB_POS]*100+parserbuf[ROLLANGULARSPEED_LSB_POS]);

	sport_data.position1 = (double)(parserbuf[POSITION1_MSB_POS]*100+parserbuf[POSITION1_LSB_POS]);
	sport_data.position2 = (double)(parserbuf[POSITION2_MSB_POS]*100+parserbuf[POSITION2_LSB_POS]);

	sensordata.voltage = sport_data.voltage ;
	sensordata.current = sport_data.current;
	sensordata.temperature1 = sport_data.temperature1 ;
	sensordata.temperature2 = sport_data.temperature2 ;
	sensordata.speed = sport_data.speed ;
	sensordata.rpm = sport_data.rpm;
	sensordata.height = sport_data.height ;
	sensordata.yawangularspeed = sport_data.yawangularspeed;
	sensordata.pitchangularspeed = sport_data.pitchangularspeed;
	sensordata.rollangularspeed = sport_data.rollangularspeed;
	sensordata.position1 = sport_data.position1;
	sensordata.position2 = sport_data.position2;

//	sensordata.timestap = hrt_absolute_tiem();


	orb_publish(ORB_ID(sportinfo), sportinfo_pub, &sensordata);
//	PX4_INFO("66666");

/*
	PX4_INFO("orb voltage %.2f\n", (double)sensordata.voltage);

	PX4_INFO("orb current %.1f\n", (double)sensordata.current);
	PX4_INFO("orb temperature1 %.2f\n", (double)sensordata.temperature1);
	PX4_INFO("orb temperature2 %.2f\n",(double) sensordata.temperature2);
	PX4_INFO("orb speed %.2f\n", (double)sensordata.speed);

	PX4_INFO("orb rpm %.1f\n", (double)sensordata.rpm);
	PX4_INFO("orb height %.2f\n", (double)sensordata.height);
	PX4_INFO("orb yawangularspeed %.1f\n", (double)sensordata.yawangularspeed);
	PX4_INFO("orb pitchangularspeed %.1f\n",(double) sensordata.pitchangularspeed);
	PX4_INFO("orb rollangularspeed %.2f\n", (double)sensordata.rollangularspeed);

	PX4_INFO("orb position1 %.1f\n", (double)sensordata.position1);
	PX4_INFO("orb position2 %.1f\n", (double)sensordata.position2);

*/
}

int SPORTINFO::init()
{
	start();
	return PX4_OK;
}

int SPORTINFO::open_serial_port(const speed_t speed)
{

	if (_file_descriptor > 0) {
		// PX4_INFO("serial port already open");
		return PX4_OK;
	}

	// Configure port flags for read/write, non-controlling, non-blocking.
	int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);

	// Open the serial port.
	_file_descriptor = ::open(_port, flags);

	if (_file_descriptor < 0) {
		PX4_ERR("open failed (%i)", errno);
		return PX4_ERROR;
	}

	termios uart_config = {};

	// Store the current port configuration. attributes.
	tcgetattr(_file_descriptor, &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 the input baud rate in the uart_config struct.
	int termios_state = cfsetispeed(&uart_config, speed);

	if (termios_state < 0) {
		PX4_ERR("CFG: %d ISPD", termios_state);
		::close(_file_descriptor);

		return PX4_ERROR;
	}

	// Set the output baud rate in the uart_config struct.
	termios_state = cfsetospeed(&uart_config, speed);

	if (termios_state < 0) {
		PX4_ERR("CFG: %d OSPD", termios_state);
		::close(_file_descriptor);
		return PX4_ERROR;
	}

	// Apply the modified port attributes.
	termios_state = tcsetattr(_file_descriptor, TCSANOW, &uart_config);

	if (termios_state < 0) {
		PX4_ERR("baud %d ATTR", termios_state);
		::close(_file_descriptor);
		return PX4_ERROR;
	}

	PX4_INFO("successfully opened UART port %s", _port);
	return PX4_OK;
}

void SPORTINFO::print_info()
{
	perf_print_counter(_sample_perf);
	perf_print_counter(_comms_errors);
}

void SPORTINFO::Run()
{
	// Ensure the serial port is open.
	open_serial_port();

	send_data();
	_frame_counter = (_frame_counter + 1) % 256;

	// Perform collection.
	collect();
}

void SPORTINFO::start()
{
	// Schedule the driver at regular intervals.
	ScheduleOnInterval(SPORTINFO_MEASURE_INTERVAL);

	PX4_INFO("driver started");
}

void SPORTINFO::stop()
{
	// Clear the work queue schedule.
	ScheduleClear();

	// Ensure the serial port is closed.
	::close(_file_descriptor);
}

注意：该部分主要实现mavlink协议的数据解析，采用状态机的方法实现。

（4）sportinfo.hpp

/**
 * @file sportinfo.cpp
 * @author Mingsong Li <lucky.mingsong@gmail.com>
 *
 * Driver for the engine control unit.
 * Make sure to disable MAVLINK messages (MAV_0_CONFIG PARAMETER)
 * on the serial port you connect the sensor,i.e TELEM2.
 *
 */

#pragma once

#include <termios.h>
#include <fcntl.h>
#include <inttypes.h>

#include <errno.h>
#include <drivers/drv_hrt.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/log.h>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sportinfo.h>


using namespace time_literals;

/* Configuration Constants */
static constexpr uint32_t SPORTINFO_MEASURE_INTERVAL{50_ms};	// 50ms default data conversion time.

/* Frame format for receive data */
static constexpr uint8_t HEAD{0xFE};
static constexpr uint8_t TAIL{0xFF};

/**
 * RX Frame format definition
 *   1B         2-41 B     42B
 * | 0xFE |  ...data... | 0XFF|
 *

 * Frame format for received data
 * Why is it written this way?
 * It's more felt than told
 *
*/
static constexpr uint8_t HEAD_POS{0};

static constexpr uint8_t VOLTAGE_MSB_POS{1};
static constexpr uint8_t VOLTAGE_LSB_POS{2};
static constexpr uint8_t CURRENT_MSB_POS{3};
static constexpr uint8_t CURRENT_LSB_POS{4};
static constexpr uint8_t TEMPERATURE1_MSB_POS{5};
static constexpr uint8_t TEMPERATURE1_LSB_POS{6};
static constexpr uint8_t TEMPERATURE2_MSB_POS{7};
static constexpr uint8_t TEMPERATURE2_LSB_POS{8};


static constexpr uint8_t SPEED_MSB_POS{9};
static constexpr uint8_t SPEED_LSB_POS{10};
static constexpr uint8_t RPM_MSB_POS{11};
static constexpr uint8_t RPM_LSB_POS{12};
static constexpr uint8_t HEIGHT_MSB_POS{13};
static constexpr uint8_t HEIGHT_LSB_POS{14};
static constexpr uint8_t YAWANGULARSPEED_MSB_POS{15};
static constexpr uint8_t YAWANGULARSPEED_LSB_POS{16};
static constexpr uint8_t PITCHANGULARSPEED_MSB_POS{17};
static constexpr uint8_t PITCHANGULARSPEED_LSB_POS{18};

static constexpr uint8_t ROLLANGULARSPEED_MSB_POS{19};
static constexpr uint8_t ROLLANGULARSPEED_LSB_POS{20};
static constexpr uint8_t POSITION1_MSB_POS{21};
static constexpr uint8_t POSITION1_LSB_POS{22};
static constexpr uint8_t POSITION2_MSB_POS{23};
static constexpr uint8_t POSITION2_LSB_POS{24};
static constexpr uint8_t TAIL_POS{25};

static constexpr uint8_t RX_PARSE_LENGTH{26};
static constexpr int8_t CHARACTER_COUNT{26};


class SPORTINFO : public px4::ScheduledWorkItem
{
public:
	/**
	 * Default Constructor
	 * @param port The serial port to open for communicating with the SPORTINFO.
	 *
	*/
	SPORTINFO(const char*port);
	~SPORTINFO();

	/**
	 * Method : init()
	 * This method initializes the general driver for the SPORTINFO.
	*/
	int init();

	struct sportinfo_data
	{
		uint8_t head = 0; //1 byte

		float voltage = 0;//2 byte
		float current = 0;//2 byte
		float temperature1 = 0;//2 byte
		float temperature2 = 0;//2 byte
		float speed = 0;//2 byte
		float rpm = 0; //2 byte
		float height = 0;//2 byte
		float yawangularspeed = 0;//2 byte
		float pitchangularspeed = 0;//2 byte
		float rollangularspeed = 0;//2 byte
		float position1 = 0;//2 byte
		float position2 =0;//2 byte

		uint8_t tail=0;//1 byte
	};
        sportinfo_data sport_data;

	/**
	 * Diagnostics - print some basic information about the driver.
	*/
	void print_info();

private:

	enum class SPORTINFO_PARSE_STATE {
		WAITING_FRAME = 0,
	//	HEAD_SATE,
		DATA,
	//	TAIL_STATE,
		CHECKSUM
	};

	/**
	 *Calculates the 8 byte crc value for the data frame.
	 * @param data_frame The data frame to compute a checksum for.
	 * @param crc8_length The length of the data frame that needs to be verified.
	*/
	uint8_t crc8_calc(const unsigned char *data_frame, uint8_t crc_length);

	/**
	 * Reads data from serial UART and places it into a buffer.
	 */
	int collect();

	int data_parser(const uint8_t check_byte, uint8_t parserbuf[RX_PARSE_LENGTH], SPORTINFO_PARSE_STATE &state,
			uint8_t &crc8);


	/**
	 * Process the received data
	 * @param parserbuf valid array
	*/
	void data_process(uint8_t parserbuf[RX_PARSE_LENGTH]);


	/**
	 * Send the data
	 *
	*/
    	int send_data();

	/**
	 * Opens and configures the UART serial communications port.
	 * @param speed The baudrate (speed) to configure the serial UART port.
	 */
	int open_serial_port(const speed_t speed = B115200);

	/**
	 * Perform a reading and sending cycle;
	 */
	void Run() override;

	/**
	 * Initialise the automatic measurement state machine and start it.
	 * @note This function is called at open and error time.  It might make sense
	 *       to make it more aggressive about resetting the bus in case of errors.
	 */
//	uORB::Publication<sportinfo_s> sportinfo_pub{ORB_ID(sportinfo)};
//	sportinfo_s _sportinfo{};

	void start();

	/**
	 * Stops the automatic measurement state machine.
	 */
	void stop();

	char _port[20] {};

	uint8_t _frame_counter = 0;
	uint16_t _thr_cmd = 1;

	int total_packets_lost = 0; // Total number of lost packets
	int total_packets_received = 0; // Number of successfully parsed packets
	int last_seq=0;

	unsigned char _frame_data[RX_PARSE_LENGTH]={HEAD,TAIL};
//	unsigned char _packet[CYL_2_T_LSB_POS]={FRAME_DIGIT2,FRAME_DIGIT1};// note: CYL_2_T_LSB_POS=10

        int _file_descriptor{-1};

	uint8_t _linebuf[85] {}; //2x data length buffer

	uint8_t _crc8{0};
        int data_index = 0;
	SPORTINFO_PARSE_STATE _parse_state{SPORTINFO_PARSE_STATE::WAITING_FRAME};

	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};


	/*初始化数据结构体，只能在这里初始化。如果在collect里初始化会导致经常发布不出来。
	但是voltage_sensor 中初始化结构体和公告、发布主题在一起。*/


	struct sportinfo_s sensordata;



};

（5） sportinfo_main.cpp

#include <px4_platform_common/cli.h>
#include <px4_platform_common/getopt.h>

#include "sportinfo.hpp"

/**
 * Local functions in support of the shell command.
 */
namespace sportinfo
{
	SPORTINFO *sportinfo;
	int reset(const char *port);
	int start(const char *port);
	int status();
	int stop();
	int usage();

/**
 * Reset the driver.
 */
int reset(const char *port)
{
	if (stop() == PX4_OK) {
		return start(port);
	}

	return PX4_ERROR;
}

/**
 * Start the driver.
 */
int start(const char *port)
{
	if (port == nullptr) {
		PX4_ERR("invalid port");
		return PX4_ERROR;
	}

	if (sportinfo != nullptr) {
		PX4_INFO("already started");
		return PX4_OK;
	}

	// Instantiate the driver.
	sportinfo = new SPORTINFO(port);

	if (sportinfo == nullptr) {
		PX4_ERR("object instantiate failed");
		return PX4_ERROR;
	}

	if (sportinfo->init() != PX4_OK) {
		PX4_ERR("driver start failed");
		delete sportinfo;
		sportinfo = nullptr;
		return PX4_ERROR;
	}

	return PX4_OK;
}

/**
 * Print the driver status.
 */
int
status()
{
	if (sportinfo == nullptr) {
		PX4_ERR("driver not running");
		return PX4_ERROR;
	}

	sportinfo->print_info();

	return PX4_OK;
}

/**
 * Stop the driver
 */
int stop()
{
	if (sportinfo != nullptr) {
		delete sportinfo;
		sportinfo = nullptr;
	}

	return PX4_ERROR;
}

int
usage()
{
	PX4_INFO("usage: sportinfo command [options]");
	PX4_INFO("command:");
	PX4_INFO("\treset|start|status|stop");
	PX4_INFO("options:");
	PX4_INFO("\t-d device_path");
	return PX4_OK;
}

}

/**
 * Driver 'main' command.
 */
extern "C" __EXPORT int sportinfo_main(int argc, char *argv[])
{
	const char *device_path = nullptr;
	int ch;
	int myoptind = 1;
	const char *myoptarg = nullptr;

	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		case 'd':
			device_path = myoptarg;
			break;

		default:
			PX4_WARN("Unknown option!");
			return sportinfo::usage();
		}
	}

	if (myoptind >= argc) {
		return sportinfo::usage();
	}

	// Reset the driver.
	if (!strcmp(argv[myoptind], "reset")) {
		return sportinfo::reset(device_path);
	}

	// Start/load the driver.
	if (!strcmp(argv[myoptind], "start")) {
		return sportinfo::start(device_path);
	}

	// Print driver information.
	if (!strcmp(argv[myoptind], "status")) {
		return sportinfo::status();
	}

	// Stop the driver
	if (!strcmp(argv[myoptind], "stop")) {
		return sportinfo::stop();
	}

	return sportinfo::usage();
}

5、在/boards/px4/fmu-v6c 文件夹中的default.px4board ，添加一行

CONFIG_DRIVERS_SPORTINFO=y

注意：此处/boards/px4/fmu-v6c 取决于飞控板的版本，本次采用的是pixhawk 6c 。

三、飞控端基于mavlink协议发送消息到地面站

在 /src/modules/mavlink/streams 中新建 SPORTINFO.hpp ，(注意是大写)

#ifndef SPORTINFO_HPP
#define SPORTINFO_HPP
#include <uORB/topics/sportinfo.h>

class MavlinkStreamSPORTINFO : public MavlinkStream
{
public:
	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSPORTINFO(mavlink); }

	static constexpr const char *get_name_static() { return "SPORTINFO"; }
	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SPORTINFO; }

	const char *get_name() const override { return get_name_static(); }
	uint16_t get_id() override { return get_id_static(); }

	unsigned get_size() override
	{
		return sportinfo_sub.advertised() ? MAVLINK_MSG_ID_SPORTINFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
	}

private:
	explicit MavlinkStreamSPORTINFO(Mavlink *mavlink) : MavlinkStream(mavlink) {}

	uORB::Subscription sportinfo_sub{ORB_ID(sportinfo)};

	bool send() override
	{
		sportinfo_s sportinfo;

		if (sportinfo_sub.update(&sportinfo)) {
			mavlink_sportinfo_t msg{};

			msg.voltage=sportinfo.voltage;
			msg.current = sportinfo.current;

			msg.temperature1 = sportinfo.temperature1;
			msg.temperature2 = sportinfo.temperature2;
			msg.speed = sportinfo.speed;
			msg.rpm = sportinfo.rpm;
			msg.height = sportinfo.height;



			msg.yawangularspeed = sportinfo.yawangularspeed;
			msg.pitchangularspeed = sportinfo.pitchangularspeed;
			msg.rollangularspeed = sportinfo.rollangularspeed;
			msg.position1 = sportinfo.position1;
			msg.position2 = sportinfo.position2;

			PX4_INFO("orb voltage %.2f\n", (double)msg.voltage);

			PX4_INFO("orb current %.1f\n", (double)msg.current);
			PX4_INFO("orb temperature1 %.2f\n", (double)msg.temperature1);
			PX4_INFO("orb temperature2 %.2f\n",(double) msg.temperature2);
			PX4_INFO("orb speed %.2f\n", (double)msg.speed);

			PX4_INFO("orb rpm %.1f\n", (double)msg.rpm);
			PX4_INFO("orb height %.2f\n", (double)msg.height);
			PX4_INFO("orb yawangularspeed %.1f\n", (double)msg.yawangularspeed);
			PX4_INFO("orb pitchangularspeed %.1f\n",(double) msg.pitchangularspeed);
			PX4_INFO("orb rollangularspeed %.2f\n", (double)msg.rollangularspeed);

			PX4_INFO("orb position1 %.1f\n", (double)msg.position1);
			PX4_INFO("orb position2 %.1f\n", (double)msg.position2);

			mavlink_msg_sportinfo_send_struct(_mavlink->get_channel(), &msg);

			return true;

		}

		return false;
	}
};

#endif // MOUNT_ORIENTATION

注意：该文件主要实现将解析的mavlink协议数据再次打包成mavlink协议，并发送给地面站。

四、头文件添加

1、打开 src/modules/mavlink/mavlink_messages.cpp ，添加头文件

#include "streams/SPORTINFO.hpp"

2、在“ static const StreamListItem streams_list[] = ” 中添加

#if defined(SPORTINFO_HPP)
  create_stream_list_item<MavlinkStreamSPORTINFO>(),
#endif // SPORTINFO_HPP

3、打开 src/modules/mavlink/mavlink_main.cpp ，

在 Mavlink::configure_streams_to_default(const char *configure_single_stream) 中的 “switch (_mode) ”之前添加

configure_stream_local("SPORTINFO", 2.0f);

五、添加串口

1、打开 ROMFS/px4fmu_common/init.d/rcS ，在568行左右添加

sportinfo start -d /dev/ttyS3

其中ttyS3 对应的是飞控板上的 telem2, 即 S.port 信号通过 arduino nano 解析后通过串口传给飞控板的 telem2端口，然后在飞控端进行继续解析。

2、波特率默认设置115200 ，此处不需修改。若要修改，在sportinfo.hpp中的

 int open_serial_port(const speed_t speed = B115200);

六、记录日志

将S.port 传感器传过来的数据实时保存在日志中，便于后续的数据分析。

打开src/modules/logger/logged_topics.cpp ，在 void LoggedTopics::add_default_topics()

中添加

add_topic("sportinfo");