mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-112 OP-1120 waypoint editor now sends and requests Waypoints and PathActions in a transacted and asynchronous manner. Board does not send Waypoints and PathActions periodically anymore.
This commit is contained in:
parent
0dc334adea
commit
398a3ab0e6
@ -31,14 +31,83 @@
|
||||
ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model)
|
||||
{
|
||||
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
||||
|
||||
Q_ASSERT(pm != NULL);
|
||||
|
||||
objManager = pm->getObject<UAVObjectManager>();
|
||||
Q_ASSERT(objManager != NULL);
|
||||
waypointObj = Waypoint::GetInstance(objManager);
|
||||
Q_ASSERT(waypointObj != NULL);
|
||||
pathactionObj = PathAction::GetInstance(objManager);
|
||||
Q_ASSERT(pathactionObj != NULL);
|
||||
}
|
||||
|
||||
void ModelUavoProxy::sendFlightPlan()
|
||||
{
|
||||
modelToObjects();
|
||||
|
||||
Waypoint *waypoint = Waypoint::GetInstance(objManager, 0);
|
||||
connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool)));
|
||||
|
||||
PathAction *action = PathAction::GetInstance(objManager, 0);
|
||||
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool)));
|
||||
|
||||
completionCount = 0;
|
||||
completionSuccessCount = 0;
|
||||
|
||||
waypoint->updatedAll();
|
||||
action->updatedAll();
|
||||
}
|
||||
|
||||
void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success)
|
||||
{
|
||||
obj->disconnect(this);
|
||||
|
||||
completionCount++;
|
||||
if (success) {
|
||||
completionSuccessCount++;
|
||||
}
|
||||
|
||||
if (completionCount == 2) {
|
||||
qDebug() << "ModelUavoProxy::flightPlanSent - success" << (completionSuccessCount == 2);
|
||||
if (completionSuccessCount == 2) {
|
||||
// TODO : popup?
|
||||
}
|
||||
else {
|
||||
// TODO : popup?
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ModelUavoProxy::receiveFlightPlan()
|
||||
{
|
||||
Waypoint *waypoint = Waypoint::GetInstance(objManager, 0);
|
||||
connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool)));
|
||||
|
||||
PathAction *action = PathAction::GetInstance(objManager, 0);
|
||||
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool)));
|
||||
|
||||
completionCount = 0;
|
||||
completionSuccessCount = 0;
|
||||
|
||||
waypoint->requestUpdateAll();
|
||||
action->requestUpdateAll();
|
||||
}
|
||||
|
||||
void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success)
|
||||
{
|
||||
obj->disconnect(this);
|
||||
|
||||
completionCount++;
|
||||
if (success) {
|
||||
completionSuccessCount++;
|
||||
}
|
||||
|
||||
if (completionCount == 2) {
|
||||
qDebug() << "ModelUavoProxy::flightPlanReceived - success" << (completionSuccessCount == 2);
|
||||
if (completionSuccessCount == 2) {
|
||||
objectsToModel();
|
||||
// TODO : popup?
|
||||
}
|
||||
else {
|
||||
// TODO : popup?
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ModelUavoProxy::modelToObjects()
|
||||
@ -80,10 +149,8 @@ void ModelUavoProxy::modelToObjects()
|
||||
action = createPathAction(actionCount, action);
|
||||
actionCount++;
|
||||
|
||||
// send action update to UAV
|
||||
// update UAVObject
|
||||
action->setData(actionData);
|
||||
action->updated();
|
||||
qDebug() << "ModelUAVProxy::modelToObjects - sent action instance :" << action->getInstID();
|
||||
}
|
||||
else {
|
||||
action->deleteLater();
|
||||
@ -104,16 +171,14 @@ void ModelUavoProxy::modelToObjects()
|
||||
// connect waypoint to path action
|
||||
waypointData.Action = action->getInstID();
|
||||
|
||||
// send waypoint update to UAV
|
||||
// update UAVObject
|
||||
waypoint->setData(waypointData);
|
||||
waypoint->updated();
|
||||
qDebug() << "ModelUAVProxy::modelToObjects - sent waypoint instance :" << waypoint->getInstID();
|
||||
}
|
||||
}
|
||||
|
||||
Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) {
|
||||
Waypoint *waypoint = NULL;
|
||||
int count = objManager->getNumInstances(waypointObj->getObjID());
|
||||
int count = objManager->getNumInstances(Waypoint::OBJID);
|
||||
if (index < count) {
|
||||
// reuse object
|
||||
qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count;
|
||||
@ -138,7 +203,7 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) {
|
||||
|
||||
PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) {
|
||||
PathAction *action = NULL;
|
||||
int count = objManager->getNumInstances(pathactionObj->getObjID());
|
||||
int count = objManager->getNumInstances(PathAction::OBJID);
|
||||
if (index < count) {
|
||||
// reuse object
|
||||
qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count;
|
||||
@ -162,7 +227,7 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) {
|
||||
}
|
||||
|
||||
PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) {
|
||||
int instancesCount = objManager->getNumInstances(pathactionObj->getObjID());
|
||||
int instancesCount = objManager->getNumInstances(PathAction::OBJID);
|
||||
int count = actionCount <= instancesCount ? actionCount : instancesCount;
|
||||
for (int i = 0; i < count; ++i) {
|
||||
PathAction *action = PathAction::GetInstance(objManager, i);
|
||||
@ -189,21 +254,29 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD
|
||||
|
||||
void ModelUavoProxy::objectsToModel()
|
||||
{
|
||||
myModel->removeRows(0, myModel->rowCount());
|
||||
int instanceCount = objManager->getNumInstances(Waypoint::OBJID);
|
||||
// TODO retain only reachable waypoints
|
||||
|
||||
int rowCount = myModel->rowCount();
|
||||
if (instanceCount < rowCount) {
|
||||
myModel->removeRows(instanceCount, rowCount - instanceCount);
|
||||
}
|
||||
else if (instanceCount > rowCount) {
|
||||
myModel->insertRows(rowCount, instanceCount - rowCount);
|
||||
}
|
||||
|
||||
int instanceCount = objManager->getNumInstances(waypointObj->getObjID());
|
||||
for (int i = 0; i < instanceCount; ++i) {
|
||||
Waypoint *waypoint = Waypoint::GetInstance(objManager, i);
|
||||
Q_ASSERT(waypoint);
|
||||
if (!waypoint) {
|
||||
continue;
|
||||
}
|
||||
myModel->insertRow(i);
|
||||
|
||||
Waypoint::DataFields waypointData = waypoint->getData();
|
||||
waypointToModel(i, waypointData);
|
||||
|
||||
PathAction *action = PathAction::GetInstance(objManager, waypointData.Action);
|
||||
int actionId = waypointData.Action;
|
||||
PathAction *action = PathAction::GetInstance(objManager, actionId);
|
||||
Q_ASSERT(action);
|
||||
if (!action) {
|
||||
continue;
|
||||
@ -226,8 +299,8 @@ void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) {
|
||||
index = myModel->index(i, flightDataModel::VELOCITY);
|
||||
velocity = myModel->data(index).toFloat();
|
||||
|
||||
data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / (180 * M_PI));
|
||||
data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / (180 * M_PI));
|
||||
data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI);
|
||||
data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI);
|
||||
data.Position[Waypoint::POSITION_DOWN] = -altitude;
|
||||
data.Velocity = velocity;
|
||||
}
|
||||
@ -237,7 +310,7 @@ void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data) {
|
||||
double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] +
|
||||
data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]);
|
||||
|
||||
double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * (180 / M_PI);
|
||||
double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI;
|
||||
if (bearing != bearing) {
|
||||
bearing = 0;
|
||||
}
|
||||
|
@ -38,15 +38,18 @@ class ModelUavoProxy : public QObject {
|
||||
public:
|
||||
explicit ModelUavoProxy(QObject *parent, flightDataModel *model);
|
||||
|
||||
public slots:
|
||||
void modelToObjects();
|
||||
void objectsToModel();
|
||||
|
||||
public slots:
|
||||
void sendFlightPlan();
|
||||
void receiveFlightPlan();
|
||||
|
||||
private:
|
||||
UAVObjectManager *objManager;
|
||||
Waypoint *waypointObj;
|
||||
PathAction *pathactionObj;
|
||||
flightDataModel *myModel;
|
||||
uint completionCount;
|
||||
uint completionSuccessCount;
|
||||
|
||||
Waypoint *createWaypoint(int index, Waypoint *newWaypoint);
|
||||
void modelToWaypoint(int i, Waypoint::DataFields &data);
|
||||
@ -56,6 +59,11 @@ private:
|
||||
PathAction *createPathAction(int index, PathAction *newAction);
|
||||
void modelToPathAction(int i, PathAction::DataFields &data);
|
||||
void pathActionToModel(int i, PathAction::DataFields &data);
|
||||
|
||||
public slots:
|
||||
void flightPlanSent(UAVObject *, bool success);
|
||||
void flightPlanReceived(UAVObject *, bool success);
|
||||
|
||||
};
|
||||
|
||||
#endif // MODELUAVOPROXY_H
|
||||
|
@ -226,8 +226,10 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
|
||||
table->setModel(model, selectionModel);
|
||||
waypoint_edit_dialog = new opmap_edit_waypoint_dialog(this, model, selectionModel);
|
||||
UAVProxy = new ModelUavoProxy(this, model);
|
||||
connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(modelToObjects()));
|
||||
connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(objectsToModel()));
|
||||
// sending and receiving is asynchronous
|
||||
// TODO : buttons should be disabled while a send or receive is in progress
|
||||
connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(sendFlightPlan()));
|
||||
connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(receiveFlightPlan()));
|
||||
#endif
|
||||
magicWayPoint = m_map->magicWPCreate();
|
||||
magicWayPoint->setVisible(false);
|
||||
|
@ -1,30 +1,24 @@
|
||||
<xml>
|
||||
<object name="PathAction" singleinstance="false" settings="false" category="Navigation">
|
||||
<description>A waypoint command the pathplanner is to use at a certain waypoint</description>
|
||||
<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
|
||||
DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
|
||||
FixedAttitude,
|
||||
SetAccessory,
|
||||
DisarmAlarm" default="Endpoint" />
|
||||
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
|
||||
|
||||
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
|
||||
DistanceToTarget,LegRemaining,BelowError,
|
||||
AboveAltitude,AboveSpeed,
|
||||
PointingTowardsNext,
|
||||
PythonScript,
|
||||
Immediate" default="None" />
|
||||
<field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
|
||||
|
||||
<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
|
||||
OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
|
||||
IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
|
||||
<field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
|
||||
<field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
|
||||
<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
|
||||
DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
|
||||
FixedAttitude, SetAccessory, DisarmAlarm" default="Endpoint" />
|
||||
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
|
||||
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
|
||||
DistanceToTarget,LegRemaining,BelowError,AboveAltitude,AboveSpeed,PointingTowardsNext,
|
||||
PythonScript,Immediate" default="None" />
|
||||
<field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
|
||||
<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
|
||||
OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
|
||||
IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
|
||||
<field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
|
||||
<field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="4000"/>
|
||||
<telemetryflight acked="true" updatemode="manual" period="0"/>
|
||||
<logging updatemode="periodic" period="1000"/>
|
||||
</object>
|
||||
</xml>
|
||||
|
@ -1,12 +1,14 @@
|
||||
<xml>
|
||||
<object name="Waypoint" singleinstance="false" settings="false" category="Navigation">
|
||||
<description>A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module</description>
|
||||
|
||||
<field name="Position" units="m" type="float" elementnames="North, East, Down"/>
|
||||
<field name="Velocity" units="m/s" type="float" elements="1"/>
|
||||
<field name="Action" units="" type="uint8" elements="1" />
|
||||
<field name="Velocity" units="m/s" type="float" elements="1"/>
|
||||
<field name="Action" units="" type="uint8" elements="1" />
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="4000"/>
|
||||
<telemetryflight acked="true" updatemode="manual" period="0"/>
|
||||
<logging updatemode="periodic" period="1000"/>
|
||||
</object>
|
||||
</xml>
|
||||
|
Loading…
x
Reference in New Issue
Block a user