diff --git a/src/QGC.cc b/src/QGC.cc index 17f4c226f16621363d51ffba79b960483e428700..179d54f5905c1c1c06006a2a68f31fc01bba2f90 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -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; } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index c5c8417e3182cb4434265525a73c3618e6eb77b5..50a2bd0fb176821094f086a5aa70fb2216787c3e 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -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; } diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 9bf493fdbe8d5919aa918893d1808be20029d18f..251d0fa4d64ef0dc65a62954bf600c862cd9a78b 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -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