diff --git a/files/flightgear/qgroundcontrol.xml b/files/flightgear/qgroundcontrol.xml
index 9f10d87..94a60de 100644
--- a/files/flightgear/qgroundcontrol.xml
+++ b/files/flightgear/qgroundcontrol.xml
@@ -5,119 +5,151 @@
newline
- ,
+ tab
- magnetos
- float
- %f
- /controls/engines/engine[0]/magnetos
-
-
-
aileron
float
/controls/flight/aileron
@@ -134,12 +166,19 @@
float
/controls/flight/rudder
+
+
+ running
+ bool
+ /engines/engine/running
+
throttle
float
/controls/engines/engine/throttle
+
diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc
index 36d66bb..3438c34 100644
--- a/src/comm/MAVLinkSimulationMAV.cc
+++ b/src/comm/MAVLinkSimulationMAV.cc
@@ -207,6 +207,19 @@ void MAVLinkSimulationMAV::mainloop()
// 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.0;
+ hil.pitch_elevator = 0.0;
+ hil.yaw_rudder = 0.05;
+ hil.throttle = 0.6;
+ // 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
diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc
index 351e6a7..1b67fbf 100644
--- a/src/comm/QGCFlightGearLink.cc
+++ b/src/comm/QGCFlightGearLink.cc
@@ -139,13 +139,13 @@ void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, float
{
// magnetos,aileron,elevator,rudder,throttle\n
- float magnetos = 3.0f;
+ //float magnetos = 3.0f;
Q_UNUSED(time);
Q_UNUSED(systemMode);
Q_UNUSED(navMode);
- QString state("%1,%2,%3,%4,%5\n");
- state = state.arg(magnetos).arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(throttle);
+ QString state("%1\t%2\t%3\t%4\t%5\n");
+ state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle);
writeBytes(state.toAscii().constData(), state.length());
//qDebug() << "Updated controls" << state;
}
@@ -199,29 +199,43 @@ void QGCFlightGearLink::readBytes()
QString state(b);
//qDebug() << "FG LINK GOT:" << state;
- QStringList values = state.split(",");
+ QStringList values = state.split("\t");
+
+ // Check length
+ if (values.size() != 17)
+ {
+ qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 17 << "BUT GOT" << values.size();
+ qDebug() << state;
+ return;
+ }
// Parse string
+ double time;
float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
- int32_t lat, lon, alt;
- int16_t vx, vy, vz, xacc, yacc, zacc;
-
- lat = values.at(0).toDouble();
- lon = values.at(1).toDouble();
- alt = values.at(2).toDouble();
- roll = values.at(3).toDouble();
- pitch = values.at(4).toDouble();
- yaw = values.at(5).toDouble();
- vx = values.at(6).toDouble();
- vy = values.at(7).toDouble();
- vz = values.at(8).toDouble();
-
- // FIXME Accelerations missing
- xacc = 0;
- yacc = 0;
- zacc = 1.0;
-
-
+ double lat, lon, alt;
+ double vx, vy, vz, xacc, yacc, zacc;
+ double airspeed;
+
+ time = values.at(0).toDouble();
+ lat = values.at(1).toDouble();
+ lon = values.at(2).toDouble();
+ alt = values.at(3).toDouble();
+ roll = values.at(4).toDouble();
+ pitch = values.at(5).toDouble();
+ yaw = values.at(6).toDouble();
+ rollspeed = values.at(7).toDouble();
+ pitchspeed = values.at(8).toDouble();
+ yawspeed = values.at(9).toDouble();
+
+ xacc = values.at(10).toDouble();
+ yacc = values.at(11).toDouble();
+ zacc = values.at(12).toDouble();
+
+ vx = values.at(13).toDouble();
+ vy = values.at(14).toDouble();
+ vz = values.at(15).toDouble();
+
+ airspeed = values.at(16).toDouble();
// Send updated state
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
@@ -294,15 +308,15 @@ bool QGCFlightGearLink::connectSimulation()
QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
-// process = new QProcess(this);
+ process = 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)));
+ 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)));
-// //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
-// // Catch process error
-// QObject::connect( process, SIGNAL(error(QProcess::ProcessError)),
-// this, SLOT(processError(QProcess::ProcessError)));
+ //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
+ // Catch process error
+ QObject::connect( process, SIGNAL(error(QProcess::ProcessError)),
+ this, SLOT(processError(QProcess::ProcessError)));
// Start Flightgear
QStringList processCall;
QString processFgfs;
@@ -340,11 +354,12 @@ bool QGCFlightGearLink::connectSimulation()
processCall << "--disable-sound";
processCall << "--disable-anti-alias-hud";
processCall << "--disable-fullscreen";
- processCall << "--disable-random-objects";
- processCall << "--disable-ai-models";
- processCall << "--wind=0@0";
+// processCall << "--disable-random-objects";
+// processCall << "--disable-ai-models";
+// processCall << "--wind=0@0";
processCall << "--fdm=jsb";
- processCall << "--prop:/engines/engine[0]/running=true";
+ processCall << "--prop:/engines/engine/running=true";
+ processCall << "--units-meters";
if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
{
// Start the remaining three motors of the quad
@@ -352,22 +367,22 @@ bool QGCFlightGearLink::connectSimulation()
processCall << "--prop:/engines/engine[2]/running=true";
processCall << "--prop:/engines/engine[3]/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());
+// 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);
-// process->start(processFgfs, processCall);
+ process->start(processFgfs, processCall);
-// emit flightGearConnected(connectState);
-// if (connectState) {
-// emit flightGearConnected();
-// connectionStartTime = QGC::groundTimeUsecs()/1000;
-// }
-// qDebug() << "STARTING SIM";
+ emit flightGearConnected(connectState);
+ if (connectState) {
+ emit flightGearConnected();
+ connectionStartTime = QGC::groundTimeUsecs()/1000;
+ }
+ qDebug() << "STARTING SIM";
qDebug() << "STARTING: " << processFgfs << processCall;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index cc0f1d4..c5c8417 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -343,8 +343,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
currentVoltage = state.voltage_battery/1000.0f;
lpVoltage = filterVoltage(currentVoltage);
- qDebug() << "BATT VOLTAGE:" << currentVoltage << "RAW" << state.voltage_battery;
-
if (startVoltage == 0) startVoltage = currentVoltage;
timeRemaining = calculateTimeRemaining();
if (!batteryRemainingEstimateEnabled && chargeLevel != -1)