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

Support for vehicle yaw to structure

QGC4.4
DonLakeFlyer 8 years ago
parent
c670117339
commit
59ddfdff6a
5 changed files with 99 additions and 24 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. 3
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  2. 2
      src/MissionManager/CameraCalc.FactMetaData.json
  3. 88
      src/MissionManager/StructureScanComplexItem.cc
  4. 5
      src/MissionManager/StructureScanComplexItem.h
  5. 25
      src/PlanView/StructureScanEditor.qml

3
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
Unescape View File

@ -277,7 +277,8 @@ QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void) @@ -277,7 +277,8 @@ QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
<< MAV_CMD_DO_MOUNT_CONTROL
<< MAV_CMD_SET_CAMERA_MODE
<< MAV_CMD_IMAGE_START_CAPTURE << MAV_CMD_IMAGE_STOP_CAPTURE << MAV_CMD_VIDEO_START_CAPTURE << MAV_CMD_VIDEO_STOP_CAPTURE
<< MAV_CMD_NAV_DELAY;
<< MAV_CMD_NAV_DELAY
<< MAV_CMD_CONDITION_YAW;
return list;
}

2
src/MissionManager/CameraCalc.FactMetaData.json
Unescape View File

@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@
"min": 0.1,
"units": "m",
"decimalPlaces": 2,
"defaultValue": 100.0
"defaultValue": 10.0
},
{
"name": "ImageDensity",

88
src/MissionManager/StructureScanComplexItem.cc
Unescape View File

@ -21,11 +21,12 @@ @@ -21,11 +21,12 @@
QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog")
const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan";
const char* StructureScanComplexItem::_altitudeFactName = "Altitude";
const char* StructureScanComplexItem::_layersFactName = "Layers";
const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc";
const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative";
const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan";
const char* StructureScanComplexItem::_altitudeFactName = "Altitude";
const char* StructureScanComplexItem::_layersFactName = "Layers";
const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc";
const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative";
const char* StructureScanComplexItem::_jsonYawVehicleToStructureKey = "yawVehicleToStructure";
QMap<QString, FactMetaData*> StructureScanComplexItem::_metaDataMap;
@ -40,6 +41,7 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa @@ -40,6 +41,7 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa
, _cameraShots (0)
, _cameraMinTriggerInterval (0)
, _cameraCalc (vehicle)
, _yawVehicleToStructure (true)
, _altitudeFact (0, _altitudeFactName, FactMetaData::valueTypeDouble)
, _layersFact (0, _layersFactName, FactMetaData::valueTypeUint32)
{
@ -73,6 +75,10 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa @@ -73,6 +75,10 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa
connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_flightPathChanged);
connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon);
connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_recalcCameraShots);
connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &StructureScanComplexItem::_recalcCameraShots);
connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcCameraShots);
}
void StructureScanComplexItem::_setScanDistance(double scanDistance)
@ -129,9 +135,10 @@ void StructureScanComplexItem::save(QJsonArray& missionItems) @@ -129,9 +135,10 @@ void StructureScanComplexItem::save(QJsonArray& missionItems)
saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;
saveObject[_altitudeFactName] = _altitudeFact.rawValue().toDouble();
saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative;
saveObject[_layersFactName] = _layersFact.rawValue().toDouble();
saveObject[_altitudeFactName] = _altitudeFact.rawValue().toDouble();
saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative;
saveObject[_layersFactName] = _layersFact.rawValue().toDouble();
saveObject[_jsonYawVehicleToStructureKey] = _yawVehicleToStructure;
QJsonObject cameraCalcObject;
_cameraCalc.save(cameraCalcObject);
@ -162,6 +169,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen @@ -162,6 +169,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
{ _jsonAltitudeRelativeKey, QJsonValue::Bool, false },
{ _layersFactName, QJsonValue::Double, true },
{ _jsonCameraCalcKey, QJsonValue::Object, true },
{ _jsonYawVehicleToStructureKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
return false;
@ -187,6 +195,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen @@ -187,6 +195,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
_altitudeFact.setRawValue (complexObject[_altitudeFactName].toDouble());
_layersFact.setRawValue (complexObject[_layersFactName].toDouble());
_altitudeRelative = complexObject[_jsonAltitudeRelativeKey].toBool(true);
_yawVehicleToStructure = complexObject[_jsonYawVehicleToStructureKey].toBool(true);
if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), errorString)) {
return false;
@ -231,19 +240,35 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO @@ -231,19 +240,35 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO
{
int seqNum = _sequenceNumber;
double baseAltitude = _altitudeFact.rawValue().toDouble();
MissionItem* item;
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_DO_MOUNT_CONTROL,
MAV_FRAME_MISSION,
0, // Gimbal pitch
0, // Gimbal roll
90, // Gimbal yaw
0, 0, 0, // param 4-6 not used
MAV_MOUNT_MODE_MAVLINK_TARGETING,
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
if (_yawVehicleToStructure) {
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_CONDITION_YAW,
MAV_FRAME_MISSION,
90.0, // Target angle
0, // Use default turn rate
1, // Clockwise turn
0, // Absolute angle specified
0, 0, 0, // param 5-7 not used
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
} else {
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_DO_MOUNT_CONTROL,
MAV_FRAME_MISSION,
0, // Gimbal pitch
0, // Gimbal roll
90, // Gimbal yaw
0, 0, 0, // param 4-6 not used
MAV_MOUNT_MODE_MAVLINK_TARGETING,
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) {
bool addTriggerStart = true;
@ -386,3 +411,26 @@ void StructureScanComplexItem::_rebuildFlightPolygon(void) @@ -386,3 +411,26 @@ void StructureScanComplexItem::_rebuildFlightPolygon(void)
_flightPolygon = _structurePolygon;
_flightPolygon.offset(_cameraCalc.distanceToSurface()->rawValue().toDouble());
}
void StructureScanComplexItem::_recalcCameraShots(void)
{
if (_flightPolygon.count() < 3) {
_setCameraShots(0);
return;
}
// Determine the distance for each polygon traverse
double distance = 0;
for (int i=0; i<_flightPolygon.count(); i++) {
QGeoCoordinate coord1 = _flightPolygon.vertexCoordinate(i);
QGeoCoordinate coord2 = _flightPolygon.vertexCoordinate(i + 1 == _flightPolygon.count() ? 0 : i + 1);
distance += coord1.distanceTo(coord2);
}
if (distance == 0.0) {
_setCameraShots(0);
return;
}
int cameraShots = distance / _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
_setCameraShots(cameraShots * _layersFact.rawValue().toInt());
}

5
src/MissionManager/StructureScanComplexItem.h
Unescape View File

@ -36,6 +36,7 @@ public: @@ -36,6 +36,7 @@ public:
Q_PROPERTY(double cameraMinTriggerInterval MEMBER _cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged)
Q_PROPERTY(QGCMapPolygon* structurePolygon READ structurePolygon CONSTANT)
Q_PROPERTY(QGCMapPolygon* flightPolygon READ flightPolygon CONSTANT)
Q_PROPERTY(bool yawVehicleToStructure MEMBER _yawVehicleToStructure NOTIFY yawVehicleToStructureChanged) ///< true: vehicle yaws to point to structure, false: gimbal yaws to point to structure
CameraCalc* cameraCalc (void) { return &_cameraCalc; }
Fact* altitude (void) { return &_altitudeFact; }
@ -91,6 +92,7 @@ signals: @@ -91,6 +92,7 @@ signals:
void timeBetweenShotsChanged (void);
void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval);
void altitudeRelativeChanged (bool altitudeRelative);
void yawVehicleToStructureChanged (bool yawVehicleToStructure);
private slots:
void _setDirty(void);
@ -100,6 +102,7 @@ private slots: @@ -100,6 +102,7 @@ private slots:
void _clearInternal(void);
void _updateCoordinateAltitudes(void);
void _rebuildFlightPolygon(void);
void _recalcCameraShots(void);
private:
void _setExitCoordinate(const QGeoCoordinate& coordinate);
@ -121,6 +124,7 @@ private: @@ -121,6 +124,7 @@ private:
double _cameraMinTriggerInterval;
double _cruiseSpeed;
CameraCalc _cameraCalc;
bool _yawVehicleToStructure;
static QMap<QString, FactMetaData*> _metaDataMap;
@ -132,6 +136,7 @@ private: @@ -132,6 +136,7 @@ private:
static const char* _jsonCameraCalcKey;
static const char* _jsonAltitudeRelativeKey;
static const char* _jsonYawVehicleToStructureKey;
};
#endif

