APMGeoFenceManager.cc 13.4 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14
/****************************************************************************
 *
 *   (c) 2009-2016 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 "APMGeoFenceManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
15
#include "ParameterManager.h"
16 17
#include "QmlObjectListModel.h"
#include "QGCQGeoCoordinate.h"
18

19 20 21
const char* APMGeoFenceManager::_fenceTotalParam =  "FENCE_TOTAL";
const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION";
const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE";
22 23 24 25

APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle)
    : GeoFenceManager(vehicle)
    , _fenceSupported(false)
26
    , _circleEnabled(false)
27
    , _polygonSupported(false)
28
    , _polygonEnabled(false)
29
    , _breachReturnSupported(vehicle->fixedWing())
30 31 32 33
    , _firstParamLoadComplete(false)
    , _readTransactionInProgress(false)
    , _writeTransactionInProgress(false)
    , _fenceTypeFact(NULL)
34 35
    , _fenceEnableFact(NULL)
    , _circleRadiusFact(NULL)
36
{
37 38
    connect(_vehicle,                       &Vehicle::mavlinkMessageReceived,           this, &APMGeoFenceManager::_mavlinkMessageReceived);
    connect(_vehicle->parameterManager(),   &ParameterManager::parametersReadyChanged,  this, &APMGeoFenceManager::_parametersReady);
39

40
    if (_vehicle->parameterManager()->parametersReady()) {
41 42
        _parametersReady();
    }
43 44 45 46 47 48 49
}

APMGeoFenceManager::~APMGeoFenceManager()
{

}

50
void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon)
51
{
52 53 54 55
    if (_vehicle->isOfflineEditingVehicle()) {
        return;
    }

56 57 58 59 60
    if (_readTransactionInProgress) {
        _sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress."));
        return;
    }

61
    if (!_fenceSupported) {
62 63 64 65
        return;
    }

    // Validate
66
    int validatedPolygonCount = 0;
67 68 69 70 71 72
    if (polygon.count() >= 3) {
        validatedPolygonCount = polygon.count();
    }
    if (polygon.count() > std::numeric_limits<uint8_t>::max()) {
        _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count()));
        validatedPolygonCount = 0;
73 74
    }

75
    _breachReturnPoint = breachReturn;
76
    _polygon.clear();
77
    if (validatedPolygonCount) {
78 79 80
        for (int i=0; i<polygon.count(); i++) {
            _polygon.append(polygon.value<QGCQGeoCoordinate*>(i)->coordinate());
        }
81
    }
82

83
    // Total point count, +1 polygon close in last index, +1 for breach in index 0
Don Gagne's avatar
Don Gagne committed
84
    _cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0;
DonLakeFlyer's avatar
DonLakeFlyer committed
85
    qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::sendToVehicle validatedPolygonCount:_cWriteFencePoints" << validatedPolygonCount << _cWriteFencePoints;
86
    _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints);
87 88 89 90 91 92

    // FIXME: No validation of correct fence received
    for (uint8_t index=0; index<_cWriteFencePoints; index++) {
        _sendFencePoint(index);
    }

93
    emit sendComplete(false /* error */);
94 95 96 97
}

void APMGeoFenceManager::loadFromVehicle(void)
{
98
    if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) {
99 100 101
        return;
    }

102
    _breachReturnPoint = QGeoCoordinate();
103 104
    _polygon.clear();

105
    if (!_fenceSupported) {
106 107 108
        return;
    }

Don Gagne's avatar
Don Gagne committed
109
    // Point 0: Breach return point (always sent, but supported by ArduPlane only)
110 111
    // Point [1,N]: Polygon points
    // Point N+1: Close polygon point (same as point 1)
112
    int cFencePoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
113
    int minFencePoints = 5;
114 115
    qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints;
    if (cFencePoints == 0) {
116 117
        // No fence
        emit loadComplete(_breachReturnPoint, _polygon);
118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135
        return;
    }
    if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < minFencePoints)) {
        _sendError(TooFewPoints, QStringLiteral("Geo-Fence information from Vehicle has too few points: %1").arg(cFencePoints));
        return;
    }
    if (cFencePoints > std::numeric_limits<uint8_t>::max()) {
        _sendError(TooManyPoints, QStringLiteral("Geo-Fence information from Vehicle has too many points: %1").arg(cFencePoints));
        return;
    }

    _readTransactionInProgress = true;
    _cReadFencePoints = cFencePoints;
    _currentFencePoint = 0;

    _requestFencePoint(_currentFencePoint);
}

