2017年6月28号更新说明:在展示的代码中删除了可能泄露个人信息的内容。
最近实验室在搭建四轴编队飞行系统的过程中遇到了一点问题,不能进入pixhawk的offboard模式。
在网上查了好多资料,发现要进入offboard模式,必须先给它发生setpoint。但是这个setpoint怎么发送呢。国外实验室有用matlab封装好的发送setpoint的函数,但是不开源,用他试用版函数试了几次发现还是不能进入还是offboard模式。于是想到自己编写地面站。
我刚才说的国外实验室做的matlab发送mavlink消息的项目叫matmav,有兴趣的可以联系他们获取试用版。
由于我的目的只是进入offboard模式,不需要复杂的界面,所以用控制台编写。
编程思路一定要清晰,要知道自己的需求是什么。要不然到后期要加的新功能越来越多,自己都不知道去哪儿找写好的程序。
我的这个程序的具体思路是开两个线程,一个读串口,一个写串口。程序开始打开连接数传的串口,通过按数字键来确定发送什么mavlink指令,收到的mavlink信息自动显示在控制台上(这个功能比较好实现,我在线调试已经能收到mavlink消息并解析。只是我不需要这个功能,在代码里没有,有需要的读者请自行用printf输出)。
void main()
{
mavlink_init();
//============================================
while(1)
{
int a;
cout << "请输入要发送的mavlink消息序号" << endl;
cout << "1 set_attitude_target " << endl;
cout << "2 heartbeat " << endl;
cout << "3 set_position_target_local_ned " << endl;
cout << "4 offboard" << endl;
cin >> a;
if (a == 1)
{
mavlink_message_t msg;
float roll, pitch, yaw, thrust;
roll = 10; pitch = 20; yaw = 30; thrust = 40;
float q[4];
q[0] = 0.01; q[1] = 0.02; q[2] = 0.03; q[3] = 0.04;
mavlink_msg_set_attitude_target_pack(100, 200, &msg, 0, system_type, autopilot_type, 0, q, roll, pitch, yaw, thrust);
BufSendLen = mavlink_msg_to_send_buffer(BufSend, &msg);
IsDataReady = 1;
}
if(a==2)
{
mavlink_message_t msg;
mavlink_msg_heartbeat_pack(100, 200, &msg, system_type, autopilot_type, 0, 0, 1);
BufSendLen = mavlink_msg_to_send_buffer(BufSend, &msg);
IsDataReady = 1;
}
if (a == 3)
{
mavlink_message_t msg;
mavlink_msg_set_position_target_local_ned_pack(100, 200, &msg,0, system_type, autopilot_type, 0, 0, 1,2,3,11,22,33,111,222,333,1111,11111);
//(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
//uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate)
BufSendLen = mavlink_msg_to_send_buffer(BufSend, &msg);
IsDataReady = 1;
}
if (a == 4)
{
mavlink_message_t msg;
mavlink_msg_set_mode_pack(100, 200, &msg, system_type, 1, 1);
//最后两个参数(base_mode,custom_mode)(1,1)
//
BufSendLen = mavlink_msg_to_send_buffer(BufSend, &msg);
IsDataReady = 1;
}
if(a != 1 && a != 2 && a != 3 && a!=4)
{
IsDataReady = 0;
cout << "请重新输入" << endl;
continue;
}
}
int a;
cin >> a;
}
bool mavlink_init()
{
//serialport
char strCOM[]="COM4";
int baud = 9600;
hSerial = CreateFile(strCOM,
GENERIC_READ | GENERIC_WRITE,
0,
0,
OPEN_EXISTING,
FILE_FLAG_OVERLAPPED,
0);
if (hSerial == INVALID_HANDLE_VALUE)
{
if (GetLastError() == ERROR_FILE_NOT_FOUND)
{
printf("Error 1: No Such Com port!\n");
}
printf("Error 2: No Such Com port!\n");
}
DCB dcbSerialParams = { 0 };
DCB dcbSerial = { 0 };
dcbSerial.DCBlength = sizeof(dcbSerialParams);
if (!GetCommState(hSerial, &dcbSerialParams))
{
printf("Error 3:Cann't open Com Port!\n");
}
dcbSerialParams.BaudRate = baud;
dcbSerialParams.ByteSize = 8;
dcbSerialParams.StopBits = ONESTOPBIT;
dcbSerialParams.Parity = NOPARITY;
if (!SetCommState(hSerial, &dcbSerialParams))
{
printf("Error 4: Set Com port: Baud, StopBits... \n");
}
COMMTIMEOUTS timeouts = { 0 };
timeouts.ReadIntervalTimeout = 1;
timeouts.ReadTotalTimeoutConstant = 0;
timeouts.ReadTotalTimeoutMultiplier = 1;
timeouts.WriteTotalTimeoutConstant = 0;
timeouts.WriteTotalTimeoutMultiplier = 5;
if (!SetCommTimeouts(hSerial, &timeouts))
{
printf("Error 5: Time out!\n");
}
looping = true;
//ThreadRead
hThreadRead = CreateThread(NULL, 0,
mavThreadRead, NULL, 0, NULL);
//ThreadWrite
hThreadWrite = CreateThread(NULL, 0,
mavThreadWrite, NULL, 0, NULL);
return TRUE;
}
DWORD WINAPI mavThreadRead(LPVOID lpParam)
{
printf("Serial read thread started\n");
int index;
char szBuff[2] = { 0 };
DWORD dwBytesRead = 0;
DWORD dwRes;
BOOL fWaitingOnRead = FALSE;
OVERLAPPED osReader = { 0 };
// Create the overlapped event. Must be closed before exiting
// to avoid a handle leak.
osReader.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
mavlink_message_t msg;
mavlink_status_t status;
while (looping)
{
if (!fWaitingOnRead)
{
// Issue read operation.
if (!ReadFile(hSerial, szBuff, 1, &dwBytesRead, &osReader))
{
if (GetLastError() != ERROR_IO_PENDING) // read not delayed?
printf("Serial port Reading error!\n");
else
{
fWaitingOnRead = TRUE;
//printf("Serial port is waiting for data!\n");
}
}
else
{
// read completed immediately
// HandleASuccessfulRead(lpBuf, dwRead);
if (mavlink_parse_char(MAVLINK_COMM_0, szBuff[0], &msg, &status))
{
//int16_t work_data = Pwork_data;
mavlink_msg_decode(Pwork_data, msg);
//mavlink_msg_decode(int16_t** Pwork_data, mavlink_message_t msg);
}
}
}
else
{
dwRes = WaitForSingleObject(osReader.hEvent, INFINITE);
switch (dwRes)
{
// Read completed.
case WAIT_OBJECT_0:
if (!GetOverlappedResult(hSerial, &osReader, &dwBytesRead, FALSE))
// Error in communications; report it.
printf("Serial port Reading error after Overlapped!\n ");
else
{
// Read completed successfully (may because reading timeout).
// HandleASuccessfulRead(lpBuf, dwRead);
if (dwBytesRead>0)
{
if (mavlink_parse_char(MAVLINK_COMM_0, szBuff[0], &msg, &status))
{
//int16_t* work_data = Pwork_data;
mavlink_msg_decode(Pwork_data, msg);
printf("成功读取!\n ");
}
}
// Reset flag so that another opertion can be issued.
fWaitingOnRead = FALSE;
// printf("%d Byte Data received after Overlapped!\n", (int)dwBytesRead);
}
break;
case WAIT_TIMEOUT:
// Operation isn't complete yet. fWaitingOnRead flag isn't
// changed since I'll loop back around, and I don't want
// to issue another read until the first one finishes.
//
// This is a good time to do some background work.
printf("STILL Serial port is waiting for data!\n ");
break;
default:
// Error in the WaitForSingleObject; abort.
// This indicates a problem with the OVERLAPPED structure's
// event handle.
break;
}
}
}
printf("Serial read thread stoped\n");
return 0;
}
DWORD WINAPI mavThreadWrite(LPVOID lpParam)
{
printf("Serial write thread started\n");
while (looping)
{
if (IsDataReady)
{
if (WriteBuffer(BufSend, (DWORD)BufSendLen))
{
printf("Write OK!\n");
}
else
{
printf("Write error\n");
}
IsDataReady = 0;
}
}
printf("Serial write thread stopped\n");
return 0;
}
BOOL WriteBuffer(uint8_t* lpBuf, DWORD dwToWrite)
{
OVERLAPPED osWrite = { 0 };
DWORD dwWritten;
DWORD dwRes;
BOOL fRes;
// Create this write operation's OVERLAPPED structure's hEvent.
osWrite.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
if (osWrite.hEvent == NULL)
// error creating overlapped event handle
return FALSE;
// Issue write.
if (!WriteFile(hSerial, lpBuf, dwToWrite, &dwWritten, &osWrite))
{
if (GetLastError() != ERROR_IO_PENDING)
{
// WriteFile failed, but isn't delayed. Report error and abort.
fRes = FALSE;
printf("Serial port Writing error!\n");
}
else
// Write is pending.
dwRes = WaitForSingleObject(osWrite.hEvent, INFINITE);
switch (dwRes)
{
// OVERLAPPED structure's event has been signaled.
case WAIT_OBJECT_0:
if (!GetOverlappedResult(hSerial, &osWrite, &dwWritten, FALSE))
fRes = FALSE;
else
if (dwToWrite > dwWritten)
printf("Serial port Writing timeout!\n");
// Write operation completed successfully.
fRes = TRUE;
break;
default:
// An error has occurred in WaitForSingleObject.
// This usually indicates a problem with the
// OVERLAPPED structure's event handle.
fRes = FALSE;
break;
}
}
else
{
// WriteFile completed immediately.
fRes = TRUE;
}
CloseHandle(osWrite.hEvent);
return fRes;
}
这里只列出几个常用的消息,如果用到其他消息,请按照格式继续往下写。调用的函数名在官方给的具体的h文件定义中。
uint8_t mavlink_msg_decode(int16_t (*Pwork_data)[10], mavlink_message_t msg)
{
int16_t (*ppPwork_data)[10] = Pwork_data;
int index = 0;
switch (msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
ppPwork_data[index][0] = ppPwork_data[index][0] + 1;
break;
}
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: //没有四元数
{
ppPwork_data[index][0] = (int16_t)mavlink_msg_set_attitude_target_get_time_boot_ms(&msg);
ppPwork_data[index][1] = (int16_t)mavlink_msg_set_attitude_target_get_target_system(&msg);
ppPwork_data[index][2] = (int16_t)mavlink_msg_set_attitude_target_get_target_component(&msg);
ppPwork_data[index][3] = (int16_t)mavlink_msg_set_attitude_target_get_type_mask(&msg);
//ppPwork_data[index][3] = (int16_t)mavlink_msg_set_attitude_target_get_q(&msg,q);
ppPwork_data[index][4] = (int16_t)mavlink_msg_set_attitude_target_get_body_roll_rate(&msg);
ppPwork_data[index][5] = (int16_t)mavlink_msg_set_attitude_target_get_body_pitch_rate(&msg);
ppPwork_data[index][6] = (int16_t)mavlink_msg_set_attitude_target_get_body_yaw_rate(&msg);
ppPwork_data[index][7] = (int16_t)mavlink_msg_set_attitude_target_get_thrust(&msg);
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
{
ppPwork_data[index][0] = (int16_t)mavlink_msg_set_position_target_local_ned_get_time_boot_ms(&msg);
ppPwork_data[index][1] = (int16_t)mavlink_msg_set_position_target_local_ned_get_target_system(&msg);
ppPwork_data[index][2] = (int16_t)mavlink_msg_set_position_target_local_ned_get_target_component(&msg);
ppPwork_data[index][3] = (int16_t)mavlink_msg_set_position_target_local_ned_get_coordinate_frame(&msg);
ppPwork_data[index][4] = (int16_t)mavlink_msg_set_position_target_local_ned_get_type_mask(&msg);
ppPwork_data[index][5] = (int16_t)mavlink_msg_set_position_target_local_ned_get_x(&msg);
ppPwork_data[index][6] = (int16_t)mavlink_msg_set_position_target_local_ned_get_y(&msg);
ppPwork_data[index][7] = (int16_t)mavlink_msg_set_position_target_local_ned_get_z(&msg);
ppPwork_data[index][8] = (int16_t)mavlink_msg_set_position_target_local_ned_get_vx(&msg);
ppPwork_data[index][9] = (int16_t)mavlink_msg_set_position_target_local_ned_get_afx(&msg);
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
ppPwork_data[index][0] = (int16_t)mavlink_msg_attitude_get_roll(&msg);
ppPwork_data[index][1] = (int16_t)mavlink_msg_attitude_get_pitch(&msg);
ppPwork_data[index][2] = (int16_t)mavlink_msg_attitude_get_yaw(&msg);
ppPwork_data[index][3] = (int16_t)mavlink_msg_attitude_get_rollspeed(&msg); //weihua, get ViconPose.X
ppPwork_data[index][4] = (int16_t)mavlink_msg_attitude_get_pitchspeed(&msg); //weihua, get ViconPose.Y
ppPwork_data[index][5] = (int16_t)mavlink_msg_attitude_get_yawspeed(&msg); //weihua, get ViconPose.Yaw
break;
}
case MAVLINK_MSG_ID_RAW_IMU:
{
ppPwork_data[index][0] = (int16_t)mavlink_msg_raw_imu_get_xacc(&msg);
ppPwork_data[index][1] = (int16_t)mavlink_msg_raw_imu_get_yacc(&msg);
ppPwork_data[index][2] = (int16_t)mavlink_msg_raw_imu_get_zacc(&msg);
ppPwork_data[index][3] = (int16_t)mavlink_msg_raw_imu_get_xgyro(&msg);
ppPwork_data[index][4] = (int16_t)mavlink_msg_raw_imu_get_ygyro(&msg);
ppPwork_data[index][5] = (int16_t)mavlink_msg_raw_imu_get_zgyro(&msg);
ppPwork_data[index][6] = (int16_t)mavlink_msg_raw_imu_get_xmag(&msg);
ppPwork_data[index][7] = (int16_t)mavlink_msg_raw_imu_get_ymag(&msg);
ppPwork_data[index][8] = (int16_t)mavlink_msg_raw_imu_get_zmag(&msg);
break;
}
case MAVLINK_MSG_ID_VFR_HUD:
{
ppPwork_data[index][0] = (int16_t)mavlink_msg_vfr_hud_get_airspeed(&msg);
ppPwork_data[index][1] = (int16_t)mavlink_msg_vfr_hud_get_groundspeed(&msg);
ppPwork_data[index][2] = (int16_t)mavlink_msg_vfr_hud_get_heading(&msg);
ppPwork_data[index][3] = (int16_t)mavlink_msg_vfr_hud_get_alt(&msg);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
ppPwork_data[index][0] = ((int16_t)mavlink_msg_global_position_int_get_lat(&msg)) / 10000000.0;
ppPwork_data[index][1] = ((int16_t)mavlink_msg_global_position_int_get_lon(&msg)) / 10000000.0;
ppPwork_data[index][2] = ((int16_t)mavlink_msg_global_position_int_get_alt(&msg)) / 1000.0;
ppPwork_data[index][3] = ((int16_t)mavlink_msg_global_position_int_get_vx(&msg)) / 100.0;
ppPwork_data[index][4] = ((int16_t)mavlink_msg_global_position_int_get_vy(&msg)) / 100.0;
ppPwork_data[index][5] = ((int16_t)mavlink_msg_global_position_int_get_vz(&msg)) / 100.0;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
ppPwork_data[index][0] = ((int16_t)mavlink_msg_rc_channels_scaled_get_chan1_scaled(&msg)) / 1000;
ppPwork_data[index][1] = ((int16_t)mavlink_msg_rc_channels_scaled_get_chan2_scaled(&msg)) / 1000;
ppPwork_data[index][2] = ((int16_t)mavlink_msg_rc_channels_scaled_get_chan3_scaled(&msg)) / 1000;
ppPwork_data[index][3] = ((int16_t)mavlink_msg_rc_channels_scaled_get_chan4_scaled(&msg)) / 1000;
ppPwork_data[index][4] = ((int16_t)mavlink_msg_rc_channels_scaled_get_chan5_scaled(&msg)) / 1000;
ppPwork_data[index][5] = ((int16_t)mavlink_msg_rc_channels_scaled_get_chan6_scaled(&msg)) / 1000;
ppPwork_data[index][6] = ((int16_t)mavlink_msg_rc_channels_scaled_get_chan7_scaled(&msg)) / 1000;
ppPwork_data[index][7] = ((int16_t)mavlink_msg_rc_channels_scaled_get_chan8_scaled(&msg)) / 1000;
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
ppPwork_data[index][0] = (int16_t)mavlink_msg_servo_output_raw_get_servo1_raw(&msg);
ppPwork_data[index][1] = (int16_t)mavlink_msg_servo_output_raw_get_servo2_raw(&msg);
ppPwork_data[index][2] = (int16_t)mavlink_msg_servo_output_raw_get_servo3_raw(&msg);
ppPwork_data[index][3] = (int16_t)mavlink_msg_servo_output_raw_get_servo4_raw(&msg);
break;
}
default:
break;
}
return (uint8_t)msg.msgid;
}
这是编程前期用来测试串口能否打开和发送数据是否成功函数。
void testfunction1()
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
//mavlink_msg_heartbeat_pack(100, 200, &msg, system_type, autopilot_type, 0, 0, 1);
//mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
// uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)
float roll, pitch, yaw, thrust;
roll = 10; pitch = 20; yaw = 30; thrust = 40;
float q[4];
q[0] = 0.01; q[1] = 0.02; q[2] = 0.03; q[3] = 0.04;
mavlink_msg_set_attitude_target_pack(100, 200, &msg, 0, system_type, autopilot_type, 0, q, roll, pitch, yaw, thrust);
//mavlink_msg_set_attitude_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
// uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q,
// float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust)
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
DWORD dwWrittenLen = 0;
//===========
OVERLAPPED osWrite = { 0 };
osWrite.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
if (osWrite.hEvent == NULL)
{
cout << "创建失败" << endl;
exit(0);
}
for (;;)
{
//mavlink_msg_heartbeat_pack(100, 200, &msg, system_type, autopilot_type, 0, 0, 1);
//len = mavlink_msg_to_send_buffer(buf, &msg);
BOOL fRes;
DWORD dwRes;
if (!WriteFile(hSerial, buf, len, &dwWrittenLen, &osWrite))
{
if (GetLastError() != ERROR_IO_PENDING)
{
// WriteFile failed, but isn't delayed. Report error and abort.
fRes = FALSE;
printf("Serial port Writing error!\n");
}
else
{
// Write is pending.
dwRes = WaitForSingleObject(osWrite.hEvent, INFINITE);
}
switch (dwRes)
{
// OVERLAPPED structure's event has been signaled.
case WAIT_OBJECT_0:
if (!GetOverlappedResult(hSerial, &osWrite, &dwWrittenLen, FALSE))
{
fRes = FALSE;
}
else
{
if (len > dwWrittenLen)
{
printf("Serial port Writing timeout!\n");
}
}
// Write operation completed successfully.
fRes = TRUE;
break;
default:
// An error has occurred in WaitForSingleObject.
// This usually indicates a problem with the
// OVERLAPPED structure's event handle.
fRes = FALSE;
break;
}
}
else
{
// WriteFile completed immediately.
fRes = TRUE;
}
}
//===========================================
int a;
cin >> a;
}
另一个测试写串口封装函数:
void testfunction2()
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint8_t* ptrBuf=buf;
int16_t len = 0;
float roll, pitch, yaw, thrust;
roll = 10; pitch = 20; yaw = 30; thrust = 40;
float q[4];
q[0] = 0.01; q[1] = 0.02; q[2] = 0.03; q[3] = 0.04;
mavlink_msg_set_attitude_target_pack(100, 200, &msg, 0, system_type, autopilot_type, 0, q, roll, pitch, yaw, thrust);
len = mavlink_msg_to_send_buffer(buf, &msg);
printf("Serial write started\n");
if (WriteBuffer(ptrBuf, (DWORD)len))
{
printf("Write OK!\n");
}
else
{
printf("Write error\n");
}
printf("Serial write stopped\n");
int a;
cin >> a;
}
可以明显看到使用封装后代码量大幅缩写,增强了代码的可读性和重用性。
#pragma once
#include
#include
#include "../mavlinklibraryv1/common/mavlink.h"
#include
using namespace std;
//Function Definition
DWORD WINAPI mavThreadRead(LPVOID lpParam);
DWORD WINAPI mavThreadWrite(LPVOID lpParam);
uint8_t mavlink_msg_decode(int16_t (*Pwork_data)[10], mavlink_message_t msg);
BOOL WriteBuffer(uint8_t* lpBuf, DWORD dwToWrite);
bool mavlink_init();
void testfunction1(); //测试直接发送
void testfunction2(); //测试发串口封装函数
//Variable Definition
int system_type = 0;
int autopilot_type = 0;
bool looping = false;
int16_t Pwork_data[1][10];
HANDLE hSerial, hThreadRead, hThreadWrite;
uint8_t BufReceive[MAVLINK_MAX_PACKET_LEN];
uint8_t BufSend[MAVLINK_MAX_PACKET_LEN];
uint16_t BufSendLen = 0;
uint16_t BufReceiveLen = 0;
uint8_t *ptrMsg_id_in;
uint8_t *ptrMsg_id_out;
bool IsDataReady = 0;
这里的../mavlinklibraryv1/common/mavlink.h文件是mavlink提供给开发者的库文件,可以在
https://github.com/mavlink/c_library_v1上打包下载。