rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15:54:15 +01:00
Code Issues Releases Activity

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.

Browse Source
This commit is contained in:
Philippe Renon 2013-12-01 19:38:23 +01:00
parent 0dc334adea
commit 398a3ab0e6
5 changed files with 128 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp
View File

@ -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;
}

14
ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h
View File

@ -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

6
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -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);

34
shared/uavobjectdefinition/pathaction.xml
View File

@ -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>

8
shared/uavobjectdefinition/waypoint.xml
View File

@ -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>