Skip to content

Commit 1828914

Browse files
rubenp02DonLakeFlyer
authored andcommitted
Add property to check if vehicle in forward flight
Added a new property in the Vehicle class to determine if the vehicle is in forward flight. This was previously done in GuidedActionsController.qml but it's useful enough to include in the main Vehicle class. The existing logic in the GuidedActionsController module has been updated to utilize this new property.
1 parent 2cdb79b commit 1828914

File tree

3 files changed

+20
-8
lines changed

3 files changed

+20
-8
lines changed

src/FlightDisplay/GuidedActionsController.qml

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -160,8 +160,8 @@ Item {
160160
property bool showSetEstimatorOrigin: _activeVehicle && !(_activeVehicle.sensorsPresentBits & Vehicle.SysStatusSensorGPS)
161161
property bool showChangeHeading: _guidedActionsEnabled && _vehicleFlying
162162

163-
property string changeSpeedTitle: _fixedWing ? changeAirspeedTitle : changeCruiseSpeedTitle
164-
property string changeSpeedMessage: _fixedWing ? changeAirspeedMessage : changeCruiseSpeedMessage
163+
property string changeSpeedTitle: _vehicleInFwdFlight ? changeAirspeedTitle : changeCruiseSpeedTitle
164+
property string changeSpeedMessage: _vehicleInFwdFlight ? changeAirspeedMessage : changeCruiseSpeedMessage
165165

166166
// Note: The '_missionItemCount - 2' is a hack to not trigger resume mission when a mission ends with an RTL item
167167
property bool showResumeMission: _activeVehicle && !_vehicleArmed && _vehicleWasFlying && _missionAvailable && _resumeMissionIndex > 0 && (_resumeMissionIndex < _missionItemCount - 2)
@@ -190,8 +190,8 @@ Item {
190190
property bool _vehicleWasFlying: false
191191
property bool _rcRSSIAvailable: _activeVehicle ? _activeVehicle.rcRSSI > 0 && _activeVehicle.rcRSSI <= 100 : false
192192
property bool _fixedWingOnApproach: _activeVehicle ? _activeVehicle.fixedWing && _vehicleLanding : false
193-
property bool _fixedWing: _activeVehicle ? _activeVehicle.fixedWing || _activeVehicle.vtolInFwdFlight : false
194-
property bool _speedLimitsAvailable: _activeVehicle && ((_fixedWing && _activeVehicle.haveFWSpeedLimits) || (!_fixedWing && _activeVehicle.haveMRSpeedLimits))
193+
property bool _vehicleInFwdFlight: _activeVehicle ? _activeVehicle.inFwdFlight : false
194+
property bool _speedLimitsAvailable: _activeVehicle && ((_vehicleInFwdFlight && _activeVehicle.haveFWSpeedLimits) || (!_vehicleInFwdFlight && _activeVehicle.haveMRSpeedLimits))
195195
property var _gripperFunction: undefined
196196

197197
// You can turn on log output for GuidedActionsController by turning on GuidedActionsControllerLog category
@@ -226,22 +226,22 @@ Item {
226226
_unitsConversion.metersToAppSettingsVerticalDistanceUnits(_activeVehicle.minimumTakeoffAltitudeMeters()),
227227
qsTr("Height (rel)"))
228228
} else if (actionCode === actionChangeSpeed) {
229-
if (_fixedWing) {
229+
if (_vehicleInFwdFlight) {
230230
guidedValueSlider.setupSlider(
231231
GuidedValueSlider.SliderType.Speed,
232232
_unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.minimumEquivalentAirspeed()).toFixed(1),
233233
_unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumEquivalentAirspeed()).toFixed(1),
234234
_unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.airSpeed.rawValue),
235235
qsTr("Airspeed"))
236-
} else if (!_fixedWing && _activeVehicle.haveMRSpeedLimits) {
236+
} else if (!_vehicleInFwdFlight && _activeVehicle.haveMRSpeedLimits) {
237237
guidedValueSlider.setupSlider(
238238
GuidedValueSlider.SliderType.Speed,
239239
_unitsConversion.metersSecondToAppSettingsSpeedUnits(0.1).toFixed(1),
240240
_unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumHorizontalSpeedMultirotor()).toFixed(1),
241241
_unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumHorizontalSpeedMultirotor()/2).toFixed(1),
242242
qsTr("Speed"))
243243
} else {
244-
console.error("setupSlider called for inapproproate change speed action", _fixedWing, _activeVehicle.haveMRSpeedLimits)
244+
console.error("setupSlider called for inapproproate change speed action", _vehicleInFwdFlight, _activeVehicle.haveMRSpeedLimits)
245245
}
246246
} else if (actionCode === actionChangeAlt || actionCode === actionOrbit || actionCode === actionGoto || actionCode === actionPause) {
247247
guidedValueSlider.setupSlider(
@@ -686,7 +686,7 @@ Item {
686686
if (_activeVehicle) {
687687
// We need to convert back to m/s as that is what mavlink standard uses for MAV_CMD_DO_CHANGE_SPEED
688688
var metersSecondSpeed = _unitsConversion.appSettingsSpeedUnitsToMetersSecond(sliderOutputValue)
689-
if (_activeVehicle.vtolInFwdFlight || _activeVehicle.fixedWing) {
689+
if (_vehicleInFwdFlight) {
690690
_activeVehicle.guidedModeChangeEquivalentAirspeedMetersSecond(metersSecondSpeed)
691691
} else {
692692
_activeVehicle.guidedModeChangeGroundSpeedMetersSecond(metersSecondSpeed)

src/Vehicle/Vehicle.cc

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -260,6 +260,9 @@ void Vehicle::_commonInit()
260260
// Initialize alt above terrain to Nan so frontend can display it correctly in case the terrain query had no response
261261
_altitudeAboveTerrFact.setRawValue(qQNaN());
262262

263+
connect(this, &Vehicle::vehicleTypeChanged, this, &Vehicle::inFwdFlightChanged);
264+
connect(this, &Vehicle::vtolInFwdFlightChanged, this, &Vehicle::inFwdFlightChanged);
265+
263266
connect(QGCPositionManager::instance(), &QGCPositionManager::gcsPositionChanged, this, &Vehicle::_updateDistanceToGCS);
264267
connect(QGCPositionManager::instance(), &QGCPositionManager::gcsPositionChanged, this, &Vehicle::_updateHomepoint);
265268

@@ -2315,6 +2318,12 @@ void Vehicle::setGuidedMode(bool guidedMode)
23152318
return _firmwarePlugin->setGuidedMode(this, guidedMode);
23162319
}
23172320

2321+
bool Vehicle::inFwdFlight() const
2322+
{
2323+
return fixedWing() || _vtolInFwdFlight;
2324+
}
2325+
2326+
23182327
void Vehicle::emergencyStop()
23192328
{
23202329
sendMavCommand(

src/Vehicle/Vehicle.h

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -206,6 +206,7 @@ class Vehicle : public VehicleFactGroup
206206
Q_PROPERTY(QVariantList staticCameraList READ staticCameraList CONSTANT)
207207
Q_PROPERTY(QGCCameraManager* cameraManager READ cameraManager NOTIFY cameraManagerChanged)
208208
Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged)
209+
Q_PROPERTY(bool inFwdFlight READ inFwdFlight NOTIFY inFwdFlightChanged)
209210
Q_PROPERTY(bool vtolInFwdFlight READ vtolInFwdFlight WRITE setVtolInFwdFlight NOTIFY vtolInFwdFlightChanged)
210211
Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY firmwareTypeChanged)
211212
Q_PROPERTY(quint64 mavlinkSentCount READ mavlinkSentCount NOTIFY mavlinkStatusChanged)
@@ -536,6 +537,7 @@ class Vehicle : public VehicleFactGroup
536537
bool flying () const { return _flying; }
537538
bool landing () const { return _landing; }
538539
bool guidedMode () const;
540+
bool inFwdFlight () const;
539541
bool vtolInFwdFlight () const { return _vtolInFwdFlight; }
540542
uint8_t baseMode () const { return _base_mode; }
541543
uint32_t customMode () const { return _custom_mode; }
@@ -833,6 +835,7 @@ public slots:
833835
void flyingChanged (bool flying);
834836
void landingChanged (bool landing);
835837
void guidedModeChanged (bool guidedMode);
838+
void inFwdFlightChanged ();
836839
void vtolInFwdFlightChanged (bool vtolInFwdFlight);
837840
void prearmErrorChanged (const QString& prearmError);
838841
void soloFirmwareChanged (bool soloFirmware);

0 commit comments

Comments
 (0)