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

Fixed flightgear HIL

QGC4.4
lm 14 years ago
parent
7dba86d0dd
commit
2899fc5fbd
4 changed files with 159 additions and 94 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. 135
      files/flightgear/qgroundcontrol.xml
  2. 13
      src/comm/MAVLinkSimulationMAV.cc
  3. 103
      src/comm/QGCFlightGearLink.cc
  4. 2
      src/uas/UAS.cc

135
files/flightgear/qgroundcontrol.xml
Unescape View File

@ -5,119 +5,151 @@ @@ -5,119 +5,151 @@
<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>
<node>/controls/flight/aileron</node>
@ -134,12 +166,19 @@ @@ -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>

13
src/comm/MAVLinkSimulationMAV.cc
Unescape View File

@ -207,6 +207,19 @@ void MAVLinkSimulationMAV::mainloop() @@ -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

103
src/comm/QGCFlightGearLink.cc
Unescape View File

@ -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
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() @@ -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() @@ -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() @@ -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() @@ -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;

2
src/uas/UAS.cc
Unescape View File

@ -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;
lpVoltage = filterVoltage(currentVoltage);
qDebug() << "BATT VOLTAGE:" << currentVoltage << "RAW" << state.voltage_battery;
if (startVoltage == 0) startVoltage = currentVoltage;
timeRemaining = calculateTimeRemaining();
if (!batteryRemainingEstimateEnabled && chargeLevel != -1)

Write Preview
Loading…
Cancel
Save