添加所有的航线相关的函数
This commit is contained in:
@@ -57,6 +57,12 @@ void MissionProcess::process()//线程函数
|
||||
QThread::msleep(1000/running_frq);
|
||||
//状态机
|
||||
|
||||
//读取航线
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//如果要读取航线，线发送读取指令，然后等待返回，如果返回，那么启动线程进入循环接收
|
||||
|
||||
@@ -75,8 +81,7 @@ void MissionProcess::MissionParse(mavlink_message_t msg)
|
||||
//航线部分
|
||||
case MAVLINK_MSG_ID_MISSION_COUNT: {//飞控发来计数值，然后开始读取
|
||||
mavlink_msg_mission_count_decode(&msg,&mission_count);
|
||||
|
||||
//Mavlink_msg_mission_request_int(0);
|
||||
isRecieveCount = true;//收到了计数，那可以开始读取第一个
|
||||
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_MISSION_ITEM: {
|
||||
@@ -107,9 +112,313 @@ void MissionProcess::MissionParse(mavlink_message_t msg)
|
||||
}
|
||||
|
||||
|
||||
/*All msg
|
||||
*
|
||||
* MISSION_REQUEST_LIST
|
||||
* MISSION_COUNT
|
||||
* MISSION_REQUEST_INT
|
||||
* MISSION_REQUEST
|
||||
* MISSION_ITEM_INT
|
||||
* MISSION_ITEM
|
||||
* MISSION_ACK
|
||||
* MISSION_CURRENT
|
||||
* MISSION_SET_CURRENT
|
||||
* STATUSTEXT
|
||||
* MISSION_CLEAR_ALL
|
||||
* MISSION_ITEM_REACHED
|
||||
* MISSION_REQUEST_PARTIAL_LIST
|
||||
* MISSION_WRITE_PARTIAL_LIST
|
||||
*
|
||||
*
|
||||
**/
|
||||
|
||||
//读航线状态机
|
||||
void MissionProcess::ReadStateMachine(void)
|
||||
{
|
||||
static uint8_t step = 0;
|
||||
|
||||
if(isReadMission == true) //如果收到读取航线的指令
|
||||
{
|
||||
if(step == 0)//第一步 发送请求
|
||||
{
|
||||
//MISSION_REQUEST_LIST
|
||||
//启动定时器，等待超时
|
||||
}
|
||||
else if(step == 1)//等待收到Count
|
||||
{
|
||||
//如果收到MISSION_COUNT
|
||||
//step ++;
|
||||
|
||||
}
|
||||
else if(step ==3)//请求航点
|
||||
{
|
||||
//
|
||||
//如果当前航点没有是第（n-1）航点，就正常传输MISSION_ITEM_INT，MISSION_ITEM
|
||||
//如果当前已经是第（n-1）个，那么下一步step++,
|
||||
}
|
||||
else if(step == 4)//航线传输结束，发送ack
|
||||
{
|
||||
//发送 MISSION_ACK
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void MissionProcess::request_list(void)//读取整列请求
|
||||
{
|
||||
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_request_list_t mission_request_list;
|
||||
|
||||
|
||||
|
||||
mission_request_list.mission_type = 0;
|
||||
mission_request_list.target_system = vehicle.sysid;
|
||||
mission_request_list.target_component = vehicle.compid;
|
||||
//打包消息(到MAVLink_msg)
|
||||
MAVLink_Len = mavlink_msg_mission_request_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request_list);
|
||||
//根据消息得到Buf和Len
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
|
||||
//通过串口发送
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
|
||||
qDebug() << "mission request list";
|
||||
|
||||
}
|
||||
|
||||
void MissionProcess::count(uint16_t count)//计数值
|
||||
{
|
||||
static mavlink_message_t MAVLink_Msg;
|
||||
uint8_t MAVLink_Buf[256+20];
|
||||
uint16_t MAVLink_Len;
|
||||
static mavlink_mission_count_t mission_count;
|
||||
|
||||
mission_count.count = count;
|
||||
mission_count.mission_type = 0;
|
||||
mission_count.target_system = vehicle.sysid;
|
||||
mission_count.target_component = vehicle.compid;
|
||||
|
||||
MAVLink_Len = mavlink_msg_mission_count_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_count);
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
}
|
||||
|
||||
void MissionProcess::request_int(uint16_t seq)//读取请求
|
||||
{
|
||||
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_request_int_t mission_request_int;
|
||||
|
||||
mission_request_int.seq = seq;
|
||||
mission_request_int.mission_type = 0;
|
||||
mission_request_int.target_system = vehicle.sysid;
|
||||
mission_request_int.target_component = vehicle.compid;
|
||||
|
||||
MAVLink_Len = mavlink_msg_mission_request_int_encode(2, MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request_int);
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
}
|
||||
|
||||
void MissionProcess::request(uint16_t seq)//读取请求
|
||||
{
|
||||
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_request_t mission_request;
|
||||
|
||||
mission_request.mission_type = 0;
|
||||
mission_request.seq = seq;
|
||||
mission_request.target_system = vehicle.sysid;
|
||||
mission_request.target_component = vehicle.compid;
|
||||
MAVLink_Len = mavlink_msg_mission_request_encode(2, MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request);
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
}
|
||||
|
||||
void MissionProcess::item_int(float param1,float param2,float param3,float param4,
|
||||
float x,float y,float z,
|
||||
uint16_t seq,uint16_t command,
|
||||
uint8_t frame,uint8_t current,
|
||||
uint8_t autocontinue,uint8_t mission_type)
|
||||
{
|
||||
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_item_t mission_item;
|
||||
|
||||
mission_item.target_system = vehicle.sysid;
|
||||
mission_item.target_component = vehicle.compid;
|
||||
|
||||
mission_item.param1 = param1;
|
||||
mission_item.param2 = param2;
|
||||
mission_item.param3 = param3;
|
||||
mission_item.param4 = param4;
|
||||
|
||||
mission_item.x = x;
|
||||
mission_item.y = y;
|
||||
mission_item.z = z;
|
||||
|
||||
mission_item.seq = seq;
|
||||
mission_item.command = command;
|
||||
mission_item.frame = frame;
|
||||
mission_item.current = current;
|
||||
mission_item.autocontinue = autocontinue;
|
||||
mission_item.mission_type = mission_type;
|
||||
|
||||
MAVLink_Len = mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_item);
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
}
|
||||
|
||||
void MissionProcess::item(float param1,float param2,float param3,float param4,
|
||||
float x,float y,float z,
|
||||
uint16_t seq,uint16_t command,
|
||||
uint8_t frame,uint8_t current,
|
||||
uint8_t autocontinue,uint8_t mission_type)
|
||||
{
|
||||
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_item_t mission_item;
|
||||
|
||||
mission_item.target_system = vehicle.sysid;
|
||||
mission_item.target_component = vehicle.compid;
|
||||
|
||||
mission_item.param1 = param1;
|
||||
mission_item.param2 = param2;
|
||||
mission_item.param3 = param3;
|
||||
mission_item.param4 = param4;
|
||||
|
||||
mission_item.x = x;
|
||||
mission_item.y = y;
|
||||
mission_item.z = z;
|
||||
|
||||
mission_item.seq = seq;
|
||||
mission_item.command = command;
|
||||
mission_item.frame = frame;
|
||||
mission_item.current = current;
|
||||
mission_item.autocontinue = autocontinue;
|
||||
mission_item.mission_type = mission_type;
|
||||
|
||||
//打包消息(到MAVLink_msg)
|
||||
MAVLink_Len = mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_item);
|
||||
//根据消息得到Buf和Len
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
|
||||
//通过串口发送
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
}
|
||||
|
||||
void MissionProcess::ack(void)
|
||||
{
|
||||
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_ack_t mission_ack;
|
||||
|
||||
mission_ack.target_system = vehicle.sysid;
|
||||
mission_ack.target_component = vehicle.compid;
|
||||
mission_ack.mission_type = 0;
|
||||
mission_ack.type = 0;
|
||||
|
||||
//打包消息(到MAVLink_msg)
|
||||
MAVLink_Len = mavlink_msg_mission_ack_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_ack);
|
||||
//根据消息得到Buf和Len
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
|
||||
//通过串口发送
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
|
||||
|
||||
}
|
||||
|
||||
void MissionProcess::current(void)
|
||||
{
|
||||
static mavlink_message_t msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_set_current_t mission_current;
|
||||
|
||||
//打包消息(到MAVLink_msg)
|
||||
MAVLink_Len = mavlink_msg_mission_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_current);
|
||||
//根据消息得到Buf和Len
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
|
||||
//通过串口发送
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
}
|
||||
|
||||
void MissionProcess::setcurrent(void)
|
||||
{
|
||||
static mavlink_message_t msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_set_current_t mission_set_current;
|
||||
|
||||
//打包消息(到MAVLink_msg)
|
||||
MAVLink_Len = mavlink_msg_mission_set_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_set_current);
|
||||
//根据消息得到Buf和Len
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
|
||||
//通过串口发送
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
}
|
||||
|
||||
void MissionProcess::clear_all(void)
|
||||
{
|
||||
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
|
||||
|
||||
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
|
||||
uint16_t MAVLink_Len; //发送的长度
|
||||
static mavlink_mission_clear_all_t mission_clear_all;
|
||||
|
||||
mission_clear_all.target_system = vehicle.sysid;
|
||||
mission_clear_all.target_component = vehicle.compid;
|
||||
mission_clear_all.mission_type = 0;
|
||||
|
||||
//打包消息(到MAVLink_msg)
|
||||
MAVLink_Len = mavlink_msg_mission_clear_all_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_clear_all);
|
||||
//根据消息得到Buf和Len
|
||||
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
|
||||
//通过串口发送
|
||||
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user