zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Update vehicle heading from possible item change

QGC4.4
DonLakeFlyer 5 years ago
parent
d5f7442e2b
commit
2695258139
4 changed files with 30 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 11
      src/MissionManager/MissionController.cc
  2. 13
      src/MissionManager/SimpleMissionItem.cc
  3. 2
      src/MissionManager/SimpleMissionItem.h
  4. 9
      src/MissionManager/VisualMissionItem.h

11
src/MissionManager/MissionController.cc
Unescape View File

@ -1484,6 +1484,7 @@ void MissionController::_recalcMissionFlightStatus() @@ -1484,6 +1484,7 @@ void MissionController::_recalcMissionFlightStatus()
bool vtolInHover = true;
bool linkStartToHome = false;
bool foundRTL = false;
bool vehicleYawSpecificallySet = false;
for (int i=0; i<_visualItems->count(); i++) {
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
@ -1614,9 +1615,14 @@ void MissionController::_recalcMissionFlightStatus() @@ -1614,9 +1615,14 @@ void MissionController::_recalcMissionFlightStatus()
if (!item->isStandaloneCoordinate()) {
firstCoordinateItem = false;
// Update vehicle yaw assuming direction to next waypoint
// Update vehicle yaw assuming direction to next waypoint and/or mission item change
if (item != lastCoordinateItemBeforeRTL) {
_missionFlightStatus.vehicleYaw = lastCoordinateItemBeforeRTL->exitCoordinate().azimuthTo(item->coordinate());
if (simpleItem && !qIsNaN(simpleItem->specifiedVehicleYaw())) {
vehicleYawSpecificallySet = true;
_missionFlightStatus.vehicleYaw = simpleItem->specifiedVehicleYaw();
} else if (!vehicleYawSpecificallySet) {
_missionFlightStatus.vehicleYaw = lastCoordinateItemBeforeRTL->exitCoordinate().azimuthTo(item->coordinate());
}
lastCoordinateItemBeforeRTL->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw);
}
@ -1868,6 +1874,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem) @@ -1868,6 +1874,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedVehicleYawChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::terrainAltitudeChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence);

13
src/MissionManager/SimpleMissionItem.cc
Unescape View File

@ -177,6 +177,7 @@ void SimpleMissionItem::_connectSignals(void) @@ -177,6 +177,7 @@ void SimpleMissionItem::_connectSignals(void)
connect(&_missionItem._param7Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged);
connect(&_missionItem._param1Fact, &Fact::valueChanged, this, &SimpleMissionItem::_possibleAdditionalTimeDelayChanged);
connect(&_missionItem._param4Fact, &Fact::valueChanged, this, &SimpleMissionItem::_possibleVehicleYawChanged);
// The following changes may also change friendlyEditAllowed
connect(&_missionItem._autoContinueFact, &Fact::valueChanged, this, &SimpleMissionItem::_sendFriendlyEditAllowedChanged);
@ -834,6 +835,18 @@ double SimpleMissionItem::specifiedGimbalPitch(void) @@ -834,6 +835,18 @@ double SimpleMissionItem::specifiedGimbalPitch(void)
return _cameraSection->available() ? _cameraSection->specifiedGimbalPitch() : missionItem().specifiedGimbalPitch();
}
double SimpleMissionItem::specifiedVehicleYaw(void)
{
return command() == MAV_CMD_NAV_WAYPOINT ? missionItem().param4() : qQNaN();
}
void SimpleMissionItem::_possibleVehicleYawChanged(void)
{
if (command() == MAV_CMD_NAV_WAYPOINT) {
emit specifiedVehicleYawChanged();
}
}
bool SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle)
{
bool sectionFound = false;

2
src/MissionManager/SimpleMissionItem.h
Unescape View File

@ -114,6 +114,7 @@ public: @@ -114,6 +114,7 @@ public:
double specifiedFlightSpeed (void) override;
double specifiedGimbalYaw (void) override;
double specifiedGimbalPitch (void) override;
double specifiedVehicleYaw (void) override;
QString mapVisualQML (void) const override { return QStringLiteral("SimpleItemMapVisual.qml"); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
@ -155,6 +156,7 @@ private slots: @@ -155,6 +156,7 @@ private slots:
void _rebuildTextFieldFacts (void);
void _possibleAdditionalTimeDelayChanged(void);
void _setDefaultsForCommand (void);
void _possibleVehicleYawChanged (void);
private:
void _connectSignals (void);

9
src/MissionManager/VisualMissionItem.h
Unescape View File

@ -70,9 +70,10 @@ public: @@ -70,9 +70,10 @@ public:
Q_PROPERTY(QString editorQml MEMBER _editorQml CONSTANT) ///< Qml code for editing this item
Q_PROPERTY(QString mapVisualQML READ mapVisualQML CONSTANT) ///< QMl code for map visuals
Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT)
Q_PROPERTY(double specifiedFlightSpeed READ specifiedFlightSpeed NOTIFY specifiedFlightSpeedChanged) ///< NaN if this item does not specify flight speed
Q_PROPERTY(double specifiedGimbalYaw READ specifiedGimbalYaw NOTIFY specifiedGimbalYawChanged) ///< Gimbal yaw, NaN for not specified
Q_PROPERTY(double specifiedGimbalPitch READ specifiedGimbalPitch NOTIFY specifiedGimbalPitchChanged) ///< Gimbal pitch, NaN for not specified
Q_PROPERTY(double specifiedFlightSpeed READ specifiedFlightSpeed NOTIFY specifiedFlightSpeedChanged) ///< NaN for not specified
Q_PROPERTY(double specifiedGimbalYaw READ specifiedGimbalYaw NOTIFY specifiedGimbalYawChanged) ///< NaN for not specified
Q_PROPERTY(double specifiedGimbalPitch READ specifiedGimbalPitch NOTIFY specifiedGimbalPitchChanged) ///< NaN for not specified
Q_PROPERTY(double specifiedVehicleYaw READ specifiedVehicleYaw NOTIFY specifiedVehicleYawChanged) ///< NaN for not specified
Q_PROPERTY(double missionGimbalYaw READ missionGimbalYaw NOTIFY missionGimbalYawChanged) ///< Current gimbal yaw state at this point in mission
Q_PROPERTY(double missionVehicleYaw READ missionVehicleYaw NOTIFY missionVehicleYawChanged) ///< Expected vehicle yaw at this point in mission
Q_PROPERTY(bool flyView READ flyView CONSTANT)
@ -140,6 +141,7 @@ public: @@ -140,6 +141,7 @@ public:
virtual double specifiedFlightSpeed (void) = 0;
virtual double specifiedGimbalYaw (void) = 0;
virtual double specifiedGimbalPitch (void) = 0;
virtual double specifiedVehicleYaw (void) { return qQNaN(); }
//-- Default implementation returns an invalid bounding cube
virtual QGCGeoBoundingCube* boundingCube (void) { return &_boundingCube; }
@ -210,6 +212,7 @@ signals: @@ -210,6 +212,7 @@ signals:
void specifiedFlightSpeedChanged (void);
void specifiedGimbalYawChanged (void);
void specifiedGimbalPitchChanged (void);
void specifiedVehicleYawChanged (void);
void lastSequenceNumberChanged (int sequenceNumber);
void missionGimbalYawChanged (double missionGimbalYaw);
void missionVehicleYawChanged (double missionVehicleYaw);

Write Preview
Loading…
Cancel
Save