Skip to content

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

Fixed a simulation issue, merged with master

parents 28826042 8bd61d71
Hide whitespace changes
Inline Side-by-side
Showing with 349 additions and 36 deletions
src/comm/MAVLinkSimulationLink.cc
View file @ 034c8afe
......@@ -36,7 +36,7 @@ This file is part of the QGROUNDCONTROL project
#include <QTime>
#include <QImage>
#include <QDebug>
#include "MG.h"
#include <QFileInfo>
#include "LinkManager.h"
#include "MAVLinkProtocol.h"
#include "MAVLinkSimulationLink.h"
......@@ -80,7 +80,7 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
simulationHeader = simulationFile->readLine();
}
receiveFile = new QFile(writeFile, this);
lastSent = MG::TIME::getGroundTimeNow() * 1000;
lastSent = QGC::groundTimeMilliseconds();
if (simulationFile->exists()) {
this->name = "Simulation: " + QFileInfo(simulationFile->fileName()).fileName();
......@@ -120,7 +120,7 @@ void MAVLinkSimulationLink::run()
static quint64 last = 0;
if (MG::TIME::getGroundTimeNow() - last >= rate) {
if (QGC::groundTimeMilliseconds() - last >= rate) {
if (_isConnected) {
mainloop();
......@@ -134,7 +134,7 @@ void MAVLinkSimulationLink::run()
readBytes();
}
last = MG::TIME::getGroundTimeNow();
last = QGC::groundTimeMilliseconds();
}
QGC::SLEEP::msleep(3);
......@@ -259,9 +259,6 @@ void MAVLinkSimulationLink::mainloop()
double d = QString(parts.at(i)).toDouble(&res);
if (!res) d = 0;
//qDebug() << "TIME" << time << "VALUE" << d;
//emit valueChanged(220, keys.at(i), d, MG::TIME::getGroundTimeNow());
if (keys.value(i, "") == "Accel._X") {
rawImuValues.xacc = d;
}
......@@ -339,7 +336,7 @@ void MAVLinkSimulationLink::mainloop()
//qDebug() << "ATTITUDE" << "BUF LEN" << bufferlength << "POINTER" << streampointer;
//qDebug() << "REALTIME" << MG::TIME::getGroundTimeNow() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
//qDebug() << "REALTIME" << QGC::groundTimeMilliseconds() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
}
......@@ -654,11 +651,6 @@ qint64 MAVLinkSimulationLink::bytesAvailable()
void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
{
//qDebug() << "Simulation received " << size << " bytes from groundstation: ";
// Increase write counter
//bitsSentTotal += size * 8;
// Parse bytes
mavlink_message_t msg;
mavlink_status_t comm;
......
src/comm/MAVLinkSimulationMAV.cc
View file @ 034c8afe
......@@ -23,20 +23,20 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
yaw(0.0),
globalNavigation(true),
firstWP(false),
// previousSPX(8.548056),
// previousSPY(47.376389),
// previousSPZ(550),
// previousSPYaw(0.0),
// nextSPX(8.548056),
// nextSPY(47.376389),
// nextSPZ(550),
previousSPX(37.480391),
previousSPY(122.282883),
previousSPZ(550),
previousSPYaw(0.0),
nextSPX(37.480391),
nextSPY(122.282883),
nextSPZ(550),
// previousSPX(8.548056),
// previousSPY(47.376389),
// previousSPZ(550),
// previousSPYaw(0.0),
// nextSPX(8.548056),
// nextSPY(47.376389),
// nextSPZ(550),
previousSPX(37.480391),
previousSPY(122.282883),
previousSPZ(550),
previousSPYaw(0.0),
nextSPX(37.480391),
nextSPY(122.282883),
nextSPZ(550),
nextSPYaw(0.0),
sys_mode(MAV_MODE_PREFLIGHT),
sys_state(MAV_STATE_STANDBY),
......
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @ 034c8afe
......@@ -449,8 +449,13 @@ MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimula
protocol_timeout(1000),
timestamp_last_send_setpoint(0),
systemid(sysid),
<<<<<<< HEAD
compid(MAV_COMP_ID_MISSIONPLANNER),
setpointDelay(10),
=======
compid(MAV_COMP_ID_WAYPOINTPLANNER),
setpointDelay(1000),
>>>>>>> master
yawTolerance(0.4f),
verbose(true),
debug(false),
......@@ -553,7 +558,7 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq)
emit messageSent(msg);
}
uint64_t now = QGC::groundTimeUsecs()/1000;
uint64_t now = QGC::groundTimeMilliseconds();
timestamp_last_send_setpoint = now;
}
}
......@@ -699,7 +704,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
//qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
uint64_t now = QGC::groundTimeUsecs()/1000;
uint64_t now = QGC::groundTimeMilliseconds();
if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) {
if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state;
current_state = PX_WPP_IDLE;
......
src/uas/PxQuadMAV.cc
View file @ 034c8afe
......@@ -132,6 +132,51 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "vis. speed z", "m/s", speed.z, time);
}
break;
<<<<<<< HEAD
=======
case MAVLINK_MSG_ID_CONTROL_STATUS:
{
mavlink_control_status_t status;
mavlink_msg_control_status_decode(&message, &status);
// Emit control status vector
emit attitudeControlEnabled(static_cast<bool>(status.control_att));
emit positionXYControlEnabled(static_cast<bool>(status.control_pos_xy));
emit positionZControlEnabled(static_cast<bool>(status.control_pos_z));
emit positionYawControlEnabled(static_cast<bool>(status.control_pos_yaw));
// Emit localization status vector
emit localizationChanged(this, status.position_fix);
emit visionLocalizationChanged(this, status.vision_fix);
emit gpsLocalizationChanged(this, status.gps_fix);
}
break;
case MAVLINK_MSG_ID_VISUAL_ODOMETRY:
{
mavlink_visual_odometry_t pos;
mavlink_msg_visual_odometry_decode(&message, &pos);
quint64 time = getUnixTime(pos.frame1_time_us);
//emit valueChanged(uasId, "vis. time", pos.usec, time);
emit valueChanged(uasId, "vis-o. roll", "rad", pos.roll, time);
emit valueChanged(uasId, "vis-o. pitch", "rad", pos.pitch, time);
emit valueChanged(uasId, "vis-o. yaw", "rad", pos.yaw, time);
emit valueChanged(uasId, "vis-o. x", "m", pos.x, time);
emit valueChanged(uasId, "vis-o. y", "m", pos.y, time);
emit valueChanged(uasId, "vis-o. z", "m", pos.z, time);
}
break;
case MAVLINK_MSG_ID_AUX_STATUS: {
mavlink_aux_status_t status;
mavlink_msg_aux_status_decode(&message, &status);
emit loadChanged(this, status.load/10.0f);
emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count);
emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count);
emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count);
emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count);
emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count);
emit valueChanged(uasId, "Load", "%", ((float)status.load)/10.0f, getUnixTime());
}
break;
>>>>>>> master
default:
// Let UAS handle the default message set
UAS::receiveMessage(link, message);
......
src/uas/UAS.cc
View file @ 034c8afe
......@@ -76,8 +76,12 @@ airframe(0),
attitudeKnown(false),
paramManager(NULL),
attitudeStamped(false),
<<<<<<< HEAD
lastAttitude(0),
simulation(new QGCFlightGearLink(this))
=======
lastAttitude(0)
>>>>>>> master
{
color = UASInterface::getNextColor();
setBattery(LIPOLY, 3);
......@@ -339,8 +343,50 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
// COMMUNICATIONS DROP RATE
<<<<<<< HEAD
// FIXME
emit dropRateChanged(this->getUASID(), state.drop_rate_comm/10000.0f);
=======
emit dropRateChanged(this->getUASID(), state.packet_drop/1000.0f);
//add for development
//emit remoteControlRSSIChanged(state.packet_drop/1000.0f);
//float en = state.packet_drop/1000.0f;
//emit remoteControlChannelRawChanged(0, en);//MAVLINK_MSG_ID_RC_CHANNELS_RAW
//emit remoteControlChannelScaledChanged(0, en/100.0f);//MAVLINK_MSG_ID_RC_CHANNELS_SCALED
//qDebug() << __FILE__ << __LINE__ << "RCV LOSS: " << state.packet_drop;
// AUDIO
if (modechanged && statechanged)
{
// Output both messages
audiostring += modeAudio + " and " + stateAudio;
}
else
{
// Output the one message
audiostring += modeAudio + stateAudio;
}
if ((int)state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY)
{
GAudioOutput::instance()->startEmergency();
}
else if (modechanged || statechanged)
{
GAudioOutput::instance()->stopEmergency();
GAudioOutput::instance()->say(audiostring);
}
if (state.status == MAV_STATE_POWEROFF)
{
emit systemRemoved(this);
emit systemRemoved();
}
>>>>>>> master
}
break;
case MAVLINK_MSG_ID_RAW_IMU:
......@@ -737,7 +783,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_roll_pitch_yaw_thrust_setpoint_t out;
mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out);
<<<<<<< HEAD
quint64 time = getUnixTimeFromMs(out.time_boot_ms);
=======
quint64 time = getUnixTime(out.time_us);
>>>>>>> master
emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "att control roll", "rad", out.roll, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "att control pitch", "rad", out.pitch, time);
......@@ -1239,14 +1289,14 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
*/
quint64 UAS::getUnixTime(quint64 time)
{
quint64 ret = 0;
if (attitudeStamped)
{
return lastAttitude;
ret = lastAttitude;
}
if (time == 0)
{
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
return QGC::groundTimeMilliseconds();
ret = QGC::groundTimeMilliseconds();
}
// Check if time is smaller than 40 years,
// assuming no system without Unix timestamp
......@@ -1275,14 +1325,15 @@ quint64 UAS::getUnixTime(quint64 time)
{
onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
}
return time/1000 + onboardTimeOffset;
ret = time/1000 + onboardTimeOffset;
}
else
{
// Time is not zero and larger than 40 years -> has to be
// a Unix epoch timestamp. Do nothing.
return time/1000;
ret = time/1000;
}
return ret;
}
QList<QString> UAS::getParameterNames(int component)
......
src/ui/map/MAV2DIcon.cc
View file @ 034c8afe
......@@ -37,7 +37,6 @@ MAV2DIcon::MAV2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* p
MAV2DIcon::~MAV2DIcon()
{
//delete pic;
}
void MAV2DIcon::setSelectedUAS(bool selected)
......@@ -53,7 +52,6 @@ void MAV2DIcon::setSelectedUAS(bool selected)
*/
void MAV2DIcon::setYaw(float yaw)
{
//// qDebug() << "MAV2Icon" << yaw;
float diff = fabs(yaw - this->yaw);
while (diff > (float)M_PI) {
diff -= (float)M_PI;
......
thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/project.pbxproj 0 → 100644
View file @ 034c8afe
// !$*UTF8*$!
{
archiveVersion = 1;
classes = {
};
objectVersion = 45;
objects = {
/* Begin PBXBuildFile section */
348868D013F512010005F759 /* mavlink_parameters.c in Sources */ = {isa = PBXBuildFile; fileRef = 348868CF13F512010005F759 /* mavlink_parameters.c */; };
34E8AFDB13F5064C001100AA /* waypoints.c in Sources */ = {isa = PBXBuildFile; fileRef = 34E8AFDA13F5064C001100AA /* waypoints.c */; };
8DD76FAC0486AB0100D96B5E /* main.c in Sources */ = {isa = PBXBuildFile; fileRef = 08FB7796FE84155DC02AAC07 /* main.c */; settings = {ATTRIBUTES = (); }; };
/* End PBXBuildFile section */
/* Begin PBXCopyFilesBuildPhase section */
8DD76FAF0486AB0100D96B5E /* CopyFiles */ = {
isa = PBXCopyFilesBuildPhase;
buildActionMask = 8;
dstPath = /usr/share/man/man1/;
dstSubfolderSpec = 0;
files = (
);
runOnlyForDeploymentPostprocessing = 1;
};
/* End PBXCopyFilesBuildPhase section */
/* Begin PBXFileReference section */
08FB7796FE84155DC02AAC07 /* main.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = main.c; sourceTree = "<group>"; };
348868CF13F512010005F759 /* mavlink_parameters.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = mavlink_parameters.c; path = ../mavlink_parameters.c; sourceTree = SOURCE_ROOT; };
348868D113F512080005F759 /* mavlink_parameters.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = mavlink_parameters.h; path = ../mavlink_parameters.h; sourceTree = SOURCE_ROOT; };
34E8AFDA13F5064C001100AA /* waypoints.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = waypoints.c; path = ../waypoints.c; sourceTree = SOURCE_ROOT; };
34E8AFDC13F50659001100AA /* waypoints.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = waypoints.h; path = ../waypoints.h; sourceTree = SOURCE_ROOT; };
8DD76FB20486AB0100D96B5E /* udptest */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = udptest; sourceTree = BUILT_PRODUCTS_DIR; };
/* End PBXFileReference section */
/* Begin PBXFrameworksBuildPhase section */
8DD76FAD0486AB0100D96B5E /* Frameworks */ = {
isa = PBXFrameworksBuildPhase;
buildActionMask = 2147483647;
files = (
);
runOnlyForDeploymentPostprocessing = 0;
};
/* End PBXFrameworksBuildPhase section */
/* Begin PBXGroup section */
08FB7794FE84155DC02AAC07 /* udptest */ = {
isa = PBXGroup;
children = (
34E8AFDC13F50659001100AA /* waypoints.h */,
08FB7795FE84155DC02AAC07 /* Source */,
C6A0FF2B0290797F04C91782 /* Documentation */,
1AB674ADFE9D54B511CA2CBB /* Products */,
);
name = udptest;
sourceTree = "<group>";
};
08FB7795FE84155DC02AAC07 /* Source */ = {
isa = PBXGroup;
children = (
348868D113F512080005F759 /* mavlink_parameters.h */,
348868CF13F512010005F759 /* mavlink_parameters.c */,
34E8AFDA13F5064C001100AA /* waypoints.c */,
08FB7796FE84155DC02AAC07 /* main.c */,
);
name = Source;
sourceTree = "<group>";
};
1AB674ADFE9D54B511CA2CBB /* Products */ = {
isa = PBXGroup;
children = (
8DD76FB20486AB0100D96B5E /* udptest */,
);
name = Products;
sourceTree = "<group>";
};
C6A0FF2B0290797F04C91782 /* Documentation */ = {
isa = PBXGroup;
children = (
);
name = Documentation;
sourceTree = "<group>";
};
/* End PBXGroup section */
/* Begin PBXNativeTarget section */
8DD76FA90486AB0100D96B5E /* udptest */ = {
isa = PBXNativeTarget;
buildConfigurationList = 1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */;
buildPhases = (
8DD76FAB0486AB0100D96B5E /* Sources */,
8DD76FAD0486AB0100D96B5E /* Frameworks */,
8DD76FAF0486AB0100D96B5E /* CopyFiles */,
);
buildRules = (
);
dependencies = (
);
name = udptest;
productInstallPath = "$(HOME)/bin";
productName = udptest;
productReference = 8DD76FB20486AB0100D96B5E /* udptest */;
productType = "com.apple.product-type.tool";
};
/* End PBXNativeTarget section */
/* Begin PBXProject section */
08FB7793FE84155DC02AAC07 /* Project object */ = {
isa = PBXProject;
buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */;
compatibilityVersion = "Xcode 3.1";
developmentRegion = English;
hasScannedForEncodings = 1;
knownRegions = (
English,
Japanese,
French,
German,
);
mainGroup = 08FB7794FE84155DC02AAC07 /* udptest */;
projectDirPath = "";
projectRoot = "";
targets = (
8DD76FA90486AB0100D96B5E /* udptest */,
);
};
/* End PBXProject section */
/* Begin PBXSourcesBuildPhase section */
8DD76FAB0486AB0100D96B5E /* Sources */ = {
isa = PBXSourcesBuildPhase;
buildActionMask = 2147483647;
files = (
8DD76FAC0486AB0100D96B5E /* main.c in Sources */,
34E8AFDB13F5064C001100AA /* waypoints.c in Sources */,
348868D013F512010005F759 /* mavlink_parameters.c in Sources */,
);
runOnlyForDeploymentPostprocessing = 0;
};
/* End PBXSourcesBuildPhase section */
/* Begin XCBuildConfiguration section */
1DEB928608733DD80010E9CD /* Debug */ = {
isa = XCBuildConfiguration;
buildSettings = {
ALWAYS_SEARCH_USER_PATHS = NO;
COPY_PHASE_STRIP = NO;
GCC_DYNAMIC_NO_PIC = NO;
GCC_ENABLE_FIX_AND_CONTINUE = YES;
GCC_MODEL_TUNING = G5;
GCC_OPTIMIZATION_LEVEL = 0;
INSTALL_PATH = /usr/local/bin;
PRODUCT_NAME = udptest;
};
name = Debug;
};
1DEB928708733DD80010E9CD /* Release */ = {
isa = XCBuildConfiguration;
buildSettings = {
ALWAYS_SEARCH_USER_PATHS = NO;
DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym";
GCC_MODEL_TUNING = G5;
INSTALL_PATH = /usr/local/bin;
PRODUCT_NAME = udptest;
};
name = Release;
};
1DEB928A08733DD80010E9CD /* Debug */ = {
isa = XCBuildConfiguration;
buildSettings = {
ARCHS = "$(ARCHS_STANDARD_32_64_BIT)";
GCC_C_LANGUAGE_STANDARD = gnu99;
GCC_OPTIMIZATION_LEVEL = 0;
GCC_WARN_ABOUT_RETURN_TYPE = YES;
GCC_WARN_UNUSED_VARIABLE = YES;
HEADER_SEARCH_PATHS = (
../../include/,
../../include/common/,
);
ONLY_ACTIVE_ARCH = YES;
PREBINDING = NO;
SDKROOT = macosx10.6;
};
name = Debug;
};
1DEB928B08733DD80010E9CD /* Release */ = {
isa = XCBuildConfiguration;
buildSettings = {
ARCHS = "$(ARCHS_STANDARD_32_64_BIT)";
GCC_C_LANGUAGE_STANDARD = gnu99;
GCC_WARN_ABOUT_RETURN_TYPE = YES;
GCC_WARN_UNUSED_VARIABLE = YES;
PREBINDING = NO;
SDKROOT = macosx10.6;
};
name = Release;
};
/* End XCBuildConfiguration section */
/* Begin XCConfigurationList section */
1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */ = {
isa = XCConfigurationList;
buildConfigurations = (
1DEB928608733DD80010E9CD /* Debug */,
1DEB928708733DD80010E9CD /* Release */,
);
defaultConfigurationIsVisible = 0;
defaultConfigurationName = Release;
};
1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */ = {
isa = XCConfigurationList;
buildConfigurations = (
1DEB928A08733DD80010E9CD /* Debug */,
1DEB928B08733DD80010E9CD /* Release */,
);
defaultConfigurationIsVisible = 0;
defaultConfigurationName = Release;
};
/* End XCConfigurationList section */
};
rootObject = 08FB7793FE84155DC02AAC07 /* Project object */;
}
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