Commit edf814c1 authored by Gus Grubba's avatar Gus Grubba

First experiment

parent c2043990
......@@ -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
......
......@@ -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 {
......
......@@ -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);
......
......@@ -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);
......
......@@ -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);
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
......
......@@ -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;
......
/****************************************************************************
*
* (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;
}
/****************************************************************************
*
* (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;
};
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment