Commit 710fffe1 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #3758 from DonLakeFlyer/CopyCoordFrame

Plan: Copy coordinate frame from prev waypoint as well
parents ddb22c89 28c0bf9e
......@@ -175,11 +175,13 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
newItem->setDefaultsForCommand();
if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) {
double lastValue;
MAV_FRAME lastFrame;
if (_findLastAcceptanceRadius(&lastValue)) {
newItem->missionItem().setParam2(lastValue);
}
if (_findLastAltitude(&lastValue)) {
if (_findLastAltitude(&lastValue, &lastFrame)) {
newItem->missionItem().setFrame(lastFrame);
newItem->missionItem().setParam7(lastValue);
}
}
......@@ -1047,23 +1049,24 @@ void MissionController::_inProgressChanged(bool inProgress)
}
}
bool MissionController::_findLastAltitude(double* lastAltitude)
bool MissionController::_findLastAltitude(double* lastAltitude, MAV_FRAME* frame)
{
bool found = false;
bool found = false;
double foundAltitude;
MAV_FRAME foundFrame;
// Don't use home position
for (int i=1; i<_visualItems->count(); i++) {
VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) {
foundAltitude = visualItem->exitCoordinate().altitude();
found = true;
if (visualItem->isSimpleItem()) {
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
if ((MAV_CMD)simpleItem->command() == MAV_CMD_NAV_TAKEOFF) {
found = false;
if ((MAV_CMD)simpleItem->command() != MAV_CMD_NAV_TAKEOFF) {
foundAltitude = simpleItem->exitCoordinate().altitude();
foundFrame = simpleItem->missionItem().frame();
found = true;
}
}
}
......@@ -1071,6 +1074,7 @@ bool MissionController::_findLastAltitude(double* lastAltitude)
if (found) {
*lastAltitude = foundAltitude;
*frame = foundFrame;
}
return found;
......
......@@ -103,7 +103,7 @@ private:
void _autoSyncSend(void);
void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
static void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
bool _findLastAltitude(double* lastAltitude);
bool _findLastAltitude(double* lastAltitude, MAV_FRAME* frame);
bool _findLastAcceptanceRadius(double* lastAcceptanceRadius);
void _addPlannedHomePosition(QmlObjectListModel* visualItems, bool addToCenter);
double _normalizeLat(double lat);
......
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