@@ -77,6 +77,11 @@ void PlanManager::writeMissionItems(const QList<MissionItem*>& missionItems)
77
77
78
78
bool skipFirstItem = _planType == MAV_MISSION_TYPE_MISSION && !_vehicle->firmwarePlugin ()->sendHomePositionToVehicle ();
79
79
80
+ if (skipFirstItem) {
81
+ // First item is not going to be moved to _writeMissionItems, free it now.
82
+ delete missionItems[0 ];
83
+ }
84
+
80
85
int firstIndex = skipFirstItem ? 1 : 0 ;
81
86
82
87
for (int i=firstIndex; i<missionItems.count (); i++) {
@@ -290,7 +295,7 @@ bool PlanManager::_checkForExpectedAck(AckType_t receivedAck)
290
295
void PlanManager::_readTransactionComplete (void )
291
296
{
292
297
qCDebug (PlanManagerLog) << " _readTransactionComplete read sequence complete" ;
293
-
298
+
294
299
SharedLinkInterfacePtr sharedLink = _vehicle->vehicleLinkManager ()->primaryLink ().lock ();
295
300
if (sharedLink) {
296
301
mavlink_message_t message;
@@ -436,7 +441,7 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message)
436
441
_vehicle->_setHomePosition (newHomePosition);
437
442
return ;
438
443
}
439
-
444
+
440
445
if (_itemIndicesToRead.contains (seq)) {
441
446
_itemIndicesToRead.removeOne (seq);
442
447
@@ -468,7 +473,7 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message)
468
473
}
469
474
470
475
emit progressPctChanged ((double )seq / (double )_missionItemCountToRead);
471
-
476
+
472
477
_retryCount = 0 ;
473
478
if (_itemIndicesToRead.count () == 0 ) {
474
479
_readTransactionComplete ();
@@ -499,7 +504,7 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message)
499
504
qCDebug (PlanManagerLog) << QStringLiteral (" _handleMissionRequest %1 Incorrect mission_type received expected:actual" ).arg (_planTypeString ()) << _planType << missionRequestMissionType;
500
505
return ;
501
506
}
502
-
507
+
503
508
if (!_checkForExpectedAck (AckMissionRequest)) {
504
509
return ;
505
510
}
@@ -520,7 +525,7 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message)
520
525
} else {
521
526
_itemIndicesToWrite.removeOne (missionRequestSeq);
522
527
}
523
-
528
+
524
529
MissionItem* item = _writeMissionItems[missionRequestSeq];
525
530
qCDebug (PlanManagerLog) << QStringLiteral (" _handleMissionRequest %1 sequenceNumber:command" ).arg (_planTypeString ()) << missionRequestSeq << item->command ();
526
531
@@ -555,7 +560,7 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message)
555
560
void PlanManager::_handleMissionAck (const mavlink_message_t & message)
556
561
{
557
562
mavlink_mission_ack_t missionAck;
558
-
563
+
559
564
mavlink_msg_mission_ack_decode (&message, &missionAck);
560
565
if (missionAck.mission_type != _planType) {
561
566
// if there was a previous transaction with a different mission_type, it can happen that we receive
@@ -575,7 +580,7 @@ void PlanManager::_handleMissionAck(const mavlink_message_t& message)
575
580
// Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what
576
581
// type of a protocol sequence we are in.
577
582
AckType_t savedExpectedAck = _expectedAck;
578
-
583
+
579
584
// We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not
580
585
// a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no
581
586
// matter what.
0 commit comments