hm/gcs-nf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改航线从缓存读取，多任务模式

Browse Source
This commit is contained in:
hm
2022-06-09 17:50:48 +08:00
parent 51b5ba03ff
commit aa199e173c
22 changed files with 989 additions and 688 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/mavlinknode.cpp
+2 -24
View File
@@ -937,22 +937,11 @@ void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
void MavLinkNode::StatusParse(mavlink_message_t msg)
{
//_vehicle vehicle = vehicleList.value(msg.sysid);
_vehicle vehicle = vehicleList.value(msg.sysid);
vehicle.sysid = msg.sysid;
vehicle.compid = msg.compid;
/*
if(LocationTime)
{
gpsTimer.ms = LocationTime->msec();
qDebug() << LocationTime->hour() << LocationTime->minute() << LocationTime->msec();
}
*/
gpsTimer.ms = QDateTime::currentDateTime().currentMSecsSinceEpoch() % 1000;
@@ -1166,17 +1155,6 @@ void MavLinkNode::StatusParse(mavlink_message_t msg)
gpsTimer.min = min;
gpsTimer.sec = sec;
/*
if(vehicle.ins1.satellites_visible >= 7)
{
if(!LocationTime)
{
LocationTime = new QTime();
LocationTime->setHMS(hour,min,sec);
LocationTime->start();
}
}
*/
@@ -1707,7 +1685,7 @@ void MavLinkNode::StatusParse(mavlink_message_t msg)
//vehicleList.insert(msg.sysid,vehicle);//直接覆盖
vehicleList.insert(msg.sysid,vehicle);//直接覆盖
//emit signal_vehicle(vehicle);
MavLinkNode/mavlinknode.h
+1 -2
View File
@@ -123,8 +123,7 @@ public:
QByteArray rtkrawdata;
QByteArray rtkrawdata;
QFile * autopilot_version_file = nullptr;
QFile * sys_status_file = nullptr;
MavLinkNode/missionprocess.cpp
+105 -1
View File
@@ -52,6 +52,93 @@ void MissionProcess::process()//线程函数
}
}
void MissionProcess::checkoutVehicle(int sysid)
{
QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(sysid);//读出一个飞机的所有航线
QMap<int,mavlink_mission_item_int_t> group1 = missionGroup.value(1);
QMap<int,mavlink_mission_item_int_t> group3 = missionGroup.value(3);
QMap<int,mavlink_mission_item_int_t> group4 = missionGroup.value(4);
QMap<int,mavlink_mission_item_int_t> group5 = missionGroup.value(5);
QMap<int,mavlink_mission_item_int_t> group6 = missionGroup.value(6);
foreach (mavlink_mission_item_int_t item, group1) {
//把航点发出去
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
item.x,item.y,item.z,
item.seq,item.mission_type,
item.command,
item.target_system,
item.target_component,
item.frame,
item.current,
item.autocontinue,
item.mission_type);
}
foreach (mavlink_mission_item_int_t item, group3) {
//把航点发出去
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
item.x,item.y,item.z,
item.seq,item.mission_type,
item.command,
item.target_system,
item.target_component,
item.frame,
item.current,
item.autocontinue,
item.mission_type);
}
foreach (mavlink_mission_item_int_t item, group4) {
//把航点发出去
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
item.x,item.y,item.z,
item.seq,item.mission_type,
item.command,
item.target_system,
item.target_component,
item.frame,
item.current,
item.autocontinue,
item.mission_type);
}
foreach (mavlink_mission_item_int_t item, group5) {
//把航点发出去
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
item.x,item.y,item.z,
item.seq,item.mission_type,
item.command,
item.target_system,
item.target_component,
item.frame,
item.current,
item.autocontinue,
item.mission_type);
}
foreach (mavlink_mission_item_int_t item, group6) {
//把航点发出去
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
item.x,item.y,item.z,
item.seq,item.mission_type,
item.command,
item.target_system,
item.target_component,
item.frame,
item.current,
item.autocontinue,
item.mission_type);
}
}
void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,int m_group)
{
if(mission_status.m_Mode == Nop_Mode)//没有任务正在下载
@@ -239,9 +326,17 @@ void MissionProcess::Parse(mavlink_message_t msg)
if(mission_item_int.seq == 0)
{
emit clearWaypoint();
QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(msg.sysid);//读出一个飞机的所有航线
QMap<int,mavlink_mission_item_int_t > points = missionGroup.value(mission_item_int.mission_type);//读出一组
points.clear();
missionGroup.insert(mission_item_int.mission_type,points);
missions.insert(msg.sysid,missionGroup);
}
//把航点发出去
//把航点发出去
emit receivedPoint(mission_item_int.param1,mission_item_int.param2,mission_item_int.param3,mission_item_int.param4,
mission_item_int.x,mission_item_int.y,mission_item_int.z,
mission_item_int.seq,mission_item_int.mission_type,
@@ -255,6 +350,15 @@ void MissionProcess::Parse(mavlink_message_t msg)
qDebug() << "recieve mission type" << mission_item_int.mission_type;
QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(msg.sysid);//读出一个飞机的所有航线
QMap<int,mavlink_mission_item_int_t > points = missionGroup.value(mission_item_int.mission_type);//读出一组
points.insert(mission_item_int.seq,mission_item_int);
missionGroup.insert(mission_item_int.mission_type,points);
missions.insert(msg.sysid,missionGroup);
mission_status.recieve.isWaitingforItem = false;//已经收到航点，不用再等待
}break;
case MAVLINK_MSG_ID_MISSION_ITEM: {
MavLinkNode/missionprocess.h
+20
View File
@@ -70,6 +70,11 @@ public:
int currentGroup = 3;
QMap<int,QMap<int,QMap<int,mavlink_mission_item_int_t >>> missions;//SysID 航线组 航线
public slots:
void Parse(mavlink_message_t msg);
@@ -104,6 +109,9 @@ public slots:
uint16_t compid,
uint16_t mission_type);
void checkoutVehicle(int sysid);
private slots:
//线程私有接口
void process();
@@ -163,6 +171,18 @@ signals:
uint8_t autocontinue,
uint8_t mission_type);
void vehicleChanged(float param1,float param2,float param3,float param4,
int32_t x,int32_t y,float z,
uint16_t seq,
uint16_t group,
uint16_t command,
uint8_t target_system,
uint8_t target_component,
uint8_t frame,
uint8_t current,
uint8_t autocontinue,
uint8_t mission_type);
//void currentGroup(int group);
void currentPoint(int seq);