Skip to content

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

Minor compile fixes, added velocities to simulation

parent 034c8afe
Show whitespace changes
Inline Side-by-side
Showing with 120 additions and 112 deletions
files/flightgear/ardupilot.xml → files/flightgear/qgroundcontrol.xml
View file @ cf8fd0d9
......@@ -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>
......
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @ cf8fd0d9
......@@ -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),
......
src/comm/QGCFlightGearLink.cc
View file @ cf8fd0d9
......@@ -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
......
src/uas/UAS.cc
View file @ cf8fd0d9
......@@ -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)
}
// 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)
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)
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)
// 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)
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)
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)
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)
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)
{
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)
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)
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)
#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,6 +1047,29 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
void UAS::setHomePosition(double lat, double lon, double alt)
{
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;
......@@ -1112,14 +1077,13 @@ void UAS::setHomePosition(double lat, double lon, double alt)
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);
}
}
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()
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)
{
return QString("UNKNOWN");
}
// If nothing matches, return unknown
return QString("UNKNOWN");
}
void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
......
src/uas/UAS.h
View file @ cf8fd0d9
......@@ -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
......
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