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 @@ ...@@ -5,118 +5,150 @@
<output> <output>
<line_separator>newline</line_separator> <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 --> <!-- Position -->
<chunk> <chunk>
<name>latitude-deg</name> <name>latitude-deg</name>
<type>float</type> <type>double</type>
<format>%f</format> <format>%.18f</format>
<node>/position/latitude-deg</node> <node>/position/latitude-deg</node>
</chunk> </chunk>
<chunk> <chunk>
<name>longitude-deg</name> <name>longitude-deg</name>
<type>float</type> <type>double</type>
<format>%f</format> <format>%.18f</format>
<node>/position/longitude-deg</node> <node>/position/longitude-deg</node>
</chunk> </chunk>
<chunk> <chunk>
<name>altitude-ft</name> <name>altitiude (m)</name>
<type>float</type> <type>double</type>
<format>%f</format> <format>%.5f</format>
<node>/position/altitude-ft</node> <node>/position/altitude-ft</node>
<factor>0.3048</factor> <!-- feet to meter -->
</chunk> </chunk>
<!-- Orientation --> <!-- Orientation -->
<chunk> <chunk>
<name>roll-deg</name> <name>roll angle</name>
<type>float</type> <type>float</type>
<format>%f</format> <format>%.5f</format>
<node>/orientation/roll-deg</node> <node>/orientation/roll-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk> </chunk>
<chunk> <chunk>
<name>pitch-deg</name> <name>pitch angle (rad)</name>
<type>float</type> <type>float</type>
<format>%f</format> <format>%.5f</format>
<node>/orientation/pitch-deg</node> <node>/orientation/pitch-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk> </chunk>
<chunk> <chunk>
<name>heading-deg</name> <name>yaw angle</name>
<type>float</type> <type>float</type>
<format>%f</format> <format>%.5f</format>
<node>/orientation/heading-deg</node> <node>/orientation/heading-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk> </chunk>
<!-- Velocities -->
<chunk> <chunk>
<name>v_n</name> <name>roll rate ("p" rad/sec)</name>
<type>float</type> <type>float</type>
<format>%2.3f</format> <format>%.6f</format>
<node>/velocities/speed-north-fps</node> <node>/fdm/jsbsim/velocities/pi-rad_sec</node>
</chunk> </chunk>
<chunk> <chunk>
<name>v_e</name> <name>pitch rate ("q" rad/sec)</name>
<type>float</type> <type>float</type>
<format>%2.3f</format> <format>%.6f</format>
<node>/velocities/speed-east-fps</node> <node>/fdm/jsbsim/velocities/qi-rad_sec</node>
</chunk> </chunk>
<chunk> <chunk>
<name>v_d</name> <name>yaw rate ("r" rad/sec)</name>
<type>float</type> <type>float</type>
<format>%2.3f</format> <format>%.6f</format>
<node>/velocities/speed-down-fps</node> <node>/fdm/jsbsim/velocities/ri-rad_sec</node>
</chunk> </chunk>
<chunk> <chunk>
<name>rate_roll</name> <name>X accel (body axis) (mps)</name>
<type>float</type> <type>float</type>
<format>%2.3f</format> <format>%.5f</format>
<node>/orientation/roll-rate-degps</node> <node>/accelerations/pilot/x-accel-fps_sec</node>
<factor>0.3048</factor> <!-- feet to meter -->
</chunk> </chunk>
<chunk> <chunk>
<name>rate_pitch</name> <name>Y accel (body axis) (mps)</name>
<type>float</type> <type>float</type>
<format>%2.3f</format> <format>%.5f</format>
<node>/orientation/pitch-rate-degps</node> <node>/accelerations/pilot/y-accel-fps_sec</node>
<factor>0.3048</factor> <!-- feet to meter -->
</chunk> </chunk>
<chunk> <chunk>
<name>rate_yaw</name> <name>Z accel (body axis) (mps)</name>
<type>float</type> <type>float</type>
<format>%2.3f</format> <format>%.5f</format>
<node>/orientation/yaw-rate-degps</node> <node>/accelerations/pilot/z-accel-fps_sec</node>
<factor>0.3048</factor> <!-- feet to meter -->
</chunk> </chunk>
<!-- Velocities -->
<chunk> <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> <type>float</type>
<format>%f</format> <format>%.8f</format>
<node>/velocities/airspeed-kt</node> <node>/velocities/airspeed-kt</node>
<factor>0.514444444444444</factor> <!-- knots to mps -->
</chunk> </chunk>
</output> </output>
<input> <input>
<line_separator>newline</line_separator> <line_separator>newline</line_separator>
<var_separator>,</var_separator> <var_separator>tab</var_separator>
<!-- Controls --> <!-- Controls -->
<chunk>
<name>magnetos</name>
<type>float</type>
<format>%f</format>
<node>/controls/engines/engine[0]/magnetos</node>
</chunk>
<chunk> <chunk>
<name>aileron</name> <name>aileron</name>
<type>float</type> <type>float</type>
...@@ -134,12 +166,19 @@ ...@@ -134,12 +166,19 @@
<type>float</type> <type>float</type>
<node>/controls/flight/rudder</node> <node>/controls/flight/rudder</node>
</chunk> </chunk>
<chunk>
<name>running</name>
<type>bool</type>
<node>/engines/engine/running</node>
</chunk>
<chunk> <chunk>
<name>throttle</name> <name>throttle</name>
<type>float</type> <type>float</type>
<node>/controls/engines/engine/throttle</node> <node>/controls/engines/engine/throttle</node>
</chunk> </chunk>
</input> </input>
......
src/comm/MAVLinkSimulationMAV.cc
View file @ 2899fc5f
...@@ -207,6 +207,19 @@ void MAVLinkSimulationMAV::mainloop() ...@@ -207,6 +207,19 @@ void MAVLinkSimulationMAV::mainloop()
// The message container to be used for sending // The message container to be used for sending
mavlink_message_t ret; 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 // Send actual controller outputs
// This message just shows the direction // This message just shows the direction
// and magnitude of the control output // 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 ...@@ -139,13 +139,13 @@ void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, float
{ {
// magnetos,aileron,elevator,rudder,throttle\n // magnetos,aileron,elevator,rudder,throttle\n
float magnetos = 3.0f; //float magnetos = 3.0f;
Q_UNUSED(time); Q_UNUSED(time);
Q_UNUSED(systemMode); Q_UNUSED(systemMode);
Q_UNUSED(navMode); Q_UNUSED(navMode);
QString state("%1,%2,%3,%4,%5\n"); QString state("%1\t%2\t%3\t%4\t%5\n");
state = state.arg(magnetos).arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(throttle); state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle);
writeBytes(state.toAscii().constData(), state.length()); writeBytes(state.toAscii().constData(), state.length());
//qDebug() << "Updated controls" << state; //qDebug() << "Updated controls" << state;
} }
...@@ -199,29 +199,43 @@ void QGCFlightGearLink::readBytes() ...@@ -199,29 +199,43 @@ void QGCFlightGearLink::readBytes()
QString state(b); QString state(b);
//qDebug() << "FG LINK GOT:" << state; //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 // Parse string
double time;
float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
int32_t lat, lon, alt; double lat, lon, alt;
int16_t vx, vy, vz, xacc, yacc, zacc; double vx, vy, vz, xacc, yacc, zacc;
double airspeed;
lat = values.at(0).toDouble();
lon = values.at(1).toDouble(); time = values.at(0).toDouble();
alt = values.at(2).toDouble(); lat = values.at(1).toDouble();
roll = values.at(3).toDouble(); lon = values.at(2).toDouble();
pitch = values.at(4).toDouble(); alt = values.at(3).toDouble();
yaw = values.at(5).toDouble(); roll = values.at(4).toDouble();
vx = values.at(6).toDouble(); pitch = values.at(5).toDouble();
vy = values.at(7).toDouble(); yaw = values.at(6).toDouble();
vz = values.at(8).toDouble(); rollspeed = values.at(7).toDouble();
pitchspeed = values.at(8).toDouble();
// FIXME Accelerations missing yawspeed = values.at(9).toDouble();
xacc = 0;
yacc = 0; xacc = values.at(10).toDouble();
zacc = 1.0; 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 // Send updated state
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
...@@ -294,15 +308,15 @@ bool QGCFlightGearLink::connectSimulation() ...@@ -294,15 +308,15 @@ bool QGCFlightGearLink::connectSimulation()
QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); 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(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(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())); //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
// // Catch process error // Catch process error
// QObject::connect( process, SIGNAL(error(QProcess::ProcessError)), QObject::connect( process, SIGNAL(error(QProcess::ProcessError)),
// this, SLOT(processError(QProcess::ProcessError))); this, SLOT(processError(QProcess::ProcessError)));
// Start Flightgear // Start Flightgear
QStringList processCall; QStringList processCall;
QString processFgfs; QString processFgfs;
...@@ -340,11 +354,12 @@ bool QGCFlightGearLink::connectSimulation() ...@@ -340,11 +354,12 @@ bool QGCFlightGearLink::connectSimulation()
processCall << "--disable-sound"; processCall << "--disable-sound";
processCall << "--disable-anti-alias-hud"; processCall << "--disable-anti-alias-hud";
processCall << "--disable-fullscreen"; processCall << "--disable-fullscreen";
processCall << "--disable-random-objects"; // processCall << "--disable-random-objects";
processCall << "--disable-ai-models"; // processCall << "--disable-ai-models";
processCall << "--wind=0@0"; // processCall << "--wind=0@0";
processCall << "--fdm=jsb"; 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) if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
{ {
// Start the remaining three motors of the quad // Start the remaining three motors of the quad
...@@ -352,22 +367,22 @@ bool QGCFlightGearLink::connectSimulation() ...@@ -352,22 +367,22 @@ bool QGCFlightGearLink::connectSimulation()
processCall << "--prop:/engines/engine[2]/running=true"; processCall << "--prop:/engines/engine[2]/running=true";
processCall << "--prop:/engines/engine[3]/running=true"; processCall << "--prop:/engines/engine[3]/running=true";
} }
processCall << QString("--lat=%1").arg(UASManager::instance()->getHomeLatitude()); // processCall << QString("--lat=%1").arg(UASManager::instance()->getHomeLatitude());
processCall << QString("--lon=%1").arg(UASManager::instance()->getHomeLongitude()); // processCall << QString("--lon=%1").arg(UASManager::instance()->getHomeLongitude());
processCall << QString("--altitude=%1").arg(UASManager::instance()->getHomeAltitude()); // processCall << QString("--altitude=%1").arg(UASManager::instance()->getHomeAltitude());
// Add new argument with this: processCall << ""; // Add new argument with this: processCall << "";
processCall << QString("--aircraft=%2").arg(aircraft); processCall << QString("--aircraft=%2").arg(aircraft);
// process->start(processFgfs, processCall); process->start(processFgfs, processCall);
// emit flightGearConnected(connectState); emit flightGearConnected(connectState);
// if (connectState) { if (connectState) {
// emit flightGearConnected(); emit flightGearConnected();
// connectionStartTime = QGC::groundTimeUsecs()/1000; connectionStartTime = QGC::groundTimeUsecs()/1000;
// } }
// qDebug() << "STARTING SIM"; qDebug() << "STARTING SIM";
qDebug() << "STARTING: " << processFgfs << processCall; qDebug() << "STARTING: " << processFgfs << processCall;
......
src/uas/UAS.cc
View file @ 2899fc5f
...@@ -343,8 +343,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -343,8 +343,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
currentVoltage = state.voltage_battery/1000.0f; currentVoltage = state.voltage_battery/1000.0f;
lpVoltage = filterVoltage(currentVoltage); lpVoltage = filterVoltage(currentVoltage);
qDebug() << "BATT VOLTAGE:" << currentVoltage << "RAW" << state.voltage_battery;
if (startVoltage == 0) startVoltage = currentVoltage; if (startVoltage == 0) startVoltage = currentVoltage;
timeRemaining = calculateTimeRemaining(); timeRemaining = calculateTimeRemaining();
if (!batteryRemainingEstimateEnabled && chargeLevel != -1) 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