zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Working HIL!

QGC4.4
lm 14 years ago
parent
b51baab709
commit
a821d12190
4 changed files with 270 additions and 146 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 112
      src/comm/MAVLinkSimulationMAV.cc
  2. 3
      src/comm/MAVLinkSimulationMAV.h
  3. 128
      src/comm/QGCFlightGearLink.cc
  4. 1
      src/comm/QGCFlightGearLink.h

112
src/comm/MAVLinkSimulationMAV.cc
Unescape View File

@ -21,6 +21,9 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy @@ -21,6 +21,9 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
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),
@ -98,6 +101,8 @@ void MAVLinkSimulationMAV::mainloop() @@ -98,6 +101,8 @@ void MAVLinkSimulationMAV::mainloop()
float zsign = (zm < 0) ? -1.0f : 1.0f;
if (!(sys_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL))
{
if (!firstWP) {
//float trueyaw = atan2f(xm, ym);
@ -126,14 +131,20 @@ void MAVLinkSimulationMAV::mainloop() @@ -126,14 +131,20 @@ void MAVLinkSimulationMAV::mainloop()
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 = z*1000.0;
pos.lat = x*1E7;
pos.lon = y*1E7;
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;
@ -144,12 +155,12 @@ void MAVLinkSimulationMAV::mainloop() @@ -144,12 +155,12 @@ void MAVLinkSimulationMAV::mainloop()
// ATTITUDE
mavlink_attitude_t attitude;
attitude.time_boot_ms = 0;
attitude.roll = 0.0f;
attitude.pitch = 0.0f;
attitude.roll = roll;
attitude.pitch = pitch;
attitude.yaw = yaw;
attitude.rollspeed = 0.0f;
attitude.pitchspeed = 0.0f;
attitude.yawspeed = 0.0f;
attitude.rollspeed = rollspeed;
attitude.pitchspeed = pitchspeed;
attitude.yawspeed = yawspeed;
mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
link->sendMAVLinkMessage(&msg);
@ -157,11 +168,11 @@ void MAVLinkSimulationMAV::mainloop() @@ -157,11 +168,11 @@ void MAVLinkSimulationMAV::mainloop()
// SYSTEM STATUS
mavlink_sys_status_t status;
status.load = 300;
// status.mode = sys_mode;
// status.nav_mode = nav_mode;
// status.mode = sys_mode;
// status.nav_mode = nav_mode;
status.errors_comm = 0;
status.voltage_battery = 10500;
// status.status = sys_state;
// status.status = sys_state;
status.battery_remaining = 90;
mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status);
link->sendMAVLinkMessage(&msg);
@ -171,7 +182,7 @@ void MAVLinkSimulationMAV::mainloop() @@ -171,7 +182,7 @@ void MAVLinkSimulationMAV::mainloop()
mavlink_vfr_hud_t hud;
hud.airspeed = pos.vx/100.0f;
hud.groundspeed = pos.vx/100.0f;
hud.alt = z;
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;
@ -211,7 +222,7 @@ void MAVLinkSimulationMAV::mainloop() @@ -211,7 +222,7 @@ void MAVLinkSimulationMAV::mainloop()
{
mavlink_hil_controls_t hil;
hil.roll_ailerons = 0.0;
hil.pitch_elevator = 0.0;
hil.pitch_elevator = 0.05;
hil.yaw_rudder = 0.05;
hil.throttle = 0.6;
// Encode the data (adding header and checksums, etc.)
@ -223,12 +234,12 @@ void MAVLinkSimulationMAV::mainloop() @@ -223,12 +234,12 @@ void MAVLinkSimulationMAV::mainloop()
// 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);
// 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
@ -395,31 +406,46 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) @@ -395,31 +406,46 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
if (systemid == mode.target_system) sys_mode = mode.base_mode;
}
break;
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t state;
mavlink_msg_hil_state_decode(&msg, &state);
roll = state.roll;
pitch = state.pitch;
yaw = state.yaw;
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);
// }
// }
// 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;
case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: {
mavlink_set_local_position_setpoint_t sp;

3
src/comm/MAVLinkSimulationMAV.h
Unescape View File

@ -36,6 +36,9 @@ protected: @@ -36,6 +36,9 @@ protected:
double roll;
double pitch;
double yaw;
double rollspeed;
double pitchspeed;
double yawspeed;
bool globalNavigation;
bool firstWP;

128
src/comm/QGCFlightGearLink.cc
Unescape View File

@ -24,6 +24,7 @@ This file is part of the QGROUNDCONTROL project @@ -24,6 +24,7 @@ This file is part of the QGROUNDCONTROL project
/**
* @file
* @brief Definition of UDP connection (server) for unmanned vehicles
* @see Flightgear Manual http://mapserver.flightgear.org/getstart.pdf
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
@ -38,7 +39,9 @@ This file is part of the QGROUNDCONTROL project @@ -38,7 +39,9 @@ This file is part of the QGROUNDCONTROL project
#include <QHostInfo>
#include "MainWindow.h"
QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString remoteHost, QHostAddress host, quint16 port)
QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString remoteHost, QHostAddress host, quint16 port) :
process(NULL),
terraSync(NULL)
{
this->host = host;
this->port = port+mav->getUASID();
@ -282,8 +285,15 @@ bool QGCFlightGearLink::disconnectSimulation() @@ -282,8 +285,15 @@ bool QGCFlightGearLink::disconnectSimulation()
delete process;
process = NULL;
}
if (terraSync)
{
terraSync->close();
delete terraSync;
terraSync = NULL;
}
if (socket)
{
socket->close();
delete socket;
socket = NULL;
}
@ -309,6 +319,7 @@ bool QGCFlightGearLink::connectSimulation() @@ -309,6 +319,7 @@ bool QGCFlightGearLink::connectSimulation()
QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
process = new QProcess(this);
terraSync = new QProcess(this);
connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
@ -317,62 +328,145 @@ bool QGCFlightGearLink::connectSimulation() @@ -317,62 +328,145 @@ bool QGCFlightGearLink::connectSimulation()
// Catch process error
QObject::connect( process, SIGNAL(error(QProcess::ProcessError)),
this, SLOT(processError(QProcess::ProcessError)));
QObject::connect( terraSync, SIGNAL(error(QProcess::ProcessError)),
this, SLOT(processError(QProcess::ProcessError)));
// Start Flightgear
QStringList processCall;
QString processFgfs;
QString processTerraSync;
QString fgRoot;
QString fgScenery;
QString aircraft("Rascal110-JSBSim");
QString aircraft;
if (mav->getSystemType() == MAV_TYPE_FIXED_WING)
{
aircraft = "Rascal110-JSBSim";
}
else if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
{
aircraft = "arducopter";
}
else
{
aircraft = "Rascal110-JSBSim";
}
#ifdef Q_OS_MACX
processFgfs = "/Applications/FlightGear.app/Contents/Resources/fgfs";
fgRoot = "--fg-root=/Applications/FlightGear.app/Contents/Resources/data";
fgScenery = "--fg-scenery=/Applications/FlightGear.app/Contents/Resources/data/Scenery:/Applications/FlightGear.app/Contents/Resources/data/Scenery-Terrasync";
processTerraSync = "/Applications/FlightGear.app/Contents/Resources/terrasync";
fgRoot = "/Applications/FlightGear.app/Contents/Resources/data";
//fgScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery";
fgScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery-TerraSync";
// /Applications/FlightGear.app/Contents/Resources/data/Scenery:
#endif
#ifdef Q_OS_WIN32
processFgfs = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs";
fgRoot = "--fg-root=C:\\Program Files (x86)\\FlightGear\\data";
fgRoot = "C:\\Program Files (x86)\\FlightGear\\data";
fgScenery = "C:\\Program Files (x86)\\FlightGear\\data\\Scenery-Terrasync";
#endif
#ifdef Q_OS_LINUX
processFgfs = "fgfs";
fgRoot = "--fg-root=/usr/share/flightgear/data";
fgRoot = "/usr/share/flightgear/data";
fgScenery = "/usr/share/flightgear/data/Scenery-Terrasync";
#endif
processCall << fgRoot;
processCall << fgScenery;
// Sanity checks
bool sane = true;
QFileInfo executable(processFgfs);
if (!executable.isExecutable())
{
MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear was not found at %1").arg(processFgfs));
sane = false;
}
QFileInfo root(fgRoot);
if (!root.isDir())
{
MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear data directory was not found at %1").arg(fgRoot));
sane = false;
}
QFileInfo scenery(fgScenery);
if (!scenery.isDir())
{
MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear scenery directory was not found at %1").arg(fgScenery));
sane = false;
}
if (!sane) return false;
// --atlas=socket,out,1,localhost,5505,udp
// terrasync -p 5505 -S -d /usr/local/share/TerraSync
processCall << QString("--fg-root=%1").arg(fgRoot);
processCall << QString("--fg-scenery=%1").arg(fgScenery);
if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
{
// FIXME ADD QUAD-Specific protocol here
processCall << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol").arg(port);
processCall << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol").arg(currentPort);
}
else
{
processCall << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol").arg(port);
processCall << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol").arg(currentPort);
}
processCall << "--atlas=socket,out,1,localhost,5505,udp";
processCall << "--in-air";
processCall << "--roll=0";
processCall << "--pitch=0";
processCall << "--vc=90";
processCall << "--heading=300";
processCall << "--timeofday=noon";
processCall << "--disable-hud-3d";
processCall << "--disable-fullscreen";
// processCall << "--control=mouse";
// processCall << "--disable-intro-music";
// processCall << "--disable-sound";
// processCall << "--disable-anti-alias-hud";
// processCall << "--disable-random-objects";
// processCall << "--disable-ai-models";
// processCall << "--wind=0@0";
processCall << "--geometry=400x300";
processCall << "--disable-anti-alias-hud";
processCall << "--wind=0@0";
processCall << "--turbulence=0.0";
processCall << "--prop:/sim/frame-rate-throttle-hz=30";
processCall << "--control=mouse";
processCall << "--disable-intro-music";
processCall << "--disable-sound";
processCall << "--disable-random-objects";
processCall << "--disable-ai-models";
processCall << "--shading-flat";
processCall << "--fog-disable";
processCall << "--disable-specular-highlight";
//processCall << "--disable-skyblend";
processCall << "--disable-random-objects";
processCall << "--disable-panel";
//processCall << "--disable-horizon-effect";
processCall << "--disable-clouds";
processCall << "--fdm=jsb";
processCall << "--prop:/engines/engine/running=true";
processCall << "--units-meters";
if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
{
// Start the remaining three motors of the quad
// Start all engines of the quad
processCall << "--prop:/engines/engine[0]/running=true";
processCall << "--prop:/engines/engine[1]/running=true";
processCall << "--prop:/engines/engine[2]/running=true";
processCall << "--prop:/engines/engine[3]/running=true";
}
else
{
processCall << "--prop:/engines/engine/running=true";
}
processCall << QString("--lat=%1").arg(UASManager::instance()->getHomeLatitude());
processCall << QString("--lon=%1").arg(UASManager::instance()->getHomeLongitude());
processCall << QString("--altitude=%1").arg(UASManager::instance()->getHomeAltitude());
// Add new argument with this: processCall << "";
processCall << QString("--aircraft=%2").arg(aircraft);
QStringList terraSyncArguments;
terraSyncArguments << "-p 5505";
terraSyncArguments << "-S";
terraSyncArguments << QString("-d=%1").arg(fgScenery);
terraSync->start(processTerraSync, terraSyncArguments);
process->start(processFgfs, processCall);

1
src/comm/QGCFlightGearLink.h
Unescape View File

@ -109,6 +109,7 @@ protected: @@ -109,6 +109,7 @@ protected:
QTimer refreshTimer;
UASInterface* mav;
QProcess* process;
QProcess* terraSync;
void setName(QString name);

Write Preview
Loading…
Cancel
Save