Gus Grubba
6 years ago
8 changed files with 250 additions and 24 deletions
@ -0,0 +1,117 @@
@@ -0,0 +1,117 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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; |
||||
} |
||||
@ -0,0 +1,67 @@
@@ -0,0 +1,67 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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; |
||||
}; |
||||
|
||||
Loading…
Reference in new issue