Commit cfd9c792 authored by LM's avatar LM
Browse files

Resolved drop count issue

parents f01ad4a2 deddf85c
......@@ -351,33 +351,36 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
// Update last message sequence ID
uint8_t expectedIndex;
if (lastIndex[message.sysid][message.compid] == -1)
if (lastIndex[message.sysid][message.compid] == -1)
{
lastIndex[message.sysid][message.compid] = message.seq;
expectedIndex = message.seq;
}
}
else
{
// NOTE: Using uint8_t here auto-wraps the number around to 0.
// NOTE: Using uint8_t here auto-wraps the number around to 0.
expectedIndex = lastIndex[message.sysid][message.compid] + 1;
}
// Make some noise if a message was skipped
}
// Make some noise if a message was skipped
//qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq;
if (message.seq != expectedIndex)
{
// Determine how many messages were skipped accounting for 0-wraparound
int16_t lostMessages = message.seq - expectedIndex;
if (lostMessages < 0)
{
lostMessages += 256;
}
// Console generates excessive load at high loss rates, needs better GUI visualization
//qDebug() << QString("Lost %1 messages for comp %4: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq).arg(message.compid);
totalLossCounter += lostMessages;
currLossCounter += lostMessages;
}
if (message.seq != expectedIndex)
{
// Determine how many messages were skipped accounting for 0-wraparound
int16_t lostMessages = message.seq - expectedIndex;
if (lostMessages < 0)
{
// Usually, this happens in the case of an out-of order packet
lostMessages = 0;
}
else
{
// Console generates excessive load at high loss rates, needs better GUI visualization
//qDebug() << QString("Lost %1 messages for comp %4: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq).arg(message.compid);
}
totalLossCounter += lostMessages;
currLossCounter += lostMessages;
}
// Update the last sequence ID
lastIndex[message.sysid][message.compid] = message.seq;
......
......@@ -282,6 +282,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit heartbeat(this);
mavlink_heartbeat_t state;
mavlink_msg_heartbeat_decode(&message, &state);
// Send the base_mode and system_status values to the plotter. This uses the ground time
// so the Ground Time checkbox must be ticked for these values to display
quint64 time = getUnixTime();
QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid);
emit valueChanged(uasId, name.arg("base_mode"), "none", state.base_mode, time);
emit valueChanged(uasId, name.arg("system_status"), "none", state.system_status, time);
// Set new type if it has changed
if (this->type != state.type)
{
......@@ -384,20 +392,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
GAudioOutput::instance()->stopEmergency();
GAudioOutput::instance()->say(audiostring.toLower());
}
// if (state.system_status == MAV_STATE_POWEROFF)
// {
// emit systemRemoved(this);
// emit systemRemoved();
// }
}
break;
// case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
// case MAVLINK_MSG_ID_NAMED_VALUE_INT:
// // Receive named value message
// receiveMessageNamedValue(message);
// break;
case MAVLINK_MSG_ID_SYS_STATUS:
{
if (multiComponentSourceDetected && wrongComponent)
......@@ -407,8 +404,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_sys_status_t state;
mavlink_msg_sys_status_decode(&message, &state);
// Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present.
quint64 time = getUnixTime();
QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid);
emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time);
emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time);
emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time);
emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time);
emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time);
emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time);
emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);
// Process CPU load.
emit loadChanged(this,state.load/10.0f);
emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);
// Battery charge/time remaining/voltage calculations
currentVoltage = state.voltage_battery/1000.0f;
lpVoltage = filterVoltage(currentVoltage);
......@@ -418,9 +429,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
chargeLevel = state.battery_remaining;
}
//qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining);
emit voltageChanged(message.sysid, state.voltage_battery/1000);
emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time);
emit voltageChanged(message.sysid, currentVoltage);
emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time);
// And if the battery current draw is measured, log that also.
if (state.current_battery != -1)
{
emit valueChanged(uasId, name.arg("battery_current"), "A", ((double)state.current_battery) / 100.0f, time);
}
// LOW BATTERY ALARM
if (lpVoltage < warnVoltage)
......@@ -432,19 +450,25 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
stopLowBattAlarm();
}
// Trigger drop rate updates as needed. Here we convert the incoming
// drop_rate_comm value from 1/100 of a percent in a uint16 to a true
// percentage as a float. We also cap the incoming value at 100% as defined
// by the MAVLink specifications.
if (state.drop_rate_comm > 10000)
{
emit dropRateChanged(this->getUASID(), 100.0f);
}
else
{
emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f);
}
}
// control_sensors_enabled:
// relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11));
emit positionYawControlEnabled(state.onboard_control_sensors_enabled & (1 << 12));
emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13));
emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14));
// Trigger drop rate updates as needed. Here we convert the incoming
// drop_rate_comm value from 1/100 of a percent in a uint16 to a true
// percentage as a float. We also cap the incoming value at 100% as defined
// by the MAVLink specifications.
if (state.drop_rate_comm > 10000)
{
state.drop_rate_comm = 10000;
}
emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f);
emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
}
break;
case MAVLINK_MSG_ID_ATTITUDE:
{
......@@ -563,6 +587,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
speedZ = pos.vz/100.0;
emit globalPositionChanged(this, latitude, longitude, altitude, time);
emit speedChanged(this, speedX, speedY, speedZ, time);
// Set internal state
if (!positionLock)
{
......@@ -584,6 +609,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// only accept values in a realistic range
// quint64 time = getUnixTime(pos.time_usec);
quint64 time = getUnixTime(pos.time_usec);
emit gpsLocalizationChanged(this, pos.fix_type);
// TODO: track localization state not only for gps but also for other loc. sources
int loc_type = pos.fix_type;
if (loc_type == 1)
{
loc_type = 0;
}
emit localizationChanged(this, loc_type);
if (pos.fix_type > 2)
{
......
......@@ -408,23 +408,28 @@ signals:
void deactivated();
/** @brief The robot is manually controlled **/
void manualControl();
/** @brief A value of the robot has changed.
*
* Typically this is used to send lowlevel information like the battery voltage to the plotting facilities of
* the groundstation
* the groundstation. The data here should be converted to human-readable values before being passed, so ideally
* SI units.
*
* @param uasId ID of this system
* @param name name of the value, e.g. "battery voltage"
* @param unit The units this variable is in as an abbreviation. For system-dependent (such as raw ADC values) use "raw", for bitfields use "bits", for true/false or on/off use "bool", for unitless values use "-".
* @param value the value that changed
* @param msec the timestamp of the message, in milliseconds
*/
//void valueChanged(const int uasId, const QString& name, const double value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec);
// void valueChanged(const int uasId, const QString& name, const double value, const quint64 msec);
// //void valueChanged(UASInterface* uas, QString name, double value, quint64 msec);
void voltageChanged(int uasId, double voltage);
void waypointUpdated(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool active);
......
......@@ -478,16 +478,13 @@ void HDDisplay::renderOverlay()
*/
void HDDisplay::setActiveUAS(UASInterface* uas)
{
// Disconnect any previously connected active UAS
if (this->uas != NULL) {
// Disconnect any previously connected active MAV
disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64)));
disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64)));
removeSource(this->uas);
}
// Now connect the new UAS
// Setup communication
connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64)));
addSource(uas);
this->uas = uas;
}
......@@ -842,28 +839,75 @@ float HDDisplay::refLineWidthToPen(float line)
return line * 2.50f;
}
// Connect a generic source
void HDDisplay::addSource(QObject* obj)
{
//genericSources.append(obj);
// FIXME XXX HACK
// if (plots.size() > 0)
// {
// Connect generic source
connect(obj, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), this, SLOT(updateValue(int,QString,QString,unsigned int,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), this, SLOT(updateValue(int,QString,QString,qint8,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), this, SLOT(updateValue(int,QString,QString,quint8,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), this, SLOT(updateValue(int,QString,QString,qint16,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), this, SLOT(updateValue(int,QString,QString,quint16,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), this, SLOT(updateValue(int,QString,QString,qint32,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), this, SLOT(updateValue(int,QString,QString,quint32,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), this, SLOT(updateValue(int,QString,QString,quint64,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), this, SLOT(updateValue(int,QString,QString,qint64,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64)));
// }
}
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec)
// Disconnect a generic source
void HDDisplay::removeSource(QObject* obj)
{
//genericSources.append(obj);
// FIXME XXX HACK
// if (plots.size() > 0)
// {
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), this, SLOT(updateValue(int,QString,QString,qint8,quint64)));
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), this, SLOT(updateValue(int,QString,QString,quint8,quint64)));
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), this, SLOT(updateValue(int,QString,QString,qint16,quint64)));
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), this, SLOT(updateValue(int,QString,QString,quint16,quint64)));
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), this, SLOT(updateValue(int,QString,QString,qint32,quint64)));
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), this, SLOT(updateValue(int,QString,QString,quint32,quint64)));
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), this, SLOT(updateValue(int,QString,QString,quint64,quint64)));
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), this, SLOT(updateValue(int,QString,QString,qint64,quint64)));
disconnect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64)));
// }
}
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec)
{
if (!intValues.contains(name)) intValues.insert(name, true);
updateValue(uasId, name, unit, (double)value, msec);
}
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec)
{
if (!intValues.contains(name)) intValues.insert(name, true);
updateValue(uasId, name, unit, (double)value, msec);
}
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec)
{
if (!intValues.contains(name)) intValues.insert(name, true);
updateValue(uasId, name, unit, (double)value, msec);
}
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec)
{
if (!intValues.contains(name)) intValues.insert(name, true);
updateValue(uasId, name, unit, (double)value, msec);
}
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec)
{
if (!intValues.contains(name)) intValues.insert(name, true);
updateValue(uasId, name, unit, (double)value, msec);
}
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec)
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec)
{
if (!intValues.contains(name)) intValues.insert(name, true);
updateValue(uasId, name, unit, (double)value, msec);
......
......@@ -64,18 +64,31 @@ public:
~HDDisplay();
public slots:
/** @brief Update a HDD double value */
void updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec);
/** @brief Update a HDD integer value */
void updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec);
/** @brief Update a HDD integer value */
void updateValue(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec);
/** @brief Update a HDD integer value */
/** @brief Update the HDD with new int8 data */
void updateValue(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec);
/** @brief Update the HDD with new uint8 data */
void updateValue(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec);
/** @brief Update the HDD with new int16 data */
void updateValue(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec);
/** @brief Update the HDD with new uint16 data */
void updateValue(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec);
/** @brief Update the HDD with new int32 data */
void updateValue(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec);
/** @brief Update the HDD with new uint32 data */
void updateValue(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec);
/** @brief Update the HDD with new int64 data */
void updateValue(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec);
/** @brief Update a HDD integer value */
/** @brief Update the HDD with new uint64 data */
void updateValue(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec);
/** @brief Update the HDD with new double data */
void updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec);
virtual void setActiveUAS(UASInterface* uas);
/** @brief Connects a source to the updateValue() signals */
void addSource(QObject* obj);
/** @brief Disconnects a source to the updateValue() signals */
void removeSource(QObject* obj);
/** @brief Removes a plot item by the action data */
void removeItemByAction();
......
......@@ -474,25 +474,45 @@ void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, b
void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock)
{
Q_UNUSED(uas);
bool changed = positionLock != lock;
positionLock = lock;
if (changed)
{
update();
}
}
void HSIDisplay::updateAttitudeControllerEnabled(bool enabled)
{
bool changed = attControlEnabled != enabled;
attControlEnabled = enabled;
attControlKnown = true;
if (changed)
{
update();
}
}
void HSIDisplay::updatePositionXYControllerEnabled(bool enabled)
{
bool changed = xyControlEnabled != enabled;
xyControlEnabled = enabled;
xyControlKnown = true;
if (changed)
{
update();
}
}
void HSIDisplay::updatePositionZControllerEnabled(bool enabled)
{
bool changed = zControlEnabled != enabled;
zControlEnabled = enabled;
zControlKnown = true;
if (changed)
{
update();
}
}
void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance)
......@@ -1187,8 +1207,8 @@ void HSIDisplay::drawGPS(QPainter &painter)
i.next();
GPSSatellite* sat = i.value();
// Check if update is not older than 5 seconds, else delete satellite
if (sat->lastUpdate + 1000000 < currTime) {
// Check if update is not older than 10 seconds, else delete satellite
if (sat->lastUpdate + 10000000 < currTime) {
// Delete and go to next satellite
gpsSatellites.remove(i.key());
if (i.hasNext()) {
......
......@@ -12,12 +12,16 @@ public:
signals:
void textMessageReceived(int uasid, int componentid, int severity, const QString& text);
void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec);
void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec);
public slots:
/** @brief Receive one message from the protocol and decode it */
......
......@@ -1167,7 +1167,6 @@ void MainWindow::addLink(LinkInterface *link)
MAVLinkSimulationLink* sim = dynamic_cast<MAVLinkSimulationLink*>(link);
if (sim)
{
//connect(sim, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64)));
connect(ui.actionSimulate, SIGNAL(triggered(bool)), sim, SLOT(connectLink(bool)));
}
}
......
......@@ -24,7 +24,7 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par
mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO;
memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256);
memset(receivedMessages, 0, sizeof(mavlink_message_t)*256);
memset(receivedMessages, 0xFF, sizeof(mavlink_message_t)*256);
connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));
QStringList header;
......@@ -95,7 +95,7 @@ void QGCMAVLinkInspector::refreshView()
{
mavlink_message_t* msg = receivedMessages+i;
// Ignore NULL values
if (!msg) continue;
if (msg->msgid == 0xFF) continue;
// Update the tree view
QString messageName("%1 (%2 Hz, #%3)");
float msgHz = (1.0f-updateHzLowpass)*messagesHz.value(msg->msgid, 0) + updateHzLowpass*((float)messageCount.value(msg->msgid, 0))/((float)updateInterval/1000.0f);
......
......@@ -294,40 +294,37 @@ void LinechartWidget::createLayout()
connect(scalingLogButton, SIGNAL(clicked()), activePlot, SLOT(setLogarithmicScaling()));
}
void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 usec)
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint8 value, quint64 usec)
{
static const QString unit("-");
if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible()))
{
// Order matters here, first append to plot, then update curve list
activePlot->appendData(curve+unit, usec, value);
// Store data
QLabel* label = curveLabels->value(curve+unit, NULL);
// Make sure the curve will be created if it does not yet exist
if(!label)
{
addCurve(curve, unit);
}
}
appendData(uasId, curve, unit, static_cast<qint64>(value), usec);
}
// Log data
if (logging)
{
if (activePlot->isVisible(curve+unit))
{
if (usec == 0) usec = QGC::groundTimeMilliseconds();
if (logStartTime == 0) logStartTime = usec;
qint64 time = usec - logStartTime;
if (time < 0) time = 0;
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint8 value, quint64 usec)
{
appendData(uasId, curve, unit, static_cast<quint64>(value), usec);
}
logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value,'g',18) + "\n").toLatin1());
logFile->flush();
}
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint16 value, quint64 usec)
{
appendData(uasId, curve, unit, static_cast<qint64>(value), usec);
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint16 value, quint64 usec)
{
appendData(uasId, curve, unit, static_cast<quint64>(value), usec);
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec)
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint32 value, quint64 usec)
{
appendData(uasId, curve, unit, static_cast<qint64>(value), usec);
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint32 value, quint64 usec)
{
appendData(uasId, curve, unit, static_cast<quint64>(value), usec);
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint64 value, quint64 usec)
{
if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible()))
{
......@@ -338,9 +335,12 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString&
// Make sure the curve will be created if it does not yet exist
if(!label)
{
//qDebug() << "ADDING CURVE IN APPENDDATE DOUBLE";
intData.insert(curve+unit, 0);
addCurve(curve, unit);
}
// Add int data
intData.insert(curve+unit, value);
}
// Log data
......@@ -353,23 +353,13 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString&
qint64 time = usec - logStartTime;
if (time < 0) time = 0;
logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value,'g',18) + "\n").toLatin1());
logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1());
logFile->flush();
}
}
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec)
{
appendData(uasId, curve, unit, static_cast<qint64>(value), usec);
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, unsigned int value, quint64 usec)
{
appendData(uasId, curve, unit, static_cast<quint64>(value), usec);
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint64 value, quint64 usec)
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint64 value, quint64 usec)
{
if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible()))
{
......@@ -404,7 +394,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString&
}
}
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint64 value, quint64 usec)
void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec)
{
if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible()))
{
......@@ -415,12 +405,9 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString&
// Make sure the curve will be created if it does not yet exist
if(!label)
{
intData.insert(curve+unit, 0);
//qDebug() << "ADDING CURVE IN APPENDDATE DOUBLE";
addCurve(curve, unit);
}
// Add int data
intData.insert(curve+unit, value);
}
// Log data
......@@ -433,7 +420,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString&
qint64 time = usec - logStartTime;
if (time < 0) time = 0;
logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1());
logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value,'g',18) + "\n").toLatin1());
logFile->flush();
}
}
......
......@@ -77,18 +77,25 @@ public slots:
void recolor();
/** @brief Set short names for curves */
void setShortNames(bool enable);
/** @brief Append data without unit */
void appendData(int uasId, QString curve, double data, quint64 usec);
/** @brief Append data with unit */
void appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec);
/** @brief Append data as int with unit */
void appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec);
/** @brief Append data as unsigned int with unit */
void appendData(int uasId, const QString& curve, const QString& unit, unsigned int value, quint64 usec);
/** @brief Append data as int64 with unit */
/** @brief Append int8 data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, qint8 value, quint64 usec);
/** @brief Append uint8 data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, quint8 value, quint64 usec);
/** @brief Append int16 data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, qint16 value, quint64 usec);
/** @brief Append uint16 data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, quint16 value, quint64 usec);
/** @brief Append int32 data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, qint32 value, quint64 usec);
/** @brief Append uint32 data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, quint32 value, quint64 usec);
/** @brief Append int64 data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, qint64 value, quint64 usec);
/** @brief Append data as uint64 with unit */
/** @brief Append uint64 data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, quint64 value, quint64 usec);
/** @brief Append double data to the given curve. */
void appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec);
void takeButtonClick(bool checked);
void setPlotWindowPosition(int scrollBarValue);
void setPlotWindowPosition(quint64 position);
......
......@@ -93,12 +93,17 @@ void Linecharts::addSystem(UASInterface* uas)
LinechartWidget* widget = new LinechartWidget(uas->getUASID(), this);
addWidget(widget);
plots.insert(uas->getUASID(), widget);
// Values without unit
//connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), widget, SLOT(appendData(int,QString,double,quint64)));
// Values with unit as double
connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64)));
// Values with unit as integer
connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), widget, SLOT(appendData(int,QString,QString,int,quint64)));
// Connect valueChanged signals
connect(uas, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), widget, SLOT(appendData(int,QString,QString,quint8,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), widget, SLOT(appendData(int,QString,QString,qint8,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), widget, SLOT(appendData(int,QString,QString,quint16,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), widget, SLOT(appendData(int,QString,QString,qint16,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), widget, SLOT(appendData(int,QString,QString,quint32,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), widget, SLOT(appendData(int,QString,QString,qint32,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), widget, SLOT(appendData(int,QString,QString,quint64,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), widget, SLOT(appendData(int,QString,QString,qint64,quint64)));
connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64)));
connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString)));
// Set system active if this is the only system
......@@ -110,11 +115,15 @@ void Linecharts::addSystem(UASInterface* uas)
// Connect generic sources
for (int i = 0; i < genericSources.count(); ++i)
{
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,int,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,unsigned int,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint64,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint8,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint8,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint16,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint16,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint32,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint32,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint64,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64)));
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64)));
}
// Select system
selectSystem(uas->getUASID());
......@@ -130,10 +139,14 @@ void Linecharts::addSource(QObject* obj)
if (plots.size() > 0)
{
// Connect generic source
connect(obj, SIGNAL(valueChanged(int,QString,QString,int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,int,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,unsigned int,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint8,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint8,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint16,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint16,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint32,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint32,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint64,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64)));
}
}
}
\ No newline at end of file
......@@ -74,8 +74,8 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
if (isMouseMoving()) {
if (calcMovement()) {
us.requestRedraw();
us.requestContinuousUpdate(true);
_thrown = true;
us.requestContinuousUpdate(false);
_thrown = false;
}
} else {
flushMouseEventStack();
......
......@@ -161,7 +161,9 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
(float)qrand() / RAND_MAX,
(float)qrand() / RAND_MAX,
0.5);
systemData.trailNode()->addDrawable(createTrail(color));
systemData.trailNode()->addDrawable(createLink(uas->getColor()));
}
// update system position
......@@ -223,7 +225,11 @@ Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component,
void
Pixhawk3DWidget::showViewParamWindow(void)
{
if (!mViewParamWidget->isVisible())
if (mViewParamWidget->isVisible())
{
mViewParamWidget->hide();
}
else
{
mViewParamWidget->show();
}
......@@ -772,7 +778,10 @@ Pixhawk3DWidget::update(void)
// updateImagery(robotX, robotY, robotZ, utmZone);
}
updateHUD(mActiveUAS, frame);
if (mActiveUAS)
{
updateHUD(mActiveUAS, frame);
}
layout()->update();
}
......@@ -817,6 +826,7 @@ void
Pixhawk3DWidget::buildLayout(void)
{
QPushButton* viewParamWindowButton = new QPushButton(this);
viewParamWindowButton->setCheckable(true);
viewParamWindowButton->setText("View Parameters");
QHBoxLayout* layoutTop = new QHBoxLayout;
......@@ -1332,6 +1342,35 @@ Pixhawk3DWidget::createTrail(const osg::Vec4& color)
return geometry;
}
osg::ref_ptr<osg::Geometry>
Pixhawk3DWidget::createLink(const QColor& color)
{
osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
geometry->setUseDisplayList(false);
osg::ref_ptr<osg::Vec3dArray> vertices(new osg::Vec3dArray());
geometry->setVertexArray(vertices);
osg::ref_ptr<osg::DrawArrays> drawArrays(new osg::DrawArrays(osg::PrimitiveSet::LINES));
geometry->addPrimitiveSet(drawArrays);
osg::ref_ptr<osg::Vec4Array> colorArray(new osg::Vec4Array);
colorArray->push_back(osg::Vec4(color.redF(), color.greenF(), color.blueF(), 1.0f));
geometry->setColorArray(colorArray);
geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
osg::ref_ptr<osg::StateSet> stateset(new osg::StateSet);
osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
linewidth->setWidth(3.0f);
stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
stateset->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON);
stateset->setMode(GL_BLEND, osg::StateAttribute::ON);
geometry->setStateSet(stateset);
return geometry;
}
osg::ref_ptr<Imagery>
Pixhawk3DWidget::createImagery(void)
{
......@@ -1523,7 +1562,8 @@ Pixhawk3DWidget::updateHUD(UASInterface* uas, MAV_FRAME frame)
mStatusText->setText(oss.str());
bool darkBackground = true;
if (mImageryNode->getImageryType() == Imagery::GOOGLE_MAP)
if (frame == MAV_FRAME_GLOBAL &&
mImageryNode->getImageryType() == Imagery::GOOGLE_MAP)
{
darkBackground = false;
}
......@@ -1545,13 +1585,15 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ,
{
it.next();
osg::Geometry* geometry = trailNode->getDrawable(it.value())->asGeometry();
// update trail
osg::Geometry* geometry = trailNode->getDrawable(it.value() * 2)->asGeometry();
osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
const QVector<osg::Vec3d>& trail = trailMap.value(it.key());
vertices->reserve(trail.size());
for (int i = 0; i < trail.size(); ++i)
{
vertices->push_back(osg::Vec3d(trail[i].y() - robotY,
......@@ -1563,6 +1605,38 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ,
drawArrays->setFirst(0);
drawArrays->setCount(vertices->size());
geometry->dirtyBound();
// update link
geometry = trailNode->getDrawable(it.value() * 2 + 1)->asGeometry();
drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
vertices = new osg::Vec3Array;
if (!trail.empty())
{
QVector3D p(trail.back().x() - robotX,
trail.back().y() - robotY,
trail.back().z() - robotZ);
double length = p.length();
p.normalize();
for (double i = 0.1; i < length - 0.1; i += 0.3)
{
QVector3D v = p * i;
vertices->push_back(osg::Vec3d(v.y(), v.x(), -v.z()));
}
}
if (vertices->size() % 2 == 1)
{
vertices->pop_back();
}
geometry->setVertexArray(vertices);
drawArrays->setFirst(0);
drawArrays->setCount(vertices->size());
geometry->dirtyBound();
}
}
......
......@@ -123,6 +123,7 @@ private:
osg::ref_ptr<osg::Geode> createLocalGrid(void);
osg::ref_ptr<osg::Geode> createWorldGrid(void);
osg::ref_ptr<osg::Geometry> createTrail(const osg::Vec4& color);
osg::ref_ptr<osg::Geometry> createLink(const QColor& color);
osg::ref_ptr<Imagery> createImagery(void);
osg::ref_ptr<osg::Geode> createPointCloud(void);
osg::ref_ptr<osg::Node> createTarget(const QColor& color);
......
Supports Markdown
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