25
src/PlanView/StructureScanEditor.qml
Unescape View File

@ -49,6 +49,11 @@ Rectangle { @@ -49,6 +49,11 @@ Rectangle {
QGCPalette { id: qgcPal; colorGroupEnabled: true }
ExclusiveGroup {
id: yawRadiosGroup
onCurrentChanged: missionItem.yawVehicleToStructure = yawVehicleRadio.checked
}
Column {
id: editorColumn
anchors.margins: _margin
@ -129,8 +134,24 @@ Rectangle { @@ -129,8 +134,24 @@ Rectangle {
}
QGCLabel { text: qsTr("Point camera to structure using:") }
QGCRadioButton { text: qsTr("Vehicle yaw"); enabled: false }
QGCRadioButton { text: qsTr("Gimbal yaw"); checked: true; enabled: false }
RowLayout {
spacing: _margin
QGCRadioButton {
id: yawVehicleRadio
text: qsTr("Vehicle yaw")
exclusiveGroup: yawRadiosGroup
checked: !!missionItem.yawVehicleToStructure
}
QGCRadioButton
{
text: qsTr("Gimbal yaw")
exclusiveGroup: yawRadiosGroup
checked: !missionItem.yawVehicleToStructure
}
}
QGCButton {
text: qsTr("Rotate entry point")

Write Preview
Loading…
Cancel
Save