VehicleObjectAvoidance.cc 4.06 KB
Newer Older
Gus Grubba's avatar
Gus Grubba committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 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;
}