First experiment

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

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 {

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);
 

src/QGCApplication.cc

@@ -95,6 +95,7 @@
 #include "MAVLinkInspectorController.h"
 #include "GeoTagController.h"
 #include "LogReplayLink.h"
+#include "VehicleObjectAvoidance.h"
 
 #ifndef __mobile__
 #include "FirmwareUpgradeController.h"
@@ -470,13 +471,14 @@ void QGCApplication::_initCommon()
     qmlRegisterUncreatableType<MissionItem>             (kQGCVehicle,                       1, 0, "MissionItem",                kRefOnly);
     qmlRegisterUncreatableType<MissionManager>          (kQGCVehicle,                       1, 0, "MissionManager",             kRefOnly);
     qmlRegisterUncreatableType<ParameterManager>        (kQGCVehicle,                       1, 0, "ParameterManager",           kRefOnly);
+    qmlRegisterUncreatableType<VehicleObjectAvoidance>  (kQGCVehicle,                       1, 0, "VehicleObjectAvoidance",     kRefOnly);
     qmlRegisterUncreatableType<QGCCameraManager>        (kQGCVehicle,                       1, 0, "QGCCameraManager",           kRefOnly);
     qmlRegisterUncreatableType<QGCCameraControl>        (kQGCVehicle,                       1, 0, "QGCCameraControl",           kRefOnly);
     qmlRegisterUncreatableType<QGCVideoStreamInfo>      (kQGCVehicle,                       1, 0, "QGCVideoStreamInfo",         kRefOnly);
     qmlRegisterUncreatableType<LinkInterface>           (kQGCVehicle,                       1, 0, "LinkInterface",              kRefOnly);
     qmlRegisterUncreatableType<MissionController>       (kQGCControllers,                   1, 0, "MissionController",          kRefOnly);
     qmlRegisterUncreatableType<GeoFenceController>      (kQGCControllers,                   1, 0, "GeoFenceController",         kRefOnly);
-    qmlRegisterUncreatableType<RallyPointController>(kQGCControllers,                       1, 0, "RallyPointController",       kRefOnly);
+    qmlRegisterUncreatableType<RallyPointController>    (kQGCControllers,                   1, 0, "RallyPointController",       kRefOnly);
     qmlRegisterUncreatableType<VisualMissionItem>       (kQGCControllers,                   1, 0, "VisualMissionItem",          kRefOnly);
 
     qmlRegisterUncreatableType<CoordinateVector>    ("QGroundControl",                      1, 0, "CoordinateVector",           kRefOnly);

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);
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 

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
@@ -653,6 +654,7 @@ public:
     Q_PROPERTY(QString  gotoFlightMode          READ gotoFlightMode                                 CONSTANT)                   ///< Flight mode vehicle is in while performing goto
 
     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;

src/Vehicle/VehicleObjectAvoidance.cc

+/****************************************************************************
+ *
+ *   (c) 2009-2019 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * 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 <cmath>
+
+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<qreal>(message->increment_f);
+    } else {
+        _increment = static_cast<qreal>(message->increment);
+    }
+    _minDistance = message->min_distance;
+    _maxDistance = message->max_distance;
+    _angleOffset = static_cast<qreal>(message->angle_offset);
+    if(_distances.count() == 0) {
+        for(int i = 0; i < MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN; i++) {
+            _distances.append(static_cast<int>(message->distances[i]));
+        }
+    } else {
+        for(int i = 0; i < MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN; i++) {
+            _distances[i] = static_cast<int>(message->distances[i]);
+        }
+    }
+    //-- Create a plottable grid with found objects
+    _objGrid.clear();
+    _objDistance.clear();
+    VehicleSetpointFactGroup* sp = dynamic_cast<VehicleSetpointFactGroup*>(_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<qreal>(_distances[i]);
+            d = d / static_cast<qreal>(_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<uint8_t>(_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<uint8_t>(_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<uint8_t>(_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;
+}

src/Vehicle/VehicleObjectAvoidance.h

+/****************************************************************************
+ *
+ *   (c) 2009-2019 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <QObject>
+#include <QVector>
+#include <QPointF>
+
+#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<int>       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<int>      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<int>      _distances;
+    QVector<QPointF>_objGrid;
+    QVector<qreal>  _objDistance;
+    qreal           _increment      = 0;
+    int             _minDistance    = 0;
+    int             _maxDistance    = 0;
+    qreal           _angleOffset    = 0;
+    Vehicle*        _vehicle        = nullptr;
+};
+