Skip to content

Commit

Permalink
Add set estimator origin support
Browse files Browse the repository at this point in the history 
Only available if vehicle has not GPS
  • Loading branch information
@DonLakeFlyer
DonLakeFlyer committed Jan 21, 2024
1 parent 6fb1094 commit 0e006c2
Show file tree
Hide file tree
Showing 4 changed files with 71 additions and 21 deletions.
15 changes: 14 additions & 1 deletion src/FlightDisplay/FlyViewMap.qml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -614,11 +614,24 @@ FlightMap {
globals.guidedControllerFlyView.confirmAction(globals.guidedControllerFlyView.actionSetHome, clickMenu.coord)
}
}

QGCButton {
Layout.fillWidth: true
text: qsTr("Set Estimator Origin")
visible: globals.guidedControllerFlyView.showSetEstimatorOrigin
onClicked: {
if (clickMenu.opened) {
clickMenu.close()
}
globals.guidedControllerFlyView.confirmAction(globals.guidedControllerFlyView.actionSetEstimatorOrigin, clickMenu.coord)
}
}
}
}

onMapClicked: (mouse) => {
if (!globals.guidedControllerFlyView.guidedUIVisible && (globals.guidedControllerFlyView.showGotoLocation || globals.guidedControllerFlyView.showOrbit || globals.guidedControllerFlyView.showROI || globals.guidedControllerFlyView.showSetHome)) {
if (!globals.guidedControllerFlyView.guidedUIVisible &&
(globals.guidedControllerFlyView.showGotoLocation || globals.guidedControllerFlyView.showOrbit || globals.guidedControllerFlyView.showROI || globals.guidedControllerFlyView.showSetHome || globals.guidedControllerFlyView.showSetEstimatorOrigin)) {
orbitMapCircle.hide()
gotoLocationItem.hide()
var clickCoord = _root.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */)
Expand Down
51 changes: 32 additions & 19 deletions src/FlightDisplay/GuidedActionsController.qml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ Item {
readonly property string roiTitle: qsTr("ROI")
readonly property string setHomeTitle: qsTr("Set Home")
readonly property string actionListTitle: qsTr("Action")
readonly property string setEstimatorOriginTitle: qsTr("Set Estimator origin")

readonly property string armMessage: qsTr("Arm the vehicle.")
readonly property string forceArmMessage: qsTr("WARNING: This will force arming of the vehicle bypassing any safety checks.")
Expand All @@ -83,6 +84,7 @@ Item {
readonly property string vtolTransitionMRMessage: qsTr("Transition VTOL to multi-rotor flight.")
readonly property string roiMessage: qsTr("Make the specified location a Region Of Interest.")
readonly property string setHomeMessage: qsTr("Set vehicle home as the specified location. This will affect Return to Home position")
readonly property string setEstimatorOriginMessage: qsTr("Make the specified location the estimator origin.")

readonly property int actionRTL: 1
readonly property int actionLand: 2
Expand Down Expand Up @@ -111,6 +113,8 @@ Item {
readonly property int actionChangeSpeed: 25
readonly property int actionGripper: 26
readonly property int actionSetHome: 27
readonly property int actionSetEstimatorOrigin: 28


property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property bool _useChecklist: QGroundControl.settingsManager.appSettings.useChecklist.rawValue && QGroundControl.corePlugin.options.preFlightChecklistUrl.toString().length
Expand All @@ -121,25 +125,27 @@ Item {
property bool _canStartMission: _activeVehicle ? (_checklistPassed && (!_activeVehicle.healthAndArmingCheckReport.supported || _activeVehicle.healthAndArmingCheckReport.canStartMission)) : false
property bool _initialConnectComplete: _activeVehicle ? _activeVehicle.initialConnectComplete : false

property bool showEmergenyStop: _guidedActionsEnabled && !_hideEmergenyStop && _vehicleArmed && _vehicleFlying
property bool showArm: _guidedActionsEnabled && !_vehicleArmed && _canArm
property bool showForceArm: _guidedActionsEnabled && !_vehicleArmed
property bool showDisarm: _guidedActionsEnabled && _vehicleArmed && !_vehicleFlying
property bool showRTL: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.guidedModeSupported && _vehicleFlying && !_vehicleInRTLMode
property bool showTakeoff: _guidedActionsEnabled && _activeVehicle.takeoffVehicleSupported && !_vehicleFlying && _canTakeoff
property bool showLand: _guidedActionsEnabled && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode
property bool showStartMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && !_vehicleFlying && _canStartMission
property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleArmed && _vehicleFlying && (_currentMissionIndex < _missionItemCount - 1)
property bool showPause: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused && !_fixedWingOnApproach
property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive
property bool showChangeSpeed: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive && _speedLimitsAvailable
property bool showOrbit: _guidedActionsEnabled && _vehicleFlying && __orbitSupported && !_missionActive
property bool showROI: _guidedActionsEnabled && _vehicleFlying && __roiSupported && !_missionActive
property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _fixedWingOnApproach
property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying
property bool showSetHome: _guidedActionsEnabled
property bool showActionList: _guidedActionsEnabled && (showStartMission || showResumeMission || showChangeAlt || showLandAbort || actionList.hasCustomActions)
property bool showGripper: _initialConnectComplete ? _activeVehicle.hasGripper : false
property bool showEmergenyStop: _guidedActionsEnabled && !_hideEmergenyStop && _vehicleArmed && _vehicleFlying
property bool showArm: _guidedActionsEnabled && !_vehicleArmed && _canArm
property bool showForceArm: _guidedActionsEnabled && !_vehicleArmed
property bool showDisarm: _guidedActionsEnabled && _vehicleArmed && !_vehicleFlying
property bool showRTL: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.guidedModeSupported && _vehicleFlying && !_vehicleInRTLMode
property bool showTakeoff: _guidedActionsEnabled && _activeVehicle.takeoffVehicleSupported && !_vehicleFlying && _canTakeoff
property bool showLand: _guidedActionsEnabled && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode
property bool showStartMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && !_vehicleFlying && _canStartMission
property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleArmed && _vehicleFlying && (_currentMissionIndex < _missionItemCount - 1)
property bool showPause: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused && !_fixedWingOnApproach
property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive
property bool showChangeSpeed: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive && _speedLimitsAvailable
property bool showOrbit: _guidedActionsEnabled && _vehicleFlying && __orbitSupported && !_missionActive
property bool showROI: _guidedActionsEnabled && _vehicleFlying && __roiSupported && !_missionActive
property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _fixedWingOnApproach
property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying
property bool showSetHome: _guidedActionsEnabled
property bool showActionList: _guidedActionsEnabled && (showStartMission || showResumeMission || showChangeAlt || showLandAbort || actionList.hasCustomActions)
property bool showGripper: _initialConnectComplete ? _activeVehicle.hasGripper : false
property bool showSetEstimatorOrigin: _activeVehicle && !(_activeVehicle.sensorsPresentBits & Vehicle.SysStatusSensorGPS)

property string changeSpeedTitle: _fixedWing ? changeAirspeedTitle : changeCruiseSpeedTitle
property string changeSpeedMessage: _fixedWing ? changeAirspeedMessage : changeCruiseSpeedMessage

Expand Down Expand Up @@ -519,6 +525,10 @@ Item {
confirmDialog.message = setHomeMessage
confirmDialog.hideTrigger = Qt.binding(function() { return !showSetHome })
break
case actionSetEstimatorOrigin:
confirmDialog.title = setEstimatorOriginTitle
confirmDialog.message = setEstimatorOriginMessage
break
default:
console.warn("Unknown actionCode", actionCode)
return
Expand Down Expand Up @@ -616,6 +626,9 @@ Item {
case actionSetHome:
_activeVehicle.doSetHome(actionData)
break
case actionSetEstimatorOrigin:
_activeVehicle.setEstimatorOrigin(actionData)
break
default:
console.warn(qsTr("Internal error: unknown actionCode"), actionCode)
break
Expand Down
25 changes: 24 additions & 1 deletion src/Vehicle/Vehicle.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4541,6 +4541,29 @@ void Vehicle::sendGripperAction(GRIPPER_OPTIONS gripperOption)
qDebug("unknown function");
break;
default:
break;
break;
}
}

void Vehicle::setEstimatorOrigin(const QGeoCoordinate& centerCoord)
{
SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock();
if (!sharedLink) {
qCDebug(VehicleLog) << "setEstimatorOrigin: primary link gone!";
return;
}

mavlink_message_t msg;
mavlink_msg_set_gps_global_origin_pack_chan(
_mavlink->getSystemId(),
_mavlink->getComponentId(),
sharedLink->mavlinkChannel(),
&msg,
id(),
centerCoord.latitude() * 1e7,
centerCoord.longitude() * 1e7,
centerCoord.altitude() * 1e3,
static_cast<float>(qQNaN())
);
sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
}
1 change: 1 addition & 0 deletions src/Vehicle/Vehicle.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -442,6 +442,7 @@ class Vehicle : public FactGroup
Q_INVOKABLE void clearMessages();

Q_INVOKABLE void sendPlan(QString planFile);
Q_INVOKABLE void setEstimatorOrigin(const QGeoCoordinate& centerCoord);

/// Used to check if running current version is equal or higher than the one being compared.
// returns 1 if current > compare, 0 if current == compare, -1 if current < compare
Expand Down

0 comments on commit 0e006c2

Please sign in to comment.