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

Add the ability to add waypoints via OPMap

Browse Source
This commit is contained in:
James Cotton 2012-06-02 14:29:22 -05:00
parent 4e1044589f
commit 64ba15cf07
2 changed files with 61 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2,7 +2,7 @@
******************************************************************************
*
* @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
@ -1898,42 +1898,19 @@ void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action)
void OPMapGadgetWidget::onAddWayPointAct_triggered()
{
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 = m_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);
}
/**

55
ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
View File

@ -93,9 +93,28 @@ int PathCompiler::loadPath(QString filename)
* @param waypoint the new waypoint to add
* @param position which position to insert it to, defaults to end
*/
void PathCompiler::doAddWaypoint(struct PathCompiler::waypoint, int /* position */)
void PathCompiler::doAddWaypoint(struct PathCompiler::waypoint waypoint, int /*position*/)
{
UAVObjectManager *objMngr;
objMngr = getObjectManager();
if (objMngr == NULL)
return;
/* TODO: If a waypoint is inserted not at the end shift them all by one and */
/* add the data there */
Waypoint *obj = new Waypoint();
Q_ASSERT(obj);
if (obj) {
// Register a new waypoint instance
quint32 newInstId = objMngr->getNumInstances(obj->getObjID());
obj->initialize(newInstId,obj->getMetaObject());
objMngr->registerObject(obj);
// Set the data in the new object
Waypoint::DataFields newWaypoint = InternalToUavo(waypoint);
obj->setData(newWaypoint);
}
}
/**
@ -181,9 +200,41 @@ struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields
* @param internal The internal structure type
* @returns The waypoint UAVO data structure
*/
Waypoint::DataFields PathCompiler::InternalToUavo(struct waypoint /*internal*/)
Waypoint::DataFields PathCompiler::InternalToUavo(struct waypoint internal)
{
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 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_NEXT;
uavo.Velocity[Waypoint::VELOCITY_NORTH] = 5;
uavo.Velocity[Waypoint::VELOCITY_EAST] = 0;
uavo.Velocity[Waypoint::VELOCITY_DOWN] = 0;
return uavo;
}