Skip to content

Q
qgroundcontrol
Commit 2899fc5f authored by lm's avatar lm
Browse files
Options

Fixed flightgear HIL

parent 7dba86d0
Hide whitespace changes
Inline Side-by-side
Showing with 159 additions and 94 deletions
files/flightgear/qgroundcontrol.xml
View file @ 2899fc5f
......@@ -5,118 +5,150 @@
<output>
<line_separator>newline</line_separator>
<var_separator>,</var_separator>
<var_separator>tab</var_separator>
<chunk>
<name>time (sec)</name>
<type>float</type>
<format>%.4f</format>
<node>/sim/time/elapsed-sec</node>
</chunk>
<!-- Position -->
<chunk>
<name>latitude-deg</name>
<type>float</type>
<format>%f</format>
<type>double</type>
<format>%.18f</format>
<node>/position/latitude-deg</node>
</chunk>
<chunk>
<name>longitude-deg</name>
<type>float</type>
<format>%f</format>
<type>double</type>
<format>%.18f</format>
<node>/position/longitude-deg</node>
</chunk>
<chunk>
<name>altitude-ft</name>
<type>float</type>
<format>%f</format>
<name>altitiude (m)</name>
<type>double</type>
<format>%.5f</format>
<node>/position/altitude-ft</node>
<factor>0.3048</factor> <!-- feet to meter -->
</chunk>
<!-- Orientation -->
<chunk>
<name>roll-deg</name>
<name>roll angle</name>
<type>float</type>
<format>%f</format>
<format>%.5f</format>
<node>/orientation/roll-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk>
<chunk>
<name>pitch-deg</name>
<name>pitch angle (rad)</name>
<type>float</type>
<format>%f</format>
<format>%.5f</format>
<node>/orientation/pitch-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk>
<chunk>
<name>heading-deg</name>
<name>yaw angle</name>
<type>float</type>
<format>%f</format>
<format>%.5f</format>
<node>/orientation/heading-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk>
<!-- Velocities -->
<chunk>
<name>v_n</name>
<name>roll rate ("p" rad/sec)</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-north-fps</node>
<format>%.6f</format>
<node>/fdm/jsbsim/velocities/pi-rad_sec</node>
</chunk>
<chunk>
<name>v_e</name>
<name>pitch rate ("q" rad/sec)</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-east-fps</node>
<format>%.6f</format>
<node>/fdm/jsbsim/velocities/qi-rad_sec</node>
</chunk>
<chunk>
<name>v_d</name>
<name>yaw rate ("r" rad/sec)</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-down-fps</node>
<format>%.6f</format>
<node>/fdm/jsbsim/velocities/ri-rad_sec</node>
</chunk>
<chunk>
<name>rate_roll</name>
<name>X accel (body axis) (mps)</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/roll-rate-degps</node>
<format>%.5f</format>
<node>/accelerations/pilot/x-accel-fps_sec</node>
<factor>0.3048</factor> <!-- feet to meter -->
</chunk>
<chunk>
<name>rate_pitch</name>
<name>Y accel (body axis) (mps)</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/pitch-rate-degps</node>
<format>%.5f</format>
<node>/accelerations/pilot/y-accel-fps_sec</node>
<factor>0.3048</factor> <!-- feet to meter -->
</chunk>
<chunk>
<name>rate_yaw</name>
<name>Z accel (body axis) (mps)</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/yaw-rate-degps</node>
<format>%.5f</format>
<node>/accelerations/pilot/z-accel-fps_sec</node>
<factor>0.3048</factor> <!-- feet to meter -->
</chunk>
<!-- Velocities -->
<chunk>
<name>airspeed-kt</name>
<name>Velocity North ("vn" mps)</name>
<type>float</type>
<format>%.8f</format>
<node>/velocities/speed-north-fps</node>
<factor>0.3048</factor> <!-- fps to mps -->
</chunk>
<chunk>
<name>Velocity East ("ve" mps)</name>
<type>float</type>
<format>%.8f</format>
<node>/velocities/speed-east-fps</node>
<factor>0.3048</factor> <!-- fps to mps -->
</chunk>
<chunk>
<name>Velocity Down ("vd" mps)</name>
<type>float</type>
<format>%.8f</format>
<node>/velocities/speed-down-fps</node>
<factor>0.3048</factor> <!-- fps to mps -->
</chunk>
<chunk>
<name>airspeed-mps</name>
<type>float</type>
<format>%f</format>
<format>%.8f</format>
<node>/velocities/airspeed-kt</node>
<factor>0.514444444444444</factor> <!-- knots to mps -->
</chunk>
</output>
<input>
<line_separator>newline</line_separator>
<var_separator>,</var_separator>
<var_separator>tab</var_separator>
<!-- Controls -->
<chunk>
<name>magnetos</name>
<type>float</type>
<format>%f</format>
<node>/controls/engines/engine[0]/magnetos</node>
</chunk>
<chunk>
<name>aileron</name>
<type>float</type>
......@@ -134,12 +166,19 @@
<type>float</type>
<node>/controls/flight/rudder</node>
</chunk>
<chunk>
<name>running</name>
<type>bool</type>
<node>/engines/engine/running</node>
</chunk>
<chunk>
<name>throttle</name>
<type>float</type>
<node>/controls/engines/engine/throttle</node>
</chunk>
</input>
......
src/comm/MAVLinkSimulationMAV.cc
View file @ 2899fc5f
......@@ -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
......
src/comm/QGCFlightGearLink.cc
View file @ 2899fc5f
......@@ -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;
......
src/uas/UAS.cc
View file @ 2899fc5f
......@@ -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)
......
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