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

Minor compile fixes, added velocities to simulation

QGC4.4
lm 14 years ago
parent
034c8afe37
commit
cf8fd0d9c8
6 changed files with 224 additions and 216 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. 44
      files/flightgear/qgroundcontrol.xml
  2. 5
      src/comm/MAVLinkSimulationWaypointPlanner.cc
  3. 5
      src/comm/QGCFlightGearLink.cc
  4. 176
      src/uas/UAS.cc
  5. 2
      src/uas/UAS.h

44
files/flightgear/ardupilot.xml → files/flightgear/qgroundcontrol.xml
Unescape View File

@ -52,6 +52,50 @@ @@ -52,6 +52,50 @@
</chunk>
<!-- Velocities -->
<chunk>
<name>v_n</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-north-fps</node>
</chunk>
<chunk>
<name>v_e</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-east-fps</node>
</chunk>
<chunk>
<name>v_d</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-down-fps</node>
</chunk>
<chunk>
<name>rate_roll</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/roll-rate-degps</node>
</chunk>
<chunk>
<name>rate_pitch</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/pitch-rate-degps</node>
</chunk>
<chunk>
<name>rate_yaw</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/yaw-rate-degps</node>
</chunk>
<chunk>
<name>airspeed-kt</name>
<type>float</type>

5
src/comm/MAVLinkSimulationWaypointPlanner.cc
Unescape View File

@ -449,13 +449,8 @@ MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimula @@ -449,13 +449,8 @@ MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimula
protocol_timeout(1000),
timestamp_last_send_setpoint(0),
systemid(sysid),
<<<<<<< HEAD
compid(MAV_COMP_ID_MISSIONPLANNER),
setpointDelay(10),
=======
compid(MAV_COMP_ID_WAYPOINTPLANNER),
setpointDelay(1000),
>>>>>>> master
yawTolerance(0.4f),
verbose(true),
debug(false),

5
src/comm/QGCFlightGearLink.cc
Unescape View File

@ -217,6 +217,11 @@ void QGCFlightGearLink::readBytes() @@ -217,6 +217,11 @@ void QGCFlightGearLink::readBytes()
roll = values.at(3).toDouble();
pitch = values.at(4).toDouble();
yaw = values.at(5).toDouble();
vx = values.at(6).toDouble();
vy = values.at(6).toDouble();
vz = values.at(6).toDouble();
// FIXME Accelerations missing

176
src/uas/UAS.cc
Unescape View File

