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

Merge branch 'navigation_map' into revo

Browse Source
This commit is contained in:
James Cotton 2012-06-03 09:48:28 -05:00
commit 96bd5ba574
19 changed files with 1397 additions and 912 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

201
flight/Modules/PathPlanner/pathplanner.c
View File

@ -54,12 +54,17 @@ static WaypointActiveData waypointActive;
static WaypointData waypoint;
// Private functions
static void advanceWaypoint();
static void checkTerminationCondition();
static void activateWaypoint();
static void pathPlannerTask(void *parameters);
static void settingsUpdated(UAVObjEvent * ev);
static void waypointsUpdated(UAVObjEvent * ev);
static void createPathBox();
static void createPathLogo();
static int32_t active_waypoint = -1;
/**
* Module initialization
*/
@ -107,11 +112,7 @@ static void pathPlannerTask(void *parameters)
WaypointActiveConnectCallback(waypointsUpdated);
FlightStatusData flightStatus;
PathDesiredData pathDesired;
PositionActualData positionActual;
const float MIN_RADIUS = 4.0f; // Radius to consider at waypoint (m)
// Main thread loop
bool pathplanner_active = false;
while (1)
@ -124,6 +125,8 @@ static void pathPlannerTask(void *parameters)
pathplanner_active = false;
continue;
}
checkTerminationCondition();
if(pathplanner_active == false) {
// This triggers callback to update variable
@ -135,71 +138,6 @@ static void pathPlannerTask(void *parameters)
continue;
}
switch(pathPlannerSettings.PathMode) {
case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT:
PositionActualGet(&positionActual);
float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) +
powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) +
powf(positionActual.Down - waypoint.Position[WAYPOINT_POSITION_DOWN], 2);
// We hit this waypoint
if (r2 < (MIN_RADIUS * MIN_RADIUS)) {
switch(waypoint.Action) {
case WAYPOINT_ACTION_NEXT:
waypointActive.Index++;
WaypointActiveSet(&waypointActive);
break;
case WAYPOINT_ACTION_RTH:
// Fly back to the home location but 20 m above it
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = -20;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
break;
default:
PIOS_DEBUG_Assert(0);
}
}
break;
case PATHPLANNERSETTINGS_PATHMODE_PATH:
PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
if (progress.fractional_progress >= 1) {
switch(waypoint.Action) {
case WAYPOINT_ACTION_NEXT:
waypointActive.Index++;
WaypointActiveSet(&waypointActive);
break;
case WAYPOINT_ACTION_RTH:
// Fly back to the home location but 20 m above it
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = -20;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
break;
default:
PIOS_DEBUG_Assert(0);
}
}
break;
}
}
}
@ -215,25 +153,109 @@ static void waypointsUpdated(UAVObjEvent * ev)
return;
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
if(active_waypoint != waypointActive.Index)
activateWaypoint(waypointActive.Index);
}
/**
* This method checks the current position against the active waypoint
* to determine if it has been reached
*/
static void checkTerminationCondition()
{
const float MIN_RADIUS = 4.0f; // Radius to consider at waypoint (m)
PositionActualData positionActual;
PathDesiredData pathDesired;
switch(waypoint.Action) {
case WAYPOINT_ACTION_ENDPOINTTONEXT:
PositionActualGet(&positionActual);
float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) +
powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) +
powf(positionActual.Down - waypoint.Position[WAYPOINT_POSITION_DOWN], 2);
// We hit this waypoint
if (r2 < (MIN_RADIUS * MIN_RADIUS))
advanceWaypoint();
break;
case WAYPOINT_ACTION_PATHTONEXT:
PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
if (progress.fractional_progress >= 1)
advanceWaypoint();
break;
case WAYPOINT_ACTION_STOP:
// Never advance even when you hit a stop waypoint
break;
}
}
/**
* Increment the waypoint index which triggers the active waypoint method
*/
static void advanceWaypoint()
{
WaypointActiveGet(&waypointActive);
waypointActive.Index++;
WaypointActiveSet(&waypointActive);
}
/**
* This method is called when a new waypoint is activated
*/
static void activateWaypoint(int idx)
{
active_waypoint = idx;
uint8_t waypoint_mode = WAYPOINT_ACTION_PATHTONEXT;
if (idx > 0) {
WaypointData prevWaypoint;
WaypointInstGet(idx - 1, &prevWaypoint);
waypoint_mode = prevWaypoint.Action;
}
PathDesiredData pathDesired;
switch(pathPlannerSettings.PathMode) {
case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT:
switch(waypoint_mode) {
case WAYPOINT_ACTION_RTH:
{
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = -50; // TODO: Get alt from somewhere?
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
}
break;
case WAYPOINT_ACTION_ENDPOINTTONEXT:
{
WaypointInstGet(idx, &waypoint);
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = -waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
}
break;
case PATHPLANNERSETTINGS_PATHMODE_PATH:
case WAYPOINT_ACTION_PATHTONEXT:
{
WaypointInstGet(idx, &waypoint);
PathDesiredData pathDesired;
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
@ -247,7 +269,7 @@ static void waypointsUpdated(UAVObjEvent * ev)
// Get current position as start point
PositionActualData positionActual;
PositionActualGet(&positionActual);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 1;
@ -256,19 +278,18 @@ static void waypointsUpdated(UAVObjEvent * ev)
// Get previous waypoint as start point
WaypointData waypointPrev;
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
pathDesired.StartingVelocity = sqrtf(powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_NORTH],2) +
powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_EAST],2));
powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_EAST],2));
}
PathDesiredSet(&pathDesired);
}
break;
}
}
void settingsUpdated(UAVObjEvent * ev) {
@ -300,7 +321,7 @@ static void createPathBox()
// Draw O
WaypointData waypoint;
waypoint.Velocity[0] = 2; // Since for now this isn't directional just set a mag
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
waypoint.Position[0] = 5;
waypoint.Position[1] = 5;
@ -337,7 +358,7 @@ static void createPathLogo()
waypoint.Position[1] = 30 * cos(i / 19.0 * 2 * M_PI);
waypoint.Position[0] = 50 * sin(i / 19.0 * 2 * M_PI);
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
}
@ -347,7 +368,7 @@ static void createPathLogo()
waypoint.Position[1] = 55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[0] = 25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
}
@ -355,7 +376,7 @@ static void createPathLogo()
waypoint.Position[1] = 35;
waypoint.Position[0] = -50;
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
WaypointInstSet(35, &waypoint);
@ -363,42 +384,42 @@ static void createPathLogo()
waypoint.Position[1] = 35;
waypoint.Position[0] = -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
WaypointInstSet(36, &waypoint);
waypoint.Position[1] = 85;
waypoint.Position[0] = -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
WaypointInstSet(37, &waypoint);
waypoint.Position[1] = 85;
waypoint.Position[0] = 60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
WaypointInstSet(38, &waypoint);
waypoint.Position[1] = -40;
waypoint.Position[0] = 60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
WaypointInstSet(39, &waypoint);
waypoint.Position[1] = -40;
waypoint.Position[0] = -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
WaypointInstSet(40, &waypoint);
waypoint.Position[1] = 35;
waypoint.Position[0] = -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance();
WaypointInstSet(41, &waypoint);

25
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp
View File

@ -40,6 +40,7 @@ namespace mapcontrol
core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height()));
core->SetMapType(MapType::GoogleHybrid);
this->SetZoom(2);
this->setFlag(ItemIsFocusable);
connect(core,SIGNAL(OnNeedInvalidation()),this,SLOT(Core_OnNeedInvalidation()));
connect(core,SIGNAL(OnMapDrag()),this,SLOT(ChildPosRefresh()));
connect(core,SIGNAL(OnMapZoomChanged()),this,SLOT(ChildPosRefresh()));
@ -196,7 +197,7 @@ namespace mapcontrol
}
}
else if(isSelected && !selectionStart.IsEmpty() && (event->modifiers() == Qt::AltModifier || event->modifiers() == Qt::ShiftModifier))
else if(isSelected && !selectionStart.IsEmpty() && (event->modifiers() == Qt::ControlModifier || event->modifiers() == Qt::ShiftModifier))
{
selectionEnd = FromLocalToLatLng(event->pos().x(), event->pos().y());
{
@ -216,11 +217,9 @@ namespace mapcontrol
void MapGraphicItem::mousePressEvent(QGraphicsSceneMouseEvent *event)
{
if(!IsMouseOverMarker())
{
if(event->button() == config->DragButton && CanDragMap()&& !((event->modifiers()==Qt::AltModifier)||(event->modifiers()==Qt::ShiftModifier)))
if(event->button() == config->DragButton && CanDragMap()&& !((event->modifiers()==Qt::ShiftModifier)||(event->modifiers()==Qt::ControlModifier)))
{
core->mouseDown.SetX(event->pos().x());
core->mouseDown.SetY(event->pos().y());
@ -232,7 +231,7 @@ namespace mapcontrol
this->update();
}
else if(!isSelected && ((event->modifiers()==Qt::AltModifier)||(event->modifiers()==Qt::ShiftModifier)))
else if(!isSelected && ((event->modifiers()==Qt::ControlModifier)||(event->modifiers()==Qt::ShiftModifier)))
{
isSelected = true;
SetSelectedArea (internals::RectLatLng::Empty);
@ -240,7 +239,6 @@ namespace mapcontrol
selectionStart = FromLocalToLatLng(event->pos().x(), event->pos().y());
}
}
}
void MapGraphicItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event)
{
@ -274,6 +272,16 @@ namespace mapcontrol
}
}
void MapGraphicItem::keyPressEvent(QKeyEvent *event)
{
if(event->modifiers()&(Qt::ShiftModifier|Qt::ControlModifier))
this->setCursor(Qt::CrossCursor);
}
void MapGraphicItem::keyReleaseEvent(QKeyEvent *event)
{
if((event->modifiers()&(Qt::ShiftModifier|Qt::ControlModifier))==0)
this->setCursor(Qt::ArrowCursor);
}
bool MapGraphicItem::SetZoomToFitRect(internals::RectLatLng const& rect)
{
int maxZoom = core->GetMaxZoomToFitRect(rect);
@ -431,11 +439,6 @@ namespace mapcontrol
}
}
}
// painter->drawRect(core->GetrenderOffset().X()-lastimagepoint.X()-3,core->GetrenderOffset().Y()-lastimagepoint.Y()-3,lastimage.width(),lastimage.height());
// painter->setPen(Qt::red);
// painter->drawLine(-10,-10,10,10);
// painter->drawLine(10,10,-10,-10);
// painter->drawRect(boundingRect().adjusted(100,100,-100,-100));
}

2
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h
View File

