121 lines
3.9 KiB
C++
121 lines
3.9 KiB
C++
/****************************************************************************
|
|
*
|
|
* (c) 2009-2020 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 "RallyPointManager.h"
|
|
#include "ParameterManager.h"
|
|
#include "Vehicle.h"
|
|
|
|
QGC_LOGGING_CATEGORY(RallyPointManagerLog, "RallyPointManagerLog")
|
|
|
|
RallyPointManager::RallyPointManager(Vehicle* vehicle)
|
|
: QObject(vehicle)
|
|
, _vehicle(vehicle)
|
|
, _planManager (vehicle, MAV_MISSION_TYPE_RALLY)
|
|
{
|
|
connect(&_planManager, &PlanManager::inProgressChanged, this, &RallyPointManager::inProgressChanged);
|
|
connect(&_planManager, &PlanManager::error, this, &RallyPointManager::error);
|
|
connect(&_planManager, &PlanManager::removeAllComplete, this, &RallyPointManager::removeAllComplete);
|
|
connect(&_planManager, &PlanManager::sendComplete, this, &RallyPointManager::_sendComplete);
|
|
connect(&_planManager, &PlanManager::newMissionItemsAvailable, this, &RallyPointManager::_planManagerLoadComplete);
|
|
}
|
|
|
|
|
|
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)
|
|
{
|
|
_planManager.loadFromVehicle();
|
|
}
|
|
|
|
void RallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
|
|
{
|
|
_rgSendPoints.clear();
|
|
for (const QGeoCoordinate& rallyPoint: rgPoints) {
|
|
_rgSendPoints.append(rallyPoint);
|
|
}
|
|
|
|
QList<MissionItem*> rallyItems;
|
|
for (int i=0; i<rgPoints.count(); i++) {
|
|
|
|
MissionItem* item = new MissionItem(0,
|
|
MAV_CMD_NAV_RALLY_POINT,
|
|
MAV_FRAME_GLOBAL_RELATIVE_ALT,
|
|
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);
|
|
}
|
|
|
|
void RallyPointManager::removeAll(void)
|
|
{
|
|
_rgPoints.clear();
|
|
_planManager.removeAll();
|
|
}
|
|
|
|
bool RallyPointManager::supported(void) const
|
|
{
|
|
return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
|
|
emit loadComplete();
|
|
}
|
|
|
|
void RallyPointManager::_sendComplete(bool error)
|
|
{
|
|
if (error) {
|
|
_rgPoints.clear();
|
|
} else {
|
|
_rgPoints = _rgSendPoints;
|
|
}
|
|
_rgSendPoints.clear();
|
|
emit sendComplete(error);
|
|
}
|