/*=================================================================== ======================================================================*/ /** * @file * @brief Represents one unmanned aerial vehicle * * @author Lorenz Meier * */ #include #include #include #include #include #include #include #include #include "UAS.h" #include "LinkInterface.h" #include "UASManager.h" #include "QGC.h" #include "GAudioOutput.h" #include "MAVLinkProtocol.h" #include "QGCMAVLink.h" #include "LinkManager.h" #include "SerialLink.h" #include "UASParameterCommsMgr.h" #include #include #ifdef QGC_PROTOBUF_ENABLED #include #endif /** * Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) * by calling readSettings. This means the new UAS will have the same settings * as the previous one created unless one calls deleteSettings in the code after * creating the UAS. */ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(), lipoFull(4.2f), lipoEmpty(3.5f), uasId(id), links(new QList()), unknownPackets(), mavlink(protocol), commStatus(COMM_DISCONNECTED), receiveDropRate(0), sendDropRate(0), statusTimeout(thread), name(""), type(MAV_TYPE_GENERIC), airframe(QGC_AIRFRAME_GENERIC), autopilot(-1), systemIsArmed(false), base_mode(0), custom_mode(0), // custom_mode not initialized status(-1), // shortModeText not initialized // shortStateText not initialized // actuatorValues not initialized // actuatorNames not initialized // motorValues not initialized // motorNames mnot initialized thrustSum(0), thrustMax(10), // batteryType not initialized // cells not initialized // fullVoltage not initialized // emptyVoltage not initialized startVoltage(-1.0f), tickVoltage(10.5f), lastTickVoltageValue(13.0f), tickLowpassVoltage(12.0f), warnVoltage(9.5f), warnLevelPercent(20.0f), currentVoltage(12.6f), lpVoltage(12.0f), currentCurrent(0.4f), batteryRemainingEstimateEnabled(false), // chargeLevel not initialized // timeRemaining not initialized lowBattAlarm(false), startTime(QGC::groundTimeMilliseconds()), onboardTimeOffset(0), controlRollManual(true), controlPitchManual(true), controlYawManual(true), controlThrustManual(true), manualRollAngle(0), manualPitchAngle(0), manualYawAngle(0), manualThrust(0), positionLock(false), isLocalPositionKnown(false), isGlobalPositionKnown(false), localX(0.0), localY(0.0), localZ(0.0), latitude(0.0), longitude(0.0), altitudeAMSL(0.0), altitudeRelative(0.0), globalEstimatorActive(false), latitude_gps(0.0), longitude_gps(0.0), altitude_gps(0.0), speedX(0.0), speedY(0.0), speedZ(0.0), nedPosGlobalOffset(0,0,0), nedAttGlobalOffset(0,0,0), #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) receivedOverlayTimestamp(0.0), receivedObstacleListTimestamp(0.0), receivedPathTimestamp(0.0), receivedPointCloudTimestamp(0.0), receivedRGBDImageTimestamp(0.0), #endif airSpeed(std::numeric_limits::quiet_NaN()), groundSpeed(std::numeric_limits::quiet_NaN()), waypointManager(this), attitudeKnown(false), attitudeStamped(false), lastAttitude(0), roll(0.0), pitch(0.0), yaw(0.0), blockHomePositionChanges(false), receivedMode(false), paramsOnceRequested(false), paramMgr(this), simulation(0), _thread(thread), // The protected members. connectionLost(false), lastVoltageWarning(0), lastNonNullTime(0), onboardTimeOffsetInvalidCount(0), hilEnabled(false), sensorHil(false), lastSendTimeGPS(0), lastSendTimeSensors(0) { moveToThread(thread); for (unsigned int i = 0; i<255;++i) { componentID[i] = -1; componentMulti[i] = false; } // Store a list of available actions for this UAS. // Basically everything exposed as a SLOT with no return value or arguments. QAction* newAction = new QAction(tr("Arm"), thread); newAction->setToolTip(tr("Enable the UAS so that all actuators are online")); connect(newAction, SIGNAL(triggered()), this, SLOT(armSystem())); actions.append(newAction); newAction = new QAction(tr("Disarm"), thread); newAction->setToolTip(tr("Disable the UAS so that all actuators are offline")); connect(newAction, SIGNAL(triggered()), this, SLOT(disarmSystem())); actions.append(newAction); newAction = new QAction(tr("Toggle armed"), thread); newAction->setToolTip(tr("Toggle between armed and disarmed")); connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy())); actions.append(newAction); newAction = new QAction(tr("Go home"), thread); newAction->setToolTip(tr("Command the UAS to return to its home position")); connect(newAction, SIGNAL(triggered()), this, SLOT(home())); actions.append(newAction); newAction = new QAction(tr("Land"), thread); newAction->setToolTip(tr("Command the UAS to land")); connect(newAction, SIGNAL(triggered()), this, SLOT(land())); actions.append(newAction); newAction = new QAction(tr("Launch"), thread); newAction->setToolTip(tr("Command the UAS to launch itself and begin its mission")); connect(newAction, SIGNAL(triggered()), this, SLOT(launch())); actions.append(newAction); newAction = new QAction(tr("Resume"), thread); newAction->setToolTip(tr("Command the UAS to continue its mission")); connect(newAction, SIGNAL(triggered()), this, SLOT(go())); actions.append(newAction); newAction = new QAction(tr("Stop"), thread); newAction->setToolTip(tr("Command the UAS to halt and hold position")); connect(newAction, SIGNAL(triggered()), this, SLOT(halt())); actions.append(newAction); newAction = new QAction(tr("Go autonomous"), thread); newAction->setToolTip(tr("Set the UAS into an autonomous control mode")); connect(newAction, SIGNAL(triggered()), this, SLOT(goAutonomous())); actions.append(newAction); newAction = new QAction(tr("Go manual"), thread); newAction->setToolTip(tr("Set the UAS into a manual control mode")); connect(newAction, SIGNAL(triggered()), this, SLOT(goManual())); actions.append(newAction); newAction = new QAction(tr("Toggle autonomy"), thread); newAction->setToolTip(tr("Toggle between manual and full-autonomy")); connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy())); actions.append(newAction); color = UASInterface::getNextColor(); setBatterySpecs(QString("")); connect(&statusTimeout, SIGNAL(timeout()), this, SLOT(updateState())); connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); statusTimeout.start(500); readSettings(); //need to init paramMgr after readSettings have been loaded, to properly set autopilot and so forth paramMgr.initWithUAS(this); // Initial signals emit disarmed(); emit armingChanged(false); } /** * Saves the settings of name, airframe, autopilot type and battery specifications * by calling writeSettings. */ UAS::~UAS() { writeSettings(); _thread->quit(); _thread->wait(); delete links; delete simulation; } /** * Saves the settings of name, airframe, autopilot type and battery specifications * for the next instantiation of UAS. */ void UAS::writeSettings() { QSettings settings; settings.beginGroup(QString("MAV%1").arg(uasId)); settings.setValue("NAME", this->name); settings.setValue("AIRFRAME", this->airframe); settings.setValue("AP_TYPE", this->autopilot); settings.setValue("BATTERY_SPECS", getBatterySpecs()); settings.endGroup(); settings.sync(); } /** * Reads in the settings: name, airframe, autopilot type, and battery specifications * for the new UAS. */ void UAS::readSettings() { QSettings settings; settings.beginGroup(QString("MAV%1").arg(uasId)); this->name = settings.value("NAME", this->name).toString(); this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt(); if (settings.contains("BATTERY_SPECS")) { setBatterySpecs(settings.value("BATTERY_SPECS").toString()); } settings.endGroup(); } /** * Deletes the settings origianally read into the UAS by readSettings. * This is in case one does not want the old values but would rather * start with the values assigned by the constructor. */ void UAS::deleteSettings() { this->name = ""; this->airframe = QGC_AIRFRAME_GENERIC; this->autopilot = -1; setBatterySpecs(QString("9V,9.5V,12.6V")); } /** * @ return the id of the uas */ int UAS::getUASID() const { return uasId; } void UAS::triggerAction(int action) { if (action >= 0 && action < actions.size()) { qDebug() << "Triggering action: '" << actions[action]->text() << "'"; actions[action]->trigger(); } } /** * Update the heartbeat. */ void UAS::updateState() { // Check if heartbeat timed out quint64 heartbeatInterval = QGC::groundTimeUsecs() - lastHeartbeat; if (!connectionLost && (heartbeatInterval > timeoutIntervalHeartbeat)) { connectionLost = true; receivedMode = false; QString audiostring = QString("Link lost to system %1").arg(this->getUASID()); GAudioOutput::instance()->say(audiostring.toLower()); } // Update connection loss time on each iteration if (connectionLost && (heartbeatInterval > timeoutIntervalHeartbeat)) { connectionLossTime = heartbeatInterval; emit heartbeatTimeout(true, heartbeatInterval/1000); } // Connection gained if (connectionLost && (heartbeatInterval < timeoutIntervalHeartbeat)) { QString audiostring = QString("Link regained to system %1").arg(this->getUASID()); GAudioOutput::instance()->say(audiostring.toLower()); connectionLost = false; connectionLossTime = 0; emit heartbeatTimeout(false, 0); } // Position lock is set by the MAVLink message handler // if no position lock is available, indicate an error if (positionLock) { positionLock = false; } else { if (((base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock) { GAudioOutput::instance()->notifyNegative(); } } } /** * If the acitve UAS (the UAS that was selected) is not the one that is currently * active, then change the active UAS to the one that was selected. */ void UAS::setSelected() { if (UASManager::instance()->getActiveUAS() != this) { UASManager::instance()->setActiveUAS(this); emit systemSelected(true); } } /** * @return if the active UAS is the current UAS **/ bool UAS::getSelected() const { return (UASManager::instance()->getActiveUAS() == this); } void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { if (!link) return; if (!links->contains(link)) { addLink(link); // qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName(); } if (!components.contains(message.compid)) { QString componentName; switch (message.compid) { case MAV_COMP_ID_ALL: { componentName = "ANONYMOUS"; break; } case MAV_COMP_ID_IMU: { componentName = "IMU #1"; break; } case MAV_COMP_ID_CAMERA: { componentName = "CAMERA"; break; } case MAV_COMP_ID_MISSIONPLANNER: { componentName = "MISSIONPLANNER"; break; } } components.insert(message.compid, componentName); emit componentCreated(uasId, message.compid, componentName); } // qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq; // Only accept messages from this system (condition 1) // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled // and we already got one attitude packet if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE)) { QString uasState; QString stateDescription; bool multiComponentSourceDetected = false; bool wrongComponent = false; switch (message.compid) { case MAV_COMP_ID_IMU_2: // Prefer IMU 2 over IMU 1 (FIXME) componentID[message.msgid] = MAV_COMP_ID_IMU_2; break; default: // Do nothing break; } // Store component ID if (componentID[message.msgid] == -1) { // Prefer the first component componentID[message.msgid] = message.compid; } else { // Got this message already if (componentID[message.msgid] != message.compid) { componentMulti[message.msgid] = true; wrongComponent = true; } } if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true; switch (message.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { if (multiComponentSourceDetected && wrongComponent) { break; } lastHeartbeat = QGC::groundTimeUsecs(); emit heartbeat(this); mavlink_heartbeat_t state; mavlink_msg_heartbeat_decode(&message, &state); // Send the base_mode and system_status values to the plotter. This uses the ground time // so the Ground Time checkbox must be ticked for these values to display quint64 time = getUnixTime(); QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid); emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time); emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time); emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time); // Set new type if it has changed if (this->type != state.type) { this->type = state.type; if (airframe == 0) { switch (type) { case MAV_TYPE_FIXED_WING: setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR); break; case MAV_TYPE_QUADROTOR: setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH); break; case MAV_TYPE_HEXAROTOR: setAirframe(UASInterface::QGC_AIRFRAME_HEXCOPTER); break; default: // Do nothing break; } } this->autopilot = state.autopilot; emit systemTypeSet(this, type); } bool currentlyArmed = state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY; if (systemIsArmed != currentlyArmed) { systemIsArmed = currentlyArmed; emit armingChanged(systemIsArmed); if (systemIsArmed) { emit armed(); } else { emit disarmed(); } } QString audiostring = QString("System %1").arg(uasId); QString stateAudio = ""; QString modeAudio = ""; QString navModeAudio = ""; bool statechanged = false; bool modechanged = false; QString audiomodeText = getAudioModeTextFor(static_cast(state.base_mode)); if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT) { statechanged = true; this->status = state.system_status; getStatusForCode((int)state.system_status, uasState, stateDescription); emit statusChanged(this, uasState, stateDescription); emit statusChanged(this->status); shortStateText = uasState; // Adjust for better audio if (uasState == QString("STANDBY")) uasState = QString("standing by"); if (uasState == QString("EMERGENCY")) uasState = QString("emergency condition"); if (uasState == QString("CRITICAL")) uasState = QString("critical condition"); if (uasState == QString("SHUTDOWN")) uasState = QString("shutting down"); stateAudio = uasState; } if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode) { modechanged = true; this->base_mode = state.base_mode; this->custom_mode = state.custom_mode; shortModeText = getShortModeTextFor(this->base_mode, this->custom_mode, this->autopilot); emit modeChanged(this->getUASID(), shortModeText, ""); modeAudio = " is now in " + audiomodeText; } // We got the mode receivedMode = true; // AUDIO if (modechanged && statechanged) { // Output both messages audiostring += modeAudio + " and " + stateAudio; } else if (modechanged || statechanged) { // Output the one message audiostring += modeAudio + stateAudio; } if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY)) { GAudioOutput::instance()->say(QString("emergency for system %1").arg(this->getUASID())); QTimer::singleShot(3000, GAudioOutput::instance(), SLOT(startEmergency())); } else if (modechanged || statechanged) { GAudioOutput::instance()->stopEmergency(); GAudioOutput::instance()->say(audiostring.toLower()); } } break; case MAVLINK_MSG_ID_SYS_STATUS: { if (multiComponentSourceDetected && wrongComponent) { break; } mavlink_sys_status_t state; mavlink_msg_sys_status_decode(&message, &state); // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present. quint64 time = getUnixTime(); QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid); emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time); emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time); emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time); emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time); emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time); emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time); emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time); // Process CPU load. emit loadChanged(this,state.load/10.0f); emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time); // Battery charge/time remaining/voltage calculations currentVoltage = state.voltage_battery/1000.0f; lpVoltage = filterVoltage(currentVoltage); tickLowpassVoltage = tickLowpassVoltage*0.8f + 0.2f*currentVoltage; // We don't want to tick above the threshold if (tickLowpassVoltage > tickVoltage) { lastTickVoltageValue = tickLowpassVoltage; } if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f) /* warn if lower than treshold */ && (lpVoltage < tickVoltage) /* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */ && (currentVoltage > 3.3f) /* warn only if current voltage is really still lower by a reasonable amount */ && ((currentVoltage - 0.2f) < tickVoltage) /* warn only every 12 seconds */ && (QGC::groundTimeUsecs() - lastVoltageWarning) > 12000000) { GAudioOutput::instance()->say(QString("voltage warning: %1 volts").arg(lpVoltage, 0, 'f', 1, QChar(' '))); lastVoltageWarning = QGC::groundTimeUsecs(); lastTickVoltageValue = tickLowpassVoltage; } if (startVoltage == -1.0f && currentVoltage > 0.1f) startVoltage = currentVoltage; timeRemaining = calculateTimeRemaining(); if (!batteryRemainingEstimateEnabled && chargeLevel != -1) { chargeLevel = state.battery_remaining; } emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), timeRemaining); emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time); // emit voltageChanged(message.sysid, currentVoltage); emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time); // And if the battery current draw is measured, log that also. if (state.current_battery != -1) { currentCurrent = ((double)state.current_battery)/100.0f; emit valueChanged(uasId, name.arg("battery_current"), "A", currentCurrent, time); } // LOW BATTERY ALARM if (lpVoltage < warnVoltage && (currentVoltage - 0.2f) < warnVoltage && (currentVoltage > 3.3)) { // An audio alarm. Does not generate any signals. startLowBattAlarm(); } else { stopLowBattAlarm(); } // control_sensors_enabled: // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11)); emit positionYawControlEnabled(state.onboard_control_sensors_enabled & (1 << 12)); emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13)); emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14)); // Trigger drop rate updates as needed. Here we convert the incoming // drop_rate_comm value from 1/100 of a percent in a uint16 to a true // percentage as a float. We also cap the incoming value at 100% as defined // by the MAVLink specifications. if (state.drop_rate_comm > 10000) { state.drop_rate_comm = 10000; } emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time); } break; case MAVLINK_MSG_ID_ATTITUDE: { mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); quint64 time = getUnixReferenceTime(attitude.time_boot_ms); emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time); if (!wrongComponent) { lastAttitude = time; setRoll(QGC::limitAngleToPMPIf(attitude.roll)); setPitch(QGC::limitAngleToPMPIf(attitude.pitch)); setYaw(QGC::limitAngleToPMPIf(attitude.yaw)); attitudeKnown = true; emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); } } break; case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: { mavlink_attitude_quaternion_t attitude; mavlink_msg_attitude_quaternion_decode(&message, &attitude); quint64 time = getUnixReferenceTime(attitude.time_boot_ms); double a = attitude.q1; double b = attitude.q2; double c = attitude.q3; double d = attitude.q4; double aSq = a * a; double bSq = b * b; double cSq = c * c; double dSq = d * d; float dcm[3][3]; dcm[0][0] = aSq + bSq - cSq - dSq; dcm[0][1] = 2.0 * (b * c - a * d); dcm[0][2] = 2.0 * (a * c + b * d); dcm[1][0] = 2.0 * (b * c + a * d); dcm[1][1] = aSq - bSq + cSq - dSq; dcm[1][2] = 2.0 * (c * d - a * b); dcm[2][0] = 2.0 * (b * d - a * c); dcm[2][1] = 2.0 * (a * b + c * d); dcm[2][2] = aSq - bSq - cSq + dSq; float phi, theta, psi; theta = asin(-dcm[2][0]); if (fabs(theta - M_PI_2) < 1.0e-3f) { phi = 0.0f; psi = (atan2(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1]) + phi); } else if (fabs(theta + M_PI_2) < 1.0e-3f) { phi = 0.0f; psi = atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1] - phi); } else { phi = atan2f(dcm[2][1], dcm[2][2]); psi = atan2f(dcm[1][0], dcm[0][0]); } emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi), QGC::limitAngleToPMPIf(theta), QGC::limitAngleToPMPIf(psi), time); if (!wrongComponent) { lastAttitude = time; setRoll(QGC::limitAngleToPMPIf(phi)); setPitch(QGC::limitAngleToPMPIf(theta)); setYaw(QGC::limitAngleToPMPIf(psi)); attitudeKnown = true; emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); } } break; case MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET: { mavlink_local_position_ned_system_global_offset_t offset; mavlink_msg_local_position_ned_system_global_offset_decode(&message, &offset); nedPosGlobalOffset.setX(offset.x); nedPosGlobalOffset.setY(offset.y); nedPosGlobalOffset.setZ(offset.z); nedAttGlobalOffset.setX(offset.roll); nedAttGlobalOffset.setY(offset.pitch); nedAttGlobalOffset.setZ(offset.yaw); } 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); } 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) { setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI)); emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); } setAltitudeAMSL(hud.alt); setGroundSpeed(hud.groundspeed); if (!isnan(hud.airspeed)) setAirSpeed(hud.airspeed); speedZ = -hud.climb; emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); emit speedChanged(this, groundSpeed, airSpeed, 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); // Emit position always with component ID emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); if (!wrongComponent) { setLocalX(pos.x); setLocalY(pos.y); setLocalZ(pos.z); speedX = pos.vx; speedY = pos.vy; speedZ = pos.vz; // Emit emit localPositionChanged(this, localX, localY, localZ, time); emit velocityChanged_NED(this, speedX, speedY, speedZ, time); // 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); } 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(); setLatitude(pos.lat/(double)1E7); setLongitude(pos.lon/(double)1E7); setAltitudeAMSL(pos.alt/1000.0); setAltitudeRelative(pos.relative_alt/1000.0); globalEstimatorActive = true; speedX = pos.vx/100.0; speedY = pos.vy/100.0; speedZ = pos.vz/100.0; emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); // We had some frame mess here, global and local axes were mixed. emit velocityChanged_NED(this, speedX, speedY, speedZ, time); setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY)); emit speedChanged(this, groundSpeed, airSpeed, 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); quint64 time = getUnixTime(pos.time_usec); emit gpsLocalizationChanged(this, pos.fix_type); // TODO: track localization state not only for gps but also for other loc. sources int loc_type = pos.fix_type; if (loc_type == 1) { loc_type = 0; } emit localizationChanged(this, loc_type); setSatelliteCount(pos.satellites_visible); if (pos.fix_type > 2) { positionLock = true; isGlobalPositionKnown = true; latitude_gps = pos.lat/(double)1E7; longitude_gps = pos.lon/(double)1E7; altitude_gps = pos.alt/1000.0; // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead. if (!globalEstimatorActive) { setLatitude(latitude_gps); setLongitude(longitude_gps); setAltitudeAMSL(altitude_gps); emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); float vel = pos.vel/100.0f; // Smaller than threshold and not NaN if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) { setGroundSpeed(vel); emit speedChanged(this, groundSpeed, airSpeed, time); } else { emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, 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++) { 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])); } setSatelliteCount(pos.satellites_visible); } 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 / 10000000.0, pos.longitude / 10000000.0, pos.altitude / 1000.0); } break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { mavlink_rc_channels_raw_t channels; mavlink_msg_rc_channels_raw_decode(&message, &channels); const unsigned int portWidth = 8; // XXX magic number emit remoteControlRSSIChanged(channels.rssi/255.0f); if (channels.chan1_raw != UINT16_MAX) emit remoteControlChannelRawChanged(channels.port * portWidth + 0, channels.chan1_raw); if (channels.chan2_raw != UINT16_MAX) emit remoteControlChannelRawChanged(channels.port * portWidth + 1, channels.chan2_raw); if (channels.chan3_raw != UINT16_MAX) emit remoteControlChannelRawChanged(channels.port * portWidth + 2, channels.chan3_raw); if (channels.chan4_raw != UINT16_MAX) emit remoteControlChannelRawChanged(channels.port * portWidth + 3, channels.chan4_raw); if (channels.chan5_raw != UINT16_MAX) emit remoteControlChannelRawChanged(channels.port * portWidth + 4, channels.chan5_raw); if (channels.chan6_raw != UINT16_MAX) emit remoteControlChannelRawChanged(channels.port * portWidth + 5, channels.chan6_raw); if (channels.chan7_raw != UINT16_MAX) emit remoteControlChannelRawChanged(channels.port * portWidth + 6, channels.chan7_raw); if (channels.chan8_raw != UINT16_MAX) emit remoteControlChannelRawChanged(channels.port * portWidth + 7, channels.chan8_raw); } break; case MAVLINK_MSG_ID_RC_CHANNELS: { mavlink_rc_channels_t channels; mavlink_msg_rc_channels_decode(&message, &channels); // UINT8_MAX indicates this value is unknown if (channels.rssi != UINT8_MAX) { emit remoteControlRSSIChanged(channels.rssi/100.0f); } if (channels.chan1_raw != UINT16_MAX && channels.chancount > 0) emit remoteControlChannelRawChanged(0, channels.chan1_raw); if (channels.chan2_raw != UINT16_MAX && channels.chancount > 1) emit remoteControlChannelRawChanged(1, channels.chan2_raw); if (channels.chan3_raw != UINT16_MAX && channels.chancount > 2) emit remoteControlChannelRawChanged(2, channels.chan3_raw); if (channels.chan4_raw != UINT16_MAX && channels.chancount > 3) emit remoteControlChannelRawChanged(3, channels.chan4_raw); if (channels.chan5_raw != UINT16_MAX && channels.chancount > 4) emit remoteControlChannelRawChanged(4, channels.chan5_raw); if (channels.chan6_raw != UINT16_MAX && channels.chancount > 5) emit remoteControlChannelRawChanged(5, channels.chan6_raw); if (channels.chan7_raw != UINT16_MAX && channels.chancount > 6) emit remoteControlChannelRawChanged(6, channels.chan7_raw); if (channels.chan8_raw != UINT16_MAX && channels.chancount > 7) emit remoteControlChannelRawChanged(7, channels.chan8_raw); if (channels.chan9_raw != UINT16_MAX && channels.chancount > 8) emit remoteControlChannelRawChanged(8, channels.chan9_raw); if (channels.chan10_raw != UINT16_MAX && channels.chancount > 9) emit remoteControlChannelRawChanged(9, channels.chan10_raw); if (channels.chan11_raw != UINT16_MAX && channels.chancount > 10) emit remoteControlChannelRawChanged(10, channels.chan11_raw); if (channels.chan12_raw != UINT16_MAX && channels.chancount > 11) emit remoteControlChannelRawChanged(11, channels.chan12_raw); if (channels.chan13_raw != UINT16_MAX && channels.chancount > 12) emit remoteControlChannelRawChanged(12, channels.chan13_raw); if (channels.chan14_raw != UINT16_MAX && channels.chancount > 13) emit remoteControlChannelRawChanged(13, channels.chan14_raw); if (channels.chan15_raw != UINT16_MAX && channels.chancount > 14) emit remoteControlChannelRawChanged(14, channels.chan15_raw); if (channels.chan16_raw != UINT16_MAX && channels.chancount > 15) emit remoteControlChannelRawChanged(15, channels.chan16_raw); if (channels.chan17_raw != UINT16_MAX && channels.chancount > 16) emit remoteControlChannelRawChanged(16, channels.chan17_raw); if (channels.chan18_raw != UINT16_MAX && channels.chancount > 17) emit remoteControlChannelRawChanged(17, channels.chan18_raw); } break; case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { mavlink_rc_channels_scaled_t channels; mavlink_msg_rc_channels_scaled_decode(&message, &channels); const unsigned int portWidth = 8; // XXX magic number emit remoteControlRSSIChanged(channels.rssi/255.0f); if (static_cast(channels.chan1_scaled) != UINT16_MAX) emit remoteControlChannelScaledChanged(channels.port * portWidth + 0, channels.chan1_scaled/10000.0f); if (static_cast(channels.chan2_scaled) != UINT16_MAX) emit remoteControlChannelScaledChanged(channels.port * portWidth + 1, channels.chan2_scaled/10000.0f); if (static_cast(channels.chan3_scaled) != UINT16_MAX) emit remoteControlChannelScaledChanged(channels.port * portWidth + 2, channels.chan3_scaled/10000.0f); if (static_cast(channels.chan4_scaled) != UINT16_MAX) emit remoteControlChannelScaledChanged(channels.port * portWidth + 3, channels.chan4_scaled/10000.0f); if (static_cast(channels.chan5_scaled) != UINT16_MAX) emit remoteControlChannelScaledChanged(channels.port * portWidth + 4, channels.chan5_scaled/10000.0f); if (static_cast(channels.chan6_scaled) != UINT16_MAX) emit remoteControlChannelScaledChanged(channels.port * portWidth + 5, channels.chan6_scaled/10000.0f); if (static_cast(channels.chan7_scaled) != UINT16_MAX) emit remoteControlChannelScaledChanged(channels.port * portWidth + 6, channels.chan7_scaled/10000.0f); if (static_cast(channels.chan8_scaled) != UINT16_MAX) emit remoteControlChannelScaledChanged(channels.port * portWidth + 7, channels.chan8_scaled/10000.0f); } break; case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t rawValue; mavlink_msg_param_value_decode(&message, &rawValue); QByteArray bytes(rawValue.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); // Construct a string stopping at the first NUL (0) character, else copy the whole // byte array (max MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN, so safe) QString parameterName(bytes); mavlink_param_union_t paramVal; paramVal.param_float = rawValue.param_value; paramVal.type = rawValue.param_type; processParamValueMsg(message, parameterName,rawValue,paramVal); processParamValueMsgHook(message, parameterName,rawValue,paramVal); } break; case MAVLINK_MSG_ID_COMMAND_ACK: { mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(&message, &ack); switch (ack.result) { case MAV_RESULT_ACCEPTED: { emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_INFO, tr("SUCCESS: Executed CMD: %1").arg(ack.command)); } break; case MAV_RESULT_TEMPORARILY_REJECTED: { emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_WARNING, tr("FAILURE: Temporarily rejected CMD: %1").arg(ack.command)); } break; case MAV_RESULT_DENIED: { emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_ERROR, tr("FAILURE: Denied CMD: %1").arg(ack.command)); } break; case MAV_RESULT_UNSUPPORTED: { emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_WARNING, tr("FAILURE: Unsupported CMD: %1").arg(ack.command)); } break; case MAV_RESULT_FAILED: { emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_ERROR, tr("FAILURE: Failed CMD: %1").arg(ack.command)); } 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); } break; case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t mc; mavlink_msg_mission_count_decode(&message, &mc); // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. if (mc.target_system == 0) { mc.target_system = mavlink->getSystemId(); } if (mc.target_component == 0) { mc.target_component = mavlink->getComponentId(); } // Check that this message applies to the UAS. if(mc.target_system == mavlink->getSystemId()) { if (mc.target_component != mavlink->getComponentId()) { qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."; qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mc.target_component; } waypointManager.handleWaypointCount(message.sysid, message.compid, mc.count); } else { qDebug() << QString("Received mission count message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mc.target_system); } } break; case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t mi; mavlink_msg_mission_item_decode(&message, &mi); // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. if (mi.target_system == 0) { mi.target_system = mavlink->getSystemId(); } if (mi.target_component == 0) { mi.target_component = mavlink->getComponentId(); } // Check that the item pertains to this UAS. if(mi.target_system == mavlink->getSystemId()) { if (mi.target_component != mavlink->getComponentId()) { qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."; qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mi.target_component; } waypointManager.handleWaypoint(message.sysid, message.compid, &mi); } else { qDebug() << QString("Received mission item message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mi.target_system); } } break; case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t ma; mavlink_msg_mission_ack_decode(&message, &ma); // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. if (ma.target_system == 0) { ma.target_system = mavlink->getSystemId(); } if (ma.target_component == 0) { ma.target_component = mavlink->getComponentId(); } // Check that the ack pertains to this UAS. if(ma.target_system == mavlink->getSystemId()) { if (ma.target_component != mavlink->getComponentId()) { qDebug() << tr("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."); qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << ma.target_component; } waypointManager.handleWaypointAck(message.sysid, message.compid, &ma); } else { qDebug() << QString("Received mission ack message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(ma.target_system); } } break; case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t mr; mavlink_msg_mission_request_decode(&message, &mr); // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. if (mr.target_system == 0) { mr.target_system = mavlink->getSystemId(); } if (mr.target_component == 0) { mr.target_component = mavlink->getComponentId(); } // Check that the request pertains to this UAS. if(mr.target_system == mavlink->getSystemId()) { if (mr.target_component != mavlink->getComponentId()) { qDebug() << QString("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."); qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mr.target_component; } waypointManager.handleWaypointRequest(message.sysid, message.compid, &mr); } else { qDebug() << QString("Received mission request message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mr.target_system); } } 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, MAV_SEVERITY_INFO, 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); } break; case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT: { if (multiComponentSourceDetected && wrongComponent) { break; } 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); } break; case MAVLINK_MSG_ID_STATUSTEXT: { QByteArray b; b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); mavlink_msg_statustext_get_text(&message, b.data()); // Ensure NUL-termination b[b.length()-1] = '\0'; QString text = QString(b); int severity = mavlink_msg_statustext_get_severity(&message); if (text.startsWith("#audio:")) { text.remove("#audio:"); emit textMessageReceived(uasId, message.compid, severity, QString("Audio message: ") + text); GAudioOutput::instance()->say(text, severity); } else { emit textMessageReceived(uasId, message.compid, severity, text); } } break; #if 0 case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: { mavlink_servo_output_raw_t raw; mavlink_msg_servo_output_raw_decode(&message, &raw); if (hilEnabled && raw.port == 0) { emit hilActuatorsChanged(static_cast(getUnixTimeFromMs(raw.time_usec)), static_cast(raw.servo1_raw), static_cast(raw.servo2_raw), static_cast(raw.servo3_raw), static_cast(raw.servo4_raw), static_cast(raw.servo5_raw), static_cast(raw.servo6_raw), static_cast(raw.servo7_raw), static_cast(raw.servo8_raw)); } } break; #endif case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: { 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(); imagePacketsArrived = 0; } 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; // 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; } ++imagePacketsArrived; // emit signal if all packets arrived if ((imagePacketsArrived >= imagePackets)) { // Restart statemachine emit imageReady(this); //qDebug() << "imageReady emitted. all packets arrived"; } } 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); } 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; } break; #endif case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { //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_nav_controller_output_t p; mavlink_msg_nav_controller_output_decode(&message,&p); setDistToWaypoint(p.wp_dist); setBearingToWaypoint(p.nav_bearing); //setAltitudeError(p.alt_error); //setSpeedError(p.aspd_error); //setCrosstrackingError(p.xtrack_error); emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error); } break; // Messages to ignore case MAVLINK_MSG_ID_RAW_IMU: case MAVLINK_MSG_ID_SCALED_IMU: case MAVLINK_MSG_ID_RAW_PRESSURE: case MAVLINK_MSG_ID_SCALED_PRESSURE: case MAVLINK_MSG_ID_OPTICAL_FLOW: case MAVLINK_MSG_ID_DEBUG_VECT: case MAVLINK_MSG_ID_DEBUG: case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: case MAVLINK_MSG_ID_NAMED_VALUE_INT: case MAVLINK_MSG_ID_MANUAL_CONTROL: case MAVLINK_MSG_ID_HIGHRES_IMU: case MAVLINK_MSG_ID_DISTANCE_SENSOR: break; default: { if (!unknownPackets.contains(message.msgid)) { unknownPackets.append(message.msgid); emit unknownPacketReceived(uasId, message.compid, message.msgid); qWarning() << "Unknown message from system:" << uasId << "message:" << message.msgid; } } break; } } } #if defined(QGC_PROTOBUF_ENABLED) /** * Receive an extended message. * @param link * @param message */ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr message) { if (!link) { return; } if (!links->contains(link)) { addLink(link); } const google::protobuf::Descriptor* descriptor = message->GetDescriptor(); if (!descriptor) { return; } const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header"); if (!headerField) { return; } const google::protobuf::Descriptor* headerDescriptor = headerField->message_type(); if (!headerDescriptor) { return; } const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid"); if (!sourceSysIdField) { return; } const google::protobuf::Reflection* reflection = message->GetReflection(); const google::protobuf::Message& headerMsg = reflection->GetMessage(*message, headerField); const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection(); int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField); if (source_sysid != uasId) { return; } #ifdef QGC_USE_PIXHAWK_MESSAGES if (message->GetTypeName() == overlay.GetTypeName()) { receivedOverlayTimestamp = QGC::groundTimeSeconds(); overlayMutex.lock(); overlay.CopyFrom(*message); overlayMutex.unlock(); emit overlayChanged(this); } else if (message->GetTypeName() == obstacleList.GetTypeName()) { receivedObstacleListTimestamp = QGC::groundTimeSeconds(); obstacleListMutex.lock(); obstacleList.CopyFrom(*message); obstacleListMutex.unlock(); emit obstacleListChanged(this); } else if (message->GetTypeName() == path.GetTypeName()) { receivedPathTimestamp = QGC::groundTimeSeconds(); pathMutex.lock(); path.CopyFrom(*message); pathMutex.unlock(); emit pathChanged(this); } else if (message->GetTypeName() == pointCloud.GetTypeName()) { receivedPointCloudTimestamp = QGC::groundTimeSeconds(); pointCloudMutex.lock(); pointCloud.CopyFrom(*message); pointCloudMutex.unlock(); emit pointCloudChanged(this); } else if (message->GetTypeName() == rgbdImage.GetTypeName()) { receivedRGBDImageTimestamp = QGC::groundTimeSeconds(); rgbdImageMutex.lock(); rgbdImage.CopyFrom(*message); rgbdImageMutex.unlock(); emit rgbdImageChanged(this); } #endif } #endif /** * Set the home position of the UAS. * @param lat The latitude fo the home position * @param lon The longitude of the home position * @param alt The altitude of the home position */ void UAS::setHomePosition(double lat, double lon, double alt) { if (blockHomePositionChanges) return; QString uasName = (getUASName() == "")? tr("UAS") + QString::number(getUASID()) : getUASName(); QMessageBox msgBox; msgBox.setIcon(QMessageBox::Warning); msgBox.setText(tr("Set a new home position for vehicle %1").arg(uasName)); msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"); 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; 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); // Send new home position to UAS mavlink_set_gps_global_origin_t home; home.target_system = uasId; home.latitude = lat*1E7; home.longitude = lon*1E7; home.altitude = alt*1000; qDebug() << "lat:" << home.latitude << " lon:" << home.longitude; mavlink_msg_set_gps_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home); sendMessage(msg); } else { blockHomePositionChanges = true; } } /** * Set the origin to the current GPS location. **/ void UAS::setLocalOriginAtCurrentGPSPosition() { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Warning); msgBox.setText("Set the home position at the current GPS position?"); msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"); 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; 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); } } /** * Set a local position setpoint. * @param x postion * @param y position * @param z position */ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) { #ifdef MAVLINK_ENABLED_PIXHAWK mavlink_message_t msg; mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw/M_PI*180.0); sendMessage(msg); #else Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); #endif } /** * Set a offset of the local position. * @param x position * @param y position * @param z position * @param yaw */ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) { #ifdef MAVLINK_ENABLED_PIXHAWK mavlink_message_t msg; mavlink_msg_set_position_control_offset_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw); sendMessage(msg); #else Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); #endif } void UAS::startRadioControlCalibration(int param) { mavlink_message_t msg; // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, param, 0, 0, 0); sendMessage(msg); } void UAS::endRadioControlCalibration() { mavlink_message_t msg; // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0, 0); sendMessage(msg); } void UAS::startDataRecording() { mavlink_message_t msg; 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_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 0, 0, 0, 0); sendMessage(msg); } 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_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); } 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_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); } 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_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); } /** * Check if time is smaller than 40 years, assuming no system without Unix * timestamp runs longer than 40 years continuously without reboot. In worst case * this will add/subtract the communication delay between GCS and MAV, it will * never alter the timestamp in a safety critical way. */ quint64 UAS::getUnixReferenceTime(quint64 time) { // Same as getUnixTime, but does not react to attitudeStamped mode if (time == 0) { // qDebug() << "XNEW time:" < has to be // a Unix epoch timestamp. Do nothing. return time/1000; } } /** * @warning If attitudeStamped is enabled, this function will not actually return * the precise time stamp of this measurement augmented to UNIX time, but will * MOVE the timestamp IN TIME to match the last measured attitude. There is no * reason why one would want this, except for system setups where the onboard * clock is not present or broken and datasets should be collected that are still * roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE * SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! */ quint64 UAS::getUnixTimeFromMs(quint64 time) { return getUnixTime(time*1000); } /** * @warning If attitudeStamped is enabled, this function will not actually return * the precise time stam of this measurement augmented to UNIX time, but will * MOVE the timestamp IN TIME to match the last measured attitude. There is no * reason why one would want this, except for system setups where the onboard * clock is not present or broken and datasets should be collected that are * still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED * RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! */ quint64 UAS::getUnixTime(quint64 time) { quint64 ret = 0; if (attitudeStamped) { ret = lastAttitude; } if (time == 0) { ret = QGC::groundTimeMilliseconds(); } // Check if time is smaller than 40 years, // assuming no system without Unix timestamp // runs longer than 40 years continuously without // reboot. In worst case this will add/subtract the // communication delay between GCS and MAV, // it will never alter the timestamp in a safety // critical way. // // Calculation: // 40 years // 365 days // 24 hours // 60 minutes // 60 seconds // 1000 milliseconds // 1000 microseconds #ifndef _MSC_VER else if (time < 1261440000000000LLU) #else else if (time < 1261440000000000) #endif { // qDebug() << "GEN time:" << time/1000 + onboardTimeOffset; if (onboardTimeOffset == 0 || time < (lastNonNullTime - 100)) { lastNonNullTime = time; onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000; } if (time > lastNonNullTime) lastNonNullTime = time; ret = time/1000 + onboardTimeOffset; } else { // Time is not zero and larger than 40 years -> has to be // a Unix epoch timestamp. Do nothing. ret = time/1000; } return ret; } /** * @param component that will be searched for in the map of parameters. */ QList UAS::getParameterNames(int component) { if (parameters.contains(component)) { return parameters.value(component)->keys(); } else { return QList(); } } QList UAS::getComponentIds() { return parameters.keys(); } /** * @param newBaseMode that UAS is to be set to. * @param newCustomMode that UAS is to be set to. */ void UAS::setMode(uint8_t newBaseMode, uint32_t newCustomMode) { if (receivedMode) { //this->mode = mode; //no call assignament, update receive message from UAS // Strip armed / disarmed call for safety reasons, this is not relevant for setting the mode newBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED; // Now set current state (request no change) newBaseMode |= this->base_mode & MAV_MODE_FLAG_SAFETY_ARMED; // // Strip HIL part, replace it with current system state // newBaseMode &= (~MAV_MODE_FLAG_HIL_ENABLED); // // Now set current state (request no change) // newBaseMode |= this->base_mode & MAV_MODE_FLAG_HIL_ENABLED; setModeArm(newBaseMode, newCustomMode); } else { qDebug() << "WARNING: setMode called before base_mode bitmask was received from UAS, new mode was not sent to system"; } } /** * @param newBaseMode that UAS is to be set to. * @param newCustomMode that UAS is to be set to. */ void UAS::setModeArm(uint8_t newBaseMode, uint32_t newCustomMode) { if (receivedMode) { mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newBaseMode, newCustomMode); qDebug() << "mavlink_msg_set_mode_pack 1"; sendMessage(msg); qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", MODE " << newBaseMode << " " << newCustomMode; } else { qDebug() << "WARNING: setModeArm called before base_mode bitmask was received from UAS, new mode was not sent to system"; } } /** * Send a message to every link that is connected. * @param message that is to be sent */ void UAS::sendMessage(mavlink_message_t message) { if (!LinkManager::instance()) { qWarning() << "LINKMANAGER NOT AVAILABLE!"; return; } if (links->count() < 1) { qDebug() << "NO LINK AVAILABLE TO SEND!"; } // Emit message on all links that are currently connected foreach (LinkInterface* link, *links) { if (LinkManager::instance()->getLinks().contains(link)) { if (link->isConnected()) sendMessage(link, message); } else { // Remove from list links->removeAt(links->indexOf(link)); } } } /** * Forward a message to all links that are currently connected. * @param message that is to be forwarded */ void UAS::forwardMessage(mavlink_message_t message) { // Emit message on all links that are currently connected QListlink_list = LinkManager::instance()->getLinksForProtocol(mavlink); foreach(LinkInterface* link, link_list) { if (link) { SerialLink* serial = dynamic_cast(link); if(serial != 0) { for(int i=0; isize(); i++) { if(serial != links->at(i)) { if (link->isConnected()) { qDebug()<<"Antenna tracking: Forwarding Over link: "<getName()<<" "<getSystemId(), mavlink->getComponentId(), link->getId(), message.len, messageKeys[message.msgid]); // If link is connected if (link->isConnected()) { // Send the portion of the buffer now occupied by the message link->writeBytes((const char*)buffer, len); } else { qDebug() << "LINK NOT CONNECTED, NOT SENDING!"; } } /** * @param value battery voltage */ float UAS::filterVoltage(float value) const { return lpVoltage * 0.6f + value * 0.4f; } /** * Get the status of the code and a description of the status. * Status can be unitialized, booting up, calibrating sensors, active * standby, cirtical, emergency, shutdown or unknown. */ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription) { switch (statusCode) { case MAV_STATE_UNINIT: uasState = tr("UNINIT"); stateDescription = tr("Unitialized, booting up."); break; case MAV_STATE_BOOT: uasState = tr("BOOT"); stateDescription = tr("Booting system, please wait."); break; case MAV_STATE_CALIBRATING: uasState = tr("CALIBRATING"); stateDescription = tr("Calibrating sensors, please wait."); break; case MAV_STATE_ACTIVE: uasState = tr("ACTIVE"); stateDescription = tr("Active, normal operation."); break; case MAV_STATE_STANDBY: uasState = tr("STANDBY"); stateDescription = tr("Standby mode, ready for launch."); break; case MAV_STATE_CRITICAL: uasState = tr("CRITICAL"); stateDescription = tr("FAILURE: Continuing operation."); break; case MAV_STATE_EMERGENCY: uasState = tr("EMERGENCY"); stateDescription = tr("EMERGENCY: Land Immediately!"); break; //case MAV_STATE_HILSIM: //uasState = tr("HIL SIM"); //stateDescription = tr("HIL Simulation, Sensors read from SIM"); //break; case MAV_STATE_POWEROFF: uasState = tr("SHUTDOWN"); stateDescription = tr("Powering off system."); break; default: uasState = tr("UNKNOWN"); stateDescription = tr("Unknown system state"); break; } } QImage UAS::getImage() { // qDebug() << "IMAGE TYPE:" << imageType; // RAW greyscale if (imageType == MAVLINK_DATA_STREAM_IMG_RAW8U) { int imgColors = 255; // Construct PGM header QString header("P5\n%1 %2\n%3\n"); header = header.arg(imageWidth).arg(imageHeight).arg(imgColors); QByteArray tmpImage(header.toStdString().c_str(), header.toStdString().size()); tmpImage.append(imageRecBuffer); //qDebug() << "IMAGE SIZE:" << tmpImage.size() << "HEADER SIZE: (15):" << header.size() << "HEADER: " << header; if (imageRecBuffer.isNull()) { qDebug()<< "could not convertToPGM()"; return QImage(); } if (!image.loadFromData(tmpImage, "PGM")) { qDebug()<< __FILE__ << __LINE__ << "could not create extracted image"; return QImage(); } } // BMP with header else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP || imageType == MAVLINK_DATA_STREAM_IMG_JPEG || imageType == MAVLINK_DATA_STREAM_IMG_PGM || imageType == MAVLINK_DATA_STREAM_IMG_PNG) { if (!image.loadFromData(imageRecBuffer)) { qDebug() << __FILE__ << __LINE__ << "Loading data from image buffer failed!"; } } // Restart statemachine imagePacketsArrived = 0; //imageRecBuffer.clear(); return image; } void UAS::requestImage() { qDebug() << "trying to get an image from the uas..."; // check if there is already an image transmission going on if (imagePacketsArrived == 0) { mavlink_message_t msg; mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, MAVLINK_DATA_STREAM_IMG_JPEG, 0, 0, 0, 0, 0, 50); sendMessage(msg); } } /* MANAGEMENT */ /** * * @return The uptime in milliseconds * */ quint64 UAS::getUptime() const { if(startTime == 0) { return 0; } else { return QGC::groundTimeMilliseconds() - startTime; } } int UAS::getCommunicationStatus() const { return commStatus; } void UAS::requestParameters() { mavlink_message_t msg; mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_ALL); sendMessage(msg); QDateTime time = QDateTime::currentDateTime(); qDebug() << __FILE__ << ":" << __LINE__ << time.toString() << "LOADING PARAM LIST"; } void UAS::writeParametersToStorage() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); qDebug() << "SENT COMMAND" << MAV_CMD_PREFLIGHT_STORAGE; sendMessage(msg); } void UAS::readParametersFromStorage() { mavlink_message_t msg; 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); } bool UAS::isRotaryWing() { switch (type) { case MAV_TYPE_QUADROTOR: /* fallthrough */ case MAV_TYPE_COAXIAL: case MAV_TYPE_HELICOPTER: case MAV_TYPE_HEXAROTOR: case MAV_TYPE_OCTOROTOR: case MAV_TYPE_TRICOPTER: return true; default: return false; } } bool UAS::isFixedWing() { switch (type) { case MAV_TYPE_FIXED_WING: return true; default: return false; } } /** * @param rate The update rate in Hz the message should be sent */ void UAS::enableAllDataTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on // 0 is a magic ID and will enable/disable the standard message set except for heartbeat stream.req_stream_id = MAV_DATA_STREAM_ALL; // Select the update rate in Hz the message should be send // All messages will be send with their default rate // TODO: use 0 to turn off and get rid of enable/disable? will require // a different magic flag for turning on defaults, possibly something really high like 1111 ? stream.req_message_rate = 0; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); } /** * @param rate The update rate in Hz the message should be sent */ void UAS::enableRawSensorDataTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSORS; // Select the update rate in Hz the message should be send stream.req_message_rate = rate; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); } /** * @param rate The update rate in Hz the message should be sent */ void UAS::enableExtendedSystemStatusTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_EXTENDED_STATUS; // Select the update rate in Hz the message should be send stream.req_message_rate = rate; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); } /** * @param rate The update rate in Hz the message should be sent */ void UAS::enableRCChannelDataTransmission(int rate) { #if defined(MAVLINK_ENABLED_UALBERTA_MESSAGES) mavlink_message_t msg; mavlink_msg_request_rc_channels_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, enabled); sendMessage(msg); #else mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_RC_CHANNELS; // Select the update rate in Hz the message should be send stream.req_message_rate = rate; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); #endif } /** * @param rate The update rate in Hz the message should be sent */ void UAS::enableRawControllerDataTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_RAW_CONTROLLER; // Select the update rate in Hz the message should be send stream.req_message_rate = rate; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); } //void UAS::enableRawSensorFusionTransmission(int rate) //{ // // Buffers to write data to // mavlink_message_t msg; // mavlink_request_data_stream_t stream; // // Select the message to request from now on // stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION; // // Select the update rate in Hz the message should be send // stream.req_message_rate = rate; // // Start / stop the message // stream.start_stop = (rate) ? 1 : 0; // // The system which should take this command // stream.target_system = uasId; // // The component / subsystem which should take this command // stream.target_component = 0; // // Encode and send the message // mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // // Send message twice to increase chance of reception // sendMessage(msg); // sendMessage(msg); //} /** * @param rate The update rate in Hz the message should be sent */ void UAS::enablePositionTransmission(int rate) { // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_POSITION; // Select the update rate in Hz the message should be send stream.req_message_rate = rate; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); } /** * @param rate The update rate in Hz the message should be sent */ void UAS::enableExtra1Transmission(int rate) { // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_EXTRA1; // Select the update rate in Hz the message should be send stream.req_message_rate = rate; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); } /** * @param rate The update rate in Hz the message should be sent */ void UAS::enableExtra2Transmission(int rate) { // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_EXTRA2; // Select the update rate in Hz the message should be send stream.req_message_rate = rate; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); } /** * @param rate The update rate in Hz the message should be sent */ void UAS::enableExtra3Transmission(int rate) { // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; // Select the message to request from now on stream.req_stream_id = MAV_DATA_STREAM_EXTRA3; // Select the update rate in Hz the message should be send stream.req_message_rate = rate; // Start / stop the message stream.start_stop = (rate) ? 1 : 0; // The system which should take this command stream.target_system = uasId; // The component / subsystem which should take this command stream.target_component = 0; // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); } /** * Set a parameter value onboard * * @param component The component to set the parameter * @param id Name of the parameter */ void UAS::setParameter(const int compId, const QString& paramId, const QVariant& value) { if (!paramId.isNull()) { mavlink_message_t msg; mavlink_param_set_t p; mavlink_param_union_t union_value; // Assign correct value based on QVariant // TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling. if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { switch ((int)value.type()) { case QVariant::Char: union_value.param_float = (unsigned char)value.toChar().toLatin1(); p.param_type = MAV_PARAM_TYPE_INT8; break; case QVariant::Int: union_value.param_float = value.toInt(); p.param_type = MAV_PARAM_TYPE_INT32; break; case QVariant::UInt: union_value.param_float = value.toUInt(); p.param_type = MAV_PARAM_TYPE_UINT32; break; case QMetaType::Float: union_value.param_float = value.toFloat(); p.param_type = MAV_PARAM_TYPE_REAL32; break; default: qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE"; return; } } else { switch ((int)value.type()) { case QVariant::Char: union_value.param_int8 = (unsigned char)value.toChar().toLatin1(); p.param_type = MAV_PARAM_TYPE_INT8; break; case QVariant::Int: union_value.param_int32 = value.toInt(); p.param_type = MAV_PARAM_TYPE_INT32; break; case QVariant::UInt: union_value.param_uint32 = value.toUInt(); p.param_type = MAV_PARAM_TYPE_UINT32; break; case QMetaType::Float: union_value.param_float = value.toFloat(); p.param_type = MAV_PARAM_TYPE_REAL32; break; default: qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE"; return; } } p.param_value = union_value.param_float; p.target_system = (uint8_t)uasId; p.target_component = (uint8_t)compId; //qDebug() << "SENT PARAM:" << value; // Copy string into buffer, ensuring not to exceed the buffer size for (unsigned int i = 0; i < sizeof(p.param_id); i++) { // String characters if ((int)i < paramId.length()) { p.param_id[i] = paramId.toLatin1()[i]; } else { // Fill rest with zeros p.param_id[i] = 0; } } mavlink_msg_param_set_encode(mavlink->getSystemId(), compId, &msg, &p); sendMessage(msg); } } //TODO update this to use the parameter manager / param data model instead void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue) { int compId = msg.compid; // Insert component if necessary if (!parameters.contains(compId)) { parameters.insert(compId, new QMap()); } // Insert parameter into registry if (parameters.value(compId)->contains(paramName)) { parameters.value(compId)->remove(paramName); } QVariant param; // Insert with correct type // TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling. switch (rawValue.param_type) { case MAV_PARAM_TYPE_REAL32: { if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { param = QVariant(paramValue.param_float); } else { param = QVariant(paramValue.param_float); } parameters.value(compId)->insert(paramName, param); // Emit change emit parameterChanged(uasId, compId, paramName, param); emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); // qDebug() << "RECEIVED PARAM:" << param; } break; case MAV_PARAM_TYPE_UINT8: { if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { param = QVariant(QChar((unsigned char)paramValue.param_float)); } else { param = QVariant(QChar((unsigned char)paramValue.param_uint8)); } parameters.value(compId)->insert(paramName, param); // Emit change emit parameterChanged(uasId, compId, paramName, param); emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); //qDebug() << "RECEIVED PARAM:" << param; } break; case MAV_PARAM_TYPE_INT8: { if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { param = QVariant(QChar((char)paramValue.param_float)); } else { param = QVariant(QChar((char)paramValue.param_int8)); } parameters.value(compId)->insert(paramName, param); // Emit change emit parameterChanged(uasId, compId, paramName, param); emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); //qDebug() << "RECEIVED PARAM:" << param; } break; case MAV_PARAM_TYPE_INT16: { if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { param = QVariant((short)paramValue.param_float); } else { param = QVariant(paramValue.param_int16); } parameters.value(compId)->insert(paramName, param); // Emit change emit parameterChanged(uasId, compId, paramName, param); emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); //qDebug() << "RECEIVED PARAM:" << param; } break; case MAV_PARAM_TYPE_UINT32: { if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { param = QVariant((unsigned int)paramValue.param_float); } else { param = QVariant(paramValue.param_uint32); } parameters.value(compId)->insert(paramName, param); // Emit change emit parameterChanged(uasId, compId, paramName, param); emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); } break; case MAV_PARAM_TYPE_INT32: { if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { param = QVariant((int)paramValue.param_float); } else { param = QVariant(paramValue.param_int32); } parameters.value(compId)->insert(paramName, param); // Emit change emit parameterChanged(uasId, compId, paramName, param); emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); // qDebug() << "RECEIVED PARAM:" << param; } break; default: qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type; } //switch (value.param_type) } /** * Request parameter, use parameter name to request it. */ void UAS::requestParameter(int component, int id) { // Request parameter, use parameter name to request it mavlink_message_t msg; mavlink_param_request_read_t read; read.param_index = id; read.param_id[0] = '\0'; // Enforce null termination read.target_system = uasId; read.target_component = component; mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read); sendMessage(msg); //qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM ID" << id; } /** * Request a parameter, use parameter name to request it. */ void UAS::requestParameter(int component, const QString& parameter) { // Request parameter, use parameter name to request it mavlink_message_t msg; mavlink_param_request_read_t read; read.param_index = -1; // Copy full param name or maximum max field size if (parameter.length() > MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN) { emit textMessageReceived(uasId, 0, MAV_SEVERITY_WARNING, QString("QGC WARNING: Parameter name %1 is more than %2 bytes long. This might lead to errors and mishaps!").arg(parameter).arg(MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN-1)); } memcpy(read.param_id, parameter.toStdString().c_str(), qMax(parameter.length(), MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN)); read.param_id[15] = '\0'; // Enforce null termination read.target_system = uasId; read.target_component = component; mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read); sendMessage(msg); //qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM NAME" << parameter; } /** * @param systemType Type of MAV. */ void UAS::setSystemType(int systemType) { if((systemType >= MAV_TYPE_GENERIC) && (systemType < MAV_TYPE_ENUM_END)) { type = systemType; // If the airframe is still generic, change it to a close default type if (airframe == 0) { switch (systemType) { case MAV_TYPE_FIXED_WING: airframe = QGC_AIRFRAME_EASYSTAR; break; case MAV_TYPE_QUADROTOR: airframe = QGC_AIRFRAME_MIKROKOPTER; break; } } emit systemSpecsChanged(uasId); } } void UAS::setUASName(const QString& name) { if (name != "") { this->name = name; writeSettings(); emit nameChanged(name); emit systemSpecsChanged(uasId); } } void UAS::executeCommand(MAV_CMD command) { mavlink_message_t msg; mavlink_command_long_t cmd; cmd.command = (uint16_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_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); sendMessage(msg); } void UAS::executeCommandAck(int num, bool success) { mavlink_message_t msg; mavlink_command_ack_t ack; ack.command = num; ack.result = (success ? 1 : 0); mavlink_msg_command_ack_encode(mavlink->getSystemId(),mavlink->getComponentId(),&msg,&ack); sendMessage(msg); } void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) { mavlink_message_t msg; mavlink_command_long_t cmd; cmd.command = (uint16_t)command; cmd.confirmation = confirmation; cmd.param1 = param1; cmd.param2 = param2; cmd.param3 = param3; cmd.param4 = param4; cmd.param5 = param5; cmd.param6 = param6; cmd.param7 = param7; cmd.target_system = uasId; cmd.target_component = component; mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); sendMessage(msg); } /** * Launches the system * */ void UAS::launch() { mavlink_message_t msg; 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); } /** * @warning Depending on the UAS, this might make the rotors of a helicopter spinning * */ void UAS::armSystem() { setModeArm(base_mode | MAV_MODE_FLAG_SAFETY_ARMED, custom_mode); } /** * @warning Depending on the UAS, this might completely stop all motors. * */ void UAS::disarmSystem() { setModeArm(base_mode & ~(MAV_MODE_FLAG_SAFETY_ARMED), custom_mode); } void UAS::toggleArmedState() { setModeArm(base_mode ^ (MAV_MODE_FLAG_SAFETY_ARMED), custom_mode); } void UAS::goAutonomous() { setMode((base_mode & ~(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)) | (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED), 0); qDebug() << __FILE__ << __LINE__ << "Going autonomous"; } void UAS::goManual() { setMode((base_mode & ~(MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)) | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0); qDebug() << __FILE__ << __LINE__ << "Going manual"; } void UAS::toggleAutonomy() { setMode(base_mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED ^ MAV_MODE_FLAG_GUIDED_ENABLED ^ MAV_MODE_FLAG_STABILIZE_ENABLED, 0); qDebug() << __FILE__ << __LINE__ << "Toggling autonomy"; } /** * Set the manual control commands. * This can only be done if the system has manual inputs enabled and is armed. */ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, JoystickInput::JOYSTICK_MODE joystickMode) { Q_UNUSED(xHat); Q_UNUSED(yHat); // Store the previous manual commands static float manualRollAngle = 0.0; static float manualPitchAngle = 0.0; static float manualYawAngle = 0.0; static float manualThrust = 0.0; static quint16 manualButtons = 0; static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission // Transmit the external setpoints only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate // if no command inputs have changed. // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz. bool sendCommand = false; if (countSinceLastTransmission++ >= 5) { sendCommand = true; countSinceLastTransmission = 0; } else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) || (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) || buttons != manualButtons) { sendCommand = true; // Ensure that another message will be sent the next time this function is called countSinceLastTransmission = 10; } // Now if we should trigger an update, let's do that if (sendCommand) { // Save the new manual control inputs manualRollAngle = roll; manualPitchAngle = pitch; manualYawAngle = yaw; manualThrust = thrust; manualButtons = buttons; mavlink_message_t message; if (joystickMode == JoystickInput::JOYSTICK_MODE_ATTITUDE) { // send an external attitude setpoint command (rate control disabled) float attitudeQuaternion[4]; mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); uint8_t typeMask = 0b111; // disable rate control mavlink_msg_attitude_setpoint_external_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, QGC::groundTimeUsecs(), this->uasId, 0, typeMask, attitudeQuaternion, 0, 0, 0, thrust ); } else if (joystickMode == JoystickInput::JOYSTICK_MODE_FORCE) { // Send the the force setpoint (local pos sp external message) float dcm[3][3]; mavlink_euler_to_dcm(roll, pitch, yaw, dcm); const float fx = -dcm[0][2]; const float fy = -dcm[1][2]; const float fz = -dcm[2][2]; uint16_t typeMask = 0b0000001000111111; // select only force control (disable everything else) mavlink_msg_local_ned_position_setpoint_external_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, QGC::groundTimeUsecs(), this->uasId, 0, MAV_FRAME_LOCAL_NED, typeMask, 0, 0, 0, 0, 0, 0, fx, fy, fz ); } else { return; } sendMessage(message); // Emit an update in control values to other UI elements, like the HSI display emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); } } void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw) { // If system has manual inputs enabled and is armed if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) { mavlink_message_t message; mavlink_msg_setpoint_6dof_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)x, (float)y, (float)z, (float)roll, (float)pitch, (float)yaw); sendMessage(message); qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGE: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw; //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); } else { qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first"; } } /** * @return the type of the system */ int UAS::getSystemType() { return this->type; } /** * Halt the uas. */ void UAS::halt() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_HOLD, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0); sendMessage(msg); } /** * Make the UAS move. */ void UAS::go() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0); sendMessage(msg); } /** * Order the robot to return home */ void UAS::home() { mavlink_message_t msg; double latitude = UASManager::instance()->getHomeLatitude(); double longitude = UASManager::instance()->getHomeLongitude(); double altitude = UASManager::instance()->getHomeAltitude(); int frame = UASManager::instance()->getHomeFrame(); mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, frame, 0, latitude, longitude, altitude); sendMessage(msg); } /** * Order the robot to land on the runway */ void UAS::land() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_LAND, 1, 0, 0, 0, 0, 0, 0, 0); sendMessage(msg); } /** * Order the robot to start receiver pairing */ void UAS::pairRX(int rxType, int rxSubType) { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0); sendMessage(msg); } /** * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation * and might differ between systems. */ void UAS::emergencySTOP() { // FIXME MAVLINKV10PORTINGNEEDED halt(); } /** * Shut down this mav - All onboard systems are immediately shut down (e.g. the * main power line is cut). * @warning This might lead to a crash. * * The command will not be executed until emergencyKILLConfirm is issues immediately afterwards */ 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; return false; } /** * If enabled, connect the flight gear link. */ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration) { QGCFlightGearLink* link = dynamic_cast(simulation); if (!link || !simulation) { // Delete wrong sim if (simulation) { stopHil(); delete simulation; } simulation = new QGCFlightGearLink(this, options); } // Connect Flight Gear Link link = dynamic_cast(simulation); link->setStartupArguments(options); link->sensorHilEnabled(sensorHil); QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float))); if (enable) { startHil(); } else { stopHil(); } } /** * If enabled, connect the JSBSim link. */ void UAS::enableHilJSBSim(bool enable, QString options) { QGCJSBSimLink* link = dynamic_cast(simulation); if (!link || !simulation) { // Delete wrong sim if (simulation) { stopHil(); delete simulation; } simulation = new QGCJSBSimLink(this, options); } // Connect Flight Gear Link link = dynamic_cast(simulation); link->setStartupArguments(options); if (enable) { startHil(); } else { stopHil(); } } /** * If enabled, connect the X-plane gear link. */ void UAS::enableHilXPlane(bool enable) { QGCXPlaneLink* link = dynamic_cast(simulation); if (!link || !simulation) { if (simulation) { stopHil(); delete simulation; } qDebug() << "CREATED NEW XPLANE LINK"; simulation = new QGCXPlaneLink(this); } // Connect X-Plane Link if (enable) { startHil(); } else { stopHil(); } } /** * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll Roll angle (rad) * @param pitch Pitch angle (rad) * @param yaw Yaw angle (rad) * @param rollspeed Roll angular speed (rad/s) * @param pitchspeed Pitch angular speed (rad/s) * @param yawspeed Yaw angular speed (rad/s) * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 * @param alt Altitude in meters, expressed as * 1000 (millimeters) * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 * @param xacc X acceleration (mg) * @param yacc Y acceleration (mg) * @param zacc Z acceleration (mg) */ void UAS::sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, double lat, double lon, double alt, float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) { Q_UNUSED(time_us); Q_UNUSED(xacc); Q_UNUSED(yacc); Q_UNUSED(zacc); // Emit attitude for cross-check emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime()); emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime()); emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime()); emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime()); emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime()); emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime()); emit valueChanged(uasId, "lat sim", "deg", lat*1e7, getUnixTime()); emit valueChanged(uasId, "lon sim", "deg", lon*1e7, getUnixTime()); emit valueChanged(uasId, "alt sim", "deg", alt*1e3, getUnixTime()); emit valueChanged(uasId, "vx sim", "m/s", vx*1e2, getUnixTime()); emit valueChanged(uasId, "vy sim", "m/s", vy*1e2, getUnixTime()); emit valueChanged(uasId, "vz sim", "m/s", vz*1e2, getUnixTime()); emit valueChanged(uasId, "IAS sim", "m/s", ind_airspeed, getUnixTime()); emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime()); } /** * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll Roll angle (rad) * @param pitch Pitch angle (rad) * @param yaw Yaw angle (rad) * @param rollspeed Roll angular speed (rad/s) * @param pitchspeed Pitch angular speed (rad/s) * @param yawspeed Yaw angular speed (rad/s) * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 * @param alt Altitude in meters, expressed as * 1000 (millimeters) * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 * @param xacc X acceleration (mg) * @param yacc Y acceleration (mg) * @param zacc Z acceleration (mg) */ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, double lat, double lon, double alt, float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) { if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED) { float q[4]; double cosPhi_2 = cos(double(roll) / 2.0); double sinPhi_2 = sin(double(roll) / 2.0); double cosTheta_2 = cos(double(pitch) / 2.0); double sinTheta_2 = sin(double(pitch) / 2.0); double cosPsi_2 = cos(double(yaw) / 2.0); double sinPsi_2 = sin(double(yaw) / 2.0); q[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2); q[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2); q[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2); q[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); mavlink_message_t msg; mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, q, rollspeed, pitchspeed, yawspeed, lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81); sendMessage(msg); } else { // Attempt to set HIL mode setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode); qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; } } /* * @param abs_pressure Absolute Pressure (hPa) * @param diff_pressure Differential Pressure (hPa) */ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) { if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED) { mavlink_message_t msg; mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); sendMessage(msg); lastSendTimeSensors = QGC::groundTimeMilliseconds(); } else { // Attempt to set HIL mode setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode); qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; } } void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) { // Only send at 10 Hz max rate if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100) return; if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED) { float course = cog; // map to 0..2pi if (course < 0) course += 2.0f * static_cast(M_PI); // scale from radians to degrees course = (course / M_PI) * 180.0f; mavlink_message_t msg; mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites); lastSendTimeGPS = QGC::groundTimeMilliseconds(); sendMessage(msg); } else { // Attempt to set HIL mode setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode); qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; } } /** * Connect flight gear link. **/ void UAS::startHil() { if (hilEnabled) return; hilEnabled = true; sensorHil = false; setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode); qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; // Connect HIL simulation link simulation->connectSimulation(); } /** * disable flight gear link. */ void UAS::stopHil() { if (simulation && simulation->isConnected()) { simulation->disconnectSimulation(); setMode(base_mode & ~MAV_MODE_FLAG_HIL_ENABLED, custom_mode); qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable."; } hilEnabled = false; sensorHil = false; } void UAS::shutdown() { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); msgBox.setText("Shutting down the UAS"); msgBox.setInformativeText("Do you want to shut down the onboard computer?"); 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) { // If the active UAS is set, execute command mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 2, 0, 0, 0, 0, 0); sendMessage(msg); } } /** * @param x position * @param y position * @param z position * @param yaw */ void UAS::setTargetPosition(float x, float y, float z, float yaw) { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 1, 1, 0, yaw, x, y, z); sendMessage(msg); } /** * @return The name of this system as string in human-readable form */ QString UAS::getUASName(void) const { QString result; if (name == "") { result = tr("MAV ") + result.sprintf("%03d", getUASID()); } else { result = name; } return result; } /** * @return the state of the uas as a short text. */ const QString& UAS::getShortState() const { return shortStateText; } /** * The mode can be autonomous, guided, manual or armed. It will also return if * hardware in the loop is being used. * @return the audio mode text for the id given. */ QString UAS::getAudioModeTextFor(int id) { QString mode; uint8_t modeid = id; // BASE MODE DECODING if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO) { mode += "autonomous"; } else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED) { mode += "guided"; } else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) { mode += "stabilized"; } else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL) { mode += "manual"; } else { // Nothing else applies, we're in preflight mode += "preflight"; } if (modeid != 0) { mode += " mode"; } // ARMED STATE DECODING if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) { mode.append(" and armed"); } // HARDWARE IN THE LOOP DECODING if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL) { mode.append(" using hardware in the loop simulation"); } return mode; } /** * The mode returned can be auto, stabilized, test, manual, preflight or unknown. * @return the short text of the mode for the id given. */ /** * The mode returned can be auto, stabilized, test, manual, preflight or unknown. * @return the short text of the mode for the id given. */ QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot) { QString mode = ""; if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { // use custom_mode - autopilot-specific if (autopilot == MAV_AUTOPILOT_PX4) { union px4_custom_mode px4_mode; px4_mode.data = custom_mode; if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { mode += "|MANUAL"; } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { mode += "|ALTCTL"; } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { mode += "|POSCTL"; } else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { mode += "|AUTO"; if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_READY) { mode += "|READY"; } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF) { mode += "|TAKEOFF"; } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER) { mode += "|LOITER"; } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_MISSION) { mode += "|MISSION"; } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL) { mode += "|RTL"; } else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LAND) { mode += "|LAND"; } } } } // fallback to using base_mode if (mode.length() == 0) { // use base_mode - not autopilot-specific if (base_mode == 0) { mode += "|PREFLIGHT"; } else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) { mode += "|AUTO"; } else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) { mode += "|MANUAL"; if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED) { mode += "|GUIDED"; } else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) { mode += "|STABILIZED"; } } } if (mode.length() == 0) { mode = "|UNKNOWN"; qDebug() << __FILE__ << __LINE__ << " Unknown mode: base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot; } // ARMED STATE DECODING if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) { mode.prepend("A"); } else { mode.prepend("D"); } // HARDWARE IN THE LOOP DECODING if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL) { mode.prepend("HIL:"); } //qDebug() << "base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot << ": " << mode; return mode; } const QString& UAS::getShortMode() const { return shortModeText; } /** * Add the link and connect a signal to it which will be set off when it is destroyed. */ void UAS::addLink(LinkInterface* link) { if (!links->contains(link)) { links->append(link); connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); } } void UAS::removeLink(QObject* object) { LinkInterface* link = dynamic_cast(object); if (link) { links->removeAt(links->indexOf(link)); } } /** * @return the list of links */ QList* UAS::getLinks() { return links; } /** * @rerturn the map of the components */ QMap UAS::getComponents() { return components; } /** * Set the battery type and the number of cells. * @param type of the battery * @param cells Number of cells. */ void UAS::setBattery(BatteryType type, int cells) { this->batteryType = type; this->cells = cells; switch (batteryType) { case NICD: break; case NIMH: break; case LIION: break; case LIPOLY: fullVoltage = this->cells * lipoFull; emptyVoltage = this->cells * lipoEmpty; break; case LIFE: break; case AGZN: break; } } /** * Set the battery specificaitons: empty voltage, warning voltage, and full voltage. * @param specifications of the battery */ void UAS::setBatterySpecs(const QString& specs) { if (specs.length() == 0 || specs.contains("%")) { batteryRemainingEstimateEnabled = false; bool ok; QString percent = specs; percent = percent.remove("%"); float temp = percent.toFloat(&ok); if (ok) { warnLevelPercent = temp; } else { emit textMessageReceived(0, 0, MAV_SEVERITY_WARNING, "Could not set battery options, format is wrong"); } } else { batteryRemainingEstimateEnabled = true; QString stringList = specs; stringList = stringList.remove("V"); stringList = stringList.remove("v"); QStringList parts = stringList.split(","); if (parts.length() == 3) { float temp; bool ok; // Get the empty voltage temp = parts.at(0).toFloat(&ok); if (ok) emptyVoltage = temp; // Get the warning voltage temp = parts.at(1).toFloat(&ok); if (ok) warnVoltage = temp; // Get the full voltage temp = parts.at(2).toFloat(&ok); if (ok) fullVoltage = temp; } else { emit textMessageReceived(0, 0, MAV_SEVERITY_WARNING, "Could not set battery options, format is wrong"); } } } /** * @return the battery specifications(empty voltage, warning voltage, full voltage) */ QString UAS::getBatterySpecs() { if (batteryRemainingEstimateEnabled) { return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage); } else { return QString("%1%").arg(warnLevelPercent); } } /** * @return the time remaining. */ int UAS::calculateTimeRemaining() { quint64 dt = QGC::groundTimeMilliseconds() - startTime; double seconds = dt / 1000.0f; double voltDifference = startVoltage - currentVoltage; if (voltDifference <= 0) voltDifference = 0.00000000001f; double dischargePerSecond = voltDifference / seconds; int remaining = static_cast((currentVoltage - emptyVoltage) / dischargePerSecond); // Can never be below 0 if (remaining < 0) remaining = 0; return remaining; } /** * @return charge level in percent - 0 - 100 */ float UAS::getChargeLevel() { if (batteryRemainingEstimateEnabled) { if (lpVoltage < emptyVoltage) { chargeLevel = 0.0f; } else if (lpVoltage > fullVoltage) { chargeLevel = 100.0f; } else { chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); } } return chargeLevel; } void UAS::startLowBattAlarm() { if (!lowBattAlarm) { GAudioOutput::instance()->alert(tr("system %1 has low battery").arg(getUASName())); QTimer::singleShot(3000, GAudioOutput::instance(), SLOT(startEmergency())); lowBattAlarm = true; } } void UAS::stopLowBattAlarm() { if (lowBattAlarm) { GAudioOutput::instance()->stopEmergency(); lowBattAlarm = false; } }