Skip to content

Q
qgroundcontrol
Commit a81ecc23 authored by LM's avatar LM
Browse files
Options

Fixed command long compile issues, fixed bug in PI wrapping, improved stability

parent bcabd2ce
Hide whitespace changes
Inline Side-by-side
Showing with 81 additions and 241 deletions
src/QGC.cc
View file @ a81ecc23
......@@ -49,28 +49,52 @@ quint64 groundTimeMilliseconds()
float limitAngleToPMPIf(float angle)
{
// while (angle > ((float)M_PI+FLT_EPSILON)) {
// angle -= 2.0f * (float)M_PI;
// }
// while (angle <= -((float)M_PI+FLT_EPSILON)) {
// angle += 2.0f * (float)M_PI;
// }
if (angle > -20*M_PI && angle < 20*M_PI)
{
while (angle > ((float)M_PI+FLT_EPSILON))
{
angle -= 2.0f * (float)M_PI;
}
while (angle <= -((float)M_PI+FLT_EPSILON))
{
angle += 2.0f * (float)M_PI;
}
}
else
{
// Approximate
angle = fmodf(angle, M_PI);
}
return angle;
}
double limitAngleToPMPId(double angle)
{
// if (angle < -M_PI) {
// while (angle < -M_PI) {
// angle += M_PI;
// }
// } else if (angle > M_PI) {
// while (angle > M_PI) {
// angle -= M_PI;
// }
// }
if (angle > -20*M_PI && angle < 20*M_PI)
{
if (angle < -M_PI)
{
while (angle < -M_PI)
{
angle += M_PI;
}
}
else if (angle > M_PI)
{
while (angle > M_PI)
{
angle -= M_PI;
}
}
}
else
{
// Approximate
angle = fmod(angle, M_PI);
}
return angle;
}
......
src/uas/UAS.cc
View file @ a81ecc23
......@@ -378,17 +378,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
pitch = QGC::limitAngleToPMPIf(attitude.pitch);
yaw = QGC::limitAngleToPMPIf(attitude.yaw);
// Emit in angles
// Convert yaw angle to compass value
// in 0 - 360 deg range
float compass = (yaw/M_PI)*180.0+360.0f;
if (compass > -10000 && compass < 10000)
{
while (compass > 360.0f) {
compass -= 360.0f;
}
}
// // Emit in angles
// // Convert yaw angle to compass value
// // in 0 - 360 deg range
// float compass = (yaw/M_PI)*180.0+360.0f;
// if (compass > -10000 && compass < 10000)
// {
// while (compass > 360.0f) {
// compass -= 360.0f;
// }
// }
// else
// {
// // Set to 0, since it is an invalid value
// compass = 0.0f;
// }
attitudeKnown = true;
emit attitudeChanged(this, roll, pitch, yaw, time);
......@@ -1006,7 +1011,7 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw)
{
#ifdef MAVLINK_ENABLED_PIXHAWK
mavlink_message_t msg;
mavlink_msg_position_control_offset_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw);
mavlink_msg_set_position_control_offset_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw);
sendMessage(msg);
#else
Q_UNUSED(x);
......@@ -1020,21 +1025,21 @@ void UAS::startRadioControlCalibration()
{
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 1);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 1, 0, 0, 0);
sendMessage(msg);
}
void UAS::startDataRecording()
{
mavlink_message_t msg;
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 2);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 2, 0, 0, 0);
sendMessage(msg);
}
void UAS::stopDataRecording()
{
mavlink_message_t msg;
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 0);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 0, 0, 0, 0);
sendMessage(msg);
}
......@@ -1042,7 +1047,7 @@ void UAS::startMagnetometerCalibration()
{
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 1, 0, 0);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 1, 0, 0, 0, 0, 0);
sendMessage(msg);
}
......@@ -1050,7 +1055,7 @@ void UAS::startGyroscopeCalibration()
{
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1, 0, 0, 0);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1, 0, 0, 0, 0, 0, 0);
sendMessage(msg);
}
......@@ -1058,7 +1063,7 @@ void UAS::startPressureCalibration()
{
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 1, 0);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 1, 0, 0, 0, 0);
sendMessage(msg);
}
......@@ -1453,14 +1458,14 @@ void UAS::requestParameters()
void UAS::writeParametersToStorage()
{
mavlink_message_t msg;
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
sendMessage(msg);
}
void UAS::readParametersFromStorage()
{
mavlink_message_t msg;
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 0, -1, -1, -1);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 0, -1, -1, -1, 0, 0, 0);
sendMessage(msg);
}
......@@ -1825,32 +1830,38 @@ void UAS::setUASName(const QString& name)
void UAS::executeCommand(MAV_CMD command)
{
mavlink_message_t msg;
mavlink_command_short_t cmd;
mavlink_command_long_t cmd;
cmd.command = (uint8_t)command;
cmd.confirmation = 0;
cmd.param1 = 0.0f;
cmd.param2 = 0.0f;
cmd.param3 = 0.0f;
cmd.param4 = 0.0f;
cmd.param5 = 0.0f;
cmd.param6 = 0.0f;
cmd.param7 = 0.0f;
cmd.target_system = uasId;
cmd.target_component = 0;
mavlink_msg_command_short_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
sendMessage(msg);
}
void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component)
{
mavlink_message_t msg;
mavlink_command_short_t cmd;
mavlink_command_long_t cmd;
cmd.command = (uint8_t)command;
cmd.confirmation = confirmation;
cmd.param1 = param1;
cmd.param2 = param2;
cmd.param3 = param3;
cmd.param4 = param4;
cmd.param5 = 0.0f;
cmd.param6 = 0.0f;
cmd.param7 = 0.0f;
cmd.target_system = uasId;
cmd.target_component = component;
mavlink_msg_command_short_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
sendMessage(msg);
}
......@@ -1880,7 +1891,7 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
void UAS::launch()
{
mavlink_message_t msg;
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_NAV_TAKEOFF, 1, 0, 0, 0, 0);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_NAV_TAKEOFF, 1, 0, 0, 0, 0, 0, 0, 0);
sendMessage(msg);
}
......@@ -2111,7 +2122,7 @@ void UAS::shutdown()
{
// If the active UAS is set, execute command
mavlink_message_t msg;
mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, 2, 0, 0, 0);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, 2, 0, 0, 0, 0, 0, 0);
sendMessage(msg);
result = true;
}
......
src/ui/HUD.cc
View file @ a81ecc23
......@@ -835,201 +835,6 @@ void HUD::paintHUD()
}
}
/*
void HUD::paintGL()
{
static float roll = 0.0;
static float pitch = 0.0;
static float yaw = 0.0;
// Read out most important values to limit hash table lookups
roll = roll * 0.5 + 0.5 * values.value("roll", 0.0f);
pitch = pitch * 0.5 + 0.5 * values.value("pitch", 0.0f);
yaw = yaw * 0.5 + 0.5 * values.value("yaw", 0.0f);
//qDebug() << __FILE__ << __LINE__ << "ROLL:" << roll << "PITCH:" << pitch << "YAW:" << yaw;
// Update scaling factor
// adjust scaling to fit both horizontally and vertically
scalingFactor = this->width()/vwidth;
double scalingFactorH = this->height()/vheight;
if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH;
makeCurrent();
glClear(GL_COLOR_BUFFER_BIT);
//if(!noCamera) glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
//glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits()); // FIXME Remove after testing
// Blue / Brown background
if (noCamera) paintCenterBackground(roll, pitch, yaw);
glFlush();
// // Store current GL model view
// glMatrixMode(GL_MODELVIEW);
// glPushMatrix();
//
// // Setup GL view
// setupGLView(0.0f, 0.0f, vwidth, vheight);
//
// // Restore previous view
// glPopMatrix();
// Now draw QPainter overlay
//painter.setRenderHint(QPainter::Antialiasing);
// Position the coordinate frame according to the setup
makeOverlayCurrent();
QPainter painter(this);
//painter.setRenderHint(QPainter::Antialiasing, true);
//painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
painter.translate((this->vwidth/2.0+xCenterOffset)*scalingFactor, (this->vheight/2.0+yCenterOffset)*scalingFactor);
// COORDINATE FRAME IS NOW (0,0) at CENTER OF WIDGET
// Draw all fixed indicators
// MODE
paintText(mode, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 10, &painter);
// STATE
paintText(state, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 15, &painter);
// BATTERY
paintText(fuelStatus, fuelColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 20, &painter);
// Waypoint
paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter);
// YAW INDICATOR
//
// .
// . .
// .......
//
const float yawIndicatorWidth = 4.0f;
const float yawIndicatorY = vheight/2.0f - 10.0f;
QPolygon yawIndicator(4);
yawIndicator.setPoint(0, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
yawIndicator.setPoint(1, QPoint(refToScreenX(yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
yawIndicator.setPoint(2, QPoint(refToScreenX(-yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
yawIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
painter.setPen(defaultColor);
painter.drawPolyline(yawIndicator);
// CENTER
// HEADING INDICATOR
//
// __ __
// \/\/
//
const float hIndicatorWidth = 7.0f;
const float hIndicatorY = -25.0f;
const float hIndicatorYLow = hIndicatorY + hIndicatorWidth / 6.0f;
const float hIndicatorSegmentWidth = hIndicatorWidth / 7.0f;
QPolygon hIndicator(7);
hIndicator.setPoint(0, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
hIndicator.setPoint(1, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f+hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
hIndicator.setPoint(2, QPoint(refToScreenX(0.0f-hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
hIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(hIndicatorY)));
hIndicator.setPoint(4, QPoint(refToScreenX(0.0f+hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
hIndicator.setPoint(5, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f-hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
hIndicator.setPoint(6, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
painter.setPen(defaultColor);
painter.drawPolyline(hIndicator);
// SETPOINT
const float centerWidth = 4.0f;
painter.setPen(defaultColor);
painter.setBrush(Qt::NoBrush);
// TODO
//painter.drawEllipse(QPointF(refToScreenX(qMin(10.0f, values.value("roll desired", 0.0f) * 10.0f)), refToScreenY(qMin(10.0f, values.value("pitch desired", 0.0f) * 10.0f))), refToScreenX(centerWidth/2.0f), refToScreenX(centerWidth/2.0f));
const float centerCrossWidth = 10.0f;
// left
painter.drawLine(QPointF(refToScreenX(-centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(-centerCrossWidth / 2.0f), refToScreenY(0.0f)));
// right
painter.drawLine(QPointF(refToScreenX(centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(centerCrossWidth / 2.0f), refToScreenY(0.0f)));
// top
painter.drawLine(QPointF(refToScreenX(0.0f), refToScreenY(-centerWidth / 2.0f)), QPointF(refToScreenX(0.0f), refToScreenY(-centerCrossWidth / 2.0f)));
// COMPASS
const float compassY = -vheight/2.0f + 10.0f;
QRectF compassRect(QPointF(refToScreenX(-5.0f), refToScreenY(compassY)), QSizeF(refToScreenX(10.0f), refToScreenY(5.0f)));
painter.setBrush(Qt::NoBrush);
painter.setPen(Qt::SolidLine);
painter.setPen(defaultColor);
painter.drawRoundedRect(compassRect, 2, 2);
QString yawAngle;
const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f;
//qDebug() << "YAW: " << yawDeg;
yawAngle.sprintf("%03d", (int)yawDeg);
paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter);
// CHANGE RATE STRIPS
drawChangeRateStrip(-51.0f, -50.0f, 15.0f, -1.0f, 1.0f, valuesDot.value("z", 0.0f), &painter);
// CHANGE RATE STRIPS
drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, valuesDot.value("x", 0.0f), &painter);
// GAUGES
// Left altitude gauge
drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, -values.value("z", 0.0f), defaultColor, &painter, false);
// Right speed gauge
drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, values.value("xSpeed", 0.0f), defaultColor, &painter, false);
glFlush();
// MOVING PARTS
// Translate for yaw
const float maxYawTrans = 60.0f;
float yawDiff = valuesDot.value("yaw", 0.0f);
if (isinf(yawDiff)) yawDiff = 0.0f;
if (yawDiff > M_PI) yawDiff = yawDiff - M_PI;
if (yawDiff < -M_PI) yawDiff = yawDiff + M_PI;
yawInt += yawDiff;
if (yawInt > M_PI) yawInt = M_PI;
if (yawInt < -M_PI) yawInt = -M_PI;
float yawTrans = yawInt * (double)maxYawTrans;
yawInt *= 0.6f;
//qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw << "asin(yawInt)" << asinYaw;
painter.translate(0, (pitch/M_PI)* -180.0f * refToScreenY(1.8));
painter.translate(refToScreenX(yawTrans), 0);
// Rotate view and draw all roll-dependent indicators
painter.rotate((roll/M_PI)* -180.0f);
//qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f);
// PITCH
paintPitchLines((pitch/M_PI)*180.0f, &painter);
painter.end();
glFlush();
}*/
/**
* @param pitch pitch angle in degrees (-180 to 180)
......@@ -1049,10 +854,10 @@ void HUD::paintPitchLines(float pitch, QPainter* painter)
float offset = pitch;
if (offset < 0) offset = -offset;
int offsetCount = 0;
// while (offset > lineDistance) {
// offset -= lineDistance;
// offsetCount++;
// }
while (offset > lineDistance) {
offset -= lineDistance;
offsetCount++;
}
int iPos = (int)(0.5f + lineDistance); ///< The first line
int iNeg = (int)(-0.5f - lineDistance); ///< The first line
......
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