Commit 0b156cdd authored by DonLakeFlyer's avatar DonLakeFlyer

Handle autopilot home position updates

parent 736947a2
......@@ -409,11 +409,6 @@ void MissionManager::_requestNextMissionItem(void)
void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt)
{
if (!_checkForExpectedAck(AckMissionItem)) {
return;
}
MAV_CMD command;
MAV_FRAME frame;
double param1;
......@@ -468,7 +463,24 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current" << seq << command << isCurrentItem;
bool ardupilotHomePositionUpdate = false;
if (!_checkForExpectedAck(AckMissionItem)) {
if (_vehicle->apmFirmware() && seq == 0) {
ardupilotHomePositionUpdate = true;
} else {
qCDebug(MissionManagerLog) << "_handleMissionItem dropping spurious item seq:command:current" << seq << command << isCurrentItem;
return;
}
}
qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current:ardupilotHomePositionUpdate" << seq << command << isCurrentItem << ardupilotHomePositionUpdate;
if (ardupilotHomePositionUpdate) {
QGeoCoordinate newHomePosition(param5, param6, param7);
_vehicle->_setHomePosition(newHomePosition);
return;
}
if (_itemIndicesToRead.contains(seq)) {
_itemIndicesToRead.removeOne(seq);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment