From dc848cebdf25100d24559ddf9fe30d228bb795c9 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Tue, 2 May 2017 11:19:48 -0700 Subject: [PATCH] Show/Hide Heading in waypoint item based on yaw to next wpt setting --- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 10 +++++++--- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 10 +++++++--- src/MissionManager/SimpleMissionItem.cc | 10 +++++++++- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 57094a7bd..86221320c 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -237,9 +237,13 @@ QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle) bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const { - if (!vehicle->isOfflineEditingVehicle() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { - Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR")); - return yawMode && yawMode->rawValue().toInt() != 0; + if (vehicle->isOfflineEditingVehicle()) { + return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle); + } else { + if (vehicle->multiRotor() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { + Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR")); + return yawMode && yawMode->rawValue().toInt() != 0; + } } return true; } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index ce622359f..65aa0ff45 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -518,9 +518,13 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const { - if (!vehicle->isOfflineEditingVehicle() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) { - Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE")); - return yawMode && yawMode->rawValue().toInt() == 1; + if (vehicle->isOfflineEditingVehicle()) { + return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle); + } else { + if (vehicle->multiRotor() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) { + Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE")); + return yawMode && yawMode->rawValue().toInt() == 1; + } } return true; } diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index bc982faa9..4c4877e98 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -445,7 +445,15 @@ void SimpleMissionItem::_rebuildNaNFacts(void) for (int i=1; i<=7; i++) { const MissionCmdParamInfo* paramInfo = uiInfo->getParamInfo(i); - if (paramInfo && paramInfo->nanUnchanged()) { + // Show hide Heading field on waypoint based on vehicle yaw to next waypoint setting. This needs to come from the actual vehicle if it exists + // and not _vehicle which is always offline. + Vehicle* firmwareVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); + if (!firmwareVehicle) { + firmwareVehicle = _vehicle; + } + bool hideWaypointHeading = command == MAV_CMD_NAV_WAYPOINT && firmwareVehicle->firmwarePlugin()->vehicleYawsToNextWaypointInMission(firmwareVehicle); + + if (paramInfo && paramInfo->nanUnchanged() && !hideWaypointHeading) { Fact* paramFact = rgParamFacts[i-1]; FactMetaData* paramMetaData = rgParamMetaData[i-1]; -- 2.22.0