@ -76,12 +76,8 @@ airframe(0), @@ -76,12 +76,8 @@ airframe(0),
attitudeKnown(false),
paramManager(NULL),
attitudeStamped(false),
<<<<<<< HEAD
lastAttitude(0),
simulation(new QGCFlightGearLink(this))
=======
lastAttitude(0)
>>>>>>> master
{
color = UASInterface::getNextColor();
setBattery(LIPOLY, 3);
@ -343,57 +339,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -343,57 +339,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
// COMMUNICATIONS DROP RATE
<<<<<<< HEAD
// FIXME
emit dropRateChanged(this->getUASID(), state.drop_rate_comm/10000.0f);
=======
emit dropRateChanged(this->getUASID(), state.packet_drop/1000.0f);
//add for development
//emit remoteControlRSSIChanged(state.packet_drop/1000.0f);
//float en = state.packet_drop/1000.0f;
//emit remoteControlChannelRawChanged(0, en);//MAVLINK_MSG_ID_RC_CHANNELS_RAW
//emit remoteControlChannelScaledChanged(0, en/100.0f);//MAVLINK_MSG_ID_RC_CHANNELS_SCALED
//qDebug() << __FILE__ << __LINE__ << "RCV LOSS: " << state.packet_drop;
// AUDIO
if (modechanged && statechanged)
{
// Output both messages
audiostring += modeAudio + " and " + stateAudio;
}
else
{
// Output the one message
audiostring += modeAudio + stateAudio;
}
if ((int)state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY)
{
GAudioOutput::instance()->startEmergency();
}
else if (modechanged || statechanged)
{
GAudioOutput::instance()->stopEmergency();
GAudioOutput::instance()->say(audiostring);
}
if (state.status == MAV_STATE_POWEROFF)
{
emit systemRemoved(this);
emit systemRemoved();
}
>>>>>>> master
}
break;
case MAVLINK_MSG_ID_RAW_IMU:
{
mavlink_raw_imu_t raw;
mavlink_msg_raw_imu_decode(&message, &raw);
quint64 time = getUnixTime(raw.time_usec);
//mavlink_raw_imu_t raw;
//mavlink_msg_raw_imu_decode(&message, &raw);
//quint64 time = getUnixTime(raw.time_usec);
// FIXME REMOVE LATER emit valueChanged(uasId, "accel x", "raw", static_cast<double>(raw.xacc), time);
// FIXME REMOVE LATER emit valueChanged(uasId, "accel y", "raw", static_cast<double>(raw.yacc), time);
@ -408,9 +362,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -408,9 +362,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break;
case MAVLINK_MSG_ID_SCALED_IMU:
{
mavlink_scaled_imu_t scaled;
mavlink_msg_scaled_imu_decode(&message, &scaled);
quint64 time = getUnixTime(scaled.time_boot_ms);
//mavlink_scaled_imu_t scaled;
//mavlink_msg_scaled_imu_decode(&message, &scaled);
//quint64 time = getUnixTime(scaled.time_boot_ms);
// FIXME REMOVE LATER emit valueChanged(uasId, "accel x", "g", scaled.xacc/1000.0f, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "accel y", "g", scaled.yacc/1000.0f, time);
@ -495,9 +449,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -495,9 +449,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break;
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
mavlink_nav_controller_output_t nav;
mavlink_msg_nav_controller_output_decode(&message, &nav);
quint64 time = getUnixTime();
//mavlink_nav_controller_output_t nav;
//mavlink_msg_nav_controller_output_decode(&message, &nav);
//quint64 time = getUnixTime();
// Update UI
// FIXME REMOVE LATER emit valueChanged(uasId, "nav roll", "deg", nav.nav_roll, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "nav pitch", "deg", nav.nav_pitch, time);
@ -556,7 +510,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -556,7 +510,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// FIXME REMOVE LATER emit valueChanged(uasId, "latitude", "deg", latitude, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "longitude", "deg", longitude, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", altitude, time);
double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ);
// double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ);
// FIXME REMOVE LATER emit valueChanged(uasId, "gps speed", "m/s", totalSpeed, time);
emit globalPositionChanged(this, latitude, longitude, altitude, time);
emit speedChanged(this, speedX, speedY, speedZ, time);
@ -635,9 +589,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -635,9 +589,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break;
case MAVLINK_MSG_ID_RAW_PRESSURE:
{
mavlink_raw_pressure_t pressure;
mavlink_msg_raw_pressure_decode(&message, &pressure);
quint64 time = this->getUnixTime(pressure.time_usec);
//mavlink_raw_pressure_t pressure;
//mavlink_msg_raw_pressure_decode(&message, &pressure);
//quint64 time = this->getUnixTime(pressure.time_usec);
// FIXME REMOVE LATER emit valueChanged(uasId, "abs pressure", "raw", pressure.press_abs, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "diff pressure 1", "raw", pressure.press_diff1, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "diff pressure 2", "raw", pressure.press_diff2, time);
@ -647,9 +601,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -647,9 +601,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
mavlink_scaled_pressure_t pressure;
mavlink_msg_scaled_pressure_decode(&message, &pressure);
quint64 time = this->getUnixTime(pressure.time_boot_ms);
//mavlink_scaled_pressure_t pressure;
//mavlink_msg_scaled_pressure_decode(&message, &pressure);
//quint64 time = this->getUnixTime(pressure.time_boot_ms);
// FIXME REMOVE LATER emit valueChanged(uasId, "abs pressure", "hPa", pressure.press_abs, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "diff pressure", "hPa", pressure.press_diff, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "temperature", "C", pressure.temperature/100.0, time);
@ -669,7 +623,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -669,7 +623,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit remoteControlChannelRawChanged(5, channels.chan6_raw);
emit remoteControlChannelRawChanged(6, channels.chan7_raw);
emit remoteControlChannelRawChanged(7, channels.chan8_raw);
quint64 time = getUnixTime();
// quint64 time = getUnixTime();
// FIXME REMOVE LATER emit valueChanged(uasId, "rc in #1", "us", channels.chan1_raw, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "rc in #2", "us", channels.chan2_raw, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "rc in #3", "us", channels.chan3_raw, time);
@ -706,14 +660,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -706,14 +660,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
val.param_float = value.param_value;
val.type = value.param_type;
// Convert to machine order if necessary
//#if MAVLINK_NEED_BYTE_SWAP
// buffer[bindex] = (b>>24)&0xff;
// buffer[bindex+1] = (b>>16)&0xff;
// buffer[bindex+2] = (b>>8)&0xff;
// buffer[bindex+3] = (b & 0xff);
//#else
// Insert component if necessary
if (!parameters.contains(component))
{
@ -783,11 +729,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -783,11 +729,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_roll_pitch_yaw_thrust_setpoint_t out;
mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out);
<<<<<<< HEAD
quint64 time = getUnixTimeFromMs(out.time_boot_ms);
=======
quint64 time = getUnixTime(out.time_us);
>>>>>>> master
emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "att control roll", "rad", out.roll, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "att control pitch", "rad", out.pitch, time);
@ -880,9 +822,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -880,9 +822,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break;
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
mavlink_servo_output_raw_t servos;
mavlink_msg_servo_output_raw_decode(&message, &servos);
quint64 time = getUnixTime();
//mavlink_servo_output_raw_t servos;
//mavlink_msg_servo_output_raw_decode(&message, &servos);
//quint64 time = getUnixTime();
// FIXME REMOVE LATER emit valueChanged(uasId, "servo #1", "us", servos.servo1_raw, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "servo #2", "us", servos.servo2_raw, time);
// FIXME REMOVE LATER emit valueChanged(uasId, "servo #3", "us", servos.servo3_raw, time);
@ -896,9 +838,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -896,9 +838,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
case MAVLINK_MSG_ID_OPTICAL_FLOW:
{
mavlink_optical_flow_t flow;
mavlink_msg_optical_flow_decode(&message, &flow);
quint64 time = getUnixTime(flow.time_usec);
//mavlink_optical_flow_t flow;
//mavlink_msg_optical_flow_decode(&message, &flow);
//quint64 time = getUnixTime(flow.time_usec);
// FIXME REMOVE LATER emit valueChanged(uasId, QString("opt_flow_%1.x").arg(flow.sensor_id), "Pixel", flow.flow_x, time);
// FIXME REMOVE LATER emit valueChanged(uasId, QString("opt_flow_%1.y").arg(flow.sensor_id), "Pixel", flow.flow_y, time);
@ -972,10 +914,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -972,10 +914,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
#endif
case MAVLINK_MSG_ID_DEBUG_VECT:
{
mavlink_debug_vect_t vect;
mavlink_msg_debug_vect_decode(&message, &vect);
QString str((const char*)vect.name);
quint64 time = getUnixTime(vect.time_usec);
//mavlink_debug_vect_t vect;
//mavlink_msg_debug_vect_decode(&message, &vect);
//QString str((const char*)vect.name);
//quint64 time = getUnixTime(vect.time_usec);
// FIXME REMOVE LATER emit valueChanged(uasId, str+".x", "raw", vect.x, time);
// FIXME REMOVE LATER emit valueChanged(uasId, str+".y", "raw", vect.y, time);
// FIXME REMOVE LATER emit valueChanged(uasId, str+".z", "raw", vect.z, time);
@ -1105,21 +1047,43 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -1105,21 +1047,43 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
void UAS::setHomePosition(double lat, double lon, double alt)
{
// Send new home position to UAS
mavlink_set_gps_global_origin_t home;
home.target_system = uasId;
home.latitude = lat*1E7;
home.longitude = lon*1E7;
home.altitude = alt*1000;
qDebug() << "lat:" << home.latitude << " lon:" << home.longitude;
mavlink_message_t msg;
mavlink_msg_set_gps_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home);
sendMessage(msg);
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Warning);
msgBox.setText("Setting new World Coordinate Frame Origin");
msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location");
msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
msgBox.setDefaultButton(QMessageBox::Cancel);
int ret = msgBox.exec();
// Close the message box shortly after the click to prevent accidental clicks
QTimer::singleShot(5000, &msgBox, SLOT(reject()));
if (ret == QMessageBox::Yes)
{
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 0, 0, 0, 0, lat, lon, alt);
// Send message twice to increase chance that it reaches its goal
sendMessage(msg);
// Wait 15 ms
QGC::SLEEP::usleep(15000);
// Send again
sendMessage(msg);
// Send new home position to UAS
mavlink_set_gps_global_origin_t home;
home.target_system = uasId;
home.latitude = lat*1E7;
home.longitude = lon*1E7;
home.altitude = alt*1000;
qDebug() << "lat:" << home.latitude << " lon:" << home.longitude;
mavlink_msg_set_gps_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home);
sendMessage(msg);
}
}
void UAS::setLocalOriginAtCurrentGPSPosition()
{
bool result = false;
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Warning);
msgBox.setText("Setting new World Coordinate Frame Origin");
@ -1134,16 +1098,14 @@ void UAS::setLocalOriginAtCurrentGPSPosition() @@ -1134,16 +1098,14 @@ void UAS::setLocalOriginAtCurrentGPSPosition()
if (ret == QMessageBox::Yes)
{
// FIXME MAVLINKV10PORTINGNEEDED
// mavlink_message_t msg;
// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN);
// // Send message twice to increase chance that it reaches its goal
// sendMessage(msg);
// // Wait 5 ms
// MG::SLEEP::usleep(5000);
// // Send again
// sendMessage(msg);
result = true;
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 1, 0, 0, 0, 0, 0, 0);
// Send message twice to increase chance that it reaches its goal
sendMessage(msg);
// Wait 15 ms
QGC::SLEEP::usleep(15000);
// Send again
sendMessage(msg);
}
}
@ -1457,6 +1419,8 @@ QString UAS::getNavModeText(int mode) @@ -1457,6 +1419,8 @@ QString UAS::getNavModeText(int mode)
{
return QString("UNKNOWN");
}
// If nothing matches, return unknown
return QString("UNKNOWN");
}
void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)

2
src/uas/UAS.h
Unescape View File

@ -159,7 +159,7 @@ protected: //COMMENTS FOR TEST UNIT @@ -159,7 +159,7 @@ protected: //COMMENTS FOR TEST UNIT
int timeRemaining; ///< Remaining time calculated based on previous and current
int mode; ///< The current mode of the MAV
int status; ///< The current status of the MAV
int navMode; ///< The current navigation mode of the MAV
uint32_t navMode; ///< The current navigation mode of the MAV
quint64 onboardTimeOffset;
bool controlRollManual; ///< status flag, true if roll is controlled manually

Write Preview
Loading…
Cancel
Save