添加MAVLINK消息解析
This commit is contained in:
OPTOC
203
app/drivers/sertrf/protocol/mavlink_control.c
Normal file
203
app/drivers/sertrf/protocol/mavlink_control.c
Normal file
@@ -0,0 +1,203 @@
|
||||
#include "mavlink_control.h"
|
||||
|
||||
|
||||
void mavlink_init(mavlink_device_t* mavlink_device)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
int mavlink_message(mavlink_device_t* mavlink_device, mavlink_message_t *message, uint8_t cp)
|
||||
{
|
||||
mavlink_status_t status;
|
||||
uint8_t msgReceived = false;
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
// PARSE MESSAGE
|
||||
// --------------------------------------------------------------------------
|
||||
|
||||
// the parsing
|
||||
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, message, &status);
|
||||
|
||||
// check for dropped packets
|
||||
if ( (mavlink_device->lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && mavlink_device->debug )
|
||||
{
|
||||
printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
|
||||
unsigned char v=cp;
|
||||
printf("%02x ", v);
|
||||
}
|
||||
mavlink_device->lastStatus = status;
|
||||
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
// mavlink_device->debugGING REPORTS
|
||||
// --------------------------------------------------------------------------
|
||||
if(msgReceived && mavlink_device->debug)
|
||||
{
|
||||
// Report info
|
||||
printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message->msgid, message->sysid, message->compid);
|
||||
|
||||
printf("Received serial data: ");
|
||||
unsigned int i;
|
||||
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
|
||||
|
||||
// check message is write length
|
||||
unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, message);
|
||||
|
||||
// message length error
|
||||
if (messageLength > MAVLINK_MAX_PACKET_LEN)
|
||||
{
|
||||
printf("\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
|
||||
}
|
||||
|
||||
// print out the buffer
|
||||
else
|
||||
{
|
||||
for (i=0; i<messageLength; i++)
|
||||
{
|
||||
// unsigned char v=buffer[i];
|
||||
// printf("%02x ", v);
|
||||
}
|
||||
// printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
// Done!
|
||||
return msgReceived;
|
||||
}
|
||||
int write_message(mavlink_device_t* mavlink_device, const mavlink_message_t *message)
|
||||
{
|
||||
char buf[300];
|
||||
|
||||
// Translate message to buffer
|
||||
unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, message);
|
||||
|
||||
// Write buffer to serial port, locks port while writing
|
||||
int bytesWritten = mavlink_device->write(buf,len,0);
|
||||
|
||||
return bytesWritten;
|
||||
}
|
||||
|
||||
void mavlik_packet_processing(mavlink_device_t* mavlink_device, mavlink_message_t* message)
|
||||
{
|
||||
mavlink_device->current_messages.sysid = message->sysid;
|
||||
mavlink_device->current_messages.compid = message->compid;
|
||||
|
||||
switch(message->msgid){
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
|
||||
mavlink_msg_heartbeat_decode(message, &(mavlink_device->current_messages.heartbeat));
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_SYS_STATUS:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
|
||||
mavlink_msg_sys_status_decode(message, &(mavlink_device->current_messages.sys_status));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_BATTERY_STATUS:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
|
||||
mavlink_msg_battery_status_decode(message, &(mavlink_device->current_messages.battery_status));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_RADIO_STATUS:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
|
||||
mavlink_msg_radio_status_decode(message, &(mavlink_device->current_messages.radio_status));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
|
||||
mavlink_msg_local_position_ned_decode(message, &(mavlink_device->current_messages.local_position_ned));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
|
||||
mavlink_msg_global_position_int_decode(message, &(mavlink_device->current_messages.global_position_int));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
|
||||
mavlink_msg_position_target_local_ned_decode(message, &(mavlink_device->current_messages.position_target_local_ned));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
|
||||
mavlink_msg_position_target_global_int_decode(message, &(mavlink_device->current_messages.position_target_global_int));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_HIGHRES_IMU:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
|
||||
mavlink_msg_highres_imu_decode(message, &(mavlink_device->current_messages.highres_imu));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_ATTITUDE:
|
||||
{
|
||||
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
|
||||
mavlink_msg_attitude_decode(message, &(mavlink_device->current_messages.attitude));
|
||||
printf("pitch %f roll %f yaw %f\n", mavlink_device->current_messages.attitude.pitch * 180 / 3.1415926, mavlink_device->current_messages.attitude.roll * 180 / 3.1415926, mavlink_device->current_messages.attitude.yaw * 180 / 3.1415926);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
// printf("Warning, did not handle message id %i\n",message.msgid);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
void mavlink_recv_loop(mavlink_device_t* mavlink_device)
|
||||
{
|
||||
bool success = false;
|
||||
|
||||
mavlink_message_t message;
|
||||
uint8_t c;
|
||||
while(mavlink_device->read(&c,1,0))
|
||||
{
|
||||
success = mavlink_message(mavlink_device,&message,c);
|
||||
if(success){
|
||||
// printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
|
||||
mavlik_packet_processing(mavlink_device,&message);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mavlink_recv_buf(mavlink_device_t* mavlink_device, void *data, uint32_t size)
|
||||
{
|
||||
bool success = false;
|
||||
|
||||
mavlink_message_t message;
|
||||
|
||||
uint32_t size_lat = size;
|
||||
uint8_t* data_char = (uint8_t*)data;
|
||||
|
||||
while(size_lat --)
|
||||
{
|
||||
uint8_t c = data_char[size - size_lat - 1];
|
||||
|
||||
success = mavlink_message(mavlink_device,&message,c);
|
||||
if(success){
|
||||
// printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
|
||||
mavlik_packet_processing(mavlink_device,&message);
|
||||
// break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
107
app/drivers/sertrf/protocol/mavlink_control.h
Normal file
107
app/drivers/sertrf/protocol/mavlink_control.h
Normal file
@@ -0,0 +1,107 @@
|
||||
#pragma once
|
||||
|
||||
#ifndef MAVLINK_CONTROL_H
|
||||
#define MAVLINK_CONTROL_H
|
||||
|
||||
//关闭针对mavlink的警告
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
|
||||
#include <common/mavlink.h>
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
|
||||
|
||||
|
||||
struct Mavlink_Messages {
|
||||
|
||||
int sysid;
|
||||
int compid;
|
||||
|
||||
// Heartbeat
|
||||
mavlink_heartbeat_t heartbeat;
|
||||
|
||||
// System Status
|
||||
mavlink_sys_status_t sys_status;
|
||||
|
||||
// Battery Status
|
||||
mavlink_battery_status_t battery_status;
|
||||
|
||||
// Radio Status
|
||||
mavlink_radio_status_t radio_status;
|
||||
|
||||
// Local Position
|
||||
mavlink_local_position_ned_t local_position_ned;
|
||||
|
||||
// Global Position
|
||||
mavlink_global_position_int_t global_position_int;
|
||||
|
||||
// Local Position Target
|
||||
mavlink_position_target_local_ned_t position_target_local_ned;
|
||||
|
||||
// Global Position Target
|
||||
mavlink_position_target_global_int_t position_target_global_int;
|
||||
|
||||
// HiRes IMU
|
||||
mavlink_highres_imu_t highres_imu;
|
||||
|
||||
// Attitude
|
||||
mavlink_attitude_t attitude;
|
||||
|
||||
// System Parameters?
|
||||
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
|
||||
mavlink_status_t lastStatus;
|
||||
struct Mavlink_Messages current_messages;
|
||||
|
||||
bool debug;
|
||||
|
||||
int (*read)(void *data, uint32_t size, uint32_t wait_ms);
|
||||
int (*write)(const void *data, uint32_t size, uint32_t wait_ms);
|
||||
}mavlink_device_t;
|
||||
/**
|
||||
* @brief 初始化mavlink
|
||||
*
|
||||
* @param mavlink_device 接口
|
||||
*/
|
||||
void mavlink_init(mavlink_device_t* mavlink_device);
|
||||
/**
|
||||
* @brief MAVLINK消息解析
|
||||
*
|
||||
* @param mavlink_device 接口
|
||||
* @param message 解析成功存放消息
|
||||
* @param cp 数据
|
||||
*/
|
||||
int mavlink_message(mavlink_device_t* mavlink_device, mavlink_message_t* message,uint8_t cp);
|
||||
/**
|
||||
* @brief MAVLINK消息发送
|
||||
*
|
||||
* @param mavlink_device 接口
|
||||
* @param message 需要发送的消息
|
||||
*/
|
||||
int write_message(mavlink_device_t* mavlink_device, const mavlink_message_t *message);
|
||||
/**
|
||||
* @brief MAVLINK消息处理
|
||||
*
|
||||
* @param mavlink_device 接口
|
||||
* @param message 需要处理的消息
|
||||
*/
|
||||
void mavlik_packet_processing(mavlink_device_t* mavlink_device, mavlink_message_t* message);
|
||||
/**
|
||||
* @brief MAVLINK消息接收 （针对需要自己进行读操作）
|
||||
*
|
||||
* @param mavlink_device 接口
|
||||
*/
|
||||
void mavlink_recv_loop(mavlink_device_t* mavlink_device);
|
||||
/**
|
||||
* @brief MAVLINK消息接收（针对已获取到数据缓存）
|
||||
*
|
||||
* @param mavlink_device 接口
|
||||
* @param data 数据
|
||||
* @param size 大小
|
||||
*/
|
||||
void mavlink_recv_buf(mavlink_device_t* mavlink_device, void *data, uint32_t size);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user