136
/// Called when a new mavlink message for our vehicle is received
137 138 139 140 141 142 143 144 145 146
void APMGeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
    if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) {
        mavlink_fence_point_t fencePoint;

        mavlink_msg_fence_point_decode(&message, &fencePoint);
        qCDebug(GeoFenceManagerLog) << "From vehicle fence_point: idx:lat:lng" << fencePoint.idx << fencePoint.lat << fencePoint.lng;

        if (fencePoint.idx != _currentFencePoint) {
            // FIXME: Protocol out of whack
147 148
            _readTransactionInProgress = false;
            emit inProgressChanged(inProgress());
149 150 151 152 153
            qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint;
            return;
        }

        if (fencePoint.idx == 0) {
154
            _breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng);
155 156 157 158 159 160 161 162 163 164 165
            qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _breachReturnPoint;
            _requestFencePoint(++_currentFencePoint);
        } else if (fencePoint.idx < _cReadFencePoints - 1) {
            QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng);
            _polygon.append(polyCoord);
            qCDebug(GeoFenceManagerLog) << "From vehicle: polygon point" << fencePoint.idx << polyCoord;
            if (fencePoint.idx < _cReadFencePoints - 2) {
                // Still more points to request
                _requestFencePoint(++_currentFencePoint);
            } else {
                // We've finished collecting fence points
166
                qCDebug(GeoFenceManagerLog) << "Fence point load complete";
167
                _readTransactionInProgress = false;
168
                emit loadComplete(_breachReturnPoint, _polygon);
169
                emit inProgressChanged(inProgress());
170 171 172 173 174 175 176 177 178 179 180
            }
        }
    }
}

void APMGeoFenceManager::_requestFencePoint(uint8_t pointIndex)
{
    mavlink_message_t   msg;
    MAVLinkProtocol*    mavlink = qgcApp()->toolbox()->mavlinkProtocol();

    qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::_requestFencePoint" << pointIndex;
181 182 183 184 185 186 187 188
    mavlink_msg_fence_fetch_point_pack_chan(mavlink->getSystemId(),
                                            mavlink->getComponentId(),
                                            _vehicle->priorityLink()->mavlinkChannel(),
                                            &msg,
                                            _vehicle->id(),
                                            _vehicle->defaultComponentId(),
                                            pointIndex);
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209
}

void APMGeoFenceManager::_sendFencePoint(uint8_t pointIndex)
{
    mavlink_message_t   msg;
    MAVLinkProtocol*    mavlink = qgcApp()->toolbox()->mavlinkProtocol();

    QGeoCoordinate fenceCoord;
    if (pointIndex == 0) {
        fenceCoord = breachReturnPoint();
    } else if (pointIndex == _cWriteFencePoints - 1) {
        // Polygon close point
        fenceCoord = _polygon[0];
    } else {
        // Polygon point
        fenceCoord = _polygon[pointIndex - 1];
    }

    // Total point count, +1 polygon close in last index, +1 for breach in index 0
    uint8_t totalPointCount = _polygon.count() + 1 + 1;

210 211 212 213 214 215 216 217 218 219 220
    mavlink_msg_fence_point_pack_chan(mavlink->getSystemId(),
                                      mavlink->getComponentId(),
                                      _vehicle->priorityLink()->mavlinkChannel(),
                                      &msg,
                                      _vehicle->id(),
                                      _vehicle->defaultComponentId(),
                                      pointIndex,                        // Index of point to set
                                      totalPointCount,
                                      fenceCoord.latitude(),
                                      fenceCoord.longitude());
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
221 222 223 224 225 226 227
}

bool APMGeoFenceManager::inProgress(void) const
{
    return _readTransactionInProgress || _writeTransactionInProgress;
}

228
void APMGeoFenceManager::_setCircleEnabled(bool circleEnabled)
229
{
230 231 232
    if (circleEnabled != _circleEnabled) {
        _circleEnabled = circleEnabled;
        emit circleEnabledChanged(circleEnabled);
233
    }
234
}
235

