RallyPointManager.cc 3.86 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9 10
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "RallyPointManager.h"
11
#include "ParameterManager.h"
12 13 14 15 16 17 18
#include "Vehicle.h"

QGC_LOGGING_CATEGORY(RallyPointManagerLog, "RallyPointManagerLog")

RallyPointManager::RallyPointManager(Vehicle* vehicle)
    : QObject(vehicle)
    , _vehicle(vehicle)
19
    , _planManager              (vehicle, MAV_MISSION_TYPE_RALLY)
20
{
21 22 23
    connect(&_planManager, &PlanManager::inProgressChanged,         this, &RallyPointManager::inProgressChanged);
    connect(&_planManager, &PlanManager::error,                     this, &RallyPointManager::error);
    connect(&_planManager, &PlanManager::removeAllComplete,         this, &RallyPointManager::removeAllComplete);
Don Gagne's avatar
Don Gagne committed
24
    connect(&_planManager, &PlanManager::sendComplete,              this, &RallyPointManager::_sendComplete);
25
    connect(&_planManager, &PlanManager::newMissionItemsAvailable,  this, &RallyPointManager::_planManagerLoadComplete);
26 27
}

28

29 30 31 32 33 34 35 36 37 38 39 40 41 42
RallyPointManager::~RallyPointManager()
{

}

void RallyPointManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
{
    qCDebug(RallyPointManagerLog) << "Sending error" << errorCode << errorMsg;

    emit error(errorCode, errorMsg);
}

void RallyPointManager::loadFromVehicle(void)
{
43
     _planManager.loadFromVehicle();
44 45 46 47
}

void RallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
{
Don Gagne's avatar
Don Gagne committed
48 49 50 51
    _rgSendPoints.clear();
    for (const QGeoCoordinate& rallyPoint: rgPoints) {
        _rgSendPoints.append(rallyPoint);
    }
52

Don Gagne's avatar
Don Gagne committed
53
    QList<MissionItem*> rallyItems;
54 55 56 57
    for (int i=0; i<rgPoints.count(); i++) {

        MissionItem* item = new MissionItem(0,
                                            MAV_CMD_NAV_RALLY_POINT,
58
                                            MAV_FRAME_GLOBAL_RELATIVE_ALT,
59 60 61 62 63 64 65 66 67 68 69 70
                                            0, 0, 0, 0,                 // param 1-4 unused
                                            rgPoints[i].latitude(),
                                            rgPoints[i].longitude(),
                                            rgPoints[i].altitude(),
                                            false,                      // autocontinue
                                            false,                      // isCurrentItem
                                            this);                      // parent
        rallyItems.append(item);
    }

    // Plan manager takes control of MissionItems, so no need to delete
    _planManager.writeMissionItems(rallyItems);
DonLakeFlyer's avatar
DonLakeFlyer committed
71 72 73 74
}

void RallyPointManager::removeAll(void)
{
75
    _rgPoints.clear();
76
    _planManager.removeAll();
77
}
78 79 80 81 82

bool RallyPointManager::supported(void) const
{
    return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200);
}
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

void RallyPointManager::_planManagerLoadComplete(bool removeAllRequested)
{
    _rgPoints.clear();

    Q_UNUSED(removeAllRequested);

    const QList<MissionItem*>& rallyItems = _planManager.missionItems();

    for (int i=0; i<rallyItems.count(); i++) {
        MissionItem* item = rallyItems[i];

        MAV_CMD command = item->command();

        if (command == MAV_CMD_NAV_RALLY_POINT) {
            _rgPoints.append(QGeoCoordinate(item->param5(), item->param6(), item->param7()));


        } else {
            qCDebug(RallyPointManagerLog) << "RallyPointManager load: Unsupported command %1" << item->command();
            break;
        }
    }


Don Gagne's avatar
Don Gagne committed
108 109 110 111 112 113 114 115 116 117 118 119
    emit loadComplete();
}

void RallyPointManager::_sendComplete(bool error)
{
    if (error) {
        _rgPoints.clear();
    } else {
        _rgPoints = _rgSendPoints;
    }
    _rgSendPoints.clear();
    emit sendComplete(error);
120
}