diff --git a/QGCApplication.pro b/QGCApplication.pro index acc9756e1ed0f7f330248c2c82467b881089f252..f32186ca295398c22ae02db366304c2c95c6f3f1 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -235,10 +235,8 @@ HEADERS += \ src/comm/LinkInterface.h \ src/comm/LinkManager.h \ src/comm/MAVLinkProtocol.h \ - src/comm/MAVLinkSimulationLink.h \ - src/comm/MAVLinkSimulationMAV.h \ - src/comm/MAVLinkSimulationWaypointPlanner.h \ - src/comm/MAVLinkSwarmSimulationLink.h \ + src/comm/MockLink.h \ + src/comm/MockLinkMissionItemHandler.h \ src/comm/ProtocolInterface.h \ src/comm/QGCFlightGearLink.h \ src/comm/QGCHilLink.h \ @@ -357,12 +355,6 @@ HEADERS += \ src/ViewWidgets/ViewWidgetController.h \ src/Waypoint.h \ -DebugBuild { -HEADERS += \ - src/comm/MockLink.h \ - src/comm/MockLinkMissionItemHandler.h -} - !AndroidBuild { HEADERS += \ src/input/JoystickInput.h \ @@ -378,10 +370,8 @@ SOURCES += \ src/comm/LinkConfiguration.cc \ src/comm/LinkManager.cc \ src/comm/MAVLinkProtocol.cc \ - src/comm/MAVLinkSimulationLink.cc \ - src/comm/MAVLinkSimulationMAV.cc \ - src/comm/MAVLinkSimulationWaypointPlanner.cc \ - src/comm/MAVLinkSwarmSimulationLink.cc \ + src/comm/MockLink.cc \ + src/comm/MockLinkMissionItemHandler.cc \ src/comm/QGCFlightGearLink.cc \ src/comm/QGCJSBSimLink.cc \ src/comm/QGCXPlaneLink.cc \ @@ -492,12 +482,6 @@ SOURCES += \ src/ViewWidgets/ViewWidgetController.cc \ src/Waypoint.cc \ -DebugBuild { -SOURCES += \ - src/comm/MockLink.cc \ - src/comm/MockLinkMissionItemHandler.cc -} - !AndroidBuild { SOURCES += \ src/input/JoystickInput.cc \ diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index b9db5f144986cbca3bc00bcd738f31077eee0cf7..90c5e5a2ba39a38e05e4da4fc7e73c86cec770e7 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -49,7 +49,6 @@ #include "QGCMessageBox.h" #include "MainWindow.h" #include "UDPLink.h" -#include "MAVLinkSimulationLink.h" #include "SerialLink.h" #include "QGCSingleton.h" #include "LinkManager.h" diff --git a/src/comm/LinkConfiguration.h b/src/comm/LinkConfiguration.h index 8f8f6009cd794ee10186a8e2a99346d7f62ccaa1..34639bc60d44f3936adfe1552aaa0dae60c40d18 100644 --- a/src/comm/LinkConfiguration.h +++ b/src/comm/LinkConfiguration.h @@ -44,14 +44,11 @@ public: TypeTcp, ///< TCP Link // TODO Below is not yet implemented #if 0 - TypeSimulation, ///< Simulation Link TypeForwarding, ///< Forwarding Link TypeXbee, ///< XBee Proprietary Link TypeOpal, ///< Opal-RT Link #endif -#ifdef QT_DEBUG TypeMock, ///< Mock Link for Unitesting -#endif TypeLast // Last type value (type >= TypeLast == invalid) }; diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc deleted file mode 100644 index 8887c3f1d20fb1067c7515e963f7812736b31d47..0000000000000000000000000000000000000000 --- a/src/comm/MAVLinkSimulationLink.cc +++ /dev/null @@ -1,860 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Implementation of simulated system link - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "LinkManager.h" -#include "MAVLinkProtocol.h" -#include "MAVLinkSimulationLink.h" -#include "QGCMAVLink.h" -#include "QGC.h" -#include "MAVLinkSimulationMAV.h" - -/** - * Create a simulated link. This link is connected to an input and output file. - * The link sends one line at a time at the specified sendrate. The timing of - * the sendrate is free of drift, which means it is stable on the long run. - * However, small deviations are mixed in to vary the sendrate slightly - * in order to simulate disturbances on a real communication link. - * - * @param readFile The file with the messages to read (must be in ASCII format, line breaks can be Unix or Windows style) - * @param writeFile The received messages are written to that file - * @param rate The rate at which the messages are sent (in intervals of milliseconds) - **/ -MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile, int rate) : - readyBytes(0), - timeOffset(0) -{ - this->rate = rate; - _isConnected = false; - - onboardParams = QMap(); - onboardParams.insert("PID_ROLL_K_P", 0.5f); - onboardParams.insert("PID_PITCH_K_P", 0.5f); - onboardParams.insert("PID_YAW_K_P", 0.5f); - onboardParams.insert("PID_XY_K_P", 100.0f); - onboardParams.insert("PID_ALT_K_P", 0.5f); - onboardParams.insert("SYS_TYPE", 1); - onboardParams.insert("SYS_ID", systemId); - onboardParams.insert("RC4_REV", 0); - onboardParams.insert("RC5_REV", 1); - onboardParams.insert("HDNG2RLL_P", 0.7f); - onboardParams.insert("HDNG2RLL_I", 0.01f); - onboardParams.insert("HDNG2RLL_D", 0.02f); - onboardParams.insert("HDNG2RLL_IMAX", 500.0f); - onboardParams.insert("RLL2SRV_P", 0.4f); - onboardParams.insert("RLL2SRV_I", 0.0f); - onboardParams.insert("RLL2SRV_D", 0.0f); - onboardParams.insert("RLL2SRV_IMAX", 500.0f); - - // Comments on the variables can be found in the header file - - simulationFile = new QFile(readFile, this); - if (simulationFile->exists() && simulationFile->open(QIODevice::ReadOnly | QIODevice::Text)) - { - simulationHeader = simulationFile->readLine(); - } - receiveFile = new QFile(writeFile, this); - lastSent = QGC::groundTimeMilliseconds(); - - if (simulationFile->exists()) - { - this->name = "Simulation: " + QFileInfo(simulationFile->fileName()).fileName(); - } - else - { - this->name = "MAVLink simulation link"; - } - - - - // Initialize the pseudo-random number generator - srand(QTime::currentTime().msec()); - maxTimeNoise = 0; - LinkManager::instance()->_addLink(this); -} - -MAVLinkSimulationLink::~MAVLinkSimulationLink() -{ - //TODO Check destructor - // fileStream->flush(); - // outStream->flush(); - // Force termination, there is no - // need for cleanup since - // this thread is not manipulating - // any relevant data - terminate(); - delete simulationFile; -} - -void MAVLinkSimulationLink::run() -{ - - status.voltage_battery = 0; - status.errors_comm = 0; - - system.base_mode = MAV_MODE_PREFLIGHT; - system.custom_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED; - system.system_status = MAV_STATE_UNINIT; - - forever - { - static quint64 last = 0; - - if (QGC::groundTimeMilliseconds() - last >= rate) - { - if (_isConnected) - { - mainloop(); - readBytes(); - } - else - { - // Sleep for substantially longer - // if not connected - QGC::SLEEP::msleep(500); - } - last = QGC::groundTimeMilliseconds(); - } - QGC::SLEEP::msleep(3); - } -} - -void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg) -{ - // Allocate buffer with packet data - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg); - - // Pack to link buffer - readyBufferMutex.lock(); - for (unsigned int i = 0; i < bufferlength; i++) - { - readyBuffer.enqueue(*(buf + i)); - } - readyBufferMutex.unlock(); -} - -void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg) -{ - // Allocate buffer with packet data - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg); - //add data into datastream - memcpy(stream+(*index),buf, bufferlength); - (*index) += bufferlength; -} - -void MAVLinkSimulationLink::mainloop() -{ - - // Test for encoding / decoding packets - - // Test data stream - streampointer = 0; - - // Fake system values - - static float fullVoltage = 4.2f * 3.0f; - static float emptyVoltage = 3.35f * 3.0f; - static float voltage = fullVoltage; - static float drainRate = 0.025f; // x.xx% of the capacity is linearly drained per second - - mavlink_attitude_t attitude; - memset(&attitude, 0, sizeof(mavlink_attitude_t)); - mavlink_raw_imu_t rawImuValues; - memset(&rawImuValues, 0, sizeof(mavlink_raw_imu_t)); - - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - int bufferlength; - mavlink_message_t msg; - - // Timers - static unsigned int rate1hzCounter = 1; - static unsigned int rate10hzCounter = 1; - static unsigned int rate50hzCounter = 1; - static unsigned int circleCounter = 0; - - // Vary values - - // VOLTAGE - // The battery is drained constantly - voltage = voltage - ((fullVoltage - emptyVoltage) * drainRate / rate); - if (voltage < 3.550f * 3.0f) voltage = 3.550f * 3.0f; - - static int state = 0; - - if (state == 0) - { - state++; - } - - - // 50 HZ TASKS - if (rate50hzCounter == 1000 / rate / 40) - { - if (simulationFile->isOpen()) - { - if (simulationFile->atEnd()) { - // We reached the end of the file, start from scratch - simulationFile->reset(); - simulationHeader = simulationFile->readLine(); - } - - // Data was made available, read one line - // first entry is the timestamp - QString values = QString(simulationFile->readLine()); - QStringList parts = values.split("\t"); - QStringList keys = simulationHeader.split("\t"); - //qDebug() << simulationHeader; - //qDebug() << values; - bool ok; - static quint64 lastTime = 0; - static quint64 baseTime = 0; - quint64 time = QString(parts.first()).toLongLong(&ok, 10); - // FIXME Remove multiplicaton by 1000 - time *= 1000; - - if (ok) { - if (timeOffset == 0) { - timeOffset = time; - baseTime = time; - } - - if (lastTime > time) { - // We have wrapped around in the logfile - // Add the measurement time interval to the base time - baseTime += lastTime - timeOffset; - } - lastTime = time; - - time = time - timeOffset + baseTime; - - // Gather individual measurement values - for (int i = 1; i < (parts.size() - 1); ++i) { - // Get one data field - bool res; - double d = QString(parts.at(i)).toDouble(&res); - if (!res) d = 0; - - if (keys.value(i, "") == "Accel._X") { - rawImuValues.xacc = d; - } - - if (keys.value(i, "") == "Accel._Y") { - rawImuValues.yacc = d; - } - - if (keys.value(i, "") == "Accel._Z") { - rawImuValues.zacc = d; - } - if (keys.value(i, "") == "Gyro_Phi") { - rawImuValues.xgyro = d; - attitude.rollspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65; - } - - if (keys.value(i, "") == "Gyro_Theta") { - rawImuValues.ygyro = d; - attitude.pitchspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65; - } - - if (keys.value(i, "") == "Gyro_Psi") { - rawImuValues.zgyro = d; - attitude.yawspeed = ((d-29.000)/3000.0)*2.7-2.7-2.65; - } - if (keys.value(i, "") == "roll_IMU") { - attitude.roll = d; - } - - if (keys.value(i, "") == "pitch_IMU") { - attitude.pitch = d; - } - - if (keys.value(i, "") == "yaw_IMU") { - attitude.yaw = d; - } - - //Accel._X Accel._Y Accel._Z Battery Bottom_Rotor CPU_Load Ground_Dist. Gyro_Phi Gyro_Psi Gyro_Theta Left_Servo Mag._X Mag._Y Mag._Z Pressure Right_Servo Temperature Top_Rotor pitch_IMU roll_IMU yaw_IMU - - } - // Send out packets - - - // ATTITUDE - attitude.time_boot_ms = time/1000; - // Pack message and get size of encoded byte string - mavlink_msg_attitude_encode(systemId, componentId, &msg, &attitude); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - - // IMU - rawImuValues.time_usec = time; - rawImuValues.xmag = 0; - rawImuValues.ymag = 0; - rawImuValues.zmag = 0; - // Pack message and get size of encoded byte string - mavlink_msg_raw_imu_encode(systemId, componentId, &msg, &rawImuValues); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - - //qDebug() << "ATTITUDE" << "BUF LEN" << bufferlength << "POINTER" << streampointer; - - //qDebug() << "REALTIME" << QGC::groundTimeMilliseconds() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll; - - } - - } - - rate50hzCounter = 1; - } - - - // 10 HZ TASKS - if (rate10hzCounter == 1000 / rate / 9) { - rate10hzCounter = 1; - - double lastX = x; - double lastY = y; - double lastZ = z; - double hackDt = 0.1f; // 100 ms - - // Move X Position - x = 12.0*sin(((double)circleCounter)/200.0); - y = 5.0*cos(((double)circleCounter)/200.0); - z = 1.8 + 1.2*sin(((double)circleCounter)/200.0); - - double xSpeed = (x - lastX)/hackDt; - double ySpeed = (y - lastY)/hackDt; - double zSpeed = (z - lastZ)/hackDt; - - - - circleCounter++; - - -// x = (x > 5.0f) ? 5.0f : x; -// y = (y > 5.0f) ? 5.0f : y; -// z = (z > 3.0f) ? 3.0f : z; - -// x = (x < -5.0f) ? -5.0f : x; -// y = (y < -5.0f) ? -5.0f : y; -// z = (z < -3.0f) ? -3.0f : z; - - mavlink_message_t ret; - - // Send back new position - mavlink_msg_local_position_ned_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed); - bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - -// // GPS RAW -// mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f); -// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); -// //add data into datastream -// memcpy(stream+streampointer,buffer, bufferlength); -// streampointer += bufferlength; - - // GLOBAL POSITION - mavlink_msg_global_position_int_pack(systemId, componentId, &ret, 0, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw); - bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - - // GLOBAL POSITION VEHICLE 2 - mavlink_msg_global_position_int_pack(systemId+1, componentId+1, &ret, 0, (473780.28137103+(x+0.00001))*1E3, (85489.9892510421+((y/2)+0.00001))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw); - bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - -// // ATTITUDE VEHICLE 2 -// mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0); -// sendMAVLinkMessage(&ret); - - -// // GLOBAL POSITION VEHICLE 3 -// mavlink_msg_global_position_int_pack(60, componentId, &ret, 0, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); -// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); -// //add data into datastream -// memcpy(stream+streampointer,buffer, bufferlength); -// streampointer += bufferlength; - - static int rcCounter = 0; - if (rcCounter == 2) { - mavlink_rc_channels_t chan; - chan.time_boot_ms = 0; - chan.chan1_raw = 1000 + ((int)(fabs(x) * 1000) % 2000); - chan.chan2_raw = 1000 + ((int)(fabs(y) * 1000) % 2000); - chan.chan3_raw = 1000 + ((int)(fabs(z) * 1000) % 2000); - chan.chan4_raw = (chan.chan1_raw + chan.chan2_raw) / 2.0f; - chan.chan5_raw = (chan.chan3_raw + chan.chan4_raw) / 2.0f; - chan.chan6_raw = (chan.chan3_raw + chan.chan2_raw) / 2.0f; - chan.chan7_raw = (chan.chan4_raw + chan.chan2_raw) / 2.0f; - chan.chan8_raw = 0; - chan.rssi = 100; - mavlink_msg_rc_channels_encode(systemId, componentId, &msg, &chan); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - rcCounter = 0; - } - rcCounter++; - - } - - // 1 HZ TASKS - if (rate1hzCounter == 1000 / rate / 1) { - // STATE - static int statusCounter = 0; - if (statusCounter == 100) { - system.base_mode = (system.base_mode + 1) % MAV_MODE_ENUM_END; - statusCounter = 0; - } - statusCounter++; - - static int detectionCounter = 6; - if (detectionCounter % 10 == 0) { - } - detectionCounter++; - - status.voltage_battery = voltage * 1000; // millivolts - status.load = 33 * detectionCounter % 1000; - - // Pack message and get size of encoded byte string - mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - - // Pack debug text message - mavlink_statustext_t text; - text.severity = 0; - strcpy((char*)(text.text), "Text message from system 32"); - mavlink_msg_statustext_encode(systemId, componentId, &msg, &text); - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - memcpy(stream+streampointer, buffer, bufferlength); - streampointer += bufferlength; - - /* - // Pack message and get size of encoded byte string - mavlink_msg_boot_pack(systemId, componentId, &msg, version); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength;*/ - - // HEARTBEAT - - static int typeCounter = 0; - uint8_t mavType; - if (typeCounter < 10) - { - mavType = MAV_TYPE_QUADROTOR; - } - else - { - mavType = typeCounter % (MAV_TYPE_GCS); - } - typeCounter++; - - // Pack message and get size of encoded byte string - mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_PIXHAWK, system.base_mode, system.custom_mode, system.system_status); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //qDebug() << "CRC:" << msg.ck_a << msg.ck_b; - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - - // Pack message and get size of encoded byte string - mavlink_msg_heartbeat_pack(systemId+1, componentId+1, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //qDebug() << "CRC:" << msg.ck_a << msg.ck_b; - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - - - // Send controller states - - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - memcpy(stream+streampointer, buffer, bufferlength); - streampointer += bufferlength; - - // Pack message and get size of encoded byte string - mavlink_msg_sys_status_encode(54, componentId, &msg, &status); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - - rate1hzCounter = 1; - } - - // FULL RATE TASKS - // Default is 50 Hz - - /* - // 50 HZ TASKS - if (rate50hzCounter == 1000 / rate / 50) - { - - //streampointer = 0; - - // Attitude - - // Pack message and get size of encoded byte string - mavlink_msg_attitude_pack(systemId, componentId, &msg, usec, roll, pitch, yaw, 0, 0, 0); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - - rate50hzCounter = 1; - }*/ - - readyBufferMutex.lock(); - for (unsigned int i = 0; i < streampointer; i++) { - readyBuffer.enqueue(*(stream + i)); - } - readyBufferMutex.unlock(); - - // Increment counters after full main loop - rate1hzCounter++; - rate10hzCounter++; - rate50hzCounter++; -} - - -void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) -{ - // Parse bytes - mavlink_message_t msg; - mavlink_status_t comm; - - uint8_t stream[2048]; - int streampointer = 0; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - int bufferlength = 0; - - // Initialize drop count to 0 so it isn't referenced uninitialized when returned at the bottom of this function - comm.packet_rx_drop_count = 0; - - // Output all bytes as hex digits - for (int i=0; i::iterator i; - // Iterate through all components / subsystems - int j = 0; - for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { - if (j != 5) { - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), MAV_PARAM_TYPE_REAL32, onboardParams.size(), j); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; - } - j++; - } - -// qDebug() << "SIMULATION SENT PARAMETERS TO GCS"; - } - } - break; - case MAVLINK_MSG_ID_PARAM_SET: - { - // Drop on even milliseconds - if (QGC::groundTimeMilliseconds() % 2 == 0) - { -// qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER"; - mavlink_param_set_t set; - mavlink_msg_param_set_decode(&msg, &set); - // if (set.target_system == systemId) - // { - QString key = QString((char*)set.param_id); - if (onboardParams.contains(key)) - { - onboardParams.remove(key); - onboardParams.insert(key, set.param_value); - - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key)); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; - } - // } - } - } - break; - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: - { -// qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER"; - mavlink_param_request_read_t read; - mavlink_msg_param_request_read_decode(&msg, &read); - QByteArray bytes((char*)read.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN); - QString key = QString(bytes); - if (onboardParams.contains(key)) - { - float paramValue = onboardParams.value(key); - - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key)); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; - //qDebug() << "Sending PARAM" << key; - } - else if (read.param_index >= 0 && read.param_index < onboardParams.keys().size()) - { - key = onboardParams.keys().at(read.param_index); - float paramValue = onboardParams.value(key); - - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key)); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; - //qDebug() << "Sending PARAM #ID" << (read.param_index) << "KEY:" << key; - } - } - break; - } - } - } - - // Log the amount and time written out for future data rate calculations. - // While this interface doesn't actually write any data to external systems, - // this data "transmit" here should still count towards the outgoing data rate. - QMutexLocker dataRateLocker(&dataRateMutex); - logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch()); - - readyBufferMutex.lock(); - for (int i = 0; i < streampointer; i++) - { - readyBuffer.enqueue(*(stream + i)); - } - readyBufferMutex.unlock(); - - // Update comm status - status.errors_comm = comm.packet_rx_drop_count; - -} - - -void MAVLinkSimulationLink::readBytes() -{ - // Lock concurrent resource readyBuffer - readyBufferMutex.lock(); - const qint64 maxLength = 2048; - char data[maxLength]; - qint64 len = qMin((qint64)readyBuffer.size(), maxLength); - - for (unsigned int i = 0; i < len; i++) { - *(data + i) = readyBuffer.takeFirst(); - } - - QByteArray b(data, len); - emit bytesReceived(this, b); - readyBufferMutex.unlock(); - - // Log the amount and time received for future data rate calculations. - QMutexLocker dataRateLocker(&dataRateMutex); - logDataRateToBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, len, QDateTime::currentMSecsSinceEpoch()); - -} - -/** - * Disconnect the connection. - * - * @return True if connection has been disconnected, false if connection - * couldn't be disconnected. - **/ -bool MAVLinkSimulationLink::_disconnect(void) -{ - - if(isConnected()) - { - // timer->stop(); - - _isConnected = false; - - emit disconnected(); - - //exit(); - } - - return true; -} - -/** - * Connect the link. - * - * @return True if connection has been established, false if connection - * couldn't be established. - **/ -bool MAVLinkSimulationLink::_connect(void) -{ - _isConnected = true; - emit connected(); - - start(LowPriority); - MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 37.480391, -122.282883); - Q_UNUSED(mav1); -// MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2, 47.375, 8.548, 1); -// Q_UNUSED(mav2); - // timer->start(rate); - return true; -} - -/** - * Check if connection is active. - * - * @return True if link is connected, false otherwise. - **/ -bool MAVLinkSimulationLink::isConnected() const -{ - return _isConnected; -} - -QString MAVLinkSimulationLink::getName() const -{ - return name; -} - -qint64 MAVLinkSimulationLink::getConnectionSpeed() const -{ - /* 100 Mbit is reasonable fast and sufficient for all embedded applications */ - return 100000000; -} - -qint64 MAVLinkSimulationLink::getCurrentInDataRate() const -{ - return 0; -} - -qint64 MAVLinkSimulationLink::getCurrentOutDataRate() const -{ - return 0; -} diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h deleted file mode 100644 index efde174699c6a965e5b2fccd450d9696fd686b82..0000000000000000000000000000000000000000 --- a/src/comm/MAVLinkSimulationLink.h +++ /dev/null @@ -1,138 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Definition of MAVLinkSimulationLink - * - * @author Lorenz Meier - * - */ - -#ifndef MAVLINKSIMULATIONLINK_H -#define MAVLINKSIMULATIONLINK_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include "QGCMAVLink.h" - -#include "LinkInterface.h" - -class MAVLinkSimulationLink : public LinkInterface -{ - Q_OBJECT -public: - MAVLinkSimulationLink(QString readFile="", QString writeFile="", int rate=5); - ~MAVLinkSimulationLink(); - bool isConnected() const; - - void run(); - void requestReset() { } - - // Extensive statistics for scientific purposes - qint64 getConnectionSpeed() const; - qint64 getCurrentInDataRate() const; - qint64 getCurrentOutDataRate() const; - - QString getName() const; - int getBaudRate() const; - int getBaudRateType() const; - int getFlowType() const; - int getParityType() const; - int getDataBitsType() const; - int getStopBitsType() const; - - // These are left unimplemented in order to cause linker errors which indicate incorrect usage of - // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager. - bool connect(void); - bool disconnect(void); - -public slots: - void writeBytes(const char* data, qint64 size); - void readBytes(); - /** @brief Mainloop simulating the mainloop of the MAV */ - virtual void mainloop(); - void sendMAVLinkMessage(const mavlink_message_t* msg); - - -protected: - - // UAS properties - float roll, pitch, yaw; - double x, y, z; - double spX, spY, spZ, spYaw; - int battery; - - QTimer* timer; - /** File which contains the input data (simulated robot messages) **/ - QFile* simulationFile; - QFile* mavlinkLogFile; - QString simulationHeader; - /** File where the commands sent by the groundstation are stored **/ - QFile* receiveFile; - QTextStream textStream; - QTextStream* fileStream; - QTextStream* outStream; - /** Buffer which can be read from connected protocols through readBytes(). **/ - QMutex readyBufferMutex; - bool _isConnected; - quint64 rate; - int maxTimeNoise; - quint64 lastSent; - static const int streamlength = 4096; - unsigned int streampointer; - //const int testoffset = 0; - uint8_t stream[streamlength]; - - int readyBytes; - QQueue readyBuffer; - - QString name; - qint64 timeOffset; - mavlink_sys_status_t status; - mavlink_heartbeat_t system; - QMap onboardParams; - - void enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg); - - static const uint8_t systemId = 220; - static const uint8_t componentId = 200; - static const uint16_t version = 1000; - -signals: - void valueChanged(int uasId, QString curve, double value, quint64 usec); - void messageReceived(const mavlink_message_t& message); - -private: - // From LinkInterface - virtual bool _connect(void); - virtual bool _disconnect(void); -}; - -#endif // MAVLINKSIMULATIONLINK_H diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc deleted file mode 100644 index af12867dd26344cabe9c49d8a5feb62fb068bc4b..0000000000000000000000000000000000000000 --- a/src/comm/MAVLinkSimulationMAV.cc +++ /dev/null @@ -1,522 +0,0 @@ -#include -#include -#include -#include - -#include "MAVLinkSimulationMAV.h" - -MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon, int version) : - QObject(parent), - link(parent), - planner(parent, systemid), - systemid(systemid), - timer25Hz(0), - timer10Hz(0), - timer1Hz(0), - latitude(lat), - longitude(lon), - altitude(550.0), - x(lat), - y(lon), - z(altitude), - roll(0.0), - pitch(0.0), - yaw(0.0), - rollspeed(0.0), - pitchspeed(0.0), - yawspeed(0.0), - globalNavigation(true), - firstWP(false), - // previousSPX(8.548056), - // previousSPY(47.376389), - // previousSPZ(550), - // previousSPYaw(0.0), - // nextSPX(8.548056), - // nextSPY(47.376389), - // nextSPZ(550), - previousSPX(37.480391), - previousSPY(122.282883), - previousSPZ(550), - previousSPYaw(0.0), - nextSPX(37.480391), - nextSPY(122.282883), - nextSPZ(550), - nextSPYaw(0.0), - sys_mode(MAV_MODE_PREFLIGHT), - sys_state(MAV_STATE_STANDBY), - nav_mode(0), - flying(false), - mavlink_version(version) -{ - // Please note: The waypoint planner is running - connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop())); - connect(&planner, SIGNAL(messageSent(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); - connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); - mainloopTimer.start(20); - mainloop(); -} - -void MAVLinkSimulationMAV::mainloop() -{ - // Calculate new simulator values - // double maxSpeed = 0.0001; // rad/s in earth coordinate frame - - // double xNew = // (nextSPX - previousSPX) - - if (flying) { - sys_state = MAV_STATE_ACTIVE; - sys_mode = MAV_MODE_AUTO_ARMED; - nav_mode = 0; - } - - // 1 Hz execution - if (timer1Hz <= 0) { - mavlink_message_t msg; - mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_TYPE_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE); - link->sendMAVLinkMessage(&msg); - planner.handleMessage(msg); - - mavlink_servo_output_raw_t servos; - servos.time_usec = 0; - servos.servo1_raw = 1000; - servos.servo2_raw = 1250; - servos.servo3_raw = 1400; - servos.servo4_raw = 1500; - servos.servo5_raw = 1800; - servos.servo6_raw = 1500; - servos.servo7_raw = 1500; - servos.servo8_raw = 2000; - servos.port = 1; // set a fake port number - - mavlink_msg_servo_output_raw_encode(systemid, MAV_COMP_ID_IMU, &msg, &servos); - link->sendMAVLinkMessage(&msg); - timer1Hz = 50; - } - - // 10 Hz execution - if (timer10Hz <= 0) { - double radPer100ms = 0.00006; - double altPer100ms = 0.4; - double xm = (nextSPX - x); - double ym = (nextSPY - y); - double zm = (nextSPZ - z); - - float zsign = (zm < 0) ? -1.0f : 1.0f; - - if (!(sys_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL)) - { - if (!firstWP) { - //float trueyaw = atan2f(xm, ym); - - float newYaw = atan2f(ym, xm); - - if (fabs(yaw - newYaw) < 90) { - yaw = yaw*0.7 + 0.3*newYaw; - } else { - yaw = newYaw; - } - - //qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw; - - //if (sqrt(xm*xm+ym*ym) > 0.0000001) - if (flying) { - x += cos(yaw)*radPer100ms; - y += sin(yaw)*radPer100ms; - z += altPer100ms*zsign; - } - - //if (xm < 0.001) xm - } else { - x = nextSPX; - y = nextSPY; - z = nextSPZ; - firstWP = false; -// qDebug() << "INIT STEP"; - } - } - else - { - // FIXME Implement heading and altitude controller - - } - - // GLOBAL POSITION - mavlink_message_t msg; - mavlink_global_position_int_t pos; - pos.alt = altitude*1000.0; - pos.lat = longitude*1E7; - pos.lon = longitude*1E7; - pos.vx = sin(yaw)*10.0f*100.0f; - pos.vy = 0; - pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f; - mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos); - link->sendMAVLinkMessage(&msg); - planner.handleMessage(msg); - - // ATTITUDE - mavlink_attitude_t attitude; - attitude.time_boot_ms = 0; - attitude.roll = roll; - attitude.pitch = pitch; - attitude.yaw = yaw; - attitude.rollspeed = rollspeed; - attitude.pitchspeed = pitchspeed; - attitude.yawspeed = yawspeed; - - mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude); - link->sendMAVLinkMessage(&msg); - - // SYSTEM STATUS - mavlink_sys_status_t status; - - // Since the simulation outputs global position, attitude and raw pressure we specify that the - // sensors that would be collecting this information are present, enabled and healthy. - - status.onboard_control_sensors_present = MAV_SYS_STATUS_SENSOR_3D_GYRO | - MAV_SYS_STATUS_SENSOR_3D_ACCEL | - MAV_SYS_STATUS_SENSOR_3D_MAG | - MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | - MAV_SYS_STATUS_SENSOR_GPS; - - status.onboard_control_sensors_enabled = status.onboard_control_sensors_present; - status.onboard_control_sensors_health = status.onboard_control_sensors_present; - status.load = 300; - status.voltage_battery = 10500; - status.current_battery = -1; // -1: autopilot does not measure the current - status.drop_rate_comm = 0; - status.errors_comm = 0; - status.errors_count1 = 0; - status.errors_count2 = 0; - status.errors_count3 = 0; - status.errors_count4 = 0; - status.battery_remaining = 90; - mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status); - link->sendMAVLinkMessage(&msg); - timer10Hz = 5; - - // VFR HUD - mavlink_vfr_hud_t hud; - hud.airspeed = pos.vx/100.0f; - hud.groundspeed = pos.vx/100.0f; - hud.alt = altitude; - hud.heading = static_cast((yaw/M_PI)*180.0f+180.0f) % 360; - hud.climb = pos.vz/100.0f; - hud.throttle = 90; - mavlink_msg_vfr_hud_encode(systemid, MAV_COMP_ID_IMU, &msg, &hud); - link->sendMAVLinkMessage(&msg); - - // NAV CONTROLLER - mavlink_nav_controller_output_t nav; - nav.nav_roll = roll; - nav.nav_pitch = pitch; - nav.nav_bearing = yaw; - nav.target_bearing = yaw; - nav.wp_dist = 2.0f; - nav.alt_error = 0.5f; - nav.xtrack_error = 0.2f; - nav.aspd_error = 0.0f; - mavlink_msg_nav_controller_output_encode(systemid, MAV_COMP_ID_IMU, &msg, &nav); - link->sendMAVLinkMessage(&msg); - - // RAW PRESSURE - mavlink_raw_pressure_t pressure; - pressure.press_abs = 1000; - pressure.press_diff1 = 2000; - pressure.press_diff2 = 5000; - pressure.temperature = 18150; // 18.15 deg Celsius - pressure.time_usec = 0; // Works also with zero timestamp - mavlink_msg_raw_pressure_encode(systemid, MAV_COMP_ID_IMU, &msg, &pressure); - link->sendMAVLinkMessage(&msg); - } - - // 25 Hz execution - if (timer25Hz <= 0) { - // The message container to be used for sending - mavlink_message_t ret; - - if (sys_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL) - { - mavlink_hil_controls_t hil; - hil.roll_ailerons = 0.0f; - hil.pitch_elevator = 0.05f; - hil.yaw_rudder = 0.05f; - hil.throttle = 0.6f; - hil.aux1 = 0.0f; - hil.aux2 = 0.0f; - hil.aux3 = 0.0f; - hil.aux4 = 0.0f; - hil.mode = MAV_MODE_FLAG_HIL_ENABLED; - hil.nav_mode = 0; // not currently used by any HIL consumers - - // Encode the data (adding header and checksums, etc.) - mavlink_msg_hil_controls_encode(systemid, MAV_COMP_ID_IMU, &ret, &hil); - // And send it - link->sendMAVLinkMessage(&ret); - } - - // Send actual controller outputs - // This message just shows the direction - // and magnitude of the control output - // mavlink_position_controller_output_t pos; - // pos.x = sin(yaw)*127.0f; - // pos.y = cos(yaw)*127.0f; - // pos.z = 0; - // mavlink_msg_position_controller_output_encode(systemid, MAV_COMP_ID_IMU, &ret, &pos); - // link->sendMAVLinkMessage(&ret); - - // Send a named value with name "FLOAT" and 0.5f as value - - // The message container to be used for sending - //mavlink_message_t ret; - // The C-struct holding the data to be sent - mavlink_named_value_float_t val; - - // Fill in the data - // Name of the value, maximum 10 characters - // see full message specs at: - // http://pixhawk.ethz.ch/wiki/mavlink/ - strcpy((char *)val.name, "FLOAT"); - // Value, in this case 0.5 - val.value = 0.5f; - - // Encode the data (adding header and checksums, etc.) - mavlink_msg_named_value_float_encode(systemid, MAV_COMP_ID_IMU, &ret, &val); - // And send it - link->sendMAVLinkMessage(&ret); - - // MICROCONTROLLER SEND CODE: - - // uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - // int16_t len = mavlink_msg_to_send_buffer(buf, &ret); - // uart0_transmit(buf, len); - - - // SEND INTEGER VALUE - - // We are reusing the "mavlink_message_t ret" - // message buffer - - // The C-struct holding the data to be sent - mavlink_named_value_int_t valint; - - // Fill in the data - // Name of the value, maximum 10 characters - // see full message specs at: - // http://pixhawk.ethz.ch/wiki/mavlink/ - strcpy((char *)valint.name, "INTEGER"); - // Value, in this case 18000 - valint.value = 18000; - - // Encode the data (adding header and checksums, etc.) - mavlink_msg_named_value_int_encode(systemid, MAV_COMP_ID_IMU, &ret, &valint); - // And send it - link->sendMAVLinkMessage(&ret); - - // MICROCONTROLLER SEND CODE: - - // uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - // int16_t len = mavlink_msg_to_send_buffer(buf, &ret); - // uart0_transmit(buf, len); - - timer25Hz = 2; - } - - timer1Hz--; - timer10Hz--; - timer25Hz--; -} - -// Uncomment to turn on debug message printing -//#define DEBUG_PRINT_MESSAGE - -#ifdef DEBUG_PRINT_MESSAGE - -//static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS]; - -static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS; -//static unsigned error_count; - -mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO; - -static void print_one_field(const mavlink_message_t *msg, const mavlink_field_info_t *f, int idx) -{ -#define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def) - switch (f->type) { - case MAVLINK_TYPE_CHAR: - qDebug(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_UINT8_T: - qDebug(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_INT8_T: - qDebug(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_UINT16_T: - qDebug(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2)); - break; - case MAVLINK_TYPE_INT16_T: - qDebug(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2)); - break; - case MAVLINK_TYPE_UINT32_T: - qDebug(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_INT32_T: - qDebug(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_UINT64_T: - qDebug(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8)); - break; - case MAVLINK_TYPE_INT64_T: - qDebug(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8)); - break; - case MAVLINK_TYPE_FLOAT: - qDebug(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_DOUBLE: - qDebug(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8)); - break; - } -} - -static void print_field(const mavlink_message_t *msg, const mavlink_field_info_t *f) -{ - qDebug("%s: ", f->name); - if (f->array_length == 0) { - print_one_field(msg, f, 0); - qDebug(" "); - } else { - unsigned i; - /* print an array */ - if (f->type == MAVLINK_TYPE_CHAR) { - qDebug("'%.*s'", f->array_length, - f->wire_offset+(const char *)_MAV_PAYLOAD(msg)); - - } else { - qDebug("[ "); - for (i=0; iarray_length; i++) { - print_one_field(msg, f, i); - if (i < f->array_length) { - qDebug(", "); - } - } - qDebug("]"); - } - } - qDebug(" "); -} -#endif - -static void print_message(const mavlink_message_t *msg) -{ -#ifdef DEBUG_PRINT_MESSAGE - const mavlink_message_info_t *m = &message_info[msg->msgid]; - const mavlink_field_info_t *f = m->fields; - unsigned i; - qDebug("%s { ", m->name); - for (i=0; inum_fields; i++) { - print_field(msg, &f[i]); - } - qDebug("}\n"); -#else - Q_UNUSED(msg); -#endif -} - -void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) -{ - if (msg.sysid != systemid) - { - print_message(&msg); -// qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid; - } - - switch(msg.msgid) { - case MAVLINK_MSG_ID_ATTITUDE: - break; - case MAVLINK_MSG_ID_SET_MODE: { - mavlink_set_mode_t mode; - mavlink_msg_set_mode_decode(&msg, &mode); - if (systemid == mode.target_system) sys_mode = mode.base_mode; - } - break; - case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: - { - mavlink_hil_state_quaternion_t state; - mavlink_msg_hil_state_quaternion_decode(&msg, &state); - - double a = state.attitude_quaternion[0]; - double b = state.attitude_quaternion[1]; - double c = state.attitude_quaternion[2]; - double d = state.attitude_quaternion[3]; - 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]); - } - - roll = phi; - pitch = theta; - yaw = psi; - rollspeed = state.rollspeed; - pitchspeed = state.pitchspeed; - yawspeed = state.yawspeed; - latitude = state.lat; - longitude = state.lon; - altitude = state.alt; - } - break; - // FIXME MAVLINKV10PORTINGNEEDED - // case MAVLINK_MSG_ID_ACTION: { - // mavlink_action_t action; - // mavlink_msg_action_decode(&msg, &action); - // if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) { - // mavlink_action_ack_t ack; - // ack.action = action.action; - //// switch (action.action) { - //// case MAV_ACTION_TAKEOFF: - //// flying = true; - //// nav_mode = MAV_NAV_LIFTOFF; - //// ack.result = 1; - //// break; - //// default: { - //// ack.result = 0; - //// } - //// break; - //// } - - // // Give feedback about action - // mavlink_message_t r_msg; - // mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack); - // link->sendMAVLinkMessage(&r_msg); - // } - // } - break; - } -} diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h deleted file mode 100644 index 3b7bea63e3fcb4ffa62e8d4d94f716aae8e57cff..0000000000000000000000000000000000000000 --- a/src/comm/MAVLinkSimulationMAV.h +++ /dev/null @@ -1,75 +0,0 @@ -#ifndef MAVLINKSIMULATIONMAV_H -#define MAVLINKSIMULATIONMAV_H - -#include -#include - -#include "MAVLinkSimulationLink.h" -#include "MAVLinkSimulationWaypointPlanner.h" - -class MAVLinkSimulationMAV : public QObject -{ - Q_OBJECT -public: - explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat=47.376389, double lon=8.548056, int version=MAVLINK_VERSION); - -signals: - -public slots: - void mainloop(); - void handleMessage(const mavlink_message_t& msg); - -protected: - MAVLinkSimulationLink* link; - MAVLinkSimulationWaypointPlanner planner; - int systemid; - QTimer mainloopTimer; - int timer25Hz; - int timer10Hz; - int timer1Hz; - double latitude; - double longitude; - double altitude; - double x; - double y; - double z; - double roll; - double pitch; - double yaw; - double rollspeed; - double pitchspeed; - double yawspeed; - - bool globalNavigation; - bool firstWP; - - double previousSPX; - double previousSPY; - double previousSPZ; - double previousSPYaw; - - double nextSPX; - double nextSPY; - double nextSPZ; - double nextSPYaw; - uint8_t sys_mode; - uint8_t sys_state; - uint8_t nav_mode; - bool flying; - int mavlink_version; - - // FIXME MAVLINKV10PORTINGNEEDED -// static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) { -// uint16_t i = 0; -// msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - -// i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) -// i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM -// i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version - -// return mavlink_finalize_message(msg, system_id, component_id, i); -// } - -}; - -#endif // MAVLINKSIMULATIONMAV_H diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc deleted file mode 100644 index aa6384166c947a50cb5761233cbcbeaf72f56efe..0000000000000000000000000000000000000000 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ /dev/null @@ -1,1131 +0,0 @@ -/*====================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009-2011 PIXHAWK PROJECT - -This file is part of the PIXHAWK project - - PIXHAWK is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - PIXHAWK is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with PIXHAWK. If not, see . - -========================================================================*/ - -/** -* @file -* @brief a program to manage waypoints and exchange them with the ground station -* -* @author Petri Tanskanen -* @author Benjamin Knecht -* @author Christian Schluchter -*/ - -#include - -#include "MAVLinkSimulationWaypointPlanner.h" -#include "QGC.h" -#include - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -class PxMatrix3x3; - - -/** - * @brief Pixhawk 3D vector class, can be cast to a local OpenCV CvMat. - * - */ -class PxVector3 -{ -public: - /** @brief standard constructor */ - PxVector3(void) {} - /** @brief copy constructor */ - PxVector3(const PxVector3 &v) { - for (int i=0; i < 3; i++) { - m_vec[i] = v.m_vec[i]; - } - } - /** @brief x,y,z constructor */ - PxVector3(const float _x, const float _y, const float _z) { - m_vec[0] = _x; - m_vec[1] = _y; - m_vec[2] = _z; - } - /** @brief broadcast constructor */ - PxVector3(const float _f) { - for (int i=0; i < 3; i++) { - m_vec[i] = _f; - } - } - -private: - /** @brief private constructor (not used here, for SSE compatibility) */ - PxVector3(const float (&_vec)[3]) { - for (int i=0; i < 3; i++) { - m_vec[i] = _vec[i]; - } - } - -public: - /** @brief assignment operator */ - void operator= (const PxVector3 &r) { - for (int i=0; i < 3; i++) { - m_vec[i] = r.m_vec[i]; - } - } - /** @brief const element access */ - float operator[] (const int i) const { - return m_vec[i]; - } - /** @brief element access */ - float &operator[] (const int i) { - return m_vec[i]; - } - - // === arithmetic operators === - /** @brief element-wise negation */ - friend PxVector3 operator- (const PxVector3 &v) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = -v.m_vec[i]; - } - return ret; - } - friend PxVector3 operator+ (const PxVector3 &l, const PxVector3 &r) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; - } - return ret; - } - friend PxVector3 operator- (const PxVector3 &l, const PxVector3 &r) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; - } - return ret; - } - friend PxVector3 operator* (const PxVector3 &l, const PxVector3 &r) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; - } - return ret; - } - friend PxVector3 operator/ (const PxVector3 &l, const PxVector3 &r) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; - } - return ret; - } - - friend void operator+= (PxVector3 &l, const PxVector3 &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; - } - } - friend void operator-= (PxVector3 &l, const PxVector3 &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; - } - } - friend void operator*= (PxVector3 &l, const PxVector3 &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; - } - } - friend void operator/= (PxVector3 &l, const PxVector3 &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; - } - } - - friend PxVector3 operator+ (const PxVector3 &l, float f) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] + f; - } - return ret; - } - friend PxVector3 operator- (const PxVector3 &l, float f) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] - f; - } - return ret; - } - friend PxVector3 operator* (const PxVector3 &l, float f) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * f; - } - return ret; - } - friend PxVector3 operator/ (const PxVector3 &l, float f) { - PxVector3 ret; - float inv = 1.f/f; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * inv; - } - return ret; - } - - friend void operator+= (PxVector3 &l, float f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] + f; - } - } - friend void operator-= (PxVector3 &l, float f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] - f; - } - } - friend void operator*= (PxVector3 &l, float f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * f; - } - } - friend void operator/= (PxVector3 &l, float f) { - float inv = 1.f/f; - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * inv; - } - } - - // === vector operators === - /** @brief dot product */ - float dot(const PxVector3 &v) const { - return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; - } - /** @brief length squared of the vector */ - float lengthSquared(void) const { - return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; - } - /** @brief length of the vector */ - float length(void) const { - return sqrt(lengthSquared()); - } - /** @brief cross product */ - PxVector3 cross(const PxVector3 &v) const { - return PxVector3(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); - } - /** @brief normalizes the vector */ - PxVector3 &normalize(void) { - const float l = 1.f / length(); - for (int i=0; i < 3; i++) { - m_vec[i] *= l; - } - return *this; - } - - friend class PxMatrix3x3; -protected: - float m_vec[3]; -}; - -/** - * @brief Pixhawk 3D vector class in double precision, can be cast to a local OpenCV CvMat. - * - */ -class PxVector3Double -{ -public: - /** @brief standard constructor */ - PxVector3Double(void) {} - /** @brief copy constructor */ - PxVector3Double(const PxVector3Double &v) { - for (int i=0; i < 3; i++) { - m_vec[i] = v.m_vec[i]; - } - } - /** @brief x,y,z constructor */ - PxVector3Double(const double _x, const double _y, const double _z) { - m_vec[0] = _x; - m_vec[1] = _y; - m_vec[2] = _z; - } - /** @brief broadcast constructor */ - PxVector3Double(const double _f) { - for (int i=0; i < 3; i++) { - m_vec[i] = _f; - } - } - -private: - /** @brief private constructor (not used here, for SSE compatibility) */ - PxVector3Double(const double (&_vec)[3]) { - for (int i=0; i < 3; i++) { - m_vec[i] = _vec[i]; - } - } - -public: - /** @brief assignment operator */ - void operator= (const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - m_vec[i] = r.m_vec[i]; - } - } - /** @brief const element access */ - double operator[] (const int i) const { - return m_vec[i]; - } - /** @brief element access */ - double &operator[] (const int i) { - return m_vec[i]; - } - - // === arithmetic operators === - /** @brief element-wise negation */ - friend PxVector3Double operator- (const PxVector3Double &v) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = -v.m_vec[i]; - } - return ret; - } - friend PxVector3Double operator+ (const PxVector3Double &l, const PxVector3Double &r) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; - } - return ret; - } - friend PxVector3Double operator- (const PxVector3Double &l, const PxVector3Double &r) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; - } - return ret; - } - friend PxVector3Double operator* (const PxVector3Double &l, const PxVector3Double &r) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; - } - return ret; - } - friend PxVector3Double operator/ (const PxVector3Double &l, const PxVector3Double &r) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; - } - return ret; - } - - friend void operator+= (PxVector3Double &l, const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; - } - } - friend void operator-= (PxVector3Double &l, const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; - } - } - friend void operator*= (PxVector3Double &l, const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; - } - } - friend void operator/= (PxVector3Double &l, const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; - } - } - - friend PxVector3Double operator+ (const PxVector3Double &l, double f) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] + f; - } - return ret; - } - friend PxVector3Double operator- (const PxVector3Double &l, double f) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] - f; - } - return ret; - } - friend PxVector3Double operator* (const PxVector3Double &l, double f) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * f; - } - return ret; - } - friend PxVector3Double operator/ (const PxVector3Double &l, double f) { - PxVector3Double ret; - double inv = 1.f/f; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * inv; - } - return ret; - } - - friend void operator+= (PxVector3Double &l, double f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] + f; - } - } - friend void operator-= (PxVector3Double &l, double f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] - f; - } - } - friend void operator*= (PxVector3Double &l, double f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * f; - } - } - friend void operator/= (PxVector3Double &l, double f) { - double inv = 1.f/f; - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * inv; - } - } - - // === vector operators === - /** @brief dot product */ - double dot(const PxVector3Double &v) const { - return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; - } - /** @brief length squared of the vector */ - double lengthSquared(void) const { - return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; - } - /** @brief length of the vector */ - double length(void) const { - return sqrt(lengthSquared()); - } - /** @brief cross product */ - PxVector3Double cross(const PxVector3Double &v) const { - return PxVector3Double(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); - } - /** @brief normalizes the vector */ - PxVector3Double &normalize(void) { - const double l = 1.f / length(); - for (int i=0; i < 3; i++) { - m_vec[i] *= l; - } - return *this; - } - - friend class PxMatrix3x3; -protected: - double m_vec[3]; -}; - -MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int sysid) : - QObject(parent), - link(parent), - idle(false), - current_active_wp_id(-1), - yawReached(false), - posReached(false), - timestamp_lastoutside_orbit(0), - timestamp_firstinside_orbit(0), - waypoints(&waypoints1), - waypoints_receive_buffer(&waypoints2), - current_state(PX_WPP_IDLE), - protocol_current_wp_id(0), - protocol_current_count(0), - protocol_current_partner_systemid(0), - protocol_current_partner_compid(0), - protocol_timestamp_lastaction(0), - protocol_timeout(1000), - timestamp_last_send_setpoint(0), - systemid(sysid), - compid(MAV_COMP_ID_MISSIONPLANNER), - setpointDelay(10), - yawTolerance(0.4f), - verbose(true), - debug(false), - silent(false) -{ - connect(parent, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); - qDebug() << "PLANNER FOR SYSTEM" << systemid << "INITIALIZED"; -} - - - -/* -* @brief Sends an waypoint ack message -*/ -void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = target_systemid; - wpa.target_component = target_compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode(systemid, compid, &msg, &wpa); - link->sendMAVLinkMessage(&msg); - - - - if (verbose) qDebug("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); -} - -/* -* @brief Broadcasts the new target waypoint and directs the MAV to fly there -* -* This function broadcasts its new active waypoint sequence number and -* sends a message to the controller, advising it to fly to the coordinates -* of the waypoint with a given orientation -* -* @param seq The waypoint sequence number the MAV should fly to. -*/ -void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq) -{ - if(seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_mission_current_encode(systemid, compid, &msg, &wpc); - link->sendMAVLinkMessage(&msg); - - - - if (verbose) qDebug("Broadcasted new current waypoint %u\n", wpc.seq); - } -} - -/* -* @brief Directs the MAV to fly to a position -* -* Sends a message to the controller, advising it to fly to the coordinates -* of the waypoint with a given orientation -* -* @param seq The waypoint sequence number the MAV should fly to. -*/ -void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) -{ - Q_UNUSED(seq); -} - -void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = target_systemid; - wpc.target_component = target_compid; - wpc.count = count; - - mavlink_msg_mission_count_encode(systemid, compid, &msg, &wpc); - link->sendMAVLinkMessage(&msg); - - if (verbose) qDebug("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - -} - -void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - if (seq < waypoints->size()) { - mavlink_message_t msg; - mavlink_mission_item_t *wp = waypoints->at(seq); - wp->target_system = target_systemid; - wp->target_component = target_compid; - - if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->command, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue); - - mavlink_msg_mission_item_encode(systemid, compid, &msg, wp); - link->sendMAVLinkMessage(&msg); - if (verbose) qDebug("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - - } else { - if (verbose) qDebug("ERROR: index out of bounds\n"); - } -} - -void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = target_systemid; - wpr.target_component = target_compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode(systemid, compid, &msg, &wpr); - link->sendMAVLinkMessage(&msg); - if (verbose) qDebug("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - -} - -/* -* @brief emits a message that a waypoint reached -* -* This function broadcasts a message that a waypoint is reached. -* -* @param seq The waypoint sequence number the MAV has reached. -*/ -void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode(systemid, compid, &msg, &wp_reached); - link->sendMAVLinkMessage(&msg); - - if (verbose) qDebug("Sent waypoint %u reached message\n", wp_reached.seq); - - -} - -float MAVLinkSimulationWaypointPlanner::distanceToSegment(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - // seq not the second last waypoint - if ((uint16_t)(seq+1) < waypoints->size()) { - mavlink_mission_item_t *next = waypoints->at(seq+1); - const PxVector3 B(next->x, next->y, next->z); - const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); - if (r >= 0 && r <= 1) { - const PxVector3 P(A + r*(B-A)); - return (P-C).length(); - } else if (r < 0.f) { - return (C-A).length(); - } else { - return (C-B).length(); - } - } else { - return (C-A).length(); - } - } - return -1.f; -} - -float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - return (C-A).length(); - } - return -1.f; -} - -float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y) -{ - if (seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, 0); - const PxVector3 C(x, y, 0); - - return (C-A).length(); - } - return -1.f; -} - -void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg) -{ - mavlink_handler(&msg); -} - -void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* msg) -{ - // Handle param messages -// paramClient->handleMAVLinkPacket(msg); - - //check for timed-out operations - - //qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid; - - uint64_t now = QGC::groundTimeMilliseconds(); - if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) { - if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state; - current_state = PX_WPP_IDLE; - protocol_current_count = 0; - protocol_current_partner_systemid = 0; - protocol_current_partner_compid = 0; - protocol_current_wp_id = -1; - - if(waypoints->size() == 0) { - current_active_wp_id = -1; - } - } - - if(now-timestamp_last_send_setpoint > setpointDelay) { - send_setpoint(current_active_wp_id); - } - - switch(msg->msgid) { - case MAVLINK_MSG_ID_ATTITUDE: { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) { - mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id); - if(wp->frame == 1) { - mavlink_attitude_t att; - mavlink_msg_attitude_decode(msg, &att); - float yaw_tolerance = yawTolerance; - //compare current yaw - if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) { - if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) - yawReached = true; - } else if(att.yaw - yaw_tolerance < 0.0f) { - float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) - yawReached = true; - } else { - float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) - yawReached = true; - } - - // FIXME HACK: Ignore yaw: - - yawReached = true; - } - } - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION_NED: { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) { - mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id); - - if(wp->frame == 1) { - mavlink_local_position_ned_t pos; - mavlink_msg_local_position_ned_decode(msg, &pos); - //qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z; - - posReached = false; - - // compare current position (given in message) with current waypoint - float orbit = wp->param1; - - float dist; - if (wp->param2 == 0) { - dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); - } else { - dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); - } - - if (dist >= 0.f && dist <= orbit && yawReached) { - posReached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) { - mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id); - - if(wp->frame == 0) { - mavlink_global_position_int_t pos; - mavlink_msg_global_position_int_decode(msg, &pos); - - float x = static_cast(pos.lat)/1E7; - float y = static_cast(pos.lon)/1E7; - //float z = static_cast(pos.alt)/1000; - - //qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z; - - posReached = false; - yawReached = true; - - // FIXME big hack for simulation! - //float oneDegreeOfLatMeters = 111131.745f; - float orbit = 0.00008f; - - // compare current position (given in message) with current waypoint - //float orbit = wp->param1; - - // Convert to degrees - - - float dist; - dist = distanceToPoint(current_active_wp_id, x, y); - - if (dist >= 0.f && dist <= orbit && yawReached) { - posReached = true; - qDebug() << "WP PLANNER: REACHED POSITION"; - } - } - } - break; - } - - case MAVLINK_MSG_ID_COMMAND_LONG: - { // special action from ground station - mavlink_command_long_t action; - mavlink_msg_command_long_decode(msg, &action); - if(action.target_system == systemid) { - if (verbose) qDebug("Waypoint: received message with action %d\n", action.command); -// switch (action.action) { -// case MAV_ACTION_LAUNCH: -// if (verbose) std::cerr << "Launch received" << std::endl; -// current_active_wp_id = 0; -// if (waypoints->size()>0) -// { -// setActive(waypoints[current_active_wp_id]); -// } -// else -// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// break; - -// case MAV_ACTION_CONTINUE: -// if (verbose) std::c -// err << "Continue received" << std::endl; -// idle = false; -// setActive(waypoints[current_active_wp_id]); -// break; - -// case MAV_ACTION_HALT: -// if (verbose) std::cerr << "Halt received" << std::endl; -// idle = true; -// break; - -// default: -// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// break; -// } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) { - if (protocol_current_wp_id == waypoints->size()-1) { - if (verbose) qDebug("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); - current_state = PX_WPP_IDLE; - protocol_current_wp_id = 0; - } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if(wpc.target_system == systemid && wpc.target_component == compid) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE) { - if (wpc.seq < waypoints->size()) { - if (verbose) qDebug("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); - current_active_wp_id = wpc.seq; - uint32_t i; - for(i = 0; i < waypoints->size(); i++) { - if (i == current_active_wp_id) { - waypoints->at(i)->current = true; - } else { - waypoints->at(i)->current = false; - } - } - if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - } else { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); - } - } - } else { - qDebug() << "SYSTEM / COMPONENT ID MISMATCH: target sys:" << wpc.target_system << "this system:" << systemid << "target comp:" << wpc.target_component << "this comp:" << compid; - } - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - if(wprl.target_system == systemid && wprl.target_component == compid) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) { - if (waypoints->size() > 0) { - if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); - current_state = PX_WPP_SENDLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - } else { - if (verbose) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - protocol_current_count = static_cast(waypoints->size()); - send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); - } else { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); - } - } else { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because not my systemid or compid.\n"); - } - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) { - if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - - current_state = PX_WPP_SENDLIST_SENDWPS; - protocol_current_wp_id = wpr.seq; - send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); - } else { - if (verbose) { - if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { - qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", current_state); - break; - } else if (current_state == PX_WPP_SENDLIST) { - if (wpr.seq != 0) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - } else if (current_state == PX_WPP_SENDLIST_SENDWPS) { - if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); - else if (wpr.seq >= waypoints->size()) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - } else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); - } - } - } else { - //we we're target but already communicating with someone else - if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - if(wpc.target_system == systemid && wpc.target_component == compid) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) { - if (wpc.count > 0) { - if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - - current_state = PX_WPP_GETLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - protocol_current_count = wpc.count; - - qDebug("clearing receive buffer and readying for receiving waypoints\n"); - while(waypoints_receive_buffer->size() > 0) { - delete waypoints_receive_buffer->back(); - waypoints_receive_buffer->pop_back(); - } - - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } else { - if (verbose) qDebug("Ignoring MAVLINK_MSG_ID_MISSION_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - } - } else { - if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT because i'm doing something else already (state=%i).\n", current_state); - else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT - FIXME: missed error description\n"); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) { - if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); - - current_state = PX_WPP_GETLIST_GETWPS; - protocol_current_wp_id = wp.seq + 1; - mavlink_mission_item_t* newwp = new mavlink_mission_item_t; - memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); - waypoints_receive_buffer->push_back(newwp); - - if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) { - if (verbose) qDebug("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); - - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - - if (current_active_wp_id > waypoints_receive_buffer->size()-1) { - current_active_wp_id = static_cast(waypoints_receive_buffer->size()) - 1; - } - - // switch the waypoints list - std::vector* waypoints_temp = waypoints; - waypoints = waypoints_receive_buffer; - waypoints_receive_buffer = waypoints_temp; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < waypoints->size(); i++) { - if (waypoints->at(i)->current == 1) { - current_active_wp_id = i; - //if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == waypoints->size()) { - current_active_wp_id = -1; - yawReached = false; - posReached = false; - timestamp_firstinside_orbit = 0; - } - - current_state = PX_WPP_IDLE; - } else { - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - } else { - if (current_state == PX_WPP_IDLE) { - //we're done receiving waypoints, answer with ack. - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - qDebug("Received MAVLINK_MSG_ID_MISSION_ITEM while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); - } - if (verbose) { - if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { - qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); - break; - } else if (current_state == PX_WPP_GETLIST) { - if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } else if (current_state == PX_WPP_GETLIST_GETWPS) { - if (!(wp.seq == protocol_current_wp_id)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); - else if (!(wp.seq < protocol_current_count)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } - } else { - // We're target but already communicating with someone else - if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); - } else if(wp.target_system == systemid && wp.target_component == compid) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) { - protocol_timestamp_lastaction = now; - - if (verbose) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - while(waypoints->size() > 0) { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - } else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); - } - break; - } - - default: { - if (debug) qDebug("Waypoint: received message of unknown type\n"); - break; - } - } - - //check if the current waypoint was reached - if ((posReached && /*yawReached &&*/ !idle)) { - if (current_active_wp_id < waypoints->size()) { - mavlink_mission_item_t *cur_wp = waypoints->at(current_active_wp_id); - - if (timestamp_firstinside_orbit == 0) { - // Announce that last waypoint was reached - if (verbose) qDebug("*** Reached waypoint %u ***\n", cur_wp->seq); - send_waypoint_reached(cur_wp->seq); - timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) { - if (cur_wp->autocontinue) { - cur_wp->current = 0; - if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 0) { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - current_active_wp_id = 0; - } else { - current_active_wp_id++; - } - - // Fly to next waypoint - timestamp_firstinside_orbit = 0; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - waypoints->at(current_active_wp_id)->current = true; - posReached = false; - //yawReached = false; - if (verbose) qDebug("Set new waypoint (%u)\n", current_active_wp_id); - } - } - } - } else { - timestamp_lastoutside_orbit = now; - } -} diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.h b/src/comm/MAVLinkSimulationWaypointPlanner.h deleted file mode 100644 index 03ba4490909b5c23a698466918348b77a5ff7c24..0000000000000000000000000000000000000000 --- a/src/comm/MAVLinkSimulationWaypointPlanner.h +++ /dev/null @@ -1,75 +0,0 @@ -#ifndef MAVLINKSIMULATIONWAYPOINTPLANNER_H -#define MAVLINKSIMULATIONWAYPOINTPLANNER_H - -#include -#include - -#include "MAVLinkSimulationLink.h" -#include "QGCMAVLink.h" - -enum PX_WAYPOINTPLANNER_STATES { - PX_WPP_IDLE = 0, - PX_WPP_SENDLIST, - PX_WPP_SENDLIST_SENDWPS, - PX_WPP_GETLIST, - PX_WPP_GETLIST_GETWPS, - PX_WPP_GETLIST_GOTALL -}; - -class MAVLinkSimulationWaypointPlanner : public QObject -{ - Q_OBJECT -public: - explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid); - -signals: - void messageSent(const mavlink_message_t& msg); - -public slots: - void handleMessage(const mavlink_message_t& msg); - -protected: - MAVLinkSimulationLink* link; - bool idle; ///< indicates if the system is following the waypoints or is waiting - uint16_t current_active_wp_id; ///< id of current waypoint - bool yawReached; ///< boolean for yaw attitude reached - bool posReached; ///< boolean for position reached - uint64_t timestamp_lastoutside_orbit;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - uint64_t timestamp_firstinside_orbit;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - - std::vector waypoints1; ///< vector1 that holds the waypoints - std::vector waypoints2; ///< vector2 that holds the waypoints - - std::vector* waypoints; ///< pointer to the currently active waypoint vector - std::vector* waypoints_receive_buffer; ///< pointer to the receive buffer waypoint vector - PX_WAYPOINTPLANNER_STATES current_state; - uint16_t protocol_current_wp_id; - uint16_t protocol_current_count; - uint8_t protocol_current_partner_systemid; - uint8_t protocol_current_partner_compid; - uint64_t protocol_timestamp_lastaction; - unsigned int protocol_timeout; - uint64_t timestamp_last_send_setpoint; - uint8_t systemid; - uint8_t compid; - unsigned int setpointDelay; - float yawTolerance; - bool verbose; - bool debug; - bool silent; - - void send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type); - void send_waypoint_current(uint16_t seq); - void send_setpoint(uint16_t seq); - void send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count); - void send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); - void send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); - void send_waypoint_reached(uint16_t seq); - float distanceToSegment(uint16_t seq, float x, float y, float z); - float distanceToPoint(uint16_t seq, float x, float y, float z); - float distanceToPoint(uint16_t seq, float x, float y); - void mavlink_handler(const mavlink_message_t* msg); - -}; - -#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H diff --git a/src/comm/MAVLinkSwarmSimulationLink.cc b/src/comm/MAVLinkSwarmSimulationLink.cc deleted file mode 100644 index e6cc882759b76f0f1081190ad21074e34f4e4d8d..0000000000000000000000000000000000000000 --- a/src/comm/MAVLinkSwarmSimulationLink.cc +++ /dev/null @@ -1,12 +0,0 @@ -#include "MAVLinkSwarmSimulationLink.h" - -MAVLinkSwarmSimulationLink::MAVLinkSwarmSimulationLink(QString readFile, QString writeFile, int rate) : - MAVLinkSimulationLink(readFile, writeFile, rate) -{ -} - - -void MAVLinkSwarmSimulationLink::mainloop() -{ - -} diff --git a/src/comm/MAVLinkSwarmSimulationLink.h b/src/comm/MAVLinkSwarmSimulationLink.h deleted file mode 100644 index 25e206535da45170501fc3700d0767f8dfd4efb7..0000000000000000000000000000000000000000 --- a/src/comm/MAVLinkSwarmSimulationLink.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef MAVLINKSWARMSIMULATIONLINK_H -#define MAVLINKSWARMSIMULATIONLINK_H - -#include "MAVLinkSimulationLink.h" - -class MAVLinkSwarmSimulationLink : public MAVLinkSimulationLink -{ - Q_OBJECT -public: - MAVLinkSwarmSimulationLink(QString readFile="", QString writeFile="", int rate=5); - -signals: - -public slots: - void mainloop(); - -}; - -#endif // MAVLINKSWARMSIMULATIONLINK_H diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 640f1fcf369380052817e0f692473efe9a2a3910..801c80e53459f80ca3b8169fb09e5aa3f53cf404 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -29,7 +29,6 @@ #include #include -#include "MAVLinkSimulationLink.h" #include "QGCMAVLink.h" /* Alreedy defined in MAVLinkSimulationLink.h above! diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index eefc271353035b4e2d0356300f91efddc23b223d..d035870866635cf8d09b80793f5cefe14bd122b7 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -40,7 +40,6 @@ This file is part of the QGROUNDCONTROL project #include #include "QGC.h" -#include "MAVLinkSimulationLink.h" #include "SerialLink.h" #include "MAVLinkProtocol.h" #include "QGCWaypointListMulti.h" @@ -149,7 +148,6 @@ MainWindow::MainWindow(QSplashScreen* splashScreen) , _lowPowerMode(false) , _showStatusBar(false) , _centerStackActionGroup(new QActionGroup(this)) - , _simulationLink(NULL) , _centralLayout(NULL) , _currentViewWidget(NULL) , _splashScreen(splashScreen) @@ -346,11 +344,6 @@ MainWindow::MainWindow(QSplashScreen* splashScreen) MainWindow::~MainWindow() { - if (_simulationLink) - { - delete _simulationLink; - _simulationLink = NULL; - } #ifndef __android__ if (joystick) { @@ -857,8 +850,6 @@ void MainWindow::connectCommonActions() // Application Settings connect(_ui.actionSettings, SIGNAL(triggered()), this, SLOT(showSettings())); - connect(_ui.actionSimulate, SIGNAL(triggered(bool)), this, SLOT(simulateLink(bool))); - // Update Tool Bar _mainToolBar->setCurrentView(_currentView); } @@ -904,19 +895,6 @@ void MainWindow::showSettings() settings.exec(); } -void MainWindow::simulateLink(bool simulate) { - if (simulate) { - if (!_simulationLink) { - _simulationLink = new MAVLinkSimulationLink(":/demo-log.txt"); - Q_CHECK_PTR(_simulationLink); - } - LinkManager::instance()->connectLink(_simulationLink); - } else { - Q_ASSERT(_simulationLink); - LinkManager::instance()->disconnectLink(_simulationLink); - } -} - void MainWindow::commsWidgetDestroyed(QObject *obj) { // Do not dynamic cast or de-reference QObject, since object is either in destructor or may have already diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index f4f9931521e94ef1e3a12d3ac5d6abc960fb7d96..54c7023f8f587f7c3d0cab8523e9b15c698396be 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -47,7 +47,6 @@ This file is part of the QGROUNDCONTROL project #include "WaypointList.h" #include "CameraView.h" #include "UASListWidget.h" -#include "MAVLinkSimulationLink.h" #ifndef __android__ #include "input/JoystickInput.h" #endif @@ -145,8 +144,6 @@ public: public slots: /** @brief Show the application settings */ void showSettings(); - /** @brief Simulate a link */ - void simulateLink(bool simulate); /** @brief Add a new UAS */ void UASCreated(UASInterface* uas); @@ -292,7 +289,6 @@ protected: QAction* returnUASAct; QAction* stopUASAct; QAction* killUASAct; - QAction* simulateUASAct; LogCompressor* comp; @@ -366,7 +362,6 @@ private: bool _lowPowerMode; ///< If enabled, QGC reduces the update rates of all widgets bool _showStatusBar; QActionGroup* _centerStackActionGroup; - MAVLinkSimulationLink* _simulationLink; QVBoxLayout* _centralLayout; QList _commsWidgetList; QWidget* _currentViewWidget; ///< Currently displayed view widget diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index bcc22f598a4ae2c5da9fea91a0135ca3af7a5d69..1d5edab7b69157eb6469a40ac9a9ff0ae579acf8 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -101,7 +101,6 @@ - @@ -170,17 +169,6 @@ Manage Communication Links - - - true - - - Simulate - - - Simulate one vehicle to test and evaluate this application - - Online Documentation diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index 9d428714ce39ade74953ac3358c61810af1b7de0..45bc0479d4b28474ea22225d55fe314e22b7de92 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -317,7 +317,8 @@ bool QGCMAVLinkLogPlayer::loadLogFile(const QString& file) if (logLink) { LinkManager::instance()->_deleteLink(logLink); } - logLink = new MAVLinkSimulationLink(""); + logLink = new MockLink(); + LinkManager::instance()->_addLink(logLink); // Select if binary or MAVLink log format is used mavlinkLogFormat = file.endsWith(".mavlink"); diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h index eb4ab1ed63ae8b9830cf8b6577bf93adc29a29c6..8e49b14d8b12e637d7c662f8f4d0324e9d592efc 100644 --- a/src/ui/QGCMAVLinkLogPlayer.h +++ b/src/ui/QGCMAVLinkLogPlayer.h @@ -6,7 +6,7 @@ #include "MAVLinkProtocol.h" #include "LinkInterface.h" -#include "MAVLinkSimulationLink.h" +#include "MockLink.h" namespace Ui { @@ -67,7 +67,7 @@ protected: quint64 logEndTime; ///< The last timestamp in the current log file. In units of microseconds since epoch UTC. float accelerationFactor; MAVLinkProtocol* mavlink; - MAVLinkSimulationLink* logLink; + MockLink* logLink; QFile logFile; QTimer loopTimer; int loopCounter; diff --git a/src/ui/uas/QGCUnconnectedInfoWidget.cc b/src/ui/uas/QGCUnconnectedInfoWidget.cc index ff4180cf21102ac435928941e86e75f2455752d8..0309c3125d79c31d22aa0497c09b35e900ab4ce0 100644 --- a/src/ui/uas/QGCUnconnectedInfoWidget.cc +++ b/src/ui/uas/QGCUnconnectedInfoWidget.cc @@ -1,7 +1,6 @@ #include "QGCUnconnectedInfoWidget.h" #include "LinkInterface.h" #include "LinkManager.h" -#include "MAVLinkSimulationLink.h" #include "MainWindow.h" #include "ui_QGCUnconnectedInfoWidget.h" @@ -24,15 +23,6 @@ QGCUnconnectedInfoWidget::~QGCUnconnectedInfoWidget() */ void QGCUnconnectedInfoWidget::simulate() { - // TODO What is this? - // Try to get reference to MAVLinkSimulationlink - QList links = LinkManager::instance()->getLinks(); - foreach(LinkInterface* link, links) { - MAVLinkSimulationLink* sim = dynamic_cast(link); - if (sim) { - LinkManager::instance()->connectLink(sim); - } - } } /** diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc index 3fe12a1a5ff6da0ed6fb381c9470f8a1e33a251e..3733a88ce06f47067e3c592dbc977a4362422263 100644 --- a/src/ui/uas/UASListWidget.cc +++ b/src/ui/uas/UASListWidget.cc @@ -41,7 +41,6 @@ This file is part of the PIXHAWK project #include "UASView.h" #include "QGCUnconnectedInfoWidget.h" #include "MainWindow.h" -#include "MAVLinkSimulationLink.h" #include "LinkManager.h" UASListWidget::UASListWidget(QWidget *parent) : QWidget(parent),