236 237 238 239 240
void APMGeoFenceManager::_setPolygonEnabled(bool polygonEnabled)
{
    if (polygonEnabled != _polygonEnabled) {
        _polygonEnabled = polygonEnabled;
        emit polygonEnabledChanged(polygonEnabled);
Don Gagne's avatar
Don Gagne committed
241
    }
242 243 244 245 246 247 248 249 250 251 252 253
}

void APMGeoFenceManager::_updateEnabledFlags(void)
{
    bool fenceEnabled = false;

    if (_fenceSupported) {
        if (_fenceEnableFact) {
            fenceEnabled = _fenceEnableFact->rawValue().toBool();
        } else {
            fenceEnabled = true;
        }
Don Gagne's avatar
Don Gagne committed
254

255 256 257 258 259 260 261 262 263 264
        bool newCircleEnabled = fenceEnabled && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2);
        _setCircleEnabled(newCircleEnabled);

        bool newPolygonEnabled = _fenceSupported && fenceEnabled &&
                ((_vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 4)) ||
                 _vehicle->fixedWing());
        _setPolygonEnabled(newPolygonEnabled);
    } else {
        _setCircleEnabled(false);
        _setPolygonEnabled(false);
Don Gagne's avatar
Don Gagne committed
265
    }
266 267 268 269 270 271 272
}

void APMGeoFenceManager::_parametersReady(void)
{
    if (!_firstParamLoadComplete) {
        _firstParamLoadComplete = true;

273 274 275
        _fenceSupported = _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) &&
                _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceActionParam) &&
                !qgcApp()->runningUnitTests();
276 277 278 279 280

        if (_fenceSupported) {
            QStringList paramNames;
            QStringList paramLabels;

281 282
            _polygonSupported = true;

283
            if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) {
284 285
                _fenceEnableFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam);
                connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
286 287
            }

288
            if (_vehicle->multiRotor()) {
289
                _breachReturnSupported = false;
290
                _fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE"));
291
                connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
292

293
                _circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS"));
294 295 296 297 298 299

                paramNames << QStringLiteral("FENCE_ENABLE") << QStringLiteral("FENCE_TYPE") << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_ALT_MAX")
                           << QStringLiteral("FENCE_RADIUS") << QStringLiteral("FENCE_MARGIN");
                paramLabels << QStringLiteral("Enabled:") << QStringLiteral("Type:") << QStringLiteral("Breach Action:") << QStringLiteral("Max Altitude:")
                            << QStringLiteral("Radius:") << QStringLiteral("Margin:");
            } if (_vehicle->fixedWing()) {
300
                _breachReturnSupported = true;
301 302 303 304 305 306 307 308 309 310
                paramNames << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_MINALT") << QStringLiteral("FENCE_MAXALT") << QStringLiteral("FENCE_RETALT")
                           << QStringLiteral("FENCE_AUTOENABLE") << QStringLiteral("FENCE_RET_RALLY");
                paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Min Altitude:") << QStringLiteral("Max Altitude:") << QStringLiteral("Return Altitude:")
                            << QStringLiteral("Auto-Enable:") << QStringLiteral("Return to Rally:");
            }

            _params.clear();
            _paramLabels.clear();
            for (int i=0; i<paramNames.count(); i++) {
                QString paramName = paramNames[i];
311 312
                if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) {
                    Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName);
313 314 315 316 317 318 319
                    _params << QVariant::fromValue(paramFact);
                    _paramLabels << paramLabels[i];
                }
            }
            emit paramsChanged(_params);
            emit paramLabelsChanged(_paramLabels);

320 321
            emit breachReturnSupportedChanged(_breachReturnSupported);
            emit polygonSupportedChanged(_polygonSupported);
322 323
        }

324
        _updateEnabledFlags();
325
    }
326
}
327 328 329

void APMGeoFenceManager::removeAll(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
330 331
    qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::removeAll";

332 333 334
    QmlObjectListModel emptyPolygon;

    sendToVehicle(_breachReturnPoint, emptyPolygon);
335
    emit removeAllComplete(false /* error */);
336
}