Skip to content

Q
qgroundcontrol
Commit 39468055 authored by lm's avatar lm
Browse files
Options

Fixed orientation calculation in simulation and several widgets

parent 52230a20
Hide whitespace changes
Inline Side-by-side
Showing with 37 additions and 29 deletions
src/comm/MAVLinkSimulationLink.cc
View file @ 39468055
......@@ -817,8 +817,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION";
mavlink_param_request_list_t read;
mavlink_msg_param_request_list_decode(&msg, &read);
if (read.target_system == systemId)
{
// if (read.target_system == systemId)
// {
// Output all params
// Iterate through all components, through all parameters and emit them
QMap<QString, float>::iterator i;
......@@ -829,7 +829,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
if (j != 5)
{
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j);
mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j);
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
......@@ -840,7 +840,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
}
qDebug() << "SIMULATION SENT PARAMETERS TO GCS";
}
// }
}
break;
case MAVLINK_MSG_ID_PARAM_SET:
......@@ -848,8 +848,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER";
mavlink_param_set_t set;
mavlink_msg_param_set_decode(&msg, &set);
if (set.target_system == systemId)
{
// if (set.target_system == systemId)
// {
QString key = QString((char*)set.param_id);
if (onboardParams.contains(key))
{
......@@ -857,14 +857,14 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
onboardParams.insert(key, set.param_value);
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key));
mavlink_msg_param_value_pack(set.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key));
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer+=bufferlength;
}
}
// }
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
......@@ -878,7 +878,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
float paramValue = onboardParams.value(key);
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
......
src/comm/MAVLinkSimulationMAV.cc
View file @ 39468055
......@@ -96,7 +96,7 @@ void MAVLinkSimulationMAV::mainloop()
{
//float trueyaw = atan2f(xm, ym);
float newYaw = atan2f(xm, ym);
float newYaw = atan2f(ym, xm);
if (fabs(yaw - newYaw) < 90)
{
......@@ -112,8 +112,8 @@ void MAVLinkSimulationMAV::mainloop()
//if (sqrt(xm*xm+ym*ym) > 0.0000001)
if (flying)
{
x += sin(yaw)*radPer100ms;
y += cos(yaw)*radPer100ms;
x += cos(yaw)*radPer100ms;
y += sin(yaw)*radPer100ms;
z += altPer100ms*zsign;
}
......@@ -135,7 +135,7 @@ void MAVLinkSimulationMAV::mainloop()
pos.alt = z*1000.0;
pos.lat = x*1E7;
pos.lon = y*1E7;
pos.vx = 10.0f*100.0f;
pos.vx = sin(yaw)*10.0f*100.0f;
pos.vy = 0;
pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f;
mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
......@@ -147,7 +147,7 @@ void MAVLinkSimulationMAV::mainloop()
attitude.usec = 0;
attitude.roll = 0.0f;
attitude.pitch = 0.0f;
attitude.yaw = yaw-M_PI_2;
attitude.yaw = yaw;
attitude.rollspeed = 0.0f;
attitude.pitchspeed = 0.0f;
attitude.yawspeed = 0.0f;
......@@ -170,11 +170,11 @@ void MAVLinkSimulationMAV::mainloop()
// VFR HUD
mavlink_vfr_hud_t hud;
hud.airspeed = pos.vx;
hud.groundspeed = pos.vx;
hud.alt = pos.alt;
hud.airspeed = pos.vx/100.0f;
hud.groundspeed = pos.vx/100.0f;
hud.alt = z;
hud.heading = static_cast<int>((yaw/M_PI)*180.0f+180.0f) % 360;
hud.climb = pos.vz;
hud.climb = pos.vz/100.0f;
hud.throttle = 90;
mavlink_msg_vfr_hud_encode(systemid, MAV_COMP_ID_IMU, &msg, &hud);
link->sendMAVLinkMessage(&msg);
......
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @ 39468055
......@@ -577,7 +577,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
float y = static_cast<double>(pos.lon)/1E7;
float z = static_cast<double>(pos.alt)/1000;
qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z;
//qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z;
posReached = false;
yawReached = true;
......
src/uas/UASWaypointManager.cc
View file @ 39468055
......@@ -258,12 +258,12 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
}
}
qDebug() << "Updated waypoints list";
//qDebug() << "Updated waypoints list";
}
emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
//emit update to UI widgets
emit currentWaypointChanged(wpc->seq);
qDebug() << "new current waypoint" << wpc->seq;
//qDebug() << "new current waypoint" << wpc->seq;
}
}
......
src/ui/HSIDisplay.cc
View file @ 39468055
......@@ -221,12 +221,14 @@ void HSIDisplay::renderOverlay()
// Draw orientation labels
// Translate and rotate coordinate frame
painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
painter.rotate((yaw/(M_PI))*180.0f);
const float yawDeg = ((yaw/M_PI)*180.0f);
int yawRotate = static_cast<int>(yawDeg) % 360;
painter.rotate(-yawRotate);
paintText(tr("N"), ringColor, 3.5f, - 1.0f, - baseRadius - 5.5f, &painter);
paintText(tr("S"), ringColor, 3.5f, - 1.0f, + baseRadius + 1.5f, &painter);
paintText(tr("E"), ringColor, 3.5f, + baseRadius + 2.0f, - 1.25f, &painter);
paintText(tr("E"), ringColor, 3.5f, + baseRadius + 3.0f, - 1.25f, &painter);
paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter);
painter.rotate((-yaw/(M_PI))*180.0f);
painter.rotate(+yawRotate);
painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
// Draw center indicator
......
src/ui/map/MAV2DIcon.cc
View file @ 39468055
......@@ -3,6 +3,8 @@
#include <qmath.h>
#include "QGC.h"
MAV2DIcon::MAV2DIcon(UASInterface* uas, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen)
: Point(uas->getLatitude(), uas->getLongitude(), name, alignment),
yaw(0.0f),
......@@ -112,7 +114,10 @@ void MAV2DIcon::drawAirframePolygon(int airframe, QPainter& painter, int radius,
// DRAW AIRPLANE
// Rotate 180 degs more since the icon is oriented downwards
float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f;
//float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f;
const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f+180.0f;
int yawRotate = static_cast<int>(yawDeg) % 360;
painter.rotate(yawRotate);
......@@ -159,7 +164,8 @@ void MAV2DIcon::drawAirframePolygon(int airframe, QPainter& painter, int radius,
{
// QUADROTOR
float iconSize = radius*0.9f;
float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f;
const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f;
int yawRotate = static_cast<int>(yawDeg) % 360;
painter.rotate(yawRotate);
......@@ -201,8 +207,8 @@ void MAV2DIcon::drawAirframePolygon(int airframe, QPainter& painter, int radius,
{
// DRAW AIRPLANE
float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f;
const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f;
int yawRotate = static_cast<int>(yawDeg) % 360;
painter.rotate(yawRotate);
//qDebug() << "ICON SIZE:" << radius;
......
src/ui/uas/UASView.cc
View file @ 39468055
......@@ -365,7 +365,7 @@ void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, do
void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 usec)
{
Q_UNUSED(usec);
totalSpeed = sqrt((pow(x, 2) + pow(y, 2) + pow(z, 2)));
totalSpeed = sqrt(x*x + y*y + z*z);
}
void UASView::currentWaypointUpdated(quint16 waypoint)
......
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