Commit dc848ceb authored by DonLakeFlyer's avatar DonLakeFlyer

Show/Hide Heading in waypoint item based on yaw to next wpt setting

parent 08f54ff7
...@@ -237,9 +237,13 @@ QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle) ...@@ -237,9 +237,13 @@ QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle)
bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{ {
if (!vehicle->isOfflineEditingVehicle() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { if (vehicle->isOfflineEditingVehicle()) {
Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR")); return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle);
return yawMode && yawMode->rawValue().toInt() != 0; } 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; return true;
} }
......
...@@ -518,9 +518,13 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag ...@@ -518,9 +518,13 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag
bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{ {
if (!vehicle->isOfflineEditingVehicle() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) { if (vehicle->isOfflineEditingVehicle()) {
Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE")); return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle);
return yawMode && yawMode->rawValue().toInt() == 1; } 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; return true;
} }
......
...@@ -445,7 +445,15 @@ void SimpleMissionItem::_rebuildNaNFacts(void) ...@@ -445,7 +445,15 @@ void SimpleMissionItem::_rebuildNaNFacts(void)
for (int i=1; i<=7; i++) { for (int i=1; i<=7; i++) {
const MissionCmdParamInfo* paramInfo = uiInfo->getParamInfo(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]; Fact* paramFact = rgParamFacts[i-1];
FactMetaData* paramMetaData = rgParamMetaData[i-1]; FactMetaData* paramMetaData = rgParamMetaData[i-1];
......
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