@ -107,6 +107,8 @@ namespace mapcontrol
void wheelEvent ( QGraphicsSceneWheelEvent * event );
void mouseReleaseEvent(QGraphicsSceneMouseEvent *event);
bool IsMouseOverMarker()const{return isMouseOverMarker;}
void keyPressEvent ( QKeyEvent * event );
void keyReleaseEvent ( QKeyEvent * event );
/**
* @brief Returns current map zoom

1
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp
View File

@ -33,6 +33,7 @@ MapRipForm::MapRipForm(QWidget *parent) :
ui(new Ui::MapRipForm)
{
ui->setupUi(this);
connect(ui->cancelButton,SIGNAL(clicked()),this,SIGNAL(cancelRequest()));
}
MapRipForm::~MapRipForm()

2
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h
View File

@ -44,6 +44,8 @@ public slots:
void SetPercentage(int const& perc);
void SetProvider(QString const& prov,int const& zoom);
void SetNumberOfTiles(int const& total,int const& actual);
signals:
void cancelRequest();
private:
Ui::MapRipForm *ui;
};

8
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.ui
View File

@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>392</width>
<width>521</width>
<height>133</height>
</rect>
</property>
@ -18,7 +18,7 @@
<rect>
<x>20</x>
<y>60</y>
<width>371</width>
<width>481</width>
<height>23</height>
</rect>
</property>
@ -31,7 +31,7 @@
<rect>
<x>30</x>
<y>10</y>
<width>321</width>
<width>481</width>
<height>16</height>
</rect>
</property>
@ -55,7 +55,7 @@
<widget class="QPushButton" name="cancelButton">
<property name="geometry">
<rect>
<x>280</x>
<x>220</x>
<y>100</y>
<width>75</width>
<height>23</height>

76
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp
View File

@ -28,17 +28,19 @@
namespace mapcontrol
{
MapRipper::MapRipper(internals::Core * core, const internals::RectLatLng & rect):sleep(100),cancel(false),progressForm(0),core(core)
MapRipper::MapRipper(internals::Core * core, const internals::RectLatLng & rect):sleep(100),cancel(false),progressForm(0),core(core),yesToAll(false)
{
if(!rect.IsEmpty())
{
type=core->GetMapType();
progressForm=new MapRipForm;
connect(progressForm,SIGNAL(cancelRequest()),this,SLOT(stopFetching()));
area=rect;
zoom=core->Zoom();
maxzoom=core->MaxZoom();
points=core->Projection()->GetAreaTileList(area,zoom,0);
this->start();
cancel=false;
progressForm->show();
connect(this,SIGNAL(percentageChanged(int)),progressForm,SLOT(SetPercentage(int)));
connect(this,SIGNAL(numberOfTilesChanged(int,int)),progressForm,SLOT(SetNumberOfTiles(int,int)));
@ -46,32 +48,54 @@ namespace mapcontrol
connect(this,SIGNAL(finished()),this,SLOT(finish()));
emit numberOfTilesChanged(0,0);
}
else
QMessageBox::information(new QWidget(),"No valid selection","Please select the area of the map to rip with Mouse+Control key");
}
void MapRipper::finish()
void MapRipper::finish()
{
if(zoom<maxzoom && !cancel)
{
if(zoom<maxzoom)
++zoom;
int ret;
if(!yesToAll)
{
++zoom;
QMessageBox msgBox;
msgBox.setText(QString("Continue Ripping at zoom level %1?").arg(zoom));
// msgBox.setInformativeText("Do you want to save your changes?");
msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
msgBox.setDefaultButton(QMessageBox::Yes);
int ret = msgBox.exec();
if(ret==QMessageBox::Yes)
{
points.clear();
points=core->Projection()->GetAreaTileList(area,zoom,0);
this->start();
}
else
{
progressForm->close();
delete progressForm;
this->deleteLater();
}
}
QMessageBox msgBox;
msgBox.setText(QString("Continue Ripping at zoom level %1?").arg(zoom));
// msgBox.setInformativeText("Do you want to save your changes?");
msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::YesAll);
msgBox.setDefaultButton(QMessageBox::Yes);
ret = msgBox.exec();
}
else
ret=QMessageBox::Yes;
if(ret==QMessageBox::Yes)
{
points.clear();
points=core->Projection()->GetAreaTileList(area,zoom,0);
this->start();
}
else if(ret==QMessageBox::YesAll)
{
yesToAll=true;
points.clear();
points=core->Projection()->GetAreaTileList(area,zoom,0);
this->start();
}
else
{
progressForm->close();
delete progressForm;
this->deleteLater();
}
}
else
{
yesToAll=false;
progressForm->close();
delete progressForm;
this->deleteLater();
}
}
void MapRipper::run()
@ -119,4 +143,10 @@ namespace mapcontrol
QThread::msleep(sleep);
}
}
void MapRipper::stopFetching()
{
QMutexLocker locker(&mutex);
cancel=true;
}
}

3
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h
View File

@ -50,6 +50,8 @@ namespace mapcontrol
MapRipForm * progressForm;
int maxzoom;
internals::Core * core;
bool yesToAll;
QMutex mutex;
signals:
void percentageChanged(int const& perc);
@ -58,6 +60,7 @@ namespace mapcontrol
public slots:
void stopFetching();
void finish();
};
}

10
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp
View File

@ -97,8 +97,14 @@ namespace mapcontrol
{
if(event->button()==Qt::LeftButton)
{
delete text;
delete textBG;
if(text) {
delete text;
text = NULL;
}
if(textBG) {
delete textBG;
textBG = NULL;
}
coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y());
isDragging=false;
RefreshToolTip();

23
ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
View File

@ -143,6 +143,29 @@ int CoordinateConversions::GetLLA(double homeLLA[3], double NED[3], double posit
return 0;
}
/**
* Get the current location in Longitude, Latitude Altitude (above WSG-48 ellipsoid)
* @param[in] BaseECEF the ECEF of the home location (in cm)
* @param[in] position three element double for position in degrees and meters
* @param[out] NED the offset from the home location (in m)
* @returns
* @arg 0 success
* @arg -1 for failure
*/
int CoordinateConversions::GetNED(double homeLLA[3], double position[3], double NED[3])
{
double T[3];
T[0] = homeLLA[2]+6.378137E6f * M_PI / 180.0;
T[1] = cosf(homeLLA[0] * M_PI / 180.0)*(homeLLA[2]+6.378137E6f) * M_PI / 180.0;
T[2] = -1.0f;
NED[0] = (position[0] - homeLLA[0]) * T[0];
NED[1] = (position[1] - homeLLA[1]) * T[1];
NED[2] = (position[2] - homeLLA[2]) * T[2];
return 0;
}
void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{
double ECEF[3];

1
ground/openpilotgcs/src/libs/utils/coordinateconversions.h
View File

@ -42,6 +42,7 @@ class QTCREATOR_UTILS_EXPORT CoordinateConversions
public:
CoordinateConversions();
int GetLLA(double LLA[3], double NED[3], double position[3]);
int GetNED(double homeLLA[3], double position[3], double NED[3]);
void RneFromLLA(double LLA[3], double Rne[3][3]);
void LLA2ECEF(double LLA[3], double ECEF[3]);
int ECEF2LLA(double ECEF[3], double LLA[3]);

43
ground/openpilotgcs/src/plugins/config/input.ui
View File

@ -239,7 +239,7 @@
<string>Configure each stabilization mode for each axis</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<item row="1" column="3">
<item row="0" column="3">
<widget class="QLabel" name="label_10">
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
@ -256,7 +256,7 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="2" column="0">
<item row="1" column="0">
<widget class="QLabel" name="label_14">
<property name="text">
<string>Stabilized1</string>
@ -266,7 +266,7 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="1" column="1">
<widget class="QComboBox" name="fmsSsPos1Roll">
<property name="minimumSize">
<size>
@ -279,7 +279,7 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="2" column="2">
<item row="1" column="2">
<widget class="QComboBox" name="fmsSsPos1Pitch">
<property name="minimumSize">
<size>
@ -292,7 +292,7 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="2" column="3">
<item row="1" column="3">
<widget class="QComboBox" name="fmsSsPos1Yaw">
<property name="minimumSize">
<size>
@ -305,7 +305,7 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="3" column="0">
<item row="2" column="0">
<widget class="QLabel" name="label_21">
<property name="text">
<string>Stabilized2</string>
@ -315,28 +315,28 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="2" column="1">
<widget class="QComboBox" name="fmsSsPos2Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="3" column="2">
<item row="2" column="2">
<widget class="QComboBox" name="fmsSsPos2Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="3" column="3">
<item row="2" column="3">
<widget class="QComboBox" name="fmsSsPos2Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="3" column="0">
<widget class="QLabel" name="label_22">
<property name="text">
<string>Stabilized3</string>
@ -346,28 +346,28 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="3" column="1">
<widget class="QComboBox" name="fmsSsPos3Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="4" column="2">
<item row="3" column="2">
<widget class="QComboBox" name="fmsSsPos3Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="4" column="3">
<item row="3" column="3">
<widget class="QComboBox" name="fmsSsPos3Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="1">
<item row="0" column="1">
<widget class="QLabel" name="label_8">
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
@ -384,20 +384,7 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="0" column="1">
<spacer name="verticalSpacer_3">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="2">
<item row="0" column="2">
<widget class="QLabel" name="label_9">
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));

96
ground/openpilotgcs/src/plugins/opmap/opmap.pro
View File

@ -1,46 +1,50 @@
TEMPLATE = lib
TARGET = OPMapGadget
include(../../openpilotgcsplugin.pri)
include(../../plugins/coreplugin/coreplugin.pri)
include(../../libs/opmapcontrol/opmapcontrol.pri)
include(../../plugins/uavobjects/uavobjects.pri)
include(../../plugins/uavobjectutil/uavobjectutil.pri)
include(../../plugins/uavtalk/uavtalk.pri)
include(../../libs/utils/utils.pri)
HEADERS += opmapplugin.h \
opmapgadgetoptionspage.h \
opmapgadgetfactory.h \
opmapgadgetconfiguration.h \
opmapgadget.h \
opmapgadgetwidget.h \
# opmap_waypointeditor_dialog.h \
# opmap_edit_waypoint_dialog.h \
opmap_zoom_slider_widget.h \
opmap_statusbar_widget.h \
opmap_overlay_widget.h
SOURCES += opmapplugin.cpp \
opmapgadgetwidget.cpp \
opmapgadgetoptionspage.cpp \
opmapgadgetfactory.cpp \
opmapgadgetconfiguration.cpp \
opmapgadget.cpp \
# opmap_waypointeditor_dialog.cpp \
# opmap_edit_waypoint_dialog.cpp \
opmap_zoom_slider_widget.cpp \
opmap_statusbar_widget.cpp \
opmap_overlay_widget.cpp
OTHER_FILES += OPMapGadget.pluginspec
FORMS += opmapgadgetoptionspage.ui \
opmap_widget.ui \
# opmap_waypointeditor_dialog.ui \
# opmap_edit_waypoint_dialog.ui \
opmap_zoom_slider_widget.ui \
opmap_statusbar_widget.ui \
opmap_overlay_widget.ui
RESOURCES += opmap.qrc
TEMPLATE = lib
TARGET = OPMapGadget
include(../../openpilotgcsplugin.pri)
include(../../plugins/coreplugin/coreplugin.pri)
include(../../libs/opmapcontrol/opmapcontrol.pri)
include(../../plugins/uavobjects/uavobjects.pri)
include(../../plugins/uavobjectutil/uavobjectutil.pri)
include(../../plugins/uavtalk/uavtalk.pri)
include(../../libs/utils/utils.pri)
HEADERS += opmapplugin.h \
opmapgadgetoptionspage.h \
opmapgadgetfactory.h \
opmapgadgetconfiguration.h \
opmapgadget.h \
opmapgadgetwidget.h \
# opmap_waypointeditor_dialog.h \
# opmap_edit_waypoint_dialog.h \
opmap_zoom_slider_widget.h \
opmap_statusbar_widget.h \
opmap_overlay_widget.h \
pathcompiler.h
SOURCES += opmapplugin.cpp \
opmapgadgetwidget.cpp \
opmapgadgetoptionspage.cpp \
opmapgadgetfactory.cpp \
opmapgadgetconfiguration.cpp \
opmapgadget.cpp \
# opmap_waypointeditor_dialog.cpp \
# opmap_edit_waypoint_dialog.cpp \
opmap_zoom_slider_widget.cpp \
opmap_statusbar_widget.cpp \
opmap_overlay_widget.cpp \
pathcompiler.cpp
OTHER_FILES += OPMapGadget.pluginspec
FORMS += opmapgadgetoptionspage.ui \
opmap_widget.ui \
# opmap_waypointeditor_dialog.ui \
# opmap_edit_waypoint_dialog.ui \
opmap_zoom_slider_widget.ui \
opmap_statusbar_widget.ui \
opmap_overlay_widget.ui
RESOURCES += opmap.qrc

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

