diff --git a/custom-example/res/CustomFlyView.qml b/custom-example/res/CustomFlyView.qml index c7e8f010a0568fab5d882fa6ca12f0dcfe29a32d..3ec772414e9baca2cfb0a2ecc73babf3dcb86b22 100644 --- a/custom-example/res/CustomFlyView.qml +++ b/custom-example/res/CustomFlyView.qml @@ -604,6 +604,27 @@ Item { } } //------------------------------------------------------------------------- + //-- Object Avoidance + Rectangle { + visible: activeVehicle && activeVehicle.objectAvoidance.available && activeVehicle.objectAvoidance.enabled + anchors.centerIn: parent + width: parent.width * 0.5 + height: parent.height * 0.5 + color: Qt.rgba(0,0,0,0.25) + Repeater { + model: activeVehicle && activeVehicle.objectAvoidance.gridSize > 0 ? activeVehicle.objectAvoidance.gridSize : [] + Rectangle { + width: ScreenTools.defaultFontPixelWidth + height: width + radius: width * 0.5 + color: distance < 0.25 ? "red" : "orange" + x: (parent.width * activeVehicle.objectAvoidance.grid(modelData).x) + (parent.width * 0.5) + y: (parent.height * activeVehicle.objectAvoidance.grid(modelData).y) + (parent.height * 0.5) + property real distance: activeVehicle.objectAvoidance.distance(modelData) + } + } + } + //------------------------------------------------------------------------- //-- Connection Lost While Armed Popup { id: connectionLostArmed diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 55ac49c3e5e56877588ad9046fd822c7f15fe73b..818ed0c97d9b2955a1ad70afed4dc05ca2189409 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -855,6 +855,7 @@ HEADERS+= \ src/Vehicle/MultiVehicleManager.h \ src/Vehicle/GPSRTKFactGroup.h \ src/Vehicle/Vehicle.h \ + src/Vehicle/VehicleObjectAvoidance.h \ src/VehicleSetup/VehicleComponent.h \ !MobileBuild { !NoSerialBuild { @@ -881,6 +882,7 @@ SOURCES += \ src/Vehicle/MultiVehicleManager.cc \ src/Vehicle/GPSRTKFactGroup.cc \ src/Vehicle/Vehicle.cc \ + src/Vehicle/VehicleObjectAvoidance.cc \ src/VehicleSetup/VehicleComponent.cc \ !MobileBuild { !NoSerialBuild { diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index f8ffcc1589e046c27c43b384c6b5db38b4bb0438..af1d28533902847dff29cc789d9a6cd1a18cff42 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -30,13 +30,13 @@ class Fact : public QObject Q_OBJECT public: - Fact(QObject* parent = NULL); - Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObject* parent = NULL); - Fact(const Fact& other, QObject* parent = NULL); + Fact(QObject* parent = nullptr); + Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObject* parent = nullptr); + Fact(const Fact& other, QObject* parent = nullptr); /// Creates a Fact using the name and type from metaData. Also calls QGCCorePlugin::adjustSettingsMetaData allowing /// custom builds to override the metadata. - Fact(const QString& settingsGroup, FactMetaData* metaData, QObject* parent = NULL); + Fact(const QString& settingsGroup, FactMetaData* metaData, QObject* parent = nullptr); const Fact& operator=(const Fact& other); @@ -51,7 +51,7 @@ public: Q_PROPERTY(QStringList enumStrings READ enumStrings NOTIFY enumsChanged) Q_PROPERTY(QString enumStringValue READ enumStringValue WRITE setEnumStringValue NOTIFY valueChanged) Q_PROPERTY(QVariantList enumValues READ enumValues NOTIFY enumsChanged) - Q_PROPERTY(QString category READ category CONSTANT) + Q_PROPERTY(QString category READ category CONSTANT) Q_PROPERTY(QString group READ group CONSTANT) Q_PROPERTY(QString longDescription READ longDescription CONSTANT) Q_PROPERTY(QVariant max READ cookedMax CONSTANT) diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 07ef945d4a75cdbef1ce2f82a62df9b2e1d7a323..28e2a9a3906aefb193e0ebcd923d7e96404fa427 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -95,6 +95,7 @@ #include "MAVLinkInspectorController.h" #include "GeoTagController.h" #include "LogReplayLink.h" +#include "VehicleObjectAvoidance.h" #ifndef __mobile__ #include "FirmwareUpgradeController.h" @@ -466,18 +467,19 @@ void QGCApplication::_initCommon() qmlRegisterType ("QGroundControl.Palette", 1, 0, "QGCPalette"); qmlRegisterType ("QGroundControl.Palette", 1, 0, "QGCMapPalette"); - qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "Vehicle", kRefOnly); - qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "MissionItem", kRefOnly); - qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "MissionManager", kRefOnly); - qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "ParameterManager", kRefOnly); - qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "QGCCameraManager", kRefOnly); - qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "QGCCameraControl", kRefOnly); - qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "QGCVideoStreamInfo", kRefOnly); - qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "LinkInterface", kRefOnly); - qmlRegisterUncreatableType (kQGCControllers, 1, 0, "MissionController", kRefOnly); - qmlRegisterUncreatableType (kQGCControllers, 1, 0, "GeoFenceController", kRefOnly); - qmlRegisterUncreatableType(kQGCControllers, 1, 0, "RallyPointController", kRefOnly); - qmlRegisterUncreatableType (kQGCControllers, 1, 0, "VisualMissionItem", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "Vehicle", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "MissionItem", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "MissionManager", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "ParameterManager", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "VehicleObjectAvoidance", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "QGCCameraManager", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "QGCCameraControl", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "QGCVideoStreamInfo", kRefOnly); + qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "LinkInterface", kRefOnly); + qmlRegisterUncreatableType (kQGCControllers, 1, 0, "MissionController", kRefOnly); + qmlRegisterUncreatableType (kQGCControllers, 1, 0, "GeoFenceController", kRefOnly); + qmlRegisterUncreatableType (kQGCControllers, 1, 0, "RallyPointController", kRefOnly); + qmlRegisterUncreatableType (kQGCControllers, 1, 0, "VisualMissionItem", kRefOnly); qmlRegisterUncreatableType ("QGroundControl", 1, 0, "CoordinateVector", kRefOnly); qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QmlObjectListModel", kRefOnly); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index eaa65792e629ae69582bc9f38540dd655aced5f2..e75f00bbf3a09c7e5d46d6b551d3794a5d6e1fbd 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -41,6 +41,8 @@ #include "VideoManager.h" #include "VideoSettings.h" #include "PositionManager.h" +#include "VehicleObjectAvoidance.h" + #if defined(QGC_AIRMAP_ENABLED) #include "AirspaceVehicleManager.h" #endif @@ -156,7 +158,6 @@ Vehicle::Vehicle(LinkInterface* link, , _geoFenceManagerInitialRequestSent(false) , _rallyPointManager(nullptr) , _rallyPointManagerInitialRequestSent(false) - , _parameterManager(nullptr) #if defined(QGC_AIRMAP_ENABLED) , _airspaceVehicleManager(nullptr) #endif @@ -359,7 +360,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _geoFenceManagerInitialRequestSent(false) , _rallyPointManager(nullptr) , _rallyPointManagerInitialRequestSent(false) - , _parameterManager(nullptr) #if defined(QGC_AIRMAP_ENABLED) , _airspaceVehicleManager(nullptr) #endif @@ -451,6 +451,8 @@ void Vehicle::_commonInit(void) _parameterManager = new ParameterManager(this); connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); + _objectAvoidance = new VehicleObjectAvoidance(this, this); + // GeoFenceManager needs to access ParameterManager so make sure to create after _geoFenceManager = new GeoFenceManager(this); connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); @@ -812,6 +814,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_MOUNT_ORIENTATION: _handleGimbalOrientation(message); break; + case MAVLINK_MSG_ID_OBSTACLE_DISTANCE: + _handleObstacleDistance(message); + break; case MAVLINK_MSG_ID_SERIAL_CONTROL: { @@ -4057,6 +4062,13 @@ void Vehicle::_handleGimbalOrientation(const mavlink_message_t& message) } } +void Vehicle::_handleObstacleDistance(const mavlink_message_t& message) +{ + mavlink_obstacle_distance_t o; + mavlink_msg_obstacle_distance_decode(&message, &o); + _objectAvoidance->update(&o); +} + //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 231e82f4acdde729470f9584f3bd8dc4e450305a..f4e35b61e1472ea7e8fbc98bf086ed36f48dfde5 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -37,6 +37,8 @@ class SettingsManager; class ADSBVehicle; class QGCCameraManager; class Joystick; +class VehicleObjectAvoidance; + #if defined(QGC_AIRMAP_ENABLED) class AirspaceVehicleManager; #endif @@ -484,7 +486,6 @@ private: #endif }; - class Vehicle : public FactGroup { Q_OBJECT @@ -652,7 +653,8 @@ public: Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported Q_PROPERTY(QString gotoFlightMode READ gotoFlightMode CONSTANT) ///< Flight mode vehicle is in while performing goto - Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT) + Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT) + Q_PROPERTY(VehicleObjectAvoidance* objectAvoidance READ objectAvoidance CONSTANT) // FactGroup object model properties @@ -989,8 +991,9 @@ public: void setConnectionLostEnabled(bool connectionLostEnabled); - ParameterManager* parameterManager(void) { return _parameterManager; } - ParameterManager* parameterManager(void) const { return _parameterManager; } + ParameterManager* parameterManager() { return _parameterManager; } + ParameterManager* parameterManager() const { return _parameterManager; } + VehicleObjectAvoidance* objectAvoidance() { return _objectAvoidance; } static const int cMaxRcChannels = 18; @@ -1295,6 +1298,7 @@ private: void _handleOrbitExecutionStatus(const mavlink_message_t& message); void _handleMessageInterval(const mavlink_message_t& message); void _handleGimbalOrientation(const mavlink_message_t& message); + void _handleObstacleDistance(const mavlink_message_t& message); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback(const mavlink_message_t& message); @@ -1425,7 +1429,8 @@ private: RallyPointManager* _rallyPointManager; bool _rallyPointManagerInitialRequestSent; - ParameterManager* _parameterManager; + ParameterManager* _parameterManager = nullptr; + VehicleObjectAvoidance* _objectAvoidance = nullptr; #if defined(QGC_AIRMAP_ENABLED) AirspaceVehicleManager* _airspaceVehicleManager; diff --git a/src/Vehicle/VehicleObjectAvoidance.cc b/src/Vehicle/VehicleObjectAvoidance.cc new file mode 100644 index 0000000000000000000000000000000000000000..dde9998523134c6e1307a1f93120093d13107b7a --- /dev/null +++ b/src/Vehicle/VehicleObjectAvoidance.cc @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * (c) 2009-2019 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "Vehicle.h" +#include "VehicleObjectAvoidance.h" +#include "ParameterManager.h" +#include + +static const char* kColPrevParam = "MPC_COL_PREV_D"; + +//----------------------------------------------------------------------------- +VehicleObjectAvoidance::VehicleObjectAvoidance(Vehicle *vehicle, QObject* parent) + : QObject(parent) + , _vehicle(vehicle) +{ +} + +//----------------------------------------------------------------------------- +void +VehicleObjectAvoidance::update(mavlink_obstacle_distance_t* message) +{ + //-- Collect raw data + if(std::isfinite(message->increment_f) && message->increment_f > 0) { + _increment = static_cast(message->increment_f); + } else { + _increment = static_cast(message->increment); + } + _minDistance = message->min_distance; + _maxDistance = message->max_distance; + _angleOffset = static_cast(message->angle_offset); + if(_distances.count() == 0) { + for(int i = 0; i < MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN; i++) { + _distances.append(static_cast(message->distances[i])); + } + } else { + for(int i = 0; i < MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN; i++) { + _distances[i] = static_cast(message->distances[i]); + } + } + //-- Create a plottable grid with found objects + _objGrid.clear(); + _objDistance.clear(); + VehicleSetpointFactGroup* sp = dynamic_cast(_vehicle->setpointFactGroup()); + qreal startAngle = sp->yaw()->rawValue().toDouble() + _angleOffset; + for(int i = 0; i < MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN; i++) { + if(_distances[i] < _maxDistance && message->distances[i] != UINT16_MAX) { + qreal d = static_cast(_distances[i]); + d = d / static_cast(_maxDistance); + qreal a = (_increment * i) - startAngle; + if(a < 0) a = a + 360; + qreal rd = (M_PI / 180.0) * a; + QPointF p = QPointF(d * cos(rd), d * sin(rd)); + _objGrid.append(p); + _objDistance.append(d); + } + } + emit objectAvoidanceChanged(); +} + +//----------------------------------------------------------------------------- +bool +VehicleObjectAvoidance::enabled() +{ + uint8_t id = static_cast(_vehicle->id()); + if(_vehicle->parameterManager()->parameterExists(id, kColPrevParam)) { + return _vehicle->parameterManager()->getParameter(id, kColPrevParam)->rawValue().toInt() >= 0; + } + return false; +} + +//----------------------------------------------------------------------------- +void +VehicleObjectAvoidance::start(int minDistance) +{ + uint8_t id = static_cast(_vehicle->id()); + if(_vehicle->parameterManager()->parameterExists(id, kColPrevParam)) { + _vehicle->parameterManager()->getParameter(id, kColPrevParam)->setRawValue(minDistance); + emit objectAvoidanceChanged(); + } +} + +//----------------------------------------------------------------------------- +void +VehicleObjectAvoidance::stop() +{ + uint8_t id = static_cast(_vehicle->id()); + if(_vehicle->parameterManager()->parameterExists(id, kColPrevParam)) { + _vehicle->parameterManager()->getParameter(id, kColPrevParam)->setRawValue(-1); + emit objectAvoidanceChanged(); + } +} + +//----------------------------------------------------------------------------- +QPointF +VehicleObjectAvoidance::grid(int i) +{ + if(i < _objGrid.count() && i >= 0) { + return _objGrid[i]; + } + return QPointF(0,0); +} + +//----------------------------------------------------------------------------- +qreal +VehicleObjectAvoidance::distance(int i) +{ + if(i < _objDistance.count() && i >= 0) { + return _objDistance[i]; + } + return 0; +} diff --git a/src/Vehicle/VehicleObjectAvoidance.h b/src/Vehicle/VehicleObjectAvoidance.h new file mode 100644 index 0000000000000000000000000000000000000000..b892285d3200e2140610c1acece80159f3b9f14e --- /dev/null +++ b/src/Vehicle/VehicleObjectAvoidance.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * (c) 2009-2019 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "QGCMAVLink.h" + +class Vehicle; + +class VehicleObjectAvoidance : public QObject +{ + Q_OBJECT +public: + VehicleObjectAvoidance(Vehicle* vehicle, QObject* parent = nullptr); + + Q_PROPERTY(bool available READ available NOTIFY objectAvoidanceChanged) + Q_PROPERTY(bool enabled READ enabled NOTIFY objectAvoidanceChanged) + Q_PROPERTY(QList distances READ distances NOTIFY objectAvoidanceChanged) + Q_PROPERTY(qreal increment READ increment NOTIFY objectAvoidanceChanged) + Q_PROPERTY(int minDistance READ minDistance NOTIFY objectAvoidanceChanged) + Q_PROPERTY(int maxDistance READ maxDistance NOTIFY objectAvoidanceChanged) + Q_PROPERTY(qreal angleOffset READ angleOffset NOTIFY objectAvoidanceChanged) + Q_PROPERTY(int gridSize READ gridSize NOTIFY objectAvoidanceChanged) + + //-- Start collision avoidance. Argument is minimum distance the vehicle should keep to all obstacles + Q_INVOKABLE void start (int minDistance); + //-- Stop collision avoidance. + Q_INVOKABLE void stop (); + //-- Object locations (in relationship to vehicle) + Q_INVOKABLE QPointF grid (int i); + Q_INVOKABLE qreal distance(int i); + + bool available () { return _distances.count() > 0; } + bool enabled (); + QList distances () { return _distances; } + qreal increment () { return _increment; } + int minDistance () { return _minDistance; } + int maxDistance () { return _maxDistance; } + qreal angleOffset () { return _angleOffset; } + int gridSize () { return _objGrid.count(); } + + void update (mavlink_obstacle_distance_t* message); + +signals: + void objectAvoidanceChanged (); + +private: + QList _distances; + QVector_objGrid; + QVector _objDistance; + qreal _increment = 0; + int _minDistance = 0; + int _maxDistance = 0; + qreal _angleOffset = 0; + Vehicle* _vehicle = nullptr; +}; +