Skip to content

Q
qgroundcontrol
Commit 000e0ef4 authored by Don Gagne's avatar Don Gagne
Browse files
Options

Remove the "Simulate" feature

parent 2f7fe9dd
Hide whitespace changes
Inline Side-by-side
Showing with 4 additions and 2896 deletions
QGCApplication.pro
View file @ 000e0ef4
......@@ -235,10 +235,6 @@ 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/ProtocolInterface.h \
src/comm/QGCFlightGearLink.h \
src/comm/QGCHilLink.h \
......@@ -378,10 +374,6 @@ 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/QGCFlightGearLink.cc \
src/comm/QGCJSBSimLink.cc \
src/comm/QGCXPlaneLink.cc \
......
src/QGCApplication.cc
View file @ 000e0ef4
......@@ -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"
......
src/comm/LinkConfiguration.h
View file @ 000e0ef4
......@@ -44,7 +44,6 @@ 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
......
src/comm/MAVLinkSimulationLink.cc deleted 100644 → 0
View file @ 2f7fe9dd
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Implementation of simulated system link
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <cmath>
#include <QTime>
#include <QImage>
#include <QDebug>
#include <QFileInfo>
#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<QString, float>();
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<size; i++)
{
if (mavlink_parse_char(getMavlinkChannel(), data[i], &msg, &comm))
{
// MESSAGE RECEIVED!
// qDebug() << "SIMULATION LINK RECEIVED MESSAGE!";
emit messageReceived(msg);
switch (msg.msgid)
{
// SET THE SYSTEM MODE
case MAVLINK_MSG_ID_SET_MODE:
{
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(&msg, &mode);
// Set mode indepent of mode.target
system.base_mode = mode.base_mode;
}
break;
// EXECUTE OPERATOR ACTIONS
case MAVLINK_MSG_ID_COMMAND_LONG:
{
mavlink_command_long_t action;
mavlink_msg_command_long_decode(&msg, &action);
// qDebug() << "SIM" << "received action" << action.command << "for system" << action.target_system;
// FIXME MAVLINKV10PORTINGNEEDED
// switch (action.action) {
// case MAV_ACTION_LAUNCH:
// status.status = MAV_STATE_ACTIVE;
// status.mode = MAV_MODE_AUTO;
// break;
// case MAV_ACTION_RETURN:
// status.status = MAV_STATE_ACTIVE;
// break;
// case MAV_ACTION_MOTORS_START:
// status.status = MAV_STATE_ACTIVE;
// status.mode = MAV_MODE_LOCKED;
// break;
// case MAV_ACTION_MOTORS_STOP:
// status.status = MAV_STATE_STANDBY;
// status.mode = MAV_MODE_LOCKED;
// break;
// case MAV_ACTION_EMCY_KILL:
// status.status = MAV_STATE_EMERGENCY;
// status.mode = MAV_MODE_MANUAL;
// break;
// case MAV_ACTION_SHUTDOWN:
// status.status = MAV_STATE_POWEROFF;
// status.mode = MAV_MODE_LOCKED;
// break;
// }
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION";
mavlink_param_request_list_t read;
mavlink_msg_param_request_list_decode(&msg, &read);
if (read.target_system == systemId)
{
// Output all params
// Iterate through all components, through all parameters and emit them
QMap<QString, float>::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;
}
src/comm/MAVLinkSimulationLink.h deleted 100644 → 0
View file @ 2f7fe9dd
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of MAVLinkSimulationLink
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#ifndef MAVLINKSIMULATIONLINK_H
#define MAVLINKSIMULATIONLINK_H
#include <QFile>
#include <QTimer>
#include <QTextStream>
#include <QQueue>
#include <QMutex>
#include <QMap>
#include <qmath.h>
#include <inttypes.h>
#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<uint8_t> readyBuffer;
QString name;
qint64 timeOffset;
mavlink_sys_status_t status;
mavlink_heartbeat_t system;
QMap<QString, float> 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
src/comm/MAVLinkSimulationMAV.cc deleted 100644 → 0
View file @ 2f7fe9dd
#include <QDebug>
#include <cmath>
#include <qmath.h>
#include <QGC.h>
#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<int>((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; i<f->array_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; i<m->num_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;
}
}
src/comm/MAVLinkSimulationMAV.h deleted 100644 → 0
View file @ 2f7fe9dd
#ifndef MAVLINKSIMULATIONMAV_H
#define MAVLINKSIMULATIONMAV_H
#include <QObject>
#include <QTimer>
#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
src/comm/MAVLinkSimulationWaypointPlanner.cc deleted 100644 → 0
View file @ 2f7fe9dd
/*======================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
(c) 2009-2011 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
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 <http://www.gnu.org/licenses/>.
========================================================================*/
/**
* @file
* @brief a program to manage waypoints and exchange them with the ground station
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Benjamin Knecht <bknecht@student.ethz.ch>
* @author Christian Schluchter <schluchc@ee.ethz.ch>
*/
#include <cmath>
#include "MAVLinkSimulationWaypointPlanner.h"
#include "QGC.h"
#include <QDebug>
#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<double>(pos.lat)/1E7;
float y = static_cast<double>(pos.lon)/1E7;
//float z = static_cast<double>(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<uint16_t>(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<uint16_t>(waypoints_receive_buffer->size()) - 1;
}
// switch the waypoints list
std::vector<mavlink_mission_item_t*>* 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;
}
}
src/comm/MAVLinkSimulationWaypointPlanner.h deleted 100644 → 0
View file @ 2f7fe9dd
#ifndef MAVLINKSIMULATIONWAYPOINTPLANNER_H
#define MAVLINKSIMULATIONWAYPOINTPLANNER_H
#include <QObject>
#include <vector>
#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<mavlink_mission_item_t*> waypoints1; ///< vector1 that holds the waypoints
std::vector<mavlink_mission_item_t*> waypoints2; ///< vector2 that holds the waypoints
std::vector<mavlink_mission_item_t*>* waypoints; ///< pointer to the currently active waypoint vector
std::vector<mavlink_mission_item_t*>* 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
src/comm/MAVLinkSwarmSimulationLink.cc deleted 100644 → 0
View file @ 2f7fe9dd
#include "MAVLinkSwarmSimulationLink.h"
MAVLinkSwarmSimulationLink::MAVLinkSwarmSimulationLink(QString readFile, QString writeFile, int rate) :
MAVLinkSimulationLink(readFile, writeFile, rate)
{
}
void MAVLinkSwarmSimulationLink::mainloop()
{
}
src/comm/MAVLinkSwarmSimulationLink.h deleted 100644 → 0
View file @ 2f7fe9dd
#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
src/comm/MockLinkMissionItemHandler.h
View file @ 000e0ef4
......@@ -29,7 +29,6 @@
#include <QObject>
#include <vector>
#include "MAVLinkSimulationLink.h"
#include "QGCMAVLink.h"
/* Alreedy defined in MAVLinkSimulationLink.h above!
......
src/ui/MainWindow.cc
View file @ 000e0ef4
......@@ -40,7 +40,6 @@ This file is part of the QGROUNDCONTROL project
#include <QDesktopWidget>
#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
......
src/ui/MainWindow.h
View file @ 000e0ef4
......@@ -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<QObject*> _commsWidgetList;
QWidget* _currentViewWidget; ///< Currently displayed view widget
......
src/ui/MainWindow.ui
View file @ 000e0ef4
......@@ -101,7 +101,6 @@
<addaction name="actionTerminalView"/>
<addaction name="actionSimulationView"/>
<addaction name="separator"/>
<addaction name="actionSimulate"/>
<addaction name="separator"/>
<addaction name="menuTools"/>
</widget>
......@@ -170,17 +169,6 @@
<string>Manage Communication Links</string>
</property>
</action>
<action name="actionSimulate">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Simulate</string>
</property>
<property name="toolTip">
<string>Simulate one vehicle to test and evaluate this application</string>
</property>
</action>
<action name="actionOnline_Documentation">
<property name="text">
<string>Online Documentation</string>
......
src/ui/QGCMAVLinkLogPlayer.cc
View file @ 000e0ef4
......@@ -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");
......
src/ui/QGCMAVLinkLogPlayer.h
View file @ 000e0ef4
......@@ -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;
......
src/ui/uas/QGCUnconnectedInfoWidget.cc
View file @ 000e0ef4
#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<LinkInterface*> links = LinkManager::instance()->getLinks();
foreach(LinkInterface* link, links) {
MAVLinkSimulationLink* sim = dynamic_cast<MAVLinkSimulationLink*>(link);
if (sim) {
LinkManager::instance()->connectLink(sim);
}
}
}
/**
......
src/ui/uas/UASListWidget.cc
View file @ 000e0ef4
......@@ -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),
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment