zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

QGC4.4
LM 14 years ago
parent
bcabd2cee3
commit
a81ecc238f
3 changed files with 81 additions and 241 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 56
      src/QGC.cc
  2. 63
      src/uas/UAS.cc
  3. 203
      src/ui/HUD.cc

56
src/QGC.cc
Unescape View File

@ -49,28 +49,52 @@ quint64 groundTimeMilliseconds() @@ -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;
}

63
src/uas/UAS.cc
Unescape View File

@ -378,17 +378,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -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) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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) @@ -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 @@ -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() @@ -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;
}

203
src/ui/HUD.cc
Unescape View File

@ -835,201 +835,6 @@ void HUD::paintHUD() @@ -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) @@ -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

Write Preview
Loading…
Cancel
Save