From 354438bea79573d17daa1669e8dbb23729a360be Mon Sep 17 00:00:00 2001 From: lm Date: Fri, 11 Feb 2011 17:26:00 +0100 Subject: [PATCH] Fixed waypoint interface to adhere to specs, fixed param name length --- src/Waypoint.cc | 27 ++++++++++++++++++++ src/Waypoint.h | 6 +++++ src/comm/MAVLinkSimulationMAV.cc | 10 ++++---- src/comm/MAVLinkSimulationWaypointPlanner.cc | 4 +-- src/uas/UAS.cc | 4 +-- src/ui/MapWidget.cc | 22 +++++++++++----- src/ui/WaypointList.cc | 2 +- src/ui/WaypointView.cc | 16 +++++++----- 8 files changed, 68 insertions(+), 23 deletions(-) diff --git a/src/Waypoint.cc b/src/Waypoint.cc index a5de8c23ee..16b2ac63ad 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -124,6 +124,33 @@ void Waypoint::setZ(double z) } } +void Waypoint::setLatitude(double lat) +{ + if (this->x != lat) + { + this->x = lat; + emit changed(this); + } +} + +void Waypoint::setLongitude(double lon) +{ + if (this->y != lon) + { + this->y = lon; + emit changed(this); + } +} + +void Waypoint::setAltitude(double altitude) +{ + if (this->z != altitude) + { + this->z = altitude; + emit changed(this); + } +} + void Waypoint::setYaw(double yaw) { if (this->yaw != yaw) diff --git a/src/Waypoint.h b/src/Waypoint.h index 0df9e96e94..d454758bcb 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -52,6 +52,9 @@ public: double getX() const { return x; } double getY() const { return y; } double getZ() const { return z; } + double getLatitude() const { return x; } + double getLongitude() const { return y; } + double getAltitude() const { return z; } double getYaw() const { return yaw; } bool getAutoContinue() const { return autocontinue; } bool getCurrent() const { return current; } @@ -95,6 +98,9 @@ public slots: void setX(double x); void setY(double y); void setZ(double z); + void setLatitude(double lat); + void setLongitude(double lon); + void setAltitude(double alt); void setYaw(double yaw); /** @brief Set the waypoint action */ void setAction(int action); diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index c32cac7f22..5be693b6df 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -15,8 +15,8 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy latitude(lat), longitude(lon), altitude(0.0), - x(lon), - y(lat), + x(lat), + y(lon), z(550), roll(0.0), pitch(0.0), @@ -133,8 +133,8 @@ void MAVLinkSimulationMAV::mainloop() mavlink_message_t msg; mavlink_global_position_int_t pos; pos.alt = z*1000.0; - pos.lat = y*1E7; - pos.lon = x*1E7; + pos.lat = x*1E7; + pos.lon = y*1E7; pos.vx = 10.0f*100.0f; pos.vy = 0; pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f; @@ -147,7 +147,7 @@ void MAVLinkSimulationMAV::mainloop() attitude.usec = 0; attitude.roll = 0.0f; attitude.pitch = 0.0f; - attitude.yaw = yaw; + attitude.yaw = yaw-M_PI_2; attitude.rollspeed = 0.0f; attitude.pitchspeed = 0.0f; attitude.yawspeed = 0.0f; diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index 23c40c1d24..08ebdefa49 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -573,8 +573,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* mavlink_global_position_int_t pos; mavlink_msg_global_position_int_decode(msg, &pos); - float x = static_cast(pos.lon)/1E7; - float y = static_cast(pos.lat)/1E7; + float x = static_cast(pos.lat)/1E7; + float y = static_cast(pos.lon)/1E7; float z = static_cast(pos.alt)/1000; qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 4e30aef473..d4567ce975 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -678,8 +678,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_param_value_t value; mavlink_msg_param_value_decode(&message, &value); - - QString parameterName = QString((char*)value.param_id); + QByteArray bytes((char*)value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QString parameterName = QString(bytes); int component = message.compid; float val = value.param_value; diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 4f994e46ef..fc4c388e15 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -417,7 +417,15 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina if (mav) { - mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y(), 0.0f, 0.0f, true)); + double altitude = 0.0; + double yaw = 0.0; + int wpListCount = mav->getWaypointManager()->getWaypointList().count(); + if (wpListCount > 0) + { + altitude = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getAltitude(); + yaw = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getYaw(); + } + mav->getWaypointManager()->addWaypoint(new Waypoint(wpListCount, coordinate.y(), coordinate.x(), altitude, yaw, true)); } else { @@ -472,8 +480,8 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView) { // Waypoint is new, a new icon is created QPointF coordinate; - coordinate.setX(wp->getX()); - coordinate.setY(wp->getY()); + coordinate.setX(wp->getLongitude()); + coordinate.setY(wp->getLatitude()); createWaypointGraphAtMap(wpindex, coordinate); } else @@ -483,8 +491,8 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView) if(!waypointIsDrag) { QPointF coordinate; - coordinate.setX(wp->getX()); - coordinate.setY(wp->getY()); + coordinate.setX(wp->getLongitude()); + coordinate.setY(wp->getLatitude()); Point* waypoint; waypoint = wps.at(wpindex); @@ -624,8 +632,8 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) if (wps.size() > index) { Waypoint* wp = wps.at(index); - wp->setX(coordinate.x()); - wp->setY(coordinate.y()); + wp->setLatitude(coordinate.y()); + wp->setLongitude(coordinate.x()); mav->getWaypointManager()->notifyOfChange(wp); } } diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 03a21e897b..fe81e324cf 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -188,7 +188,7 @@ void WaypointList::add() else { // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT); + wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT); uas->getWaypointManager()->addWaypoint(wp); } } diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 8b6df9bb33..20e0d1e9c9 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -76,8 +76,8 @@ 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->lonSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); - connect(m_ui->latSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(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))); //hidden degree to radian conversion of the yaw angle @@ -267,6 +267,10 @@ void WaypointView::changeViewMode(QGC_WAYPOINTVIEW_MODE mode) switch (mode) { case QGC_WAYPOINTVIEW_MODE_NAV: + case QGC_WAYPOINTVIEW_MODE_CONDITION: + // Hide everything, show condition widget + // TODO + case QGC_WAYPOINTVIEW_MODE_DO: break; case QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING: @@ -399,13 +403,13 @@ void WaypointView::updateValues() break; case(MAV_FRAME_GLOBAL): { - if (m_ui->lonSpinBox->value() != wp->getX()) + if (m_ui->latSpinBox->value() != wp->getX()) { - m_ui->lonSpinBox->setValue(wp->getX()); + m_ui->latSpinBox->setValue(wp->getX()); } - if (m_ui->latSpinBox->value() != wp->getY()) + if (m_ui->lonSpinBox->value() != wp->getY()) { - m_ui->latSpinBox->setValue(wp->getY()); + m_ui->lonSpinBox->setValue(wp->getY()); } if (m_ui->altSpinBox->value() != wp->getZ()) { -- GitLab