@ -2,12 +2,12 @@
******************************************************************************
*
* @file opmapgadgetwidget.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup OPMapPlugin OpenPilot Map Plugin
* @{
* @brief The OpenPilot Map plugin
* @brief The OpenPilot Map plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -40,7 +40,7 @@
#include "homelocation.h"
#include "positionactual.h"
#include <pathcompiler.h>
#include <math.h>
#include "utils/stylehelper.h"
@ -90,61 +90,65 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
m_mouse_waypoint = NULL;
pm = NULL;
obm = NULL;
obum = NULL;
pm = NULL;
obm = NULL;
obum = NULL;
m_prev_tile_number = 0;
m_prev_tile_number = 0;
m_min_zoom = m_max_zoom = 0;
m_min_zoom = m_max_zoom = 0;
m_map_mode = Normal_MapMode;
m_maxUpdateRate = max_update_rate_list[4]; // 2 seconds
m_maxUpdateRate = max_update_rate_list[4]; // 2 seconds
m_telemetry_connected = false;
m_telemetry_connected = false;
m_context_menu_lat_lon = m_mouse_lat_lon = internals::PointLatLng(0, 0);
m_context_menu_lat_lon = m_mouse_lat_lon = internals::PointLatLng(0, 0);
setMouseTracking(true);
pm = ExtensionSystem::PluginManager::instance();
if (pm)
{
obm = pm->getObject<UAVObjectManager>();
obum = pm->getObject<UAVObjectUtilManager>();
}
pm = ExtensionSystem::PluginManager::instance();
if (pm)
{
obm = pm->getObject<UAVObjectManager>();
obum = pm->getObject<UAVObjectUtilManager>();
}
// **************
// **************
// get current location
double latitude = 0;
double longitude = 0;
double altitude = 0;
// current position
getUAVPosition(latitude, longitude, altitude);
// current position
getUAVPosition(latitude, longitude, altitude);
internals::PointLatLng pos_lat_lon = internals::PointLatLng(latitude, longitude);
// **************
// default home position
m_home_position.coord = pos_lat_lon;
m_home_position.altitude = altitude;
m_home_position.locked = false;
m_home_position.coord = pos_lat_lon;
m_home_position.altitude = altitude;
m_home_position.locked = false;
// **************
// default magic waypoint params
m_magic_waypoint.map_wp_item = NULL;
m_magic_waypoint.coord = m_home_position.coord;
m_magic_waypoint.altitude = altitude;
m_magic_waypoint.description = "Magic waypoint";
m_magic_waypoint.locked = false;
m_magic_waypoint.time_seconds = 0;
m_magic_waypoint.hold_time_seconds = 0;
m_magic_waypoint.map_wp_item = NULL;
m_magic_waypoint.coord = m_home_position.coord;
m_magic_waypoint.altitude = altitude;
m_magic_waypoint.description = "Magic waypoint";
m_magic_waypoint.locked = false;
m_magic_waypoint.time_seconds = 0;
m_magic_waypoint.hold_time_seconds = 0;
// Connect to the path compiler to get updates from the waypoints
pathCompiler = new PathCompiler(this);
connect(pathCompiler,SIGNAL(visualizationChanged(QList<PathCompiler::waypoint>)),
this, SLOT(doVisualizationChanged(QList<PathCompiler::waypoint>)));
// **************
// create the widget that holds the user controls and the map
@ -164,8 +168,8 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
m_widget->horizontalSliderZoom->setMinimum(m_map->MinZoom()); //
m_widget->horizontalSliderZoom->setMaximum(m_map->MaxZoom() + max_digital_zoom); //
m_min_zoom = m_widget->horizontalSliderZoom->minimum(); // minimum zoom we can accept
m_max_zoom = m_widget->horizontalSliderZoom->maximum(); // maximum zoom we can accept
m_min_zoom = m_widget->horizontalSliderZoom->minimum(); // minimum zoom we can accept
m_max_zoom = m_widget->horizontalSliderZoom->maximum(); // maximum zoom we can accept
m_map->SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionWithoutCenter); // set how the mouse wheel zoom functions
m_map->SetFollowMouse(true); // we want a contiuous mouse position reading
@ -173,20 +177,20 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
m_map->SetShowHome(true); // display the HOME position on the map
m_map->SetShowUAV(true); // display the UAV position on the map
m_map->Home->SetSafeArea(safe_area_radius_list[0]); // set radius (meters)
m_map->Home->SetSafeArea(safe_area_radius_list[0]); // set radius (meters)
m_map->Home->SetShowSafeArea(true); // show the safe area
m_map->UAV->SetTrailTime(uav_trail_time_list[0]); // seconds
m_map->UAV->SetTrailDistance(uav_trail_distance_list[1]); // meters
m_map->UAV->SetTrailType(UAVTrailType::ByTimeElapsed);
// m_map->UAV->SetTrailType(UAVTrailType::ByDistance);
// m_map->UAV->SetTrailType(UAVTrailType::ByDistance);
m_map->GPS->SetTrailTime(uav_trail_time_list[0]); // seconds
m_map->GPS->SetTrailDistance(uav_trail_distance_list[1]); // meters
m_map->GPS->SetTrailType(UAVTrailType::ByTimeElapsed);
// m_map->GPS->SetTrailType(UAVTrailType::ByDistance);
// m_map->GPS->SetTrailType(UAVTrailType::ByDistance);
// **************
@ -205,24 +209,24 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
switch (m_map_mode)
{
case Normal_MapMode:
m_widget->toolButtonMagicWaypointMapMode->setChecked(false);
m_widget->toolButtonNormalMapMode->setChecked(true);
hideMagicWaypointControls();
break;
case Normal_MapMode:
m_widget->toolButtonMagicWaypointMapMode->setChecked(false);
m_widget->toolButtonNormalMapMode->setChecked(true);
hideMagicWaypointControls();
break;
case MagicWaypoint_MapMode:
m_widget->toolButtonNormalMapMode->setChecked(false);
m_widget->toolButtonMagicWaypointMapMode->setChecked(true);
showMagicWaypointControls();
break;
case MagicWaypoint_MapMode:
m_widget->toolButtonNormalMapMode->setChecked(false);
m_widget->toolButtonMagicWaypointMapMode->setChecked(true);
showMagicWaypointControls();
break;
default:
m_map_mode = Normal_MapMode;
m_widget->toolButtonMagicWaypointMapMode->setChecked(false);
m_widget->toolButtonNormalMapMode->setChecked(true);
hideMagicWaypointControls();
break;
default:
m_map_mode = Normal_MapMode;
m_widget->toolButtonMagicWaypointMapMode->setChecked(false);
m_widget->toolButtonNormalMapMode->setChecked(true);
hideMagicWaypointControls();
break;
}
m_widget->labelUAVPos->setText("---");
@ -241,12 +245,12 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
m_widget->progressBarMap->setMaximum(1);
/*
/*
#if defined(Q_OS_MAC)
#elif defined(Q_OS_WIN)
m_widget->comboBoxFindPlace->clear();
loadComboBoxLines(m_widget->comboBoxFindPlace, QCoreApplication::applicationDirPath() + "/opmap_find_place_history.txt");
m_widget->comboBoxFindPlace->setCurrentIndex(-1);
m_widget->comboBoxFindPlace->clear();
loadComboBoxLines(m_widget->comboBoxFindPlace, QCoreApplication::applicationDirPath() + "/opmap_find_place_history.txt");
m_widget->comboBoxFindPlace->setCurrentIndex(-1);
#else
#endif
*/
@ -269,10 +273,10 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
connect(m_map, SIGNAL(WPInserted(int const&, WayPointItem*)), this, SLOT(WPInserted(int const&, WayPointItem*)));
connect(m_map, SIGNAL(WPDeleted(int const&)), this, SLOT(WPDeleted(int const&)));
m_map->SetCurrentPosition(m_home_position.coord); // set the map position
m_map->Home->SetCoord(m_home_position.coord); // set the HOME position
m_map->UAV->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position
m_map->GPS->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position
m_map->SetCurrentPosition(m_home_position.coord); // set the map position
m_map->Home->SetCoord(m_home_position.coord); // set the HOME position
m_map->UAV->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position
m_map->GPS->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position
// **************
// create various context menu (mouse right click menu) actions
@ -282,39 +286,32 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
// **************
// connect to the UAVObject updates we require to become a bit aware of our environment:
if (pm)
{
// Register for Home Location state changes
if (obm)
{
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(obm->getObject(QString("HomeLocation")));
if (obj)
{
connect(obj, SIGNAL(objectUpdated(UAVObject *)), this , SLOT(homePositionUpdated(UAVObject *)));
}
}
Q_ASSERT(pm);
Q_ASSERT(obm);
// Listen to telemetry connection events
TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
if (telMngr)
{
connect(telMngr, SIGNAL(connected()), this, SLOT(onTelemetryConnect()));
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onTelemetryDisconnect()));
}
}
// Register for Home Location state changes
HomeLocation *obj = HomeLocation::GetInstance(obm);
Q_ASSERT(obj != NULL);
connect(obj, SIGNAL(objectUpdated(UAVObject *)), this , SLOT(homePositionUpdated(UAVObject *)));
// Listen to telemetry connection events
TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
Q_ASSERT(telMngr);
connect(telMngr, SIGNAL(connected()), this, SLOT(onTelemetryConnect()));
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onTelemetryDisconnect()));
// **************
// create the desired timers
m_updateTimer = new QTimer();
m_updateTimer->setInterval(m_maxUpdateRate);
m_updateTimer->setInterval(m_maxUpdateRate);
connect(m_updateTimer, SIGNAL(timeout()), this, SLOT(updatePosition()));
m_updateTimer->start();
m_statusUpdateTimer = new QTimer();
m_statusUpdateTimer->setInterval(200);
// m_statusUpdateTimer->setInterval(m_maxUpdateRate);
connect(m_statusUpdateTimer, SIGNAL(timeout()), this, SLOT(updateMousePos()));
m_statusUpdateTimer->setInterval(200);
// m_statusUpdateTimer->setInterval(m_maxUpdateRate);
connect(m_statusUpdateTimer, SIGNAL(timeout()), this, SLOT(updateMousePos()));
m_statusUpdateTimer->start();
// **************
@ -325,22 +322,14 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
// destructor
OPMapGadgetWidget::~OPMapGadgetWidget()
{
if (m_map)
{
disconnect(m_map, 0, 0, 0);
m_map->SetShowHome(false); // doing this appears to stop the map lib crashing on exit
m_map->SetShowUAV(false); // " "
}
if (m_map)
{
disconnect(m_map, 0, 0, 0);
m_map->SetShowHome(false); // doing this appears to stop the map lib crashing on exit
m_map->SetShowUAV(false); // " "
}
// this destructor doesn't appear to be called at shutdown???
// #if defined(Q_OS_MAC)
// #elif defined(Q_OS_WIN)
// saveComboBoxLines(m_widget->comboBoxFindPlace, QCoreApplication::applicationDirPath() + "/opmap_find_place_history.txt");
// #else
// #endif
m_waypoint_list_mutex.lock();
foreach (t_waypoint *wp, m_waypoint_list)
{
@ -355,11 +344,11 @@ OPMapGadgetWidget::~OPMapGadgetWidget()
m_waypoint_list_mutex.unlock();
m_waypoint_list.clear();
if (m_map)
{
delete m_map;
m_map = NULL;
}
if (m_map)
{
delete m_map;
m_map = NULL;
}
}
// *************************************************************************************
@ -382,7 +371,7 @@ void OPMapGadgetWidget::mouseMoveEvent(QMouseEvent *event)
if (event->buttons() & Qt::LeftButton)
{
// QPoint pos = event->pos();
// QPoint pos = event->pos();
}
QWidget::mouseMoveEvent(event);
@ -393,22 +382,22 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event)
QString s;
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
if (event->reason() != QContextMenuEvent::Mouse)
return; // not a mouse click event
// current mouse position
QPoint p = m_map->mapFromGlobal(event->globalPos());
m_context_menu_lat_lon = m_map->GetFromLocalToLatLng(p);
// m_context_menu_lat_lon = m_map->currentMousePosition();
m_context_menu_lat_lon = m_map->GetFromLocalToLatLng(p);
// m_context_menu_lat_lon = m_map->currentMousePosition();
if (!m_map->contentsRect().contains(p))
return; // the mouse click was not on the map
// show the mouse position
s = QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7);
s = QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7);
m_widget->labelMousePos->setText(s);
// find out if we have a waypoint under the mouse cursor
@ -433,18 +422,22 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event)
menu.addSeparator();
QMenu maxUpdateRateSubMenu(tr("&Max Update Rate ") + "(" + QString::number(m_maxUpdateRate) + " ms)", this);
for (int i = 0; i < maxUpdateRateAct.count(); i++)
maxUpdateRateSubMenu.addAction(maxUpdateRateAct.at(i));
menu.addMenu(&maxUpdateRateSubMenu);
menu.addAction(ripAct);
menu.addSeparator();
menu.addSeparator();
QMenu maxUpdateRateSubMenu(tr("&Max Update Rate ") + "(" + QString::number(m_maxUpdateRate) + " ms)", this);
for (int i = 0; i < maxUpdateRateAct.count(); i++)
maxUpdateRateSubMenu.addAction(maxUpdateRateAct.at(i));
menu.addMenu(&maxUpdateRateSubMenu);
menu.addSeparator();
switch (m_map_mode)
{
case Normal_MapMode: s = tr(" (Normal)"); break;
case MagicWaypoint_MapMode: s = tr(" (Magic Waypoint)"); break;
default: s = tr(" (Unknown)"); break;
case Normal_MapMode: s = tr(" (Normal)"); break;
case MagicWaypoint_MapMode: s = tr(" (Magic Waypoint)"); break;
default: s = tr(" (Unknown)"); break;
}
for (int i = 0; i < mapModeAct.count(); i++)
{ // set the menu to checked (or not)
@ -473,12 +466,14 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event)
menu.addSeparator();
*/
menu.addAction(showSafeAreaAct);
QMenu safeArea("Safety Area definitions");
// menu.addAction(showSafeAreaAct);
QMenu safeAreaSubMenu(tr("Safe Area Radius") + " (" + QString::number(m_map->Home->SafeArea()) + "m)", this);
for (int i = 0; i < safeAreaAct.count(); i++)
safeAreaSubMenu.addAction(safeAreaAct.at(i));
menu.addMenu(&safeAreaSubMenu);
safeArea.addMenu(&safeAreaSubMenu);
safeArea.addAction(showSafeAreaAct);
menu.addMenu(&safeArea);
menu.addSeparator();
@ -543,39 +538,39 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event)
// *********
qDebug() << "Testing mode";
switch (m_map_mode)
{
case Normal_MapMode:
case Normal_MapMode:
qDebug() << "Normal mode";
// only show the waypoint stuff if not in 'magic waypoint' mode
/*
menu.addSeparator()->setText(tr("Waypoints"));
menu.addSeparator()->setText(tr("Waypoints"));
menu.addAction(wayPointEditorAct);
menu.addAction(addWayPointAct);
menu.addAction(wayPointEditorAct);
menu.addAction(addWayPointAct);
if (m_mouse_waypoint)
{ // we have a waypoint under the mouse
menu.addAction(editWayPointAct);
if (m_mouse_waypoint)
{ // we have a waypoint under the mouse
menu.addAction(editWayPointAct);
lockWayPointAct->setChecked(waypoint_locked);
menu.addAction(lockWayPointAct);
lockWayPointAct->setChecked(waypoint_locked);
menu.addAction(lockWayPointAct);
if (!waypoint_locked)
menu.addAction(deleteWayPointAct);
}
if (!waypoint_locked)
menu.addAction(deleteWayPointAct);
}
m_waypoint_list_mutex.lock();
if (m_waypoint_list.count() > 0)
menu.addAction(clearWayPointsAct); // we have waypoints
m_waypoint_list_mutex.unlock();
*/
m_waypoint_list_mutex.lock();
if (m_waypoint_list.count() > 0)
menu.addAction(clearWayPointsAct); // we have waypoints
m_waypoint_list_mutex.unlock();
break;
break;
case MagicWaypoint_MapMode:
menu.addSeparator()->setText(tr("Waypoints"));
menu.addAction(homeMagicWaypointAct);
break;
case MagicWaypoint_MapMode:
menu.addSeparator()->setText(tr("Waypoints"));
menu.addAction(homeMagicWaypointAct);
break;
}
// *********
@ -595,32 +590,32 @@ void OPMapGadgetWidget::keyPressEvent(QKeyEvent* event)
switch (event->key())
{
case Qt::Key_Escape:
break;
case Qt::Key_Escape:
break;
case Qt::Key_F1:
break;
case Qt::Key_F1:
break;
case Qt::Key_F2:
break;
case Qt::Key_F2:
break;
case Qt::Key_Up:
break;
case Qt::Key_Up:
break;
case Qt::Key_Down:
break;
case Qt::Key_Down:
break;
case Qt::Key_Left:
break;
case Qt::Key_Left:
break;
case Qt::Key_Right:
break;
case Qt::Key_Right:
break;
case Qt::Key_PageUp:
break;
case Qt::Key_PageUp:
break;
case Qt::Key_PageDown:
break;
case Qt::Key_PageDown:
break;
}
}
@ -637,71 +632,71 @@ void OPMapGadgetWidget::keyPressEvent(QKeyEvent* event)
*/
void OPMapGadgetWidget::updatePosition()
{
double uav_latitude, uav_longitude, uav_altitude, uav_yaw;
double gps_latitude, gps_longitude, gps_altitude, gps_heading;
double uav_latitude, uav_longitude, uav_altitude, uav_yaw;
double gps_latitude, gps_longitude, gps_altitude, gps_heading;
internals::PointLatLng uav_pos;
internals::PointLatLng gps_pos;
internals::PointLatLng uav_pos;
internals::PointLatLng gps_pos;
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
QMutexLocker locker(&m_map_mutex);
// Pip I'm sorry, I know this was here with a purpose vvv
// from Pip: let you off :)
//if (!telemetry_connected)
// return;
// Pip I'm sorry, I know this was here with a purpose vvv
// from Pip: let you off :)
//if (!telemetry_connected)
// return;
// *************
// get the current UAV details
// *************
// get the current UAV details
// get current UAV position
if (!getUAVPosition(uav_latitude, uav_longitude, uav_altitude))
if (!getUAVPosition(uav_latitude, uav_longitude, uav_altitude))
return;
// get current UAV heading
uav_yaw = getUAV_Yaw();
uav_yaw = getUAV_Yaw();
uav_pos = internals::PointLatLng(uav_latitude, uav_longitude);
uav_pos = internals::PointLatLng(uav_latitude, uav_longitude);
// *************
// get the current GPS details
// *************
// get the current GPS details
// get current GPS position
if (!getGPSPosition(gps_latitude, gps_longitude, gps_altitude))
return;
// get current GPS position
if (!getGPSPosition(gps_latitude, gps_longitude, gps_altitude))
return;
// get current GPS heading
// gps_heading = getGPS_Heading();
gps_heading = 0;
// get current GPS heading
// gps_heading = getGPS_Heading();
gps_heading = 0;
gps_pos = internals::PointLatLng(gps_latitude, gps_longitude);
gps_pos = internals::PointLatLng(gps_latitude, gps_longitude);
// *************
// display the UAV position
// *************
// display the UAV position
QString str =
"lat: " + QString::number(uav_pos.Lat(), 'f', 7) +
" lon: " + QString::number(uav_pos.Lng(), 'f', 7) +
" " + QString::number(uav_yaw, 'f', 1) + "deg" +
" " + QString::number(uav_altitude, 'f', 1) + "m";
// " " + QString::number(uav_ground_speed_meters_per_second, 'f', 1) + "m/s";
" " + QString::number(uav_yaw, 'f', 1) + "deg" +
" " + QString::number(uav_altitude, 'f', 1) + "m";
// " " + QString::number(uav_ground_speed_meters_per_second, 'f', 1) + "m/s";
m_widget->labelUAVPos->setText(str);
// *************
// set the UAV icon position on the map
// *************
// set the UAV icon position on the map
m_map->UAV->SetUAVPos(uav_pos, uav_altitude); // set the maps UAV position
// qDebug()<<"UAVPOSITION"<<uav_pos.ToString();
m_map->UAV->SetUAVHeading(uav_yaw); // set the maps UAV heading
m_map->UAV->SetUAVPos(uav_pos, uav_altitude); // set the maps UAV position
// qDebug()<<"UAVPOSITION"<<uav_pos.ToString();
m_map->UAV->SetUAVHeading(uav_yaw); // set the maps UAV heading
// *************
// set the GPS icon position on the map
// *************
// set the GPS icon position on the map
m_map->GPS->SetUAVPos(gps_pos, gps_altitude); // set the maps GPS position
m_map->GPS->SetUAVHeading(gps_heading); // set the maps GPS heading
m_map->GPS->SetUAVPos(gps_pos, gps_altitude); // set the maps GPS position
m_map->GPS->SetUAVHeading(gps_heading); // set the maps GPS heading
// *************
// *************
}
/**
@ -710,8 +705,8 @@ void OPMapGadgetWidget::updatePosition()
*/
void OPMapGadgetWidget::updateMousePos()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
QMutexLocker locker(&m_map_mutex);
@ -721,7 +716,7 @@ void OPMapGadgetWidget::updateMousePos()
if (!m_map->contentsRect().contains(p))
return; // the mouse is not on the map
// internals::PointLatLng lat_lon = m_map->currentMousePosition(); // fetch the current lat/lon mouse position
// internals::PointLatLng lat_lon = m_map->currentMousePosition(); // fetch the current lat/lon mouse position
QGraphicsItem *item = m_map->itemAt(p);
@ -734,14 +729,14 @@ void OPMapGadgetWidget::updateMousePos()
// find out if we have a waypoint under the mouse cursor
mapcontrol::WayPointItem *wp = qgraphicsitem_cast<mapcontrol::WayPointItem *>(item);
if (m_mouse_lat_lon == lat_lon)
if (m_mouse_lat_lon == lat_lon)
return; // the mouse has not moved
m_mouse_lat_lon = lat_lon; // yes it has!
m_mouse_lat_lon = lat_lon; // yes it has!
internals::PointLatLng home_lat_lon = m_map->Home->Coord();
QString s = QString::number(m_mouse_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_mouse_lat_lon.Lng(), 'f', 7);
QString s = QString::number(m_mouse_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_mouse_lat_lon.Lng(), 'f', 7);
if (wp)
{
s += " wp[" + QString::number(wp->Number()) + "]";
@ -752,33 +747,33 @@ void OPMapGadgetWidget::updateMousePos()
s += " " + QString::number(bear, 'f', 1) + "deg";
}
else
if (home)
{
s += " home";
double dist = distance(home_lat_lon, m_mouse_lat_lon);
double bear = bearing(home_lat_lon, m_mouse_lat_lon);
s += " " + QString::number(dist * 1000, 'f', 1) + "m";
s += " " + QString::number(bear, 'f', 1) + "deg";
}
else
if (uav)
{
s += " uav";
double latitude;
double longitude;
double altitude;
if (getUAVPosition(latitude, longitude, altitude)) // get current UAV position
if (home)
{
internals::PointLatLng uav_pos = internals::PointLatLng(latitude, longitude);
s += " home";
// double dist = distance(home_lat_lon, uav_pos);
// double bear = bearing(home_lat_lon, uav_pos);
// s += " " + QString::number(dist * 1000, 'f', 1) + "m";
// s += " " + QString::number(bear, 'f', 1) + "deg";
double dist = distance(home_lat_lon, m_mouse_lat_lon);
double bear = bearing(home_lat_lon, m_mouse_lat_lon);
s += " " + QString::number(dist * 1000, 'f', 1) + "m";
s += " " + QString::number(bear, 'f', 1) + "deg";
}
}
else
if (uav)
{
s += " uav";
double latitude;
double longitude;
double altitude;
if (getUAVPosition(latitude, longitude, altitude)) // get current UAV position
{
internals::PointLatLng uav_pos = internals::PointLatLng(latitude, longitude);
// double dist = distance(home_lat_lon, uav_pos);
// double bear = bearing(home_lat_lon, uav_pos);
// s += " " + QString::number(dist * 1000, 'f', 1) + "m";
// s += " " + QString::number(bear, 'f', 1) + "deg";
}
}
m_widget->labelMousePos->setText(s);
}
@ -791,24 +786,24 @@ void OPMapGadgetWidget::updateMousePos()
*/
void OPMapGadgetWidget::zoomChanged(double zoomt, double zoom, double zoomd)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
QString s = "tot:" + QString::number(zoomt, 'f', 1) + " rea:" + QString::number(zoom, 'f', 1) + " dig:" + QString::number(zoomd, 'f', 1);
m_widget->labelMapZoom->setText(s);
int i_zoom = (int)(zoomt + 0.5);
if (i_zoom < m_min_zoom) i_zoom = m_min_zoom;
if (i_zoom < m_min_zoom) i_zoom = m_min_zoom;
else
if (i_zoom > m_max_zoom) i_zoom = m_max_zoom;
if (i_zoom > m_max_zoom) i_zoom = m_max_zoom;
if (m_widget->horizontalSliderZoom->value() != i_zoom)
m_widget->horizontalSliderZoom->setValue(i_zoom); // set the GUI zoom slider position
m_widget->horizontalSliderZoom->setValue(i_zoom); // set the GUI zoom slider position
int index0_zoom = i_zoom - m_min_zoom; // zoom level starting at index level '0'
int index0_zoom = i_zoom - m_min_zoom; // zoom level starting at index level '0'
if (index0_zoom < zoomAct.count())
zoomAct.at(index0_zoom)->setChecked(true); // set the right-click context menu zoom level
zoomAct.at(index0_zoom)->setChecked(true); // set the right-click context menu zoom level
}
void OPMapGadgetWidget::OnMapDrag()
@ -817,8 +812,8 @@ void OPMapGadgetWidget::OnMapDrag()
void OPMapGadgetWidget::OnCurrentPositionChanged(internals::PointLatLng point)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
QString coord_str = QString::number(point.Lat(), 'f', 7) + " " + QString::number(point.Lng(), 'f', 7) + " ";
m_widget->labelMapPos->setText(coord_str);
@ -829,20 +824,20 @@ void OPMapGadgetWidget::OnCurrentPositionChanged(internals::PointLatLng point)
*/
void OPMapGadgetWidget::OnTilesStillToLoad(int number)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
// if (prev_tile_number < number || m_widget->progressBarMap->maximum() < number)
// m_widget->progressBarMap->setMaximum(number);
// if (prev_tile_number < number || m_widget->progressBarMap->maximum() < number)
// m_widget->progressBarMap->setMaximum(number);
if (m_widget->progressBarMap->maximum() < number)
m_widget->progressBarMap->setMaximum(number);
if (m_widget->progressBarMap->maximum() < number)
m_widget->progressBarMap->setMaximum(number);
m_widget->progressBarMap->setValue(m_widget->progressBarMap->maximum() - number); // update the progress bar
m_widget->progressBarMap->setValue(m_widget->progressBarMap->maximum() - number); // update the progress bar
// m_widget->labelNumTilesToLoad->setText(QString::number(number));
// m_widget->labelNumTilesToLoad->setText(QString::number(number));
m_prev_tile_number = number;
m_prev_tile_number = number;
}
/**
@ -850,8 +845,8 @@ void OPMapGadgetWidget::OnTilesStillToLoad(int number)
*/
void OPMapGadgetWidget::OnTileLoadStart()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_widget->progressBarMap->setVisible(true);
}
@ -864,8 +859,8 @@ void OPMapGadgetWidget::OnTileLoadStart()
void OPMapGadgetWidget::OnTileLoadComplete()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_widget->progressBarMap->setVisible(false);
}
@ -894,38 +889,38 @@ void OPMapGadgetWidget::WPNumberChanged(int const &oldnumber, int const &newnumb
void OPMapGadgetWidget::WPValuesChanged(WayPointItem *waypoint)
{
// qDebug("opmap: WPValuesChanged");
// qDebug("opmap: WPValuesChanged");
switch (m_map_mode)
{
case Normal_MapMode:
m_waypoint_list_mutex.lock();
foreach (t_waypoint *wp, m_waypoint_list)
{ // search for the waypoint in our own waypoint list and update it
if (!wp) continue;
if (!wp->map_wp_item) continue;
if (wp->map_wp_item != waypoint) continue;
// found the waypoint in our list
wp->coord = waypoint->Coord();
wp->altitude = waypoint->Altitude();
wp->description = waypoint->Description();
break;
}
m_waypoint_list_mutex.unlock();
case Normal_MapMode:
m_waypoint_list_mutex.lock();
foreach (t_waypoint *wp, m_waypoint_list)
{ // search for the waypoint in our own waypoint list and update it
if (!wp) continue;
if (!wp->map_wp_item) continue;
if (wp->map_wp_item != waypoint) continue;
// found the waypoint in our list
wp->coord = waypoint->Coord();
wp->altitude = waypoint->Altitude();
wp->description = waypoint->Description();
break;
}
m_waypoint_list_mutex.unlock();
break;
case MagicWaypoint_MapMode:
// update our copy of the magic waypoint
if (m_magic_waypoint.map_wp_item && m_magic_waypoint.map_wp_item == waypoint)
{
m_magic_waypoint.coord = waypoint->Coord();
m_magic_waypoint.altitude = waypoint->Altitude();
m_magic_waypoint.description = waypoint->Description();
case MagicWaypoint_MapMode:
// update our copy of the magic waypoint
if (m_magic_waypoint.map_wp_item && m_magic_waypoint.map_wp_item == waypoint)
{
m_magic_waypoint.coord = waypoint->Coord();
m_magic_waypoint.altitude = waypoint->Altitude();
m_magic_waypoint.description = waypoint->Description();
// move the UAV to the magic waypoint position
// moveToMagicWaypointPosition();
}
break;
// move the UAV to the magic waypoint position
// moveToMagicWaypointPosition();
}
break;
}
}
@ -968,8 +963,8 @@ void OPMapGadgetWidget::on_toolButtonMapHome_clicked()
void OPMapGadgetWidget::on_toolButtonMapUAV_clicked()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
QMutexLocker locker(&m_map_mutex);
@ -978,16 +973,16 @@ void OPMapGadgetWidget::on_toolButtonMapUAV_clicked()
void OPMapGadgetWidget::on_toolButtonMapUAVheading_clicked()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
followUAVheadingAct->toggle();
}
void OPMapGadgetWidget::on_horizontalSliderZoom_sliderMoved(int position)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
QMutexLocker locker(&m_map_mutex);
@ -1020,30 +1015,30 @@ void OPMapGadgetWidget::on_toolButtonMoveToWP_clicked()
void OPMapGadgetWidget::onTelemetryConnect()
{
m_telemetry_connected = true;
m_telemetry_connected = true;
if (!obum) return;
if (!obum) return;
bool set;
double LLA[3];
bool set;
double LLA[3];
// ***********************
// fetch the home location
// ***********************
// fetch the home location
if (obum->getHomeLocation(set, LLA) < 0)
return; // error
if (obum->getHomeLocation(set, LLA) < 0)
return; // error
setHome(internals::PointLatLng(LLA[0], LLA[1]));
setHome(internals::PointLatLng(LLA[0], LLA[1]));
if (m_map)
m_map->SetCurrentPosition(m_home_position.coord); // set the map position
m_map->SetCurrentPosition(m_home_position.coord); // set the map position
// ***********************
}
void OPMapGadgetWidget::onTelemetryDisconnect()
{
m_telemetry_connected = false;
m_telemetry_connected = false;
}
// Updates the Home position icon whenever the HomePosition object is updated
@ -1065,21 +1060,21 @@ void OPMapGadgetWidget::homePositionUpdated(UAVObject *hp)
*/
void OPMapGadgetWidget::setHome(QPointF pos)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
double latitude = pos.x();
double longitude = pos.y();
if (latitude > 90) latitude = 90;
else
if (latitude < -90) latitude = -90;
if (latitude < -90) latitude = -90;
if (longitude != longitude) longitude = 0; // nan detection
else
if (longitude > 180) longitude = 180;
else
if (longitude < -180) longitude = -180;
if (longitude > 180) longitude = 180;
else
if (longitude < -180) longitude = -180;
setHome(internals::PointLatLng(latitude, longitude));
}
@ -1089,8 +1084,8 @@ void OPMapGadgetWidget::setHome(QPointF pos)
*/
void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
if (pos_lat_lon.Lat() != pos_lat_lon.Lat() || pos_lat_lon.Lng() != pos_lat_lon.Lng())
return;; // nan prevention
@ -1100,21 +1095,21 @@ void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon)
if (latitude != latitude) latitude = 0; // nan detection
else
if (latitude > 90) latitude = 90;
else
if (latitude < -90) latitude = -90;
if (latitude > 90) latitude = 90;
else
if (latitude < -90) latitude = -90;
if (longitude != longitude) longitude = 0; // nan detection
else
if (longitude > 180) longitude = 180;
else
if (longitude < -180) longitude = -180;
if (longitude > 180) longitude = 180;
else
if (longitude < -180) longitude = -180;
// *********
m_home_position.coord = internals::PointLatLng(latitude, longitude);
m_home_position.coord = internals::PointLatLng(latitude, longitude);
m_map->Home->SetCoord(m_home_position.coord);
m_map->Home->SetCoord(m_home_position.coord);
m_map->Home->RefreshPos();
// move the magic waypoint to keep it within the safe area boundry
@ -1127,74 +1122,74 @@ void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon)
*/
void OPMapGadgetWidget::goHome()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
followUAVpositionAct->setChecked(false);
internals::PointLatLng home_pos = m_home_position.coord; // get the home location
internals::PointLatLng home_pos = m_home_position.coord; // get the home location
m_map->SetCurrentPosition(home_pos); // center the map onto the home location
}
void OPMapGadgetWidget::zoomIn()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
int zoom = m_map->ZoomTotal() + 1;
if (zoom < m_min_zoom) zoom = m_min_zoom;
if (zoom < m_min_zoom) zoom = m_min_zoom;
else
if (zoom > m_max_zoom) zoom = m_max_zoom;
if (zoom > m_max_zoom) zoom = m_max_zoom;
m_map->SetZoom(zoom);
}
void OPMapGadgetWidget::zoomOut()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
int zoom = m_map->ZoomTotal() - 1;
if (zoom < m_min_zoom) zoom = m_min_zoom;
if (zoom < m_min_zoom) zoom = m_min_zoom;
else
if (zoom > m_max_zoom) zoom = m_max_zoom;
if (zoom > m_max_zoom) zoom = m_max_zoom;
m_map->SetZoom(zoom);
}
void OPMapGadgetWidget::setMaxUpdateRate(int update_rate)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
int list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]);
int min_rate = max_update_rate_list[0];
int max_rate = max_update_rate_list[list_size - 1];
int list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]);
int min_rate = max_update_rate_list[0];
int max_rate = max_update_rate_list[list_size - 1];
if (update_rate < min_rate) update_rate = min_rate;
else
if (update_rate > max_rate) update_rate = max_rate;
if (update_rate < min_rate) update_rate = min_rate;
else
if (update_rate > max_rate) update_rate = max_rate;
m_maxUpdateRate = update_rate;
m_maxUpdateRate = update_rate;
if (m_updateTimer)
m_updateTimer->setInterval(m_maxUpdateRate);
if (m_updateTimer)
m_updateTimer->setInterval(m_maxUpdateRate);
// if (m_statusUpdateTimer)
// m_statusUpdateTimer->setInterval(m_maxUpdateRate);
// if (m_statusUpdateTimer)
// m_statusUpdateTimer->setInterval(m_maxUpdateRate);
}
void OPMapGadgetWidget::setZoom(int zoom)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
if (zoom < m_min_zoom) zoom = m_min_zoom;
if (zoom < m_min_zoom) zoom = m_min_zoom;
else
if (zoom > m_max_zoom) zoom = m_max_zoom;
if (zoom > m_max_zoom) zoom = m_max_zoom;
internals::MouseWheelZoomType::Types zoom_type = m_map->GetMouseWheelZoomType();
m_map->SetMouseWheelZoomType(internals::MouseWheelZoomType::ViewCenter);
@ -1206,8 +1201,8 @@ void OPMapGadgetWidget::setZoom(int zoom)
void OPMapGadgetWidget::setPosition(QPointF pos)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
double latitude = pos.y();
double longitude = pos.x();
@ -1217,86 +1212,86 @@ void OPMapGadgetWidget::setPosition(QPointF pos)
if (latitude > 90) latitude = 90;
else
if (latitude < -90) latitude = -90;
if (latitude < -90) latitude = -90;
if (longitude > 180) longitude = 180;
else
if (longitude < -180) longitude = -180;
if (longitude < -180) longitude = -180;
m_map->SetCurrentPosition(internals::PointLatLng(latitude, longitude));
}
void OPMapGadgetWidget::setMapProvider(QString provider)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->SetMapType(mapcontrol::Helper::MapTypeFromString(provider));
}
void OPMapGadgetWidget::setAccessMode(QString accessMode)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->configuration->SetAccessMode(mapcontrol::Helper::AccessModeFromString(accessMode));
}
void OPMapGadgetWidget::setUseOpenGL(bool useOpenGL)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->SetUseOpenGL(useOpenGL);
}
void OPMapGadgetWidget::setShowTileGridLines(bool showTileGridLines)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->SetShowTileGridLines(showTileGridLines);
}
void OPMapGadgetWidget::setUseMemoryCache(bool useMemoryCache)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->configuration->SetUseMemoryCache(useMemoryCache);
}
void OPMapGadgetWidget::setCacheLocation(QString cacheLocation)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
cacheLocation = cacheLocation.simplified(); // remove any surrounding spaces
if (cacheLocation.isEmpty()) return;
// #if defined(Q_WS_WIN)
// if (!cacheLocation.endsWith('\\')) cacheLocation += '\\';
// #elif defined(Q_WS_X11)
if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator();
// #elif defined(Q_WS_MAC)
// if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator();
// #endif
// #if defined(Q_WS_WIN)
// if (!cacheLocation.endsWith('\\')) cacheLocation += '\\';
// #elif defined(Q_WS_X11)
if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator();
// #elif defined(Q_WS_MAC)
// if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator();
// #endif
QDir dir;
if (!dir.exists(cacheLocation))
if (!dir.mkpath(cacheLocation))
return;
// qDebug() << "opmap: map cache dir: " << cacheLocation;
// qDebug() << "opmap: map cache dir: " << cacheLocation;
m_map->configuration->SetCacheLocation(cacheLocation);
}
void OPMapGadgetWidget::setMapMode(opMapModeType mode)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
if (mode != Normal_MapMode && mode != MagicWaypoint_MapMode)
mode = Normal_MapMode; // fix error
@ -1305,87 +1300,70 @@ void OPMapGadgetWidget::setMapMode(opMapModeType mode)
{ // no change in map mode
switch (mode)
{ // make sure the UI buttons are set correctly
case Normal_MapMode:
m_widget->toolButtonMagicWaypointMapMode->setChecked(false);
m_widget->toolButtonNormalMapMode->setChecked(true);
break;
case MagicWaypoint_MapMode:
m_widget->toolButtonNormalMapMode->setChecked(false);
m_widget->toolButtonMagicWaypointMapMode->setChecked(true);
break;
case Normal_MapMode:
m_widget->toolButtonMagicWaypointMapMode->setChecked(false);
m_widget->toolButtonNormalMapMode->setChecked(true);
break;
case MagicWaypoint_MapMode:
m_widget->toolButtonNormalMapMode->setChecked(false);
m_widget->toolButtonMagicWaypointMapMode->setChecked(true);
break;
}
return;
}
switch (mode)
{
case Normal_MapMode:
m_map_mode = Normal_MapMode;
case Normal_MapMode:
m_map_mode = Normal_MapMode;
m_widget->toolButtonMagicWaypointMapMode->setChecked(false);
m_widget->toolButtonNormalMapMode->setChecked(true);
m_widget->toolButtonMagicWaypointMapMode->setChecked(false);
m_widget->toolButtonNormalMapMode->setChecked(true);
hideMagicWaypointControls();
hideMagicWaypointControls();
// delete the magic waypoint from the map
if (m_magic_waypoint.map_wp_item)
{
m_magic_waypoint.coord = m_magic_waypoint.map_wp_item->Coord();
m_magic_waypoint.altitude = m_magic_waypoint.map_wp_item->Altitude();
m_magic_waypoint.description = m_magic_waypoint.map_wp_item->Description();
m_magic_waypoint.map_wp_item = NULL;
}
m_map->WPDeleteAll();
// delete the magic waypoint from the map
if (m_magic_waypoint.map_wp_item)
{
m_magic_waypoint.coord = m_magic_waypoint.map_wp_item->Coord();
m_magic_waypoint.altitude = m_magic_waypoint.map_wp_item->Altitude();
m_magic_waypoint.description = m_magic_waypoint.map_wp_item->Description();
m_magic_waypoint.map_wp_item = NULL;
}
m_map->WPDeleteAll();
// restore the normal waypoints on the map
m_waypoint_list_mutex.lock();
foreach (t_waypoint *wp, m_waypoint_list)
{
if (!wp) continue;
wp->map_wp_item = m_map->WPCreate(wp->coord, wp->altitude, wp->description);
if (!wp->map_wp_item) continue;
wp->map_wp_item->setZValue(10 + wp->map_wp_item->Number());
wp->map_wp_item->setFlag(QGraphicsItem::ItemIsMovable, !wp->locked);
if (!wp->locked)
wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png"));
else
wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png"));
wp->map_wp_item->update();
}
m_waypoint_list_mutex.unlock();
break;
break;
case MagicWaypoint_MapMode:
m_map_mode = MagicWaypoint_MapMode;
case MagicWaypoint_MapMode:
m_map_mode = MagicWaypoint_MapMode;
m_widget->toolButtonNormalMapMode->setChecked(false);
m_widget->toolButtonMagicWaypointMapMode->setChecked(true);
m_widget->toolButtonNormalMapMode->setChecked(false);
m_widget->toolButtonMagicWaypointMapMode->setChecked(true);
showMagicWaypointControls();
showMagicWaypointControls();
// delete the normal waypoints from the map
m_waypoint_list_mutex.lock();
foreach (t_waypoint *wp, m_waypoint_list)
{
if (!wp) continue;
if (!wp->map_wp_item) continue;
wp->coord = wp->map_wp_item->Coord();
wp->altitude = wp->map_wp_item->Altitude();
wp->description = wp->map_wp_item->Description();
wp->locked = (wp->map_wp_item->flags() & QGraphicsItem::ItemIsMovable) == 0;
wp->map_wp_item = NULL;
}
m_map->WPDeleteAll();
m_waypoint_list_mutex.unlock();
// delete the normal waypoints from the map
m_waypoint_list_mutex.lock();
foreach (t_waypoint *wp, m_waypoint_list)
{
if (!wp) continue;
if (!wp->map_wp_item) continue;
wp->coord = wp->map_wp_item->Coord();
wp->altitude = wp->map_wp_item->Altitude();
wp->description = wp->map_wp_item->Description();
wp->locked = (wp->map_wp_item->flags() & QGraphicsItem::ItemIsMovable) == 0;
wp->map_wp_item = NULL;
}
m_map->WPDeleteAll();
m_waypoint_list_mutex.unlock();
// restore the magic waypoint on the map
m_magic_waypoint.map_wp_item = m_map->WPCreate(m_magic_waypoint.coord, m_magic_waypoint.altitude, m_magic_waypoint.description);
m_magic_waypoint.map_wp_item->setZValue(10 + m_magic_waypoint.map_wp_item->Number());
m_magic_waypoint.map_wp_item->SetShowNumber(false);
m_magic_waypoint.map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png"));
// restore the magic waypoint on the map
m_magic_waypoint.map_wp_item = m_map->WPCreate(m_magic_waypoint.coord, m_magic_waypoint.altitude, m_magic_waypoint.description);
m_magic_waypoint.map_wp_item->setZValue(10 + m_magic_waypoint.map_wp_item->Number());
m_magic_waypoint.map_wp_item->SetShowNumber(false);
m_magic_waypoint.map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png"));
break;
break;
}
}
@ -1394,10 +1372,10 @@ void OPMapGadgetWidget::setMapMode(opMapModeType mode)
void OPMapGadgetWidget::createActions()
{
int list_size;
int list_size;
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
// ***********************
// create menu actions
@ -1413,6 +1391,10 @@ void OPMapGadgetWidget::createActions()
reloadAct->setStatusTip(tr("Reload the map tiles"));
connect(reloadAct, SIGNAL(triggered()), this, SLOT(onReloadAct_triggered()));
ripAct = new QAction(tr("&Rip map"), this);
ripAct->setStatusTip(tr("Rip the map tiles"));
connect(ripAct, SIGNAL(triggered()), this, SLOT(onRipAct_triggered()));
copyMouseLatLonToClipAct = new QAction(tr("Mouse latitude and longitude"), this);
copyMouseLatLonToClipAct->setStatusTip(tr("Copy the mouse latitude and longitude to the clipboard"));
connect(copyMouseLatLonToClipAct, SIGNAL(triggered()), this, SLOT(onCopyMouseLatLonToClipAct_triggered()));
@ -1472,9 +1454,9 @@ void OPMapGadgetWidget::createActions()
setHomeAct = new QAction(tr("Set the home location"), this);
setHomeAct->setStatusTip(tr("Set the home location to where you clicked"));
#if !defined(allow_manual_home_location_move)
setHomeAct->setEnabled(false);
#endif
#if !defined(allow_manual_home_location_move)
setHomeAct->setEnabled(false);
#endif
connect(setHomeAct, SIGNAL(triggered()), this, SLOT(onSetHomeAct_triggered()));
goHomeAct = new QAction(tr("Go to &Home location"), this);
@ -1501,16 +1483,12 @@ void OPMapGadgetWidget::createActions()
followUAVheadingAct->setChecked(false);
connect(followUAVheadingAct, SIGNAL(toggled(bool)), this, SLOT(onFollowUAVheadingAct_toggled(bool)));
/*
TODO: Waypoint support is disabled for v1.0
*/
/*
/* Waypoint stuff */
wayPointEditorAct = new QAction(tr("&Waypoint editor"), this);
wayPointEditorAct->setShortcut(tr("Ctrl+W"));
wayPointEditorAct->setStatusTip(tr("Open the waypoint editor"));
wayPointEditorAct->setEnabled(false); // temporary
connect(wayPointEditorAct, SIGNAL(triggered()), this, SLOT(onOpenWayPointEditorAct_triggered()));
//connect(wayPointEditorAct, SIGNAL(triggered()), this, SLOT(onOpenWayPointEditorAct_triggered()));
addWayPointAct = new QAction(tr("&Add waypoint"), this);
addWayPointAct->setShortcut(tr("Ctrl+A"));
@ -1537,7 +1515,6 @@ void OPMapGadgetWidget::createActions()
clearWayPointsAct->setShortcut(tr("Ctrl+C"));
clearWayPointsAct->setStatusTip(tr("Clear waypoints"));
connect(clearWayPointsAct, SIGNAL(triggered()), this, SLOT(onClearWayPointsAct_triggered()));
*/
homeMagicWaypointAct = new QAction(tr("Home magic waypoint"), this);
homeMagicWaypointAct->setStatusTip(tr("Move the magic waypoint to the home position"));
@ -1565,7 +1542,7 @@ void OPMapGadgetWidget::createActions()
zoomActGroup = new QActionGroup(this);
connect(zoomActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onZoomActGroup_triggered(QAction *)));
zoomAct.clear();
for (int i = m_min_zoom; i <= m_max_zoom; i++)
for (int i = m_min_zoom; i <= m_max_zoom; i++)
{
QAction *zoom_act = new QAction(QString::number(i), zoomActGroup);
zoom_act->setCheckable(true);
@ -1573,22 +1550,22 @@ void OPMapGadgetWidget::createActions()
zoomAct.append(zoom_act);
}
maxUpdateRateActGroup = new QActionGroup(this);
connect(maxUpdateRateActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onMaxUpdateRateActGroup_triggered(QAction *)));
maxUpdateRateAct.clear();
list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]);
for (int i = 0; i < list_size; i++)
{
QAction *maxUpdateRate_act;
int j = max_update_rate_list[i];
maxUpdateRate_act = new QAction(QString::number(j), maxUpdateRateActGroup);
maxUpdateRate_act->setCheckable(true);
maxUpdateRate_act->setData(j);
maxUpdateRate_act->setChecked(j == m_maxUpdateRate);
maxUpdateRateAct.append(maxUpdateRate_act);
}
maxUpdateRateActGroup = new QActionGroup(this);
connect(maxUpdateRateActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onMaxUpdateRateActGroup_triggered(QAction *)));
maxUpdateRateAct.clear();
list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]);
for (int i = 0; i < list_size; i++)
{
QAction *maxUpdateRate_act;
int j = max_update_rate_list[i];
maxUpdateRate_act = new QAction(QString::number(j), maxUpdateRateActGroup);
maxUpdateRate_act->setCheckable(true);
maxUpdateRate_act->setData(j);
maxUpdateRate_act->setChecked(j == m_maxUpdateRate);
maxUpdateRateAct.append(maxUpdateRate_act);
}
// *****
// *****
// safe area
showSafeAreaAct = new QAction(tr("Show Safe Area"), this);
@ -1600,8 +1577,8 @@ void OPMapGadgetWidget::createActions()
safeAreaActGroup = new QActionGroup(this);
connect(safeAreaActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onSafeAreaActGroup_triggered(QAction *)));
safeAreaAct.clear();
list_size = sizeof(safe_area_radius_list) / sizeof(safe_area_radius_list[0]);
for (int i = 0; i < list_size; i++)
list_size = sizeof(safe_area_radius_list) / sizeof(safe_area_radius_list[0]);
for (int i = 0; i < list_size; i++)
{
int safeArea = safe_area_radius_list[i];
QAction *safeArea_act = new QAction(QString::number(safeArea) + "m", safeAreaActGroup);
@ -1647,8 +1624,8 @@ void OPMapGadgetWidget::createActions()
uavTrailTimeActGroup = new QActionGroup(this);
connect(uavTrailTimeActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onUAVTrailTimeActGroup_triggered(QAction *)));
uavTrailTimeAct.clear();
list_size = sizeof(uav_trail_time_list) / sizeof(uav_trail_time_list[0]);
for (int i = 0; i < list_size; i++)
list_size = sizeof(uav_trail_time_list) / sizeof(uav_trail_time_list[0]);
for (int i = 0; i < list_size; i++)
{
int uav_trail_time = uav_trail_time_list[i];
QAction *uavTrailTime_act = new QAction(QString::number(uav_trail_time) + " sec", uavTrailTimeActGroup);
@ -1661,8 +1638,8 @@ void OPMapGadgetWidget::createActions()
uavTrailDistanceActGroup = new QActionGroup(this);
connect(uavTrailDistanceActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onUAVTrailDistanceActGroup_triggered(QAction *)));
uavTrailDistanceAct.clear();
list_size = sizeof(uav_trail_distance_list) / sizeof(uav_trail_distance_list[0]);
for (int i = 0; i < list_size; i++)
list_size = sizeof(uav_trail_distance_list) / sizeof(uav_trail_distance_list[0]);
for (int i = 0; i < list_size; i++)
{
int uav_trail_distance = uav_trail_distance_list[i];
QAction *uavTrailDistance_act = new QAction(QString::number(uav_trail_distance) + " meters", uavTrailDistanceActGroup);
@ -1679,59 +1656,64 @@ void OPMapGadgetWidget::createActions()
void OPMapGadgetWidget::onReloadAct_triggered()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->ReloadMap();
}
void OPMapGadgetWidget::onRipAct_triggered()
{
m_map->RipMap();
}
void OPMapGadgetWidget::onCopyMouseLatLonToClipAct_triggered()
{
QClipboard *clipboard = QApplication::clipboard();
clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + ", " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard);
clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + ", " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard);
}
void OPMapGadgetWidget::onCopyMouseLatToClipAct_triggered()
{
QClipboard *clipboard = QApplication::clipboard();
clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7), QClipboard::Clipboard);
clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7), QClipboard::Clipboard);
}
void OPMapGadgetWidget::onCopyMouseLonToClipAct_triggered()
{
QClipboard *clipboard = QApplication::clipboard();
clipboard->setText(QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard);
clipboard->setText(QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard);
}
void OPMapGadgetWidget::onShowCompassAct_toggled(bool show)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->SetShowCompass(show);
}
void OPMapGadgetWidget::onShowDiagnostics_toggled(bool show)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->SetShowDiagnostics(show);
}
void OPMapGadgetWidget::onShowHomeAct_toggled(bool show)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->Home->setVisible(show);
}
void OPMapGadgetWidget::onShowUAVAct_toggled(bool show)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->UAV->setVisible(show);
m_map->GPS->setVisible(show);
@ -1739,8 +1721,8 @@ void OPMapGadgetWidget::onShowUAVAct_toggled(bool show)
void OPMapGadgetWidget::onShowTrailAct_toggled(bool show)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->UAV->SetShowTrail(show);
m_map->GPS->SetShowTrail(show);
@ -1748,8 +1730,8 @@ void OPMapGadgetWidget::onShowTrailAct_toggled(bool show)
void OPMapGadgetWidget::onShowTrailLineAct_toggled(bool show)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->UAV->SetShowTrailLine(show);
m_map->GPS->SetShowTrailLine(show);
@ -1777,55 +1759,55 @@ void OPMapGadgetWidget::onGoZoomOutAct_triggered()
void OPMapGadgetWidget::onZoomActGroup_triggered(QAction *action)
{
if (!m_widget || !m_map || !action)
return;
if (!m_widget || !m_map || !action)
return;
setZoom(action->data().toInt());
}
void OPMapGadgetWidget::onMaxUpdateRateActGroup_triggered(QAction *action)
{
if (!m_widget || !m_map || !action)
return;
if (!m_widget || !m_map || !action)
return;
setMaxUpdateRate(action->data().toInt());
setMaxUpdateRate(action->data().toInt());
}
void OPMapGadgetWidget::onGoMouseClickAct_triggered()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->SetCurrentPosition(m_map->currentMousePosition()); // center the map onto the mouse position
}
void OPMapGadgetWidget::onSetHomeAct_triggered()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
setHome(m_context_menu_lat_lon);
setHome(m_context_menu_lat_lon);
setHomeLocationObject(); // update the HomeLocation UAVObject
}
void OPMapGadgetWidget::onGoHomeAct_triggered()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
goHome();
}
void OPMapGadgetWidget::onGoUAVAct_triggered()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
double latitude;
double longitude;
double altitude;
if (getUAVPosition(latitude, longitude, altitude)) // get current UAV position
if (getUAVPosition(latitude, longitude, altitude)) // get current UAV position
{
internals::PointLatLng uav_pos = internals::PointLatLng(latitude, longitude); // current UAV position
internals::PointLatLng map_pos = m_map->CurrentPosition(); // current MAP position
@ -1835,8 +1817,8 @@ void OPMapGadgetWidget::onGoUAVAct_triggered()
void OPMapGadgetWidget::onFollowUAVpositionAct_toggled(bool checked)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
if (m_widget->toolButtonMapUAV->isChecked() != checked)
m_widget->toolButtonMapUAV->setChecked(checked);
@ -1846,8 +1828,8 @@ void OPMapGadgetWidget::onFollowUAVpositionAct_toggled(bool checked)
void OPMapGadgetWidget::onFollowUAVheadingAct_toggled(bool checked)
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
if (m_widget->toolButtonMapUAVheading->isChecked() != checked)
m_widget->toolButtonMapUAVheading->setChecked(checked);
@ -1857,8 +1839,8 @@ void OPMapGadgetWidget::onFollowUAVheadingAct_toggled(bool checked)
void OPMapGadgetWidget::onUAVTrailTypeActGroup_triggered(QAction *action)
{
if (!m_widget || !m_map || !action)
return;
if (!m_widget || !m_map || !action)
return;
int trail_type_idx = action->data().toInt();
@ -1870,8 +1852,8 @@ void OPMapGadgetWidget::onUAVTrailTypeActGroup_triggered(QAction *action)
void OPMapGadgetWidget::onClearUAVtrailAct_triggered()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
m_map->UAV->DeleteTrail();
m_map->GPS->DeleteTrail();
@ -1879,8 +1861,8 @@ void OPMapGadgetWidget::onClearUAVtrailAct_triggered()
void OPMapGadgetWidget::onUAVTrailTimeActGroup_triggered(QAction *action)
{
if (!m_widget || !m_map || !action)
return;
if (!m_widget || !m_map || !action)
return;
int trail_time = (double)action->data().toInt();
@ -1889,8 +1871,8 @@ void OPMapGadgetWidget::onUAVTrailTimeActGroup_triggered(QAction *action)
void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action)
{
if (!m_widget || !m_map || !action)
return;
if (!m_widget || !m_map || !action)
return;
int trail_distance = action->data().toInt();
@ -1898,49 +1880,25 @@ void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action)
}
/**
* TODO: unused for v1.0
**/
/*
* Called when the add waypoint menu item is selected
* the coordinate clicked is in m_context_menu_lat_lon
*/
void OPMapGadgetWidget::onAddWayPointAct_triggered()
{
if (!m_widget || !m_map)
return;
Q_ASSERT(m_widget);
Q_ASSERT(m_map);
if (!m_widget || !m_map)
return;
if (m_map_mode != Normal_MapMode)
return;
m_waypoint_list_mutex.lock();
// create a waypoint on the map at the last known mouse position
t_waypoint *wp = new t_waypoint;
wp->map_wp_item = NULL;
wp->coord = context_menu_lat_lon;
wp->altitude = 0;
wp->description = "";
wp->locked = false;
wp->time_seconds = 0;
wp->hold_time_seconds = 0;
wp->map_wp_item = m_map->WPCreate(wp->coord, wp->altitude, wp->description);
wp->map_wp_item->setZValue(10 + wp->map_wp_item->Number());
wp->map_wp_item->setFlag(QGraphicsItem::ItemIsMovable, !wp->locked);
if (wp->map_wp_item)
{
if (!wp->locked)
wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png"));
else
wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png"));
wp->map_wp_item->update();
}
// and remember it in our own local waypoint list
m_waypoint_list.append(wp);
m_waypoint_list_mutex.unlock();
struct PathCompiler::waypoint newWaypoint;
newWaypoint.latitude = m_context_menu_lat_lon.Lat();
newWaypoint.longitude = m_context_menu_lat_lon.Lng();
pathCompiler->doAddWaypoint(newWaypoint);
}
*/
/**
* Called when the user asks to edit a waypoint from the map
@ -1948,11 +1906,10 @@ void OPMapGadgetWidget::onAddWayPointAct_triggered()
* TODO: should open an interface to edit waypoint properties, or
* propagate the signal to a specific WP plugin (tbd).
**/
/*
void OPMapGadgetWidget::onEditWayPointAct_triggered()
{
if (!m_widget || !m_map)
return;
if (!m_widget || !m_map)
return;
if (m_map_mode != Normal_MapMode)
return;
@ -1964,12 +1921,7 @@ void OPMapGadgetWidget::onEditWayPointAct_triggered()
m_mouse_waypoint = NULL;
}
*/
/**
* TODO: unused for v1.0
*/
/*
void OPMapGadgetWidget::onLockWayPointAct_triggered()
{
if (!m_widget || !m_map || !m_mouse_waypoint)
@ -1989,94 +1941,56 @@ void OPMapGadgetWidget::onLockWayPointAct_triggered()
m_mouse_waypoint = NULL;
}
*/
/**
* TODO: unused for v1.0
* Called when the delete waypoint menu item is selected
* the waypoint clicked is in m_mouse_waypoint
*/
/*
void OPMapGadgetWidget::onDeleteWayPointAct_triggered()
{
Q_ASSERT(m_widget);
Q_ASSERT(m_map);
if (!m_widget || !m_map)
return;
if (m_map_mode != Normal_MapMode)
return;
if (!m_mouse_waypoint)
return;
Q_ASSERT(m_mouse_waypoint);
if(m_mouse_waypoint) {
int waypointIdx = m_mouse_waypoint->Number();
bool locked = (m_mouse_waypoint->flags() & QGraphicsItem::ItemIsMovable) == 0;
if (locked) return; // waypoint is locked
QMutexLocker locker(&m_waypoint_list_mutex);
for (int i = 0; i < m_waypoint_list.count(); i++)
{
t_waypoint *wp = m_waypoint_list.at(i);
if (!wp) continue;
if (!wp->map_wp_item || wp->map_wp_item != m_mouse_waypoint) continue;
// delete the waypoint from the map
m_map->WPDelete(wp->map_wp_item);
// delete the waypoint from our local waypoint list
m_waypoint_list.removeAt(i);
delete wp;
break;
if(waypointIdx < 0) {
qDebug() << "WTF Map gadget. Wrong number";
return;
}
Q_ASSERT(pathCompiler);
if(pathCompiler)
pathCompiler->doDelWaypoint(waypointIdx);
}
//
// foreach (t_waypoint *wp, m_waypoint_list)
// {
// if (!wp) continue;
// if (!wp->map_wp_item || wp->map_wp_item != m_mouse_waypoint) continue;
//
// // delete the waypoint from the map
// m_map->WPDelete(wp->map_wp_item);
//
// // delete the waypoint from our local waypoint list
// m_waypoint_list.removeOne(wp);
//
// delete wp;
//
// break;
// }
m_mouse_waypoint = NULL;
}
*/
/**
* TODO: No Waypoint support in v1.0
*/
/*
void OPMapGadgetWidget::onClearWayPointsAct_triggered()
{
Q_ASSERT(m_widget);
Q_ASSERT(m_map);
if (!m_widget || !m_map)
return;
if (m_map_mode != Normal_MapMode)
return;
QMutexLocker locker(&m_waypoint_list_mutex);
m_map->WPDeleteAll();
foreach (t_waypoint *wp, m_waypoint_list)
{
if (wp)
{
delete wp;
wp = NULL;
}
}
m_waypoint_list.clear();
Q_ASSERT(pathCompiler);
if(pathCompiler)
pathCompiler->doDelAllWaypoints();
}
*/
void OPMapGadgetWidget::onHomeMagicWaypointAct_triggered()
{
@ -2095,7 +2009,7 @@ void OPMapGadgetWidget::onShowSafeAreaAct_toggled(bool show)
void OPMapGadgetWidget::onSafeAreaActGroup_triggered(QAction *action)
{
if (!m_widget || !m_map || !action)
if (!m_widget || !m_map || !action)
return;
int radius = action->data().toInt();
@ -2118,10 +2032,10 @@ void OPMapGadgetWidget::homeMagicWaypoint()
if (m_map_mode != MagicWaypoint_MapMode)
return;
m_magic_waypoint.coord = m_home_position.coord;
m_magic_waypoint.coord = m_home_position.coord;
if (m_magic_waypoint.map_wp_item)
m_magic_waypoint.map_wp_item->SetCoord(m_magic_waypoint.coord);
if (m_magic_waypoint.map_wp_item)
m_magic_waypoint.map_wp_item->SetCoord(m_magic_waypoint.coord);
}
// *************************************************************************************
@ -2135,8 +2049,8 @@ void OPMapGadgetWidget::moveToMagicWaypointPosition()
if (m_map_mode != MagicWaypoint_MapMode)
return;
// internals::PointLatLng coord = magic_waypoint.coord;
// double altitude = magic_waypoint.altitude;
// internals::PointLatLng coord = magic_waypoint.coord;
// double altitude = magic_waypoint.altitude;
// ToDo:
@ -2205,11 +2119,11 @@ void OPMapGadgetWidget::showMagicWaypointControls()
m_widget->lineWaypoint->setVisible(true);
m_widget->toolButtonHomeWaypoint->setVisible(true);
#if defined(allow_manual_home_location_move)
m_widget->toolButtonMoveToWP->setVisible(true);
#else
m_widget->toolButtonMoveToWP->setVisible(false);
#endif
#if defined(allow_manual_home_location_move)
m_widget->toolButtonMoveToWP->setVisible(true);
#else
m_widget->toolButtonMoveToWP->setVisible(false);
#endif
}
// *************************************************************************************
@ -2219,25 +2133,25 @@ void OPMapGadgetWidget::keepMagicWaypointWithInSafeArea()
{
// calcute the bearing and distance from the home position to the magic waypoint
double dist = distance(m_home_position.coord, m_magic_waypoint.coord);
double bear = bearing(m_home_position.coord, m_magic_waypoint.coord);
double dist = distance(m_home_position.coord, m_magic_waypoint.coord);
double bear = bearing(m_home_position.coord, m_magic_waypoint.coord);
// get the maximum safe distance - in kilometers
double boundry_dist = (double)m_map->Home->SafeArea() / 1000;
// if (dist <= boundry_dist)
// return; // the magic waypoint is still within the safe area, don't move it
// if (dist <= boundry_dist)
// return; // the magic waypoint is still within the safe area, don't move it
if (dist > boundry_dist) dist = boundry_dist;
// move the magic waypoint
m_magic_waypoint.coord = destPoint(m_home_position.coord, bear, dist);
m_magic_waypoint.coord = destPoint(m_home_position.coord, bear, dist);
if (m_map_mode == MagicWaypoint_MapMode)
{ // move the on-screen waypoint
if (m_magic_waypoint.map_wp_item)
m_magic_waypoint.map_wp_item->SetCoord(m_magic_waypoint.coord);
if (m_magic_waypoint.map_wp_item)
m_magic_waypoint.map_wp_item->SetCoord(m_magic_waypoint.coord);
}
}
@ -2254,7 +2168,7 @@ double OPMapGadgetWidget::distance(internals::PointLatLng from, internals::Point
// ***********************
// Haversine formula
/*
/*
double delta_lat = lat2 - lat1;
double delta_lon = lon2 - lon1;
@ -2284,7 +2198,7 @@ double OPMapGadgetWidget::bearing(internals::PointLatLng from, internals::PointL
double lat2 = to.Lat() * deg_to_rad;
double lon2 = to.Lng() * deg_to_rad;
// double delta_lat = lat2 - lat1;
// double delta_lat = lat2 - lat1;
double delta_lon = lon2 - lon1;
double y = sin(delta_lon) * cos(lat2);
@ -2364,33 +2278,33 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub
double OPMapGadgetWidget::getUAV_Yaw()
{
if (!obm)
return 0;
if (!obm)
return 0;
UAVObject *obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("AttitudeActual")));
double yaw = obj->getField(QString("Yaw"))->getDouble();
UAVObject *obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("AttitudeActual")));
double yaw = obj->getField(QString("Yaw"))->getDouble();
if (yaw != yaw) yaw = 0; // nan detection
if (yaw != yaw) yaw = 0; // nan detection
while (yaw < 0) yaw += 360;
while (yaw >= 360) yaw -= 360;
while (yaw < 0) yaw += 360;
while (yaw >= 360) yaw -= 360;
return yaw;
return yaw;
}
bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, double &altitude)
{
double LLA[3];
double LLA[3];
if (!obum)
return false;
if (!obum)
return false;
if (obum->getGPSPosition(LLA) < 0)
return false; // error
if (obum->getGPSPosition(LLA) < 0)
return false; // error
latitude = LLA[0];
longitude = LLA[1];
altitude = LLA[2];
latitude = LLA[0];
longitude = LLA[1];
altitude = LLA[2];
return true;
}
@ -2408,18 +2322,18 @@ void OPMapGadgetWidget::setMapFollowingMode()
m_map->SetRotate(0); // reset map rotation to 0deg
}
else
if (!followUAVheadingAct->isChecked())
{
m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap);
m_map->SetRotate(0); // reset map rotation to 0deg
}
else
{
m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap); // the map library won't let you reset the uav rotation if it's already in rotate mode
if (!followUAVheadingAct->isChecked())
{
m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap);
m_map->SetRotate(0); // reset map rotation to 0deg
}
else
{
m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap); // the map library won't let you reset the uav rotation if it's already in rotate mode
m_map->UAV->SetUAVHeading(0); // reset the UAV heading to 0deg
m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterAndRotateMap);
}
m_map->UAV->SetUAVHeading(0); // reset the UAV heading to 0deg
m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterAndRotateMap);
}
}
// *************************************************************************************
@ -2427,11 +2341,11 @@ void OPMapGadgetWidget::setMapFollowingMode()
bool OPMapGadgetWidget::setHomeLocationObject()
{
if (!obum)
return false;
if (!obum)
return false;
double LLA[3] = {m_home_position.coord.Lat(), m_home_position.coord.Lng(), m_home_position.altitude};
return (obum->setHomeLocation(LLA, true) >= 0);
double LLA[3] = {m_home_position.coord.Lat(), m_home_position.coord.Lng(), m_home_position.altitude};
return (obum->setHomeLocation(LLA, true) >= 0);
}
// *************************************************************************************
@ -2440,3 +2354,23 @@ void OPMapGadgetWidget::SetUavPic(QString UAVPic)
{
m_map->SetUavPic(UAVPic);
}
/**
* Called from path compiler whenever the path to visualize has changed
*/
void OPMapGadgetWidget::doVisualizationChanged(QList<PathCompiler::waypoint> waypoints)
{
m_map->WPDeleteAll();
int index = 0;
foreach (PathCompiler::waypoint waypoint, waypoints) {
internals::PointLatLng position(waypoint.latitude, waypoint.longitude);
WayPointItem * wayPointItem = m_map->WPCreate(position, 0, QString(index));
Q_ASSERT(wayPointItem);
if(wayPointItem) {
wayPointItem->setFlag(QGraphicsItem::ItemIsMovable, true);
wayPointItem->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png"));
index++;
}
}
}

92
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h
View File

@ -7,7 +7,7 @@
* @{
* @addtogroup OPMapPlugin OpenPilot Map Plugin
* @{
* @brief The OpenPilot Map plugin
* @brief The OpenPilot Map plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -53,11 +53,12 @@
#include "uavobject.h"
#include "objectpersistence.h"
#include <pathcompiler.h>
// ******************************************************
namespace Ui
{
class OPMap_Widget;
class OPMap_Widget;
}
using namespace mapcontrol;
@ -96,7 +97,7 @@ class OPMapGadgetWidget : public QWidget
public:
OPMapGadgetWidget(QWidget *parent = 0);
~OPMapGadgetWidget();
~OPMapGadgetWidget();
/**
* @brief public functions
@ -115,8 +116,8 @@ public:
void setUseMemoryCache(bool useMemoryCache);
void setCacheLocation(QString cacheLocation);
void setMapMode(opMapModeType mode);
void SetUavPic(QString UAVPic);
void setMaxUpdateRate(int update_rate);
void SetUavPic(QString UAVPic);
void setMaxUpdateRate(int update_rate);
public slots:
void homePositionUpdated(UAVObject *);
@ -142,21 +143,21 @@ private slots:
*
* Some are currently disabled for the v1.0 plugin version.
*/
// void comboBoxFindPlace_returnPressed();
// void on_toolButtonFindPlace_clicked();
// void comboBoxFindPlace_returnPressed();
// void on_toolButtonFindPlace_clicked();
void on_toolButtonZoomM_clicked();
void on_toolButtonZoomP_clicked();
void on_toolButtonMapHome_clicked();
void on_toolButtonMapUAV_clicked();
void on_toolButtonMapUAVheading_clicked();
void on_horizontalSliderZoom_sliderMoved(int position);
// void on_toolButtonAddWaypoint_clicked();
// void on_treeViewWaypoints_clicked(QModelIndex index);
// void on_toolButtonHome_clicked();
// void on_toolButtonNextWaypoint_clicked();
// void on_toolButtonPrevWaypoint_clicked();
// void on_toolButtonHoldPosition_clicked();
// void on_toolButtonGo_clicked();
// void on_toolButtonAddWaypoint_clicked();
// void on_treeViewWaypoints_clicked(QModelIndex index);
// void on_toolButtonHome_clicked();
// void on_toolButtonNextWaypoint_clicked();
// void on_toolButtonPrevWaypoint_clicked();
// void on_toolButtonHoldPosition_clicked();
// void on_toolButtonGo_clicked();
void on_toolButtonMagicWaypointMapMode_clicked();
void on_toolButtonNormalMapMode_clicked();
void on_toolButtonHomeWaypoint_clicked();
@ -182,15 +183,17 @@ private slots:
void WPValuesChanged(WayPointItem* waypoint);
void WPInserted(int const& number, WayPointItem* waypoint);
void WPDeleted(int const& number);
void doVisualizationChanged(QList<PathCompiler::waypoint>);
/**
* @brief mouse right click context menu signals
*/
void onReloadAct_triggered();
void onRipAct_triggered();
void onCopyMouseLatLonToClipAct_triggered();
void onCopyMouseLatToClipAct_triggered();
void onCopyMouseLonToClipAct_triggered();
// void onFindPlaceAct_triggered();
// void onFindPlaceAct_triggered();
void onShowCompassAct_toggled(bool show);
void onShowDiagnostics_toggled(bool show);
void onShowUAVAct_toggled(bool show);
@ -205,14 +208,12 @@ private slots:
void onGoUAVAct_triggered();
void onFollowUAVpositionAct_toggled(bool checked);
void onFollowUAVheadingAct_toggled(bool checked);
/*
void onOpenWayPointEditorAct_triggered();
//void onOpenWayPointEditorAct_triggered();
void onAddWayPointAct_triggered();
void onEditWayPointAct_triggered();
void onLockWayPointAct_triggered();
void onDeleteWayPointAct_triggered();
void onClearWayPointsAct_triggered();
*/
void onMapModeActGroup_triggered(QAction *action);
void onZoomActGroup_triggered(QAction *action);
void onHomeMagicWaypointAct_triggered();
@ -222,31 +223,31 @@ private slots:
void onClearUAVtrailAct_triggered();
void onUAVTrailTimeActGroup_triggered(QAction *action);
void onUAVTrailDistanceActGroup_triggered(QAction *action);
void onMaxUpdateRateActGroup_triggered(QAction *action);
void onMaxUpdateRateActGroup_triggered(QAction *action);
private:
// *****
// *****
int m_min_zoom;
int m_max_zoom;
int m_min_zoom;
int m_max_zoom;
double m_heading; // uav heading
internals::PointLatLng m_mouse_lat_lon;
internals::PointLatLng m_context_menu_lat_lon;
internals::PointLatLng m_mouse_lat_lon;
internals::PointLatLng m_context_menu_lat_lon;
int m_prev_tile_number;
int m_prev_tile_number;
opMapModeType m_map_mode;
int m_maxUpdateRate;
int m_maxUpdateRate;
t_home m_home_position;
t_home m_home_position;
t_waypoint m_magic_waypoint;
t_waypoint m_magic_waypoint;
QStringList findPlaceWordList;
QStringList findPlaceWordList;
QCompleter *findPlaceCompleter;
QTimer *m_updateTimer;
@ -256,11 +257,11 @@ private:
mapcontrol::OPMapWidget *m_map;
ExtensionSystem::PluginManager *pm;
UAVObjectManager *obm;
UAVObjectUtilManager *obum;
ExtensionSystem::PluginManager *pm;
UAVObjectManager *obm;
UAVObjectUtilManager *obum;
//opmap_waypointeditor_dialog waypoint_editor_dialog;
//opmap_waypointeditor_dialog waypoint_editor_dialog;
//opmap_edit_waypoint_dialog waypoint_edit_dialog;
@ -268,21 +269,24 @@ private:
mapcontrol::WayPointItem *m_mouse_waypoint;
PathCompiler *pathCompiler;
QList<t_waypoint *> m_waypoint_list;
QMutex m_waypoint_list_mutex;
QMutex m_map_mutex;
bool m_telemetry_connected;
bool m_telemetry_connected;
// *****
// *****
void createActions();
QAction *closeAct1;
QAction *closeAct2;
QAction *reloadAct;
QAction *copyMouseLatLonToClipAct;
QAction *ripAct;
QAction *copyMouseLatLonToClipAct;
QAction *copyMouseLatToClipAct;
QAction *copyMouseLonToClipAct;
QAction *findPlaceAct;
@ -298,14 +302,12 @@ private:
QAction *goUAVAct;
QAction *followUAVpositionAct;
QAction *followUAVheadingAct;
/*
QAction *wayPointEditorAct;
QAction *addWayPointAct;
QAction *editWayPointAct;
QAction *lockWayPointAct;
QAction *deleteWayPointAct;
QAction *clearWayPointsAct;
*/
QAction *homeMagicWaypointAct;
QAction *showSafeAreaAct;
@ -328,12 +330,12 @@ private:
QActionGroup *zoomActGroup;
QList<QAction *> zoomAct;
QActionGroup *maxUpdateRateActGroup;
QList<QAction *> maxUpdateRateAct;
QActionGroup *maxUpdateRateActGroup;
QList<QAction *> maxUpdateRateAct;
// *****
// *****
void homeMagicWaypoint();
void homeMagicWaypoint();
void moveToMagicWaypointPosition();
@ -349,13 +351,13 @@ private:
double bearing(internals::PointLatLng from, internals::PointLatLng to);
internals::PointLatLng destPoint(internals::PointLatLng source, double bear, double dist);
bool getUAVPosition(double &latitude, double &longitude, double &altitude);
bool getGPSPosition(double &latitude, double &longitude, double &altitude);
bool getUAVPosition(double &latitude, double &longitude, double &altitude);
bool getGPSPosition(double &latitude, double &longitude, double &altitude);
double getUAV_Yaw();
void setMapFollowingMode();
bool setHomeLocationObject();
bool setHomeLocationObject();
};
#endif /* OPMAP_GADGETWIDGET_H_ */

332
ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp Normal file
View File

@ -0,0 +1,332 @@
/**
******************************************************************************
*
* @file pathcompiler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup OPMapPlugin OpenPilot Map Plugin
* @{
* @brief The OpenPilot Map plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pathcompiler.h>
#include <extensionsystem/pluginmanager.h>
#include <utils/coordinateconversions.h>
#include <uavobjectmanager.h>
#include <waypoint.h>
#include <homelocation.h>
#include <QDebug>
PathCompiler::PathCompiler(QObject *parent) :
QObject(parent)
{
Waypoint *waypoint = NULL;
HomeLocation *homeLocation = NULL;
/* Connect the object updates */
waypoint = Waypoint::GetInstance(getObjectManager());
Q_ASSERT(waypoint);
if(waypoint)
connect(waypoint, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doUpdateFromUAV()));
homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
if(homeLocation)
connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doUpdateFromUAV()));
}
/**
* Helper method to get the uavobjectamanger
*/
UAVObjectManager * PathCompiler::getObjectManager()
{
ExtensionSystem::PluginManager *pm = NULL;
UAVObjectManager *objMngr = NULL;
pm = ExtensionSystem::PluginManager::instance();
Q_ASSERT(pm);
if(pm)
objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
return objMngr;
}
/**
* This method opens a dialog (if filename is null) and saves the path
* @param filename The file to save the path to
* @returns -1 for failure, 0 for success
*/
int PathCompiler::savePath(QString filename)
{
Q_UNUSED(filename);
return -1;
}
/**
* This method opens a dialog (if filename is null) and loads the path
* @param filename The file to load from
* @returns -1 for failure, 0 for success
*/
int PathCompiler::loadPath(QString filename)
{
Q_UNUSED(filename);
return -1;
}
/**
* add a waypoint
* @param waypoint the new waypoint to add
* @param position which position to insert it to, defaults to end
*/
void PathCompiler::doAddWaypoint(struct PathCompiler::waypoint waypoint, int /*position*/)
{
/* TODO: If a waypoint is inserted not at the end shift them all by one and */
/* add the data there */
UAVObjectManager *objManager = getObjectManager();
// Format the data from the map into a UAVO
Waypoint::DataFields newWaypoint = InternalToUavo(waypoint);
// Search for any waypoints set to stop, because if they exist we
// should add the waypoint immediately after that one
int numWaypoints = objManager->getNumInstances(Waypoint::OBJID);
int i;
for (i = 0; i < numWaypoints; i++) {
Waypoint *waypoint = Waypoint::GetInstance(objManager, i);
Q_ASSERT(waypoint);
if(waypoint == NULL)
return;
Waypoint::DataFields waypointData = waypoint->getData();
if(waypointData.Action == Waypoint::ACTION_STOP) {
waypointData.Action = Waypoint::ACTION_PATHTONEXT;
waypoint->setData(waypointData);
break;
}
}
if (i >= numWaypoints - 1) {
// We reached end of list so new waypoint needs to be registered
Waypoint *waypoint = new Waypoint();
Q_ASSERT(waypoint);
if (waypoint) {
// Register a new waypoint instance
quint32 newInstId = objManager->getNumInstances(waypoint->getObjID());
waypoint->initialize(newInstId,waypoint->getMetaObject());
objManager->registerObject(waypoint);
// Set the data in the new object
waypoint->setData(newWaypoint);
waypoint->updated();
}
} else {
Waypoint *waypoint = Waypoint::GetInstance(objManager, i + 1);
Q_ASSERT(waypoint);
if (waypoint) {
waypoint->setData(newWaypoint);
waypoint->updated();
}
}
}
/**
* Delete a waypoint
* @param index which waypoint to delete
*/
void PathCompiler::doDelWaypoint(int index)
{
// This method is awkward because there is no support
// on the FC for actually deleting a waypoint. We need
// to shift them all by one and set the new "last" waypoint
// to a stop action
UAVObjectManager *objManager = getObjectManager();
Waypoint *waypoint = Waypoint::GetInstance(objManager);
Q_ASSERT(waypoint);
if (!waypoint)
return;
int numWaypoints = objManager->getNumInstances(waypoint->getObjID());
for (int i = index; i < numWaypoints - 1; i++) {
qDebug() << "Copying from" << i+1 << "to" << i;
Waypoint *waypointDest = Waypoint::GetInstance(objManager, i);
Q_ASSERT(waypointDest);
Waypoint *waypointSrc = Waypoint::GetInstance(objManager, i + 1);
Q_ASSERT(waypointSrc);
if (!waypointDest || !waypointSrc)
return;
// Copy the data down an index
Waypoint::DataFields waypoint = waypointSrc->getData();
waypointDest->setData(waypoint);
waypointDest->updated();
}
// Set the second to last waypoint to stop (and last for safety)
// the functional equivalent to deleting
for (int i = numWaypoints - 2; i < numWaypoints; i++) {
qDebug() << "Stopping" << i;
waypoint = Waypoint::GetInstance(objManager, i);
Q_ASSERT(waypoint);
if (waypoint) {
Waypoint::DataFields waypointData = waypoint->getData();
waypointData.Action = Waypoint::ACTION_STOP;
waypoint->setData(waypointData);
waypoint->updated();
}
}
}
/**
* Delete all the waypoints
*/
void PathCompiler::doDelAllWaypoints()
{
Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager(), 0);
Q_ASSERT(waypointObj);
if (waypointObj == NULL)
return;
int numWaypoints = getObjectManager()->getNumInstances(waypointObj->getObjID());
for (int i = 0; i < numWaypoints; i++) {
Waypoint *waypoint = Waypoint::GetInstance(getObjectManager(), i);
Q_ASSERT(waypoint);
if (waypoint) {
Waypoint::DataFields waypointData = waypoint->getData();
waypointData.Action = Waypoint::ACTION_STOP;
waypoint->setData(waypointData);
}
}
waypointObj->updated();
}
/**
* When the UAV waypoints change trigger the pathcompiler to
* get the latest version and then update the visualization
*/
void PathCompiler::doUpdateFromUAV()
{
UAVObjectManager *objManager = getObjectManager();
if (!objManager)
return;
Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager());
Q_ASSERT(waypointObj);
if (waypointObj == NULL)
return;
/* Get all the waypoints from the UAVO and create a representation for the visualization */
QList <struct PathCompiler::waypoint> waypoints;
waypoints.clear();
int numWaypoints = objManager->getNumInstances(waypointObj->getObjID());
bool stopped = false;
for (int i = 0; i < numWaypoints && !stopped; i++) {
Waypoint *waypoint = Waypoint::GetInstance(objManager, i);
Q_ASSERT(waypoint);
if(waypoint == NULL)
return;
Waypoint::DataFields waypointData = waypoint->getData();
waypoints.append(UavoToInternal(waypointData));
stopped = waypointData.Action == Waypoint::ACTION_STOP;
}
/* Inform visualization */
emit visualizationChanged(waypoints);
}
/**
* Convert a UAVO waypoint to the local structure
* @param uavo The UAVO data representation
* @return The waypoint structure for visualization
*/
struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields uavo)
{
double homeLLA[3];
double LLA[3];
double NED[3];
struct PathCompiler::waypoint internalWaypoint;
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
if (homeLocation == NULL)
return internalWaypoint;
HomeLocation::DataFields homeLocationData = homeLocation->getData();
homeLLA[0] = homeLocationData.Latitude / 10e6;
homeLLA[1] = homeLocationData.Longitude / 10e6;
homeLLA[2] = homeLocationData.Altitude;
NED[0] = uavo.Position[Waypoint::POSITION_NORTH];
NED[1] = uavo.Position[Waypoint::POSITION_EAST];
NED[2] = uavo.Position[Waypoint::POSITION_DOWN];
Utils::CoordinateConversions().GetLLA(homeLLA, NED, LLA);
internalWaypoint.latitude = LLA[0];
internalWaypoint.longitude = LLA[1];
return internalWaypoint;
}
/**
* Convert a UAVO waypoint to the local structure
* @param internal The internal structure type
* @returns The waypoint UAVO data structure
*/
Waypoint::DataFields PathCompiler::InternalToUavo(struct waypoint internal)
{
Waypoint::DataFields uavo;
double homeLLA[3];
double LLA[3];
double NED[3];
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
if (homeLocation == NULL)
return uavo;
HomeLocation::DataFields homeLocationData = homeLocation->getData();
homeLLA[0] = homeLocationData.Latitude / 10e6;
homeLLA[1] = homeLocationData.Longitude / 10e6;
homeLLA[2] = homeLocationData.Altitude;
// TODO: Give the point a concept of altitude
LLA[0] = internal.latitude;
LLA[1] = internal.longitude;
LLA[2] = -50;
Utils::CoordinateConversions().GetNED(homeLLA, LLA, NED);
uavo.Position[Waypoint::POSITION_NORTH] = NED[0];
uavo.Position[Waypoint::POSITION_EAST] = NED[1];
uavo.Position[Waypoint::POSITION_DOWN] = NED[2];
uavo.Action = Waypoint::ACTION_PATHTONEXT;
uavo.Velocity[Waypoint::VELOCITY_NORTH] = 5;
uavo.Velocity[Waypoint::VELOCITY_EAST] = 0;
uavo.Velocity[Waypoint::VELOCITY_DOWN] = 0;
return uavo;
}

134
ground/openpilotgcs/src/plugins/opmap/pathcompiler.h Normal file
View File

@ -0,0 +1,134 @@
/**
******************************************************************************
*
* @file pathcompiler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup OPMapPlugin OpenPilot Map Plugin
* @{
* @brief The OpenPilot Map plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PATHCOMPILER_H
#define PATHCOMPILER_H
#include <QObject>
#include <uavobject.h>
#include <uavobjectmanager.h>
#include <waypoint.h>
// TODO: Make this a singleton class and separate from map library. Not sure of the proper design pattern in Qt.
// factory? static variables?
/**
* This class is a two way adapter between a visualization of a path and the
* UAVObject representation on the flight controller. It also can support multiple
* ways of converting a path from what the user clicked to the underlying representation
* to achieve the desired end flight trajectory
*
* So the chain of data for the map lib is:
* FC <-> PathCompiler <-> OPMapGadget <-> OPMapLib
*
* The goal is that PathCompiler be as state free as is possible. Eventually for more
* complicated path compilation this will probably not be achievable. That means it
* should not cache a copy of waypoints locally if that can be avoided (i.e. it should
* refer directly to what is stored on the FC).
*
* For the visualization to have the ability to manipulate the path though it needs to
* be able to map unambiguously from the graphical items to the internal waypoints. It
* must cache a lookup from the graphical item to the index from this tool.
*/
class PathCompiler : public QObject
{
Q_OBJECT
public:
explicit PathCompiler(QObject *parent = 0);
//! This method opens a dialog (if filename is null) and saves the path
int savePath(QString filename = NULL);
//! This method opens a dialog (if filename is null) and loads the path
int loadPath(QString filename = NULL);
//! Waypoint representation that is exchanged between visualization
struct waypoint {
double latitude;
double longitude;
};
private:
//! Helper method to get uavobject manager
UAVObjectManager * getObjectManager();
//! Convert a UAVO waypoint to the local structure
struct PathCompiler::waypoint UavoToInternal(Waypoint::DataFields);
//! Convert a UAVO waypoint to the local structure
Waypoint::DataFields InternalToUavo(struct waypoint);
signals:
/**
* Indicates something changed the waypoints and the map should
* update the display
*/
void visualizationChanged(QList<struct PathCompiler::waypoint>);
public slots:
/**
* These are slots that the visualization can call to manipulate the path.
* It is an important design detail that the visualiation _not_ attempt to maintain
* the list of waypoints itself. This starts the slippery of moving the path logic
* into the view.
*/
/**
* add a waypoint
* @param waypoint the new waypoint to add
* @param position which position to insert it to, defaults to end
*/
void doAddWaypoint(struct PathCompiler::waypoint, int position = -1);
/**
* Update waypoint
*/
//void doUpdateWaypoints(struct PathCompiler::waypoint, int position);
/**
* Delete a waypoint
* @param index which waypoint to delete
*/
void doDelWaypoint(int index);
/**
* Delete all the waypoints
*/
void doDelAllWaypoints();
public slots:
/**
* These are slots that the UAV can call to update the path.
*/
/**
* When the UAV waypoints change trigger the pathcompiler to
* get the latest version and then update the visualization
*/
void doUpdateFromUAV();
};
#endif // PATHCOMPILER_H

2
shared/uavobjectdefinition/pathdesired.xml
View File

@ -7,7 +7,7 @@
<field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/>
<field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/>
<field name="Mode" units="" type="enum" elements="1" options="Endpoint,Path" default="0"/>
<field name="Mode" units="" type="enum" elements="1" options="Endpoint,Path,Land" default="0"/>
<!-- Endpoint Mode - Fly to end location and attempt to stay there -->
<!-- Path Mode - Attempt to fly a line from Start to End using specifed velocity -->

2
shared/uavobjectdefinition/waypoint.xml
View File

@ -4,7 +4,7 @@
<field name="Position" units="m" type="float" elementnames="North, East, Down"/>
<field name="Velocity" units="m/s" type="float" elementnames="North, East, Down"/>
<field name="YawDesired" units="deg" type="float" elements="1"/>
<field name="Action" units="" type="enum" elements="1" options="Next,RTH,Loiter10s,Land"/>
<field name="Action" units="" type="enum" elements="1" options="PathToNext,EndpointToNext,RTH,Loiter10s,Land,Stop"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="4000"/>