Browse Source

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

QGC4.4
DonLakeFlyer 8 years ago
parent
commit
dc848cebdf
  1. 10
      src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
  2. 10
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  3. 10
      src/MissionManager/SimpleMissionItem.cc

10
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@ -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
{
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;
}

10
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@ -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
{
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;
}

10
src/MissionManager/SimpleMissionItem.cc

@ -445,7 +445,15 @@ void SimpleMissionItem::_rebuildNaNFacts(void) @@ -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];

Loading…
Cancel
Save