diff --git a/.gitignore b/.gitignore index 9f3c6c93f53d0f844eb2b14afb9e4749ac49f334..9d9d56637ade43ae979c2b0c1928a504230bb573 100644 --- a/.gitignore +++ b/.gitignore @@ -22,7 +22,6 @@ debug release qgroundcontrol mavlinkgen-build-desktop -*.wav qgroundcontrol.xcodeproj/** doc/html doc/doxy.log diff --git a/audio/alert.wav b/files/audio/alert.wav similarity index 100% rename from audio/alert.wav rename to files/audio/alert.wav diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index bb241b7228c6fa0752d825aaa75ccab80283853c..6bf00d51aab75de9eb4fe8114c01fe289fc1bd68 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -81,8 +81,6 @@ macx|macx-g++42|macx-g++: { ICON = $$BASEDIR/images/icons/macx.icns - # Copy audio files if needed - QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/qgroundcontrol.app/Contents/MacOS # Copy contributed files QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR/qgroundcontrol.app/Contents/MacOS # Copy google earth starter file @@ -274,7 +272,6 @@ message("Compiling for linux 32") } # Validated copy commands - QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR @@ -367,7 +364,6 @@ linux-g++-64 { QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/debug } DESTDIR = $$TARGETDIR/debug - QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/debug QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR/debug QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/debug QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/debug/images @@ -378,7 +374,6 @@ linux-g++-64 { QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/release } DESTDIR = $$TARGETDIR/release - QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/release QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR/release QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/release QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/release/images @@ -455,7 +450,6 @@ DEFINES += QGC_OSG_ENABLED exists($$TARGETDIR/debug) { QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll" "$$TARGETDIR_WIN\\debug"$$escape_expand(\\n)) - QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\audio" "$$TARGETDIR_WIN\\debug\\audio" /E /I $$escape_expand(\\n)) QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\files" "$$TARGETDIR_WIN\\debug\\files" /E /I $$escape_expand(\\n)) QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\models" "$$TARGETDIR_WIN\\debug\\models" /E /I $$escape_expand(\\n)) QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\images\\earth.html" "$$TARGETDIR_WIN\\debug"$$escape_expand(\\n)) @@ -476,7 +470,6 @@ DEFINES += QGC_OSG_ENABLED exists($$TARGETDIR/release) { QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll" "$$TARGETDIR_WIN\\release"$$escape_expand(\\n)) - QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\audio" "$$TARGETDIR_WIN\\release\\audio" /E /I $$escape_expand(\\n)) QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\files" "$$TARGETDIR_WIN\\release\\files" /E /I $$escape_expand(\\n)) QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\models" "$$TARGETDIR_WIN\\release\\models" /E /I $$escape_expand(\\n)) QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\images\\earth.html" "$$TARGETDIR_WIN\\release\\earth.html" $$escape_expand(\\n)) @@ -543,13 +536,13 @@ win32-g++ { message("Using cp to copy image and audio files to executable") debug { QMAKE_POST_LINK += && cp $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/debug/SDL.dll - QMAKE_POST_LINK += && cp -r $$BASEDIR/audio $$TARGETDIR/debug/audio + QMAKE_POST_LINK += && cp -r $$BASEDIR/files $$TARGETDIR/debug/files QMAKE_POST_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/debug/models } release { QMAKE_POST_LINK += && cp $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/release/SDL.dll - QMAKE_POST_LINK += && cp -r $$BASEDIR/audio $$TARGETDIR/release/audio + QMAKE_POST_LINK += && cp -r $$BASEDIR/files $$TARGETDIR/release/files QMAKE_POST_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/release/models } @@ -561,14 +554,14 @@ win32-g++ { exists($$TARGETDIR/debug) { QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll\" \"$$TARGETDIR_WIN\\debug\\SDL.dll\" - QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\audio\" \"$$TARGETDIR_WIN\\debug\\audio\\\" /S /E /Y + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\files\" \"$$TARGETDIR_WIN\\debug\\files\\\" /S /E /Y QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\models\" \"$$TARGETDIR_WIN\\debug\\models\\\" /S /E /Y QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\images\\earth.html\" \"$$TARGETDIR_WIN\\debug\\earth.html\" } exists($$TARGETDIR/release) { QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll\" \"$$TARGETDIR_WIN\\release\\SDL.dll\" - QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\audio\" \"$$TARGETDIR_WIN\\release\\audio\\\" /S /E /Y + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\files\" \"$$TARGETDIR_WIN\\release\\files\\\" /S /E /Y QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\models\" \"$$TARGETDIR_WIN\\release\\models\\\" /S /E /Y QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\images\\earth.html\" \"$$TARGETDIR_WIN\\release\\earth.html\" } diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc index b356fd4eddf78f0d8e70cdf7da7e903619192732..364ca018c5821884145eae4db3b5e39ef7f1a24f 100644 --- a/src/GAudioOutput.cc +++ b/src/GAudioOutput.cc @@ -77,7 +77,8 @@ extern "C" { GAudioOutput* GAudioOutput::instance() { static GAudioOutput* _instance = 0; - if(_instance == 0) { + if(_instance == 0) + { _instance = new GAudioOutput(); // Set the application as parent to ensure that this object // will be destroyed when the main application exits @@ -106,12 +107,16 @@ GAudioOutput::GAudioOutput(QObject* parent) : QObject(parent), #if _MSC_VER2 ISpVoice * pVoice = NULL; - if (FAILED(::CoInitialize(NULL))) { + if (FAILED(::CoInitialize(NULL))) + { qDebug("Creating COM object for audio output failed!"); - } else { + } + else + { HRESULT hr = CoCreateInstance(CLSID_SpVoice, NULL, CLSCTX_ALL, IID_ISpVoice, (void **)&pVoice;); - if( SUCCEEDED( hr ) ) { + if( SUCCEEDED( hr ) ) + { hr = pVoice->Speak(L"Hello world", 0, NULL); pVoice->Release(); pVoice = NULL; @@ -146,7 +151,8 @@ GAudioOutput::GAudioOutput(QObject* parent) : QObject(parent), void GAudioOutput::mute(bool mute) { - if (mute != muted) { + if (mute != muted) + { this->muted = mute; QSettings settings; settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", this->muted); @@ -162,11 +168,13 @@ bool GAudioOutput::isMuted() bool GAudioOutput::say(QString text, int severity) { - if (!muted) { + if (!muted) + { // TODO Add severity filter Q_UNUSED(severity); bool res = false; - if (!emergency) { + if (!emergency) + { // Speech synthesis is only supported with MSVC compiler #ifdef _MSC_VER2 @@ -202,7 +210,9 @@ bool GAudioOutput::say(QString text, int severity) #endif } return res; - } else { + } + else + { return false; } } @@ -212,22 +222,26 @@ bool GAudioOutput::say(QString text, int severity) */ bool GAudioOutput::alert(QString text) { - if (!emergency || !muted) { + if (!emergency || !muted) + { // Play alert sound beep(); // Say alert message say(text, 2); return true; - } else { + } + else + { return false; } } void GAudioOutput::notifyPositive() { - if (!muted) { + if (!muted) + { // Use QFile to transform path for all OS - QFile f(QCoreApplication::applicationDirPath()+QString("/audio/double_notify.wav")); + QFile f(QCoreApplication::applicationDirPath()+QString("/files/audio/double_notify.wav")); //m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); //m_media->play(); } @@ -235,9 +249,10 @@ void GAudioOutput::notifyPositive() void GAudioOutput::notifyNegative() { - if (!muted) { + if (!muted) + { // Use QFile to transform path for all OS - QFile f(QCoreApplication::applicationDirPath()+QString("/audio/flat_notify.wav")); + QFile f(QCoreApplication::applicationDirPath()+QString("/files/audio/flat_notify.wav")); //m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); //m_media->play(); } @@ -252,7 +267,8 @@ void GAudioOutput::notifyNegative() */ bool GAudioOutput::startEmergency() { - if (!emergency) { + if (!emergency) + { emergency = true; // Beep immediately and then start timer if (!muted) beep(); @@ -279,9 +295,10 @@ bool GAudioOutput::stopEmergency() void GAudioOutput::beep() { - if (!muted) { + if (!muted) + { // Use QFile to transform path for all OS - QFile f(QCoreApplication::applicationDirPath()+QString("/audio/alert.wav")); + QFile f(QCoreApplication::applicationDirPath()+QString("/files/audio/alert.wav")); qDebug() << "FILE:" << f.fileName(); //m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); //m_media->play(); diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 27056fbf060447bc711fe80aa0aaabecc910942c..d5546cf1b6a909a98ef8980c3fc7b228179cb1f4 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -678,8 +678,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) int bufferlength = 0; // Output all bytes as hex digits - int i; - for (i=0; iid, data[i], &msg, &comm)) { @@ -687,16 +686,19 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) qDebug() << "SIMULATION LINK RECEIVED MESSAGE!"; emit messageReceived(msg); - switch (msg.msgid) { + switch (msg.msgid) + { // SET THE SYSTEM MODE - case MAVLINK_MSG_ID_SET_MODE: { + case MAVLINK_MSG_ID_SET_MODE: + { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(&msg, &mode); // Set mode indepent of mode.target system.base_mode = mode.base_mode; } break; - case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: { + case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: + { mavlink_set_local_position_setpoint_t set; mavlink_msg_set_local_position_setpoint_decode(&msg, &set); spX = set.x; @@ -714,7 +716,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } break; // EXECUTE OPERATOR ACTIONS - case MAVLINK_MSG_ID_COMMAND_LONG: { + case MAVLINK_MSG_ID_COMMAND_LONG: + { mavlink_command_long_t action; mavlink_msg_command_long_decode(&msg, &action); @@ -756,44 +759,48 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } break; #endif - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + { 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) -// { - // Output all params - // Iterate through all components, through all parameters and emit them - QMap::iterator i; - // Iterate through all components / subsystems - int j = 0; - for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { - if (j != 5) { - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), MAVLINK_TYPE_FLOAT, onboardParams.size(), j); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; + if (read.target_system == systemId) + { + // Output all params + // Iterate through all components, through all parameters and emit them + QMap::iterator i; + // Iterate through all components / subsystems + int j = 0; + for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { + if (j != 5) { + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), MAVLINK_TYPE_FLOAT, onboardParams.size(), j); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer+=bufferlength; + } + j++; } - j++; - } - qDebug() << "SIMULATION SENT PARAMETERS TO GCS"; -// } + qDebug() << "SIMULATION SENT PARAMETERS TO GCS"; + } } - break; - case MAVLINK_MSG_ID_PARAM_SET: { + break; + case MAVLINK_MSG_ID_PARAM_SET: + { // Drop on even milliseconds - if (QGC::groundTimeMilliseconds() % 2 == 0) { + if (QGC::groundTimeMilliseconds() % 2 == 0) + { qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER"; mavlink_param_set_t set; mavlink_msg_param_set_decode(&msg, &set); // if (set.target_system == systemId) // { QString key = QString((char*)set.param_id); - if (onboardParams.contains(key)) { + if (onboardParams.contains(key)) + { onboardParams.remove(key); onboardParams.insert(key, set.param_value); @@ -809,13 +816,15 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } } break; - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: + { qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER"; mavlink_param_request_read_t read; mavlink_msg_param_request_read_decode(&msg, &read); QByteArray bytes((char*)read.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN); QString key = QString(bytes); - if (onboardParams.contains(key)) { + if (onboardParams.contains(key)) + { float paramValue = onboardParams.value(key); // Pack message and get size of encoded byte string @@ -826,7 +835,9 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) memcpy(stream+streampointer,buffer, bufferlength); streampointer+=bufferlength; //qDebug() << "Sending PARAM" << key; - } else if (read.param_index < onboardParams.size()) { + } + else if (read.param_index < onboardParams.size()) + { key = onboardParams.keys().at(read.param_index); float paramValue = onboardParams.value(key); @@ -842,8 +853,6 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } break; } - - } unsigned char v=data[i]; fprintf(stderr,"%02x ", v); @@ -851,7 +860,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) fprintf(stderr,"\n"); readyBufferMutex.lock(); - for (int i = 0; i < streampointer; i++) { + for (int i = 0; i < streampointer; i++) + { readyBuffer.enqueue(*(stream + i)); } readyBufferMutex.unlock(); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 25749d956fee009ec650e437b37617fcdc8873e1..48c38a1a5826c8d3927744a06c32649b641854ab 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -75,12 +75,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), pitch(0.0), yaw(0.0), statusTimeout(new QTimer(this)), -#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) + #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) receivedPointCloudTimestamp(0.0), receivedRGBDImageTimestamp(0.0), receivedObstacleListTimestamp(0.0), receivedPathTimestamp(0.0), -#endif + #endif paramsOnceRequested(false), airframe(QGC_AIRFRAME_EASYSTAR), attitudeKnown(false), @@ -202,25 +202,25 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) switch (message.compid) { case MAV_COMP_ID_ALL: - { - componentName = "ANONYMOUS"; - break; - } + { + componentName = "ANONYMOUS"; + break; + } case MAV_COMP_ID_IMU: - { - componentName = "IMU #1"; - break; - } + { + componentName = "IMU #1"; + break; + } case MAV_COMP_ID_CAMERA: - { - componentName = "CAMERA"; - break; - } + { + componentName = "CAMERA"; + break; + } case MAV_COMP_ID_MISSIONPLANNER: - { - componentName = "MISSIONPLANNER"; - break; - } + { + componentName = "MISSIONPLANNER"; + break; + } } components.insert(message.compid, componentName); @@ -274,6 +274,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { case MAVLINK_MSG_ID_HEARTBEAT: { + if (multiComponentSourceDetected && wrongComponent) + { + break; + } lastHeartbeat = QGC::groundTimeUsecs(); emit heartbeat(this); mavlink_heartbeat_t state; @@ -381,338 +385,338 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) GAudioOutput::instance()->say(audiostring.toLower()); } -// if (state.system_status == MAV_STATE_POWEROFF) -// { -// emit systemRemoved(this); -// emit systemRemoved(); -// } -} + // if (state.system_status == MAV_STATE_POWEROFF) + // { + // emit systemRemoved(this); + // emit systemRemoved(); + // } + } break; -// case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: -// case MAVLINK_MSG_ID_NAMED_VALUE_INT: -// // Receive named value message -// receiveMessageNamedValue(message); -// break; + // case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + // case MAVLINK_MSG_ID_NAMED_VALUE_INT: + // // Receive named value message + // receiveMessageNamedValue(message); + // break; case MAVLINK_MSG_ID_SYS_STATUS: { - if (multiComponentSourceDetected && message.compid != MAV_COMP_ID_IMU_2) - { - break; - } - mavlink_sys_status_t state; - mavlink_msg_sys_status_decode(&message, &state); - - emit loadChanged(this,state.load/10.0f); + if (multiComponentSourceDetected && wrongComponent) + { + break; + } + mavlink_sys_status_t state; + mavlink_msg_sys_status_decode(&message, &state); - currentVoltage = state.voltage_battery/1000.0f; - lpVoltage = filterVoltage(currentVoltage); + emit loadChanged(this,state.load/10.0f); - if (startVoltage == 0) startVoltage = currentVoltage; - timeRemaining = calculateTimeRemaining(); - if (!batteryRemainingEstimateEnabled && chargeLevel != -1) - { - chargeLevel = state.battery_remaining; - } - //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining; - emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining); - emit voltageChanged(message.sysid, state.voltage_battery/1000); + currentVoltage = state.voltage_battery/1000.0f; + lpVoltage = filterVoltage(currentVoltage); - // LOW BATTERY ALARM - if (lpVoltage < warnVoltage) - { - startLowBattAlarm(); - } - else - { - stopLowBattAlarm(); - } + if (startVoltage == 0) startVoltage = currentVoltage; + timeRemaining = calculateTimeRemaining(); + if (!batteryRemainingEstimateEnabled && chargeLevel != -1) + { + chargeLevel = state.battery_remaining; + } + //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining; + emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining); + emit voltageChanged(message.sysid, state.voltage_battery/1000); - // COMMUNICATIONS DROP RATE - // FIXME - emit dropRateChanged(this->getUASID(), state.drop_rate_comm/10000.0f); + // LOW BATTERY ALARM + if (lpVoltage < warnVoltage) + { + startLowBattAlarm(); } - break; - case MAVLINK_MSG_ID_ATTITUDE: + else { - if (wrongComponent) break; - - mavlink_attitude_t attitude; - mavlink_msg_attitude_decode(&message, &attitude); - quint64 time = getUnixReferenceTime(attitude.time_boot_ms); - lastAttitude = time; - roll = QGC::limitAngleToPMPIf(attitude.roll); - 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; -// } -// } -// else -// { -// // Set to 0, since it is an invalid value -// compass = 0.0f; -// } - - attitudeKnown = true; - emit attitudeChanged(this, roll, pitch, yaw, time); - emit attitudeChanged(this, message.compid, roll, pitch, yaw, time); - emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + stopLowBattAlarm(); } + + // COMMUNICATIONS DROP RATE + // FIXME + emit dropRateChanged(this->getUASID(), state.drop_rate_comm/10000.0f); + } + break; + case MAVLINK_MSG_ID_ATTITUDE: + { + if (wrongComponent) break; + + mavlink_attitude_t attitude; + mavlink_msg_attitude_decode(&message, &attitude); + quint64 time = getUnixReferenceTime(attitude.time_boot_ms); + lastAttitude = time; + roll = QGC::limitAngleToPMPIf(attitude.roll); + 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; + // } + // } + // else + // { + // // Set to 0, since it is an invalid value + // compass = 0.0f; + // } + + attitudeKnown = true; + emit attitudeChanged(this, roll, pitch, yaw, time); + emit attitudeChanged(this, message.compid, roll, pitch, yaw, time); + emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + } break; case MAVLINK_MSG_ID_HIL_CONTROLS: - { - mavlink_hil_controls_t hil; - mavlink_msg_hil_controls_decode(&message, &hil); - emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); - } + { + mavlink_hil_controls_t hil; + mavlink_msg_hil_controls_decode(&message, &hil); + emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); + } break; case MAVLINK_MSG_ID_VFR_HUD: - { - mavlink_vfr_hud_t hud; - mavlink_msg_vfr_hud_decode(&message, &hud); - quint64 time = getUnixTime(); - // Display updated values - emit thrustChanged(this, hud.throttle/100.0); - - if (!attitudeKnown) - { - yaw = QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI); - emit attitudeChanged(this, roll, pitch, yaw, time); - } + { + mavlink_vfr_hud_t hud; + mavlink_msg_vfr_hud_decode(&message, &hud); + quint64 time = getUnixTime(); + // Display updated values + emit thrustChanged(this, hud.throttle/100.0); - emit altitudeChanged(uasId, hud.alt); - emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time); + if (!attitudeKnown) + { + yaw = QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI); + emit attitudeChanged(this, roll, pitch, yaw, time); } + + emit altitudeChanged(uasId, hud.alt); + emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time); + } break; case MAVLINK_MSG_ID_LOCAL_POSITION_NED: //std::cerr << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; - { - mavlink_local_position_ned_t pos; - mavlink_msg_local_position_ned_decode(&message, &pos); - quint64 time = getUnixTime(pos.time_boot_ms); + { + mavlink_local_position_ned_t pos; + mavlink_msg_local_position_ned_decode(&message, &pos); + quint64 time = getUnixTime(pos.time_boot_ms); - // Emit position always with component ID - emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); + // Emit position always with component ID + emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); - if (!wrongComponent) - { + if (!wrongComponent) + { - localX = pos.x; - localY = pos.y; - localZ = pos.z; + localX = pos.x; + localY = pos.y; + localZ = pos.z; - // Emit + // Emit - emit localPositionChanged(this, pos.x, pos.y, pos.z, time); - emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); + emit localPositionChanged(this, pos.x, pos.y, pos.z, time); + emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); - // Set internal state - if (!positionLock) { - // If position was not locked before, notify positive - GAudioOutput::instance()->notifyPositive(); - } - positionLock = true; - isLocalPositionKnown = true; + // Set internal state + if (!positionLock) { + // If position was not locked before, notify positive + GAudioOutput::instance()->notifyPositive(); } + positionLock = true; + isLocalPositionKnown = true; } + } break; case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: - { - mavlink_global_vision_position_estimate_t pos; - mavlink_msg_global_vision_position_estimate_decode(&message, &pos); - quint64 time = getUnixTime(pos.usec); - emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); - emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time); - } + { + mavlink_global_vision_position_estimate_t pos; + mavlink_msg_global_vision_position_estimate_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); + emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time); + } break; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: //std::cerr << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; + { + mavlink_global_position_int_t pos; + mavlink_msg_global_position_int_decode(&message, &pos); + quint64 time = getUnixTime(); + latitude = pos.lat/(double)1E7; + longitude = pos.lon/(double)1E7; + altitude = pos.alt/1000.0; + speedX = pos.vx/100.0; + speedY = pos.vy/100.0; + speedZ = pos.vz/100.0; + emit globalPositionChanged(this, latitude, longitude, altitude, time); + emit speedChanged(this, speedX, speedY, speedZ, time); + // Set internal state + if (!positionLock) + { + // If position was not locked before, notify positive + GAudioOutput::instance()->notifyPositive(); + } + positionLock = true; + isGlobalPositionKnown = true; + //TODO fix this hack for forwarding of global position for patch antenna tracking + forwardMessage(message); + } + break; + case MAVLINK_MSG_ID_GPS_RAW_INT: + { + mavlink_gps_raw_int_t pos; + mavlink_msg_gps_raw_int_decode(&message, &pos); + + // SANITY CHECK + // only accept values in a realistic range + // quint64 time = getUnixTime(pos.time_usec); + quint64 time = getUnixTime(pos.time_usec); + + if (pos.fix_type > 2) { - mavlink_global_position_int_t pos; - mavlink_msg_global_position_int_decode(&message, &pos); - quint64 time = getUnixTime(); + emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time); latitude = pos.lat/(double)1E7; longitude = pos.lon/(double)1E7; altitude = pos.alt/1000.0; - speedX = pos.vx/100.0; - speedY = pos.vy/100.0; - speedZ = pos.vz/100.0; - emit globalPositionChanged(this, latitude, longitude, altitude, time); - emit speedChanged(this, speedX, speedY, speedZ, time); - // Set internal state - if (!positionLock) - { - // If position was not locked before, notify positive - GAudioOutput::instance()->notifyPositive(); - } positionLock = true; isGlobalPositionKnown = true; - //TODO fix this hack for forwarding of global position for patch antenna tracking - forwardMessage(message); - } - break; - case MAVLINK_MSG_ID_GPS_RAW_INT: - { - mavlink_gps_raw_int_t pos; - mavlink_msg_gps_raw_int_decode(&message, &pos); - // SANITY CHECK - // only accept values in a realistic range - // quint64 time = getUnixTime(pos.time_usec); - quint64 time = getUnixTime(pos.time_usec); - - if (pos.fix_type > 2) + // Check for NaN + int alt = pos.alt; + if (!isnan(alt) && !isinf(alt)) { - emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time); - latitude = pos.lat/(double)1E7; - longitude = pos.lon/(double)1E7; - altitude = pos.alt/1000.0; - positionLock = true; - isGlobalPositionKnown = true; - - // Check for NaN - int alt = pos.alt; - if (!isnan(alt) && !isinf(alt)) - { - alt = 0; - //emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); - } - // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time); - // Smaller than threshold and not NaN + alt = 0; + //emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); + } + // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time); + // Smaller than threshold and not NaN - float vel = pos.vel/100.0f; + float vel = pos.vel/100.0f; - if (vel < 1000000 && !isnan(vel) && !isinf(vel)) - { - // FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time); - //qDebug() << "GOT GPS RAW"; - // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); - } - else - { - emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); - } + if (vel < 1000000 && !isnan(vel) && !isinf(vel)) + { + // FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time); + //qDebug() << "GOT GPS RAW"; + // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); + } + else + { + emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); } } + } break; case MAVLINK_MSG_ID_GPS_STATUS: + { + mavlink_gps_status_t pos; + mavlink_msg_gps_status_decode(&message, &pos); + for(int i = 0; i < (int)pos.satellites_visible; i++) { - mavlink_gps_status_t pos; - mavlink_msg_gps_status_decode(&message, &pos); - for(int i = 0; i < (int)pos.satellites_visible; i++) - { - emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast(pos.satellite_used[i])); - } + emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast(pos.satellite_used[i])); } + } break; case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: - { - mavlink_gps_global_origin_t pos; - mavlink_msg_gps_global_origin_decode(&message, &pos); - emit homePositionChanged(uasId, pos.latitude, pos.longitude, pos.altitude); - } + { + mavlink_gps_global_origin_t pos; + mavlink_msg_gps_global_origin_decode(&message, &pos); + emit homePositionChanged(uasId, pos.latitude, pos.longitude, pos.altitude); + } break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: - { - mavlink_rc_channels_raw_t channels; - mavlink_msg_rc_channels_raw_decode(&message, &channels); - emit remoteControlRSSIChanged(channels.rssi/255.0f); - emit remoteControlChannelRawChanged(0, channels.chan1_raw); - emit remoteControlChannelRawChanged(1, channels.chan2_raw); - emit remoteControlChannelRawChanged(2, channels.chan3_raw); - emit remoteControlChannelRawChanged(3, channels.chan4_raw); - emit remoteControlChannelRawChanged(4, channels.chan5_raw); - emit remoteControlChannelRawChanged(5, channels.chan6_raw); - emit remoteControlChannelRawChanged(6, channels.chan7_raw); - emit remoteControlChannelRawChanged(7, channels.chan8_raw); - } + { + mavlink_rc_channels_raw_t channels; + mavlink_msg_rc_channels_raw_decode(&message, &channels); + emit remoteControlRSSIChanged(channels.rssi/255.0f); + emit remoteControlChannelRawChanged(0, channels.chan1_raw); + emit remoteControlChannelRawChanged(1, channels.chan2_raw); + emit remoteControlChannelRawChanged(2, channels.chan3_raw); + emit remoteControlChannelRawChanged(3, channels.chan4_raw); + emit remoteControlChannelRawChanged(4, channels.chan5_raw); + emit remoteControlChannelRawChanged(5, channels.chan6_raw); + emit remoteControlChannelRawChanged(6, channels.chan7_raw); + emit remoteControlChannelRawChanged(7, channels.chan8_raw); + } break; case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: - { - mavlink_rc_channels_scaled_t channels; - mavlink_msg_rc_channels_scaled_decode(&message, &channels); - emit remoteControlRSSIChanged(channels.rssi/255.0f); - emit remoteControlChannelScaledChanged(0, channels.chan1_scaled/10000.0f); - emit remoteControlChannelScaledChanged(1, channels.chan2_scaled/10000.0f); - emit remoteControlChannelScaledChanged(2, channels.chan3_scaled/10000.0f); - emit remoteControlChannelScaledChanged(3, channels.chan4_scaled/10000.0f); - emit remoteControlChannelScaledChanged(4, channels.chan5_scaled/10000.0f); - emit remoteControlChannelScaledChanged(5, channels.chan6_scaled/10000.0f); - emit remoteControlChannelScaledChanged(6, channels.chan7_scaled/10000.0f); - emit remoteControlChannelScaledChanged(7, channels.chan8_scaled/10000.0f); - } + { + mavlink_rc_channels_scaled_t channels; + mavlink_msg_rc_channels_scaled_decode(&message, &channels); + emit remoteControlRSSIChanged(channels.rssi/255.0f); + emit remoteControlChannelScaledChanged(0, channels.chan1_scaled/10000.0f); + emit remoteControlChannelScaledChanged(1, channels.chan2_scaled/10000.0f); + emit remoteControlChannelScaledChanged(2, channels.chan3_scaled/10000.0f); + emit remoteControlChannelScaledChanged(3, channels.chan4_scaled/10000.0f); + emit remoteControlChannelScaledChanged(4, channels.chan5_scaled/10000.0f); + emit remoteControlChannelScaledChanged(5, channels.chan6_scaled/10000.0f); + emit remoteControlChannelScaledChanged(6, channels.chan7_scaled/10000.0f); + emit remoteControlChannelScaledChanged(7, channels.chan8_scaled/10000.0f); + } break; case MAVLINK_MSG_ID_PARAM_VALUE: + { + mavlink_param_value_t value; + mavlink_msg_param_value_decode(&message, &value); + QByteArray bytes(value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QString parameterName = QString(bytes); + int component = message.compid; + mavlink_param_union_t val; + val.param_float = value.param_value; + val.type = value.param_type; + + // Insert component if necessary + if (!parameters.contains(component)) { - mavlink_param_value_t value; - mavlink_msg_param_value_decode(&message, &value); - QByteArray bytes(value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - QString parameterName = QString(bytes); - int component = message.compid; - mavlink_param_union_t val; - val.param_float = value.param_value; - val.type = value.param_type; - - // Insert component if necessary - if (!parameters.contains(component)) - { - parameters.insert(component, new QMap()); - } + parameters.insert(component, new QMap()); + } - // Insert parameter into registry - if (parameters.value(component)->contains(parameterName)) parameters.value(component)->remove(parameterName); + // Insert parameter into registry + if (parameters.value(component)->contains(parameterName)) parameters.value(component)->remove(parameterName); - // Insert with correct type - switch (value.param_type) - { - case MAVLINK_TYPE_FLOAT: - { - // Variant - QVariant param(val.param_float); - parameters.value(component)->insert(parameterName, param); - // Emit change - emit parameterChanged(uasId, message.compid, parameterName, param); - emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); - qDebug() << "RECEIVED PARAM:" << param; - } - break; - case MAVLINK_TYPE_UINT32_T: - { - // Variant - QVariant param(val.param_uint32); - parameters.value(component)->insert(parameterName, param); - // Emit change - emit parameterChanged(uasId, message.compid, parameterName, param); - emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); - qDebug() << "RECEIVED PARAM:" << param; - } - break; - case MAVLINK_TYPE_INT32_T: - { - // Variant - QVariant param(val.param_int32); - parameters.value(component)->insert(parameterName, param); - // Emit change - emit parameterChanged(uasId, message.compid, parameterName, param); - emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); - qDebug() << "RECEIVED PARAM:" << param; - } - break; - default: - qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << value.param_type; - } + // Insert with correct type + switch (value.param_type) + { + case MAVLINK_TYPE_FLOAT: + { + // Variant + QVariant param(val.param_float); + parameters.value(component)->insert(parameterName, param); + // Emit change + emit parameterChanged(uasId, message.compid, parameterName, param); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); + qDebug() << "RECEIVED PARAM:" << param; } + break; + case MAVLINK_TYPE_UINT32_T: + { + // Variant + QVariant param(val.param_uint32); + parameters.value(component)->insert(parameterName, param); + // Emit change + emit parameterChanged(uasId, message.compid, parameterName, param); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); + qDebug() << "RECEIVED PARAM:" << param; + } + break; + case MAVLINK_TYPE_INT32_T: + { + // Variant + QVariant param(val.param_int32); + parameters.value(component)->insert(parameterName, param); + // Emit change + emit parameterChanged(uasId, message.compid, parameterName, param); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); + qDebug() << "RECEIVED PARAM:" << param; + } + break; + default: + qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << value.param_type; + } + } break; case MAVLINK_MSG_ID_COMMAND_ACK: mavlink_command_ack_t ack; @@ -727,268 +731,268 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; case MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT: - { - mavlink_roll_pitch_yaw_thrust_setpoint_t out; - mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out); - quint64 time = getUnixTimeFromMs(out.time_boot_ms); - emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time); - } + { + mavlink_roll_pitch_yaw_thrust_setpoint_t out; + mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out); + quint64 time = getUnixTimeFromMs(out.time_boot_ms); + emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time); + } break; case MAVLINK_MSG_ID_MISSION_COUNT: + { + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(&message, &wpc); + if (wpc.target_system == mavlink->getSystemId()) { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(&message, &wpc); - if (wpc.target_system == mavlink->getSystemId()) - { - waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count); - } - else - { - qDebug() << "Got waypoint message, but was not for me"; - } + waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count); } + else + { + qDebug() << "Got waypoint message, but was not for me"; + } + } break; case MAVLINK_MSG_ID_MISSION_ITEM: + { + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(&message, &wp); + //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z; + if(wp.target_system == mavlink->getSystemId()) { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(&message, &wp); - //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z; - if(wp.target_system == mavlink->getSystemId()) - { - waypointManager.handleWaypoint(message.sysid, message.compid, &wp); - } - else - { - qDebug() << "Got waypoint message, but was not for me"; - } + waypointManager.handleWaypoint(message.sysid, message.compid, &wp); + } + else + { + qDebug() << "Got waypoint message, but was not for me"; } + } break; case MAVLINK_MSG_ID_MISSION_ACK: + { + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(&message, &wpa); + if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(&message, &wpa); - if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) - { - waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa); - } + waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa); } + } break; case MAVLINK_MSG_ID_MISSION_REQUEST: + { + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(&message, &wpr); + if(wpr.target_system == mavlink->getSystemId()) { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(&message, &wpr); - if(wpr.target_system == mavlink->getSystemId()) - { - waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr); - } - else - { - qDebug() << "Got waypoint message, but was not for me"; - } + waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr); + } + else + { + qDebug() << "Got waypoint message, but was not for me"; } + } break; case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: - { - mavlink_mission_item_reached_t wpr; - mavlink_msg_mission_item_reached_decode(&message, &wpr); - waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr); - QString text = QString("System %1 reached waypoint %2").arg(getUASName()).arg(wpr.seq); - GAudioOutput::instance()->say(text); - emit textMessageReceived(message.sysid, message.compid, 0, text); - } + { + mavlink_mission_item_reached_t wpr; + mavlink_msg_mission_item_reached_decode(&message, &wpr); + waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr); + QString text = QString("System %1 reached waypoint %2").arg(getUASName()).arg(wpr.seq); + GAudioOutput::instance()->say(text); + emit textMessageReceived(message.sysid, message.compid, 0, text); + } break; case MAVLINK_MSG_ID_MISSION_CURRENT: - { - mavlink_mission_current_t wpc; - mavlink_msg_mission_current_decode(&message, &wpc); - waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc); - } + { + mavlink_mission_current_t wpc; + mavlink_msg_mission_current_decode(&message, &wpc); + waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc); + } break; case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT: - { - mavlink_local_position_setpoint_t p; - mavlink_msg_local_position_setpoint_decode(&message, &p); - emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs()); - } + { + mavlink_local_position_setpoint_t p; + mavlink_msg_local_position_setpoint_decode(&message, &p); + emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs()); + } break; case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: - { - mavlink_set_local_position_setpoint_t p; - mavlink_msg_set_local_position_setpoint_decode(&message, &p); - emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw); - } + { + mavlink_set_local_position_setpoint_t p; + mavlink_msg_set_local_position_setpoint_decode(&message, &p); + emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw); + } break; case MAVLINK_MSG_ID_STATUSTEXT: - { - QByteArray b; - b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); - mavlink_msg_statustext_get_text(&message, b.data()); - //b.append('\0'); - QString text = QString(b); - int severity = mavlink_msg_statustext_get_severity(&message); - //qDebug() << "RECEIVED STATUS:" << text;false - //emit statusTextReceived(severity, text); - emit textMessageReceived(uasId, message.compid, severity, text); - } + { + QByteArray b; + b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); + mavlink_msg_statustext_get_text(&message, b.data()); + //b.append('\0'); + QString text = QString(b); + int severity = mavlink_msg_statustext_get_severity(&message); + //qDebug() << "RECEIVED STATUS:" << text;false + //emit statusTextReceived(severity, text); + emit textMessageReceived(uasId, message.compid, severity, text); + } break; #ifdef MAVLINK_ENABLED_PIXHAWK case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: - { - qDebug() << "RECIEVED ACK TO GET IMAGE"; - mavlink_data_transmission_handshake_t p; - mavlink_msg_data_transmission_handshake_decode(&message, &p); - imageSize = p.size; - imagePackets = p.packets; - imagePayload = p.payload; - imageQuality = p.jpg_quality; - imageType = p.type; - imageWidth = p.width; - imageHeight = p.height; - imageStart = QGC::groundTimeMilliseconds(); - } + { + qDebug() << "RECIEVED ACK TO GET IMAGE"; + mavlink_data_transmission_handshake_t p; + mavlink_msg_data_transmission_handshake_decode(&message, &p); + imageSize = p.size; + imagePackets = p.packets; + imagePayload = p.payload; + imageQuality = p.jpg_quality; + imageType = p.type; + imageWidth = p.width; + imageHeight = p.height; + imageStart = QGC::groundTimeMilliseconds(); + } break; case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - { - mavlink_encapsulated_data_t img; - mavlink_msg_encapsulated_data_decode(&message, &img); - int seq = img.seqnr; - int pos = seq * imagePayload; + { + mavlink_encapsulated_data_t img; + mavlink_msg_encapsulated_data_decode(&message, &img); + int seq = img.seqnr; + int pos = seq * imagePayload; - // Check if we have a valid transaction - if (imagePackets == 0) - { - // NO VALID TRANSACTION - ABORT - // Restart statemachine - imagePacketsArrived = 0; - } + // Check if we have a valid transaction + if (imagePackets == 0) + { + // NO VALID TRANSACTION - ABORT + // Restart statemachine + imagePacketsArrived = 0; + } - for (int i = 0; i < imagePayload; ++i) - { - if (pos <= imageSize) { - imageRecBuffer[pos] = img.data[i]; - } - ++pos; + for (int i = 0; i < imagePayload; ++i) + { + if (pos <= imageSize) { + imageRecBuffer[pos] = img.data[i]; } + ++pos; + } - ++imagePacketsArrived; + ++imagePacketsArrived; - // emit signal if all packets arrived - if ((imagePacketsArrived >= imagePackets)) - { - // Restart statemachine - imagePacketsArrived = 0; - emit imageReady(this); - qDebug() << "imageReady emitted. all packets arrived"; - } + // emit signal if all packets arrived + if ((imagePacketsArrived >= imagePackets)) + { + // Restart statemachine + imagePacketsArrived = 0; + emit imageReady(this); + qDebug() << "imageReady emitted. all packets arrived"; } + } break; #endif -// case MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT: -// { -// mavlink_object_detection_event_t event; -// mavlink_msg_object_detection_event_decode(&message, &event); -// QString str(event.name); -// emit objectDetected(event.time, event.object_id, event.type, str, event.quality, event.bearing, event.distance); -// } -// break; - // WILL BE ENABLED ONCE MESSAGE IS IN COMMON MESSAGE SET -// case MAVLINK_MSG_ID_MEMORY_VECT: -// { -// mavlink_memory_vect_t vect; -// mavlink_msg_memory_vect_decode(&message, &vect); -// QString str("mem_%1"); -// quint64 time = getUnixTime(0); -// int16_t *mem0 = (int16_t *)&vect.value[0]; -// uint16_t *mem1 = (uint16_t *)&vect.value[0]; -// int32_t *mem2 = (int32_t *)&vect.value[0]; -// // uint32_t *mem3 = (uint32_t *)&vect.value[0]; causes overload problem -// float *mem4 = (float *)&vect.value[0]; -// if ( vect.ver == 0) vect.type = 0, vect.ver = 1; else ; -// if ( vect.ver == 1) -// { -// switch (vect.type) { -// default: -// case 0: -// for (int i = 0; i < 16; i++) -// // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "i16", mem0[i], time); -// break; -// case 1: -// for (int i = 0; i < 16; i++) -// // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "ui16", mem1[i], time); -// break; -// case 2: -// for (int i = 0; i < 16; i++) -// // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "Q15", (float)mem0[i]/32767.0, time); -// break; -// case 3: -// for (int i = 0; i < 16; i++) -// // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "1Q14", (float)mem0[i]/16383.0, time); -// break; -// case 4: -// for (int i = 0; i < 8; i++) -// // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time); -// break; -// case 5: -// for (int i = 0; i < 8; i++) -// // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time); -// break; -// case 6: -// for (int i = 0; i < 8; i++) -// // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "float", mem4[i], time); -// break; -// } -// } -// } -// break; + // case MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT: + // { + // mavlink_object_detection_event_t event; + // mavlink_msg_object_detection_event_decode(&message, &event); + // QString str(event.name); + // emit objectDetected(event.time, event.object_id, event.type, str, event.quality, event.bearing, event.distance); + // } + // break; + // WILL BE ENABLED ONCE MESSAGE IS IN COMMON MESSAGE SET + // case MAVLINK_MSG_ID_MEMORY_VECT: + // { + // mavlink_memory_vect_t vect; + // mavlink_msg_memory_vect_decode(&message, &vect); + // QString str("mem_%1"); + // quint64 time = getUnixTime(0); + // int16_t *mem0 = (int16_t *)&vect.value[0]; + // uint16_t *mem1 = (uint16_t *)&vect.value[0]; + // int32_t *mem2 = (int32_t *)&vect.value[0]; + // // uint32_t *mem3 = (uint32_t *)&vect.value[0]; causes overload problem + // float *mem4 = (float *)&vect.value[0]; + // if ( vect.ver == 0) vect.type = 0, vect.ver = 1; else ; + // if ( vect.ver == 1) + // { + // switch (vect.type) { + // default: + // case 0: + // for (int i = 0; i < 16; i++) + // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "i16", mem0[i], time); + // break; + // case 1: + // for (int i = 0; i < 16; i++) + // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "ui16", mem1[i], time); + // break; + // case 2: + // for (int i = 0; i < 16; i++) + // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "Q15", (float)mem0[i]/32767.0, time); + // break; + // case 3: + // for (int i = 0; i < 16; i++) + // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "1Q14", (float)mem0[i]/16383.0, time); + // break; + // case 4: + // for (int i = 0; i < 8; i++) + // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time); + // break; + // case 5: + // for (int i = 0; i < 8; i++) + // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time); + // break; + // case 6: + // for (int i = 0; i < 8; i++) + // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "float", mem4[i], time); + // break; + // } + // } + // } + // break; #ifdef MAVLINK_ENABLED_UALBERTA case MAVLINK_MSG_ID_NAV_FILTER_BIAS: - { - mavlink_nav_filter_bias_t bias; - mavlink_msg_nav_filter_bias_decode(&message, &bias); - quint64 time = getUnixTime(); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time); - } + { + mavlink_nav_filter_bias_t bias; + mavlink_msg_nav_filter_bias_decode(&message, &bias); + quint64 time = getUnixTime(); + // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time); + // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time); + // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time); + // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time); + // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time); + // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time); + } break; case MAVLINK_MSG_ID_RADIO_CALIBRATION: - { - mavlink_radio_calibration_t radioMsg; - mavlink_msg_radio_calibration_decode(&message, &radioMsg); - QVector aileron; - QVector elevator; - QVector rudder; - QVector gyro; - QVector pitch; - QVector throttle; - - for (int i=0; i radioData = new RadioCalibrationData(aileron, elevator, rudder, gyro, pitch, throttle); - emit radioCalibrationReceived(radioData); - delete radioData; - } + { + mavlink_radio_calibration_t radioMsg; + mavlink_msg_radio_calibration_decode(&message, &radioMsg); + QVector aileron; + QVector elevator; + QVector rudder; + QVector gyro; + QVector pitch; + QVector throttle; + + for (int i=0; i radioData = new RadioCalibrationData(aileron, elevator, rudder, gyro, pitch, throttle); + emit radioCalibrationReceived(radioData); + delete radioData; + } break; #endif @@ -1006,17 +1010,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_NAMED_VALUE_INT: break; default: + { + if (!unknownPackets.contains(message.msgid)) { - if (!unknownPackets.contains(message.msgid)) - { - unknownPackets.append(message.msgid); - QString errString = tr("UNABLE TO DECODE MESSAGE NUMBER %1").arg(message.msgid); - GAudioOutput::instance()->say(errString+tr(", please check console for details.")); - emit textMessageReceived(uasId, message.compid, 255, errString); - std::cout << "Unable to decode message from system " << std::dec << static_cast(message.sysid) << " with message id:" << static_cast(message.msgid) << std::endl; - //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast(message.acid) << " with message id:" << static_cast(message.msgid) << std::endl; - } + unknownPackets.append(message.msgid); + QString errString = tr("UNABLE TO DECODE MESSAGE NUMBER %1").arg(message.msgid); + GAudioOutput::instance()->say(errString+tr(", please check console for details.")); + emit textMessageReceived(uasId, message.compid, 255, errString); + std::cout << "Unable to decode message from system " << std::dec << static_cast(message.sysid) << " with message id:" << static_cast(message.msgid) << std::endl; + //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast(message.acid) << " with message id:" << static_cast(message.msgid) << std::endl; } + } break; } } @@ -1127,10 +1131,6 @@ void UAS::setHomePosition(double lat, double lon, double alt) mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 0, 0, 0, 0, lat, lon, alt); // Send message twice to increase chance that it reaches its goal sendMessage(msg); - // Wait 15 ms - QGC::SLEEP::usleep(15000); - // Send again - sendMessage(msg); // Send new home position to UAS mavlink_set_gps_global_origin_t home; @@ -1164,10 +1164,6 @@ void UAS::setLocalOriginAtCurrentGPSPosition() mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 1, 0, 0, 0, 0, 0, 0); // Send message twice to increase chance that it reaches its goal sendMessage(msg); - // Wait 15 ms - QGC::SLEEP::usleep(15000); - // Send again - sendMessage(msg); } } @@ -1272,9 +1268,9 @@ quint64 UAS::getUnixReferenceTime(quint64 time) #ifndef _MSC_VER else if (time < 1261440000000000LLU) #else - else if (time < 1261440000000000) + else if (time < 1261440000000000) #endif - { + { // qDebug() << "GEN time:" << time/1000 + onboardTimeOffset; if (onboardTimeOffset == 0) { @@ -1341,9 +1337,9 @@ quint64 UAS::getUnixTime(quint64 time) #ifndef _MSC_VER else if (time < 1261440000000000LLU) #else - else if (time < 1261440000000000) + else if (time < 1261440000000000) #endif - { + { // qDebug() << "GEN time:" << time/1000 + onboardTimeOffset; if (onboardTimeOffset == 0) { @@ -1457,14 +1453,14 @@ QString UAS::getNavModeText(int mode) { if (autopilot == MAV_AUTOPILOT_PIXHAWK) { - switch (mode) - { - case 0: - return QString("PREFLIGHT"); - break; - default: - return QString("UNKNOWN"); - } + switch (mode) + { + case 0: + return QString("PREFLIGHT"); + break; + default: + return QString("UNKNOWN"); + } } else if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) { @@ -1568,9 +1564,9 @@ QImage UAS::getImage() imageType == MAVLINK_DATA_STREAM_IMG_PGM || imageType == MAVLINK_DATA_STREAM_IMG_PNG) { - if (!image.loadFromData(imageRecBuffer)) + if (!image.loadFromData(imageRecBuffer)) { - qDebug() << "Loading data from image buffer failed!"; + qDebug() << "Loading data from image buffer failed!"; } } // Restart statemachine @@ -1578,7 +1574,7 @@ QImage UAS::getImage() //imageRecBuffer.clear(); return image; #else - return QImage(); + return QImage(); #endif } @@ -1614,7 +1610,7 @@ quint64 UAS::getUptime() const } else { - return MG::TIME::getGroundTimeNow() - startTime; + return QGC::groundTimeMilliseconds() - startTime; } } @@ -1626,7 +1622,7 @@ int UAS::getCommunicationStatus() const void UAS::requestParameters() { mavlink_message_t msg; - mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 25); + mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_ALL); sendMessage(msg); } @@ -2088,7 +2084,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double sendMessage(message); qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; - emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow()); + emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); } else { @@ -2168,30 +2164,30 @@ bool UAS::emergencyKILL() { halt(); // FIXME MAVLINKV10PORTINGNEEDED -// bool result = false; -// QMessageBox msgBox; -// msgBox.setIcon(QMessageBox::Critical); -// msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS"); -// msgBox.setInformativeText("Do you want to cut power on all systems?"); -// msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); -// msgBox.setDefaultButton(QMessageBox::Cancel); -// int ret = msgBox.exec(); - -// // Close the message box shortly after the click to prevent accidental clicks -// QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - -// if (ret == QMessageBox::Yes) -// { -// mavlink_message_t msg; -// // TODO Replace MG System ID with static function call and allow to change ID in GUI -// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL); -// // Send message twice to increase chance of reception -// sendMessage(msg); -// sendMessage(msg); -// result = true; -// } -// return result; + // bool result = false; + // QMessageBox msgBox; + // msgBox.setIcon(QMessageBox::Critical); + // msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS"); + // msgBox.setInformativeText("Do you want to cut power on all systems?"); + // msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); + // msgBox.setDefaultButton(QMessageBox::Cancel); + // int ret = msgBox.exec(); + + // // Close the message box shortly after the click to prevent accidental clicks + // QTimer::singleShot(5000, &msgBox, SLOT(reject())); + + + // if (ret == QMessageBox::Yes) + // { + // mavlink_message_t msg; + // // TODO Replace MG System ID with static function call and allow to change ID in GUI + // mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL); + // // Send message twice to increase chance of reception + // sendMessage(msg); + // sendMessage(msg); + // result = true; + // } + // return result; return false; } @@ -2227,8 +2223,8 @@ void UAS::enableHil(bool enable) * @param zacc Z acceleration (mg) */ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, - int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) + float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, + int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) { mavlink_message_t msg; mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); @@ -2322,19 +2318,19 @@ QString UAS::getShortModeTextFor(int id) } if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED) { - mode += "GUIDED"; + mode += "|GUID"; } if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) { - mode += "STABILIZED"; + mode += "|STAB"; } if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_TEST) { - mode += "TEST"; + mode += "|TEST"; } if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL) { - mode += "MANUAL"; + mode += "|MAN"; } if (modeid == 0) @@ -2345,11 +2341,11 @@ QString UAS::getShortModeTextFor(int id) // ARMED STATE DECODING if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) { - mode.prepend("A|"); + mode.prepend("A/"); } else { - mode.prepend("D|"); + mode.prepend("D/"); } // HARDWARE IN THE LOOP DECODING @@ -2480,7 +2476,7 @@ QString UAS::getBatterySpecs() int UAS::calculateTimeRemaining() { - quint64 dt = MG::TIME::getGroundTimeNow() - startTime; + quint64 dt = QGC::groundTimeMilliseconds() - startTime; double seconds = dt / 1000.0f; double voltDifference = startVoltage - currentVoltage; if (voltDifference <= 0) voltDifference = 0.00000000001f; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index e70bc521fc8e38f293940c2b064eb68011032a7e..d6284d005c763795ff79639a5f76840f83f396eb 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -218,7 +218,8 @@ protected: //COMMENTS FOR TEST UNIT bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life float chargeLevel; ///< Charge level of battery, in percent int timeRemaining; ///< Remaining time calculated based on previous and current - uint8_t mode; ///< The current mode of the MAV + uint8_t mode; ///< The current mode of the MAV + uint32_t custom_mode; ///< The current mode of the MAV int status; ///< The current status of the MAV uint32_t navMode; ///< The current navigation mode of the MAV quint64 onboardTimeOffset; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 9edb21991c45806d62e984967c92947c2ae64046..6cf37724d6fb978c4f078476eb69c845a1bfde4e 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -104,6 +104,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) : leftDragStarted(false), mouseHasMoved(false), actionPending(false), + directSending(false), userSetPointSet(false), userXYSetPointSet(false) { @@ -637,37 +638,53 @@ void HSIDisplay::keyPressEvent(QKeyEvent* event) statusClearTimer.start(); sendBodySetPointCoordinates(); } - else if ((event->key() == Qt::Key_Up)) + else if ((event->key() == Qt::Key_W)) { - setBodySetpointCoordinateXY(0.5, 0); + setBodySetpointCoordinateXY(1.0, 0); + setBodySetpointCoordinateZ(uas->getLocalZ()); + setBodySetpointCoordinateYaw(uas->getYaw()); } - else if ((event->key() == Qt::Key_Down)) + else if ((event->key() == Qt::Key_S)) { - setBodySetpointCoordinateXY(-0.5, 0); + setBodySetpointCoordinateXY(-1.0, 0); + setBodySetpointCoordinateZ(uas->getLocalZ()); + setBodySetpointCoordinateYaw(uas->getYaw()); } - else if ((event->key() == Qt::Key_Left)) + else if ((event->key() == Qt::Key_A)) { - setBodySetpointCoordinateXY(0, -0.5); + setBodySetpointCoordinateXY(0, -1.0); + setBodySetpointCoordinateZ(uas->getLocalZ()); + setBodySetpointCoordinateYaw(uas->getYaw()); } - else if ((event->key() == Qt::Key_Right)) + else if ((event->key() == Qt::Key_D)) { - setBodySetpointCoordinateXY(0, 0.5); + setBodySetpointCoordinateXY(0, 1.0); + setBodySetpointCoordinateZ(uas->getLocalZ()); + setBodySetpointCoordinateYaw(uas->getYaw()); } - else if ((event->key() == Qt::Key_Plus)) + else if ((event->key() == Qt::Key_Up)) { - setBodySetpointCoordinateZ(-0.2); + setBodySetpointCoordinateXY(0, 0); + setBodySetpointCoordinateZ(-0.5+uas->getLocalZ()); + setBodySetpointCoordinateYaw(uas->getYaw()); } - else if ((event->key() == Qt::Key_Minus)) + else if ((event->key() == Qt::Key_Down)) { - setBodySetpointCoordinateZ(+0.2); + setBodySetpointCoordinateZ(+0.5+uas->getLocalZ()); + setBodySetpointCoordinateXY(0, 0); + setBodySetpointCoordinateYaw(uas->getYaw()); } - else if ((event->key() == Qt::Key_L)) + else if ((event->key() == Qt::Key_Left)) { - setBodySetpointCoordinateYaw(-0.1); + setBodySetpointCoordinateXY(0, 0); + setBodySetpointCoordinateZ(uas->getLocalZ()); + setBodySetpointCoordinateYaw(-0.2+uas->getYaw()); } - else if ((event->key() == Qt::Key_R)) + else if ((event->key() == Qt::Key_Right)) { - setBodySetpointCoordinateYaw(0.1); + setBodySetpointCoordinateXY(0, 0); + setBodySetpointCoordinateZ(uas->getLocalZ()); + setBodySetpointCoordinateYaw(0.2+uas->getYaw()); } HDDisplay::keyPressEvent(event); } @@ -811,7 +828,7 @@ void HSIDisplay::sendBodySetPointCoordinates() double dx = uiXSetCoordinate - uas->getLocalX(); double dy = uiYSetCoordinate - uas->getLocalY(); double dz = uiZSetCoordinate - uas->getLocalZ(); - bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 1.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate); + bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 3.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate); if (valid) { uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet); diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index fd815e705f935c0daf3f15f3bc7490818a58dc96..6397baa23e4330f8b69daf01f5b46d87b1376a6b 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -175,6 +175,7 @@ protected: QTimer statusClearTimer; QString statusMessage; bool actionPending; + bool directSending; /** * @brief Private data container class to be used within the HSI widget