Commit 004183d5 authored by lm's avatar lm

Stable waypoint drag-and-drop, fixed a number of funky waypoint update methods.

parent e869bac6
......@@ -108,7 +108,7 @@ void Waypoint::setId(quint16 id)
void Waypoint::setX(double x)
{
if (this->x != x) {
if (this->x != x && (this->frame == MAV_FRAME_LOCAL)) {
this->x = x;
emit changed(this);
}
......@@ -116,7 +116,7 @@ void Waypoint::setX(double x)
void Waypoint::setY(double y)
{
if (this->y != y) {
if (this->y != y && (this->frame == MAV_FRAME_LOCAL)) {
this->y = y;
emit changed(this);
}
......@@ -124,7 +124,7 @@ void Waypoint::setY(double y)
void Waypoint::setZ(double z)
{
if (this->z != z) {
if (this->z != z && (this->frame == MAV_FRAME_LOCAL)) {
this->z = z;
emit changed(this);
}
......@@ -132,27 +132,24 @@ void Waypoint::setZ(double z)
void Waypoint::setLatitude(double lat)
{
if (this->x != lat) {
if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) {
this->x = lat;
this->frame = MAV_FRAME_GLOBAL;
emit changed(this);
}
}
void Waypoint::setLongitude(double lon)
{
if (this->y != lon) {
if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) {
this->y = lon;
this->frame = MAV_FRAME_GLOBAL;
emit changed(this);
}
}
void Waypoint::setAltitude(double altitude)
{
if (this->z != altitude) {
if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) {
this->z = altitude;
this->frame = MAV_FRAME_GLOBAL;
emit changed(this);
}
}
......
......@@ -4,7 +4,7 @@
#include <QString>
/** @brief Polling interval in ms */
#define SERIAL_POLL_INTERVAL 7
#define SERIAL_POLL_INTERVAL 9
/** @brief Heartbeat emission rate, in Hertz (times per second) */
#define MAVLINK_HEARTBEAT_DEFAULT_RATE 1
......@@ -12,14 +12,14 @@
#define WITH_TEXT_TO_SPEECH 1
#define QGC_APPLICATION_NAME "QGroundControl"
#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha RC7)"
#define QGC_APPLICATION_VERSION "v. 0.9.0 (Alpha RC1)"
namespace QGC
{
const QString APPNAME = "QGROUNDCONTROL";
const QString COMPANYNAME = "OPENMAV";
const int APPLICATIONVERSION = 83; // 0.8.0
const QString COMPANYNAME = "QGROUNDCONTROL";
const int APPLICATIONVERSION = 90; // 0.9.0
}
#endif // QGC_CONFIGURATION_H
......@@ -455,17 +455,15 @@ namespace mapcontrol
}
internals::PointLatLng MapGraphicItem::FromLocalToLatLng(int x, int y)
{
// It is IMPORTANT here to use full precision during the calculations!
double lat, lon;
if(MapRenderTransform!=1)
{
lat=x+((static_cast<double>(boundingRect().width())*MapRenderTransform)-(boundingRect().width()))/2.0f;
lon=y+((static_cast<double>(boundingRect().height())*MapRenderTransform)-(boundingRect().height()))/2.0f;
x=x+((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2;
y=y+((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2;
lat = (x / MapRenderTransform);
lon = (y / MapRenderTransform);
x = (int) (x / MapRenderTransform);
y = (int) (y / MapRenderTransform);
}
return core->FromLocalToLatLng(lat, lon);
return core->FromLocalToLatLng(x, y);
}
float MapGraphicItem::metersToPixels(double meters, internals::PointLatLng coord)
{
......
......@@ -103,6 +103,8 @@ namespace mapcontrol
delete text;
delete textBG;
coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y());
QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6);
qDebug() << "WP MOVE:" << coord_str << __FILE__ << __LINE__;
isDragging=false;
RefreshToolTip();
......@@ -118,6 +120,7 @@ namespace mapcontrol
coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y());
QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6);
text->setText(coord_str);
qDebug() << "WP DRAG:" << coord_str << __FILE__ << __LINE__;
textBG->setRect(text->boundingRect());
emit WPValuesChanged(this);
......
......@@ -71,9 +71,9 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
connect(m_ui->posESpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double)));
connect(m_ui->posDSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double)));
connect(m_ui->latSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double)));
connect(m_ui->lonSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double)));
connect(m_ui->altSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double)));
connect(m_ui->latSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setLatitude(double)));
connect(m_ui->lonSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setLongitude(double)));
connect(m_ui->altSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setAltitude(double)));
connect(m_ui->yawSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setYaw(int)));
connect(m_ui->upButton, SIGNAL(clicked()), this, SLOT(moveUp()));
......@@ -391,14 +391,26 @@ void WaypointView::updateValues()
break;
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_RELATIVE_ALT: {
if (m_ui->latSpinBox->value() != wp->getX()) {
m_ui->latSpinBox->setValue(wp->getX());
if (m_ui->latSpinBox->value() != wp->getLatitude()) {
// Rounding might occur, prevent spin box from
// firing back changes
m_ui->latSpinBox->blockSignals(true);
m_ui->latSpinBox->setValue(wp->getLatitude());
m_ui->latSpinBox->blockSignals(false);
}
if (m_ui->lonSpinBox->value() != wp->getY()) {
m_ui->lonSpinBox->setValue(wp->getY());
if (m_ui->lonSpinBox->value() != wp->getLongitude()) {
// Rounding might occur, prevent spin box from
// firing back changes
m_ui->lonSpinBox->blockSignals(true);
m_ui->lonSpinBox->setValue(wp->getLongitude());
m_ui->lonSpinBox->blockSignals(false);
}
if (m_ui->altSpinBox->value() != wp->getZ()) {
m_ui->altSpinBox->setValue(wp->getZ());
if (m_ui->altSpinBox->value() != wp->getAltitude()) {
// Rounding might occur, prevent spin box from
// firing back changes
m_ui->altSpinBox->blockSignals(true);
m_ui->altSpinBox->setValue(wp->getAltitude());
m_ui->altSpinBox->blockSignals(false);
}
}
break;
......
......@@ -133,9 +133,9 @@ void QGCMapWidget::loadSettings()
settings.endGroup();
// SET INITIAL POSITION AND ZOOM
SetZoom(lastZoom); // set map zoom level
internals::PointLatLng pos_lat_lon = internals::PointLatLng(lastLat, lastLon);
SetCurrentPosition(pos_lat_lon); // set the map position
SetZoom(lastZoom); // set map zoom level
}
void QGCMapWidget::storeSettings()
......@@ -152,7 +152,7 @@ void QGCMapWidget::storeSettings()
void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
{
OPMapWidget::mouseDoubleClickEvent(event);
//OPMapWidget::mouseDoubleClickEvent(event);
if (currEditMode == EDIT_MODE_WAYPOINTS)
{
// If a waypoint manager is available
......@@ -372,14 +372,24 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
// Update WP values
internals::PointLatLng pos = waypoint->Coord();
// Block waypoint signals
wp->blockSignals(true);
wp->setLatitude(pos.Lat());
wp->setLongitude(pos.Lng());
wp->setAltitude(waypoint->Altitude());
wp->blockSignals(false);
qDebug() << "WP: LAT:" << pos.Lat() << "LON:" << pos.Lng();
emit waypointChanged(wp);
internals::PointLatLng coord = waypoint->Coord();
QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6);
qDebug() << "MAP WP COORD (MAP):" << coord_str << __FILE__ << __LINE__;
QString wp_str = QString::number(wp->getLatitude(), 'f', 6) + " " + QString::number(wp->getLongitude(), 'f', 6);
qDebug() << "MAP WP COORD (WP):" << wp_str << __FILE__ << __LINE__;
firingWaypointChange = NULL;
emit waypointChanged(wp);
}
// WAYPOINT UPDATE FUNCTIONS
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment