Don Gagne
10 years ago
19 changed files with 8 additions and 2910 deletions
@ -1,860 +0,0 @@
@@ -1,860 +0,0 @@
|
||||
/*=====================================================================
|
||||
|
||||
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; |
||||
} |
||||
@ -1,138 +0,0 @@
@@ -1,138 +0,0 @@
|
||||
/*=====================================================================
|
||||
|
||||
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
|
||||
@ -1,522 +0,0 @@
@@ -1,522 +0,0 @@
|
||||
#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; |
||||
} |
||||
} |
||||
@ -1,75 +0,0 @@
@@ -1,75 +0,0 @@
|
||||
#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
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,75 +0,0 @@
@@ -1,75 +0,0 @@
|
||||
#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
|
||||
@ -1,12 +0,0 @@
@@ -1,12 +0,0 @@
|
||||
#include "MAVLinkSwarmSimulationLink.h" |
||||
|
||||
MAVLinkSwarmSimulationLink::MAVLinkSwarmSimulationLink(QString readFile, QString writeFile, int rate) : |
||||
MAVLinkSimulationLink(readFile, writeFile, rate) |
||||
{ |
||||
} |
||||
|
||||
|
||||
void MAVLinkSwarmSimulationLink::mainloop() |
||||
{ |
||||
|
||||
} |
||||
@ -1,19 +0,0 @@
@@ -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
|
||||
Loading…
Reference in new issue