Commit 7cedf391 authored by dongfang's avatar dongfang

more PFD integration

parent ccd2ba2f
Messages interpreted in UAS.cc:
MAVLINK_MSG_ID_HEARTBEAT
MAVLINK_MSG_ID_SYS_STATUS
MAVLINK_MSG_ID_ATTITUDE
MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET
MAVLINK_MSG_ID_HIL_CONTROLS
MAVLINK_MSG_ID_VFR_HUD
MAVLINK_MSG_ID_LOCAL_POSITION_NED
MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE
MAVLINK_MSG_ID_GLOBAL_POSITION_INT
MAVLINK_MSG_ID_GPS_RAW_INT
MAVLINK_MSG_ID_GPS_STATUS
MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN
MAVLINK_MSG_ID_RC_CHANNELS_RAW
MAVLINK_MSG_ID_RC_CHANNELS_SCALED
MAVLINK_MSG_ID_PARAM_VALUE
MAVLINK_MSG_ID_COMMAND_ACK
MAVLINK_MSG_ID_MISSION_COUNT
MAVLINK_MSG_ID_MISSION_ITEM
MAVLINK_MSG_ID_MISSION_ACK
MAVLINK_MSG_ID_MISSION_REQUEST
MAVLINK_MSG_ID_MISSION_ITEM_REACHED (we want a new signal for this one)
currentWaypointChanged
MAVLINK_MSG_ID_MISSION_CURRENT
MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT
MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT
MAVLINK_MSG_ID_STATUSTEXT
MAVLINK_MSG_ID_SERVO_OUTPUT_RAW
MAVLINK_MSG_ID_RAW_IMU
MAVLINK_MSG_ID_SCALED_IMU
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT
MAVLINK_MSG_ID_RAW_PRESSURE
MAVLINK_MSG_ID_SCALED_PRESSURE
MAVLINK_MSG_ID_OPTICAL_FLOW
MAVLINK_MSG_ID_DEBUG_VECT
MAVLINK_MSG_ID_DEBUG
MAVLINK_MSG_ID_NAMED_VALUE_FLOAT
MAVLINK_MSG_ID_NAMED_VALUE_INT
MAVLINK_MSG_ID_MANUAL_CONTROL
MAVLINK_MSG_ID_HIGHRES_IMU
Signals of interest:
On UAS.cc:
// General
value_changed (custom value tracking)
// Mode:
armingChanged (ARMED etc)
statusChanged
modechanged
navModeChanged
// System Status
loadChanged
Do we already have these in some form? Can they be merged?
batteryChanged
voltageChanged
gpsLocalizationChanged
satelliteCountChanged
// Mission status
currentWaypointChanged (on WaypointManager)
new type for MAVLINK_MSG_ID_MISSION_ITEM_REACHED
distToWaypointChanged
// Link Status
heartbeatTimeout
dropRateChanged
// Attitude & speed
attitudeChanged
speedChanged
speedChanged (when message is MAVLINK_MSG_ID_VFR_HUD: x is airspeed, y is zero and z is VV)
// Position (if used)
globalPositionChanged
homePositionChanged (for pointer to home. Or we ignore signals and suck the data
off the UAS instance)
......@@ -679,3 +679,6 @@ win32-msvc2008|win32-msvc2010|win32-msvc2012 {
}
unix:!macx:!symbian: LIBS += -losg
OTHER_FILES += \
dongfang_notes.txt
......@@ -57,6 +57,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
warnLevelPercent(20.0f),
currentVoltage(12.6f),
lpVoltage(12.0f),
currentCurrent(0.4f),
batteryRemainingEstimateEnabled(true),
mode(-1),
status(-1),
......@@ -447,7 +448,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
QString audiomodeText = getAudioModeTextFor(static_cast<int>(state.base_mode));
if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT)
{
statechanged = true;
......@@ -539,7 +539,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
lpVoltage = filterVoltage(currentVoltage);
tickLowpassVoltage = tickLowpassVoltage*0.8f + 0.2f*currentVoltage;
// We don't want to tick above the threshold
if (tickLowpassVoltage > tickVoltage)
{
......@@ -567,20 +566,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
chargeLevel = state.battery_remaining;
}
emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining);
emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), timeRemaining);
emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time);
emit voltageChanged(message.sysid, currentVoltage);
// 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);
currentCurrent = ((double)state.current_battery)/100.0f;
emit valueChanged(uasId, name.arg("battery_current"), "A", currentCurrent, time);
}
// LOW BATTERY ALARM
if (lpVoltage < warnVoltage && (currentVoltage - 0.2f) < warnVoltage && (currentVoltage > 3.3))
{
// An audio alarm. Does not generate any signals.
startLowBattAlarm();
}
else
......@@ -682,7 +684,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
// dongfang: For APM, this altitude is the mix altitude[m]
emit altitudeChanged(uasId, hud.alt);
// dongfang: For APM, airspeed is airspeed or AHRS estimated airspeed
// dongfang: For APM, climb rate is barometric
// dongfang: The signal has no parameter for groundspeed.
// dongfang: The signal is emitted also from other places,
// such as GPS xyz speeds. This will cause a mix of signals
// from different sensors, which will probably not be so good.
float weAlsoWantGroundSpeedPlease = hud.groundspeed;
emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time);
}
break;
......
......@@ -354,6 +354,7 @@ protected: //COMMENTS FOR TEST UNIT
float warnLevelPercent; ///< Warning level, in percent
double currentVoltage; ///< Voltage currently measured
float lpVoltage; ///< Low-pass filtered voltage
double currentCurrent; ///< Battery current currently measured
bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life
float chargeLevel; ///< Charge level of battery, in percent
int timeRemaining; ///< Remaining time calculated based on previous and current
......
......@@ -470,7 +470,7 @@ signals:
* @param percent remaining capacity in percent
* @param seconds estimated remaining flight time in seconds
*/
void batteryChanged(UASInterface* uas, double voltage, double percent, int seconds);
void batteryChanged(UASInterface* uas, double voltage, double current, double percent, int seconds);
void statusChanged(UASInterface* uas, QString status);
void actuatorChanged(UASInterface*, int actId, double value);
void thrustChanged(UASInterface*, double thrust);
......
......@@ -261,9 +261,9 @@ void HUD::setActiveUAS(UASInterface* uas)
{
if (this->uas != NULL) {
// Disconnect any previously connected active MAV
disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
disconnect(this->uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*, double, double, double, quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int, double, double, double, quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
disconnect(this->uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int)));
disconnect(this->uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
disconnect(this->uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
......@@ -286,7 +286,7 @@ void HUD::setActiveUAS(UASInterface* uas)
// Setup communication
connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int)));
connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
......@@ -338,10 +338,11 @@ void HUD::updateAttitude(UASInterface* uas, int component, double roll, double p
}
}
void HUD::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
void HUD::updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds)
{
Q_UNUSED(uas);
Q_UNUSED(seconds);
Q_UNUSED(current);
fuelStatus = tr("BAT [%1% | %2V]").arg(percent, 2, 'f', 0, QChar('0')).arg(voltage, 4, 'f', 1, QChar('0'));
if (percent < 20.0f) {
fuelColor = warningColor;
......
......@@ -69,7 +69,7 @@ public slots:
/** @brief Attitude from one specific component / redundant autopilot */
void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
// void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
void updateBattery(UASInterface*, double, double, int);
void updateBattery(UASInterface*, double, double, double, int);
void receiveHeartbeat(UASInterface*);
void updateThrust(UASInterface*, double);
void updateLocalPosition(UASInterface*,double,double,double,quint64);
......
This diff is collapsed.
......@@ -5,7 +5,6 @@
#include <QPen>
#include "UASInterface.h"
#define SEPARATE_LAYOUT
#define LINEWIDTH 0.007
// all in units of display height
......@@ -26,7 +25,7 @@
//#define USE_TAPE_COMPASS
//#define USE_DISK_COMPASS
#define USE_DISK2_COMPASS
//#define USE_DISK2_COMPASS
// We want no numbers on the scale, just principal winds or half-winds and ticks.
// With whole winds there are 45 deg per wind, with half-winds 22.5
......@@ -48,7 +47,7 @@
#define COMPASS_DISK2_ARROWTICK 45
#define COMPASS_DISK2_MAJORLINEWIDTH 0.006
#define COMPASS_DISK2_MINORLINEWIDTH 0.004
#define COMPASS_SCALE_TEXT_SIZE 0.03
#define COMPASS_SCALE_TEXT_SIZE 0.16
// The altitude difference between top and bottom of scale
#define ALTIMETER_LINEAR_SPAN 35
......@@ -65,15 +64,16 @@
// every 10 meters there is a number
#define ALTIMETER_PROJECTED_MAJOR_RESOLUTION 10
// min. and max. vertical velocity
//#define ALTIMETER_PROJECTED
#define TAPES_TEXT_SIZE 0.028
#define ALTIMETER_VVI_SPAN 10
#define ALTIMETER_VVI_LOGARITHMIC true
#define SHOW_ZERO_ON_SCALES true
#define SCALE_TEXT_SIZE 0.042
#define SMALL_TEXT_SIZE 0.035
#define SCALE_TEXT_SIZE 0.040
#define PANELS_TEXT_SIZE 0.030
#define UNKNOWN_BATTERY -1
......@@ -90,7 +90,7 @@ public slots:
/** @brief Attitude from one specific component / redundant autopilot */
void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
// void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
void updateBattery(UASInterface*, double, double, int);
void updateBattery(UASInterface*, double, double, double, int);
void receiveHeartbeat(UASInterface*);
void updateThrust(UASInterface*, double);
void updateLocalPosition(UASInterface*,double,double,double,quint64);
......@@ -102,6 +102,17 @@ public slots:
void selectWaypoint(int uasId, int id);
protected:
enum Layout {
FEATUREPANELS_IN_CORNERS, // For a wide and low container.
FEATUREPANELS_AT_BOTTOM, // For higher container.
COMPASS_SEPARATED // For a very high container. Feature panels are at bottom.
};
enum Style {
OPAGUE_TAPES, // Hzon not visible through tapes nor through feature panels. Frames with margins between.
TRANSLUCENT_TAPES // Hzon visible through tapes and (frameless) feature panels.
};
void paintEvent(QPaintEvent *event);
void resizeEvent(QResizeEvent *e);
......@@ -119,6 +130,7 @@ protected:
protected:
// dongfang: What is that?
// dongfang: OK it's for UI interaction. Presently, there is none.
void createActions();
public slots:
......@@ -126,7 +138,6 @@ public slots:
virtual void setActiveUAS(UASInterface* uas);
protected slots:
void paintOnTimer();
signals:
void visibilityChanged(bool visible);
......@@ -145,14 +156,11 @@ private:
void drawPitchScale(QPainter& painter, QRectF area, bool drawNumbersLeft, bool drawNumbersRight);
void drawRollScale(QPainter& painter, QRectF area, bool drawTicks, bool drawNumbers);
void drawAIAttitudeScales(QPainter& painter, QRectF area);
#if defined(USE_DISK_COMPASS) || defined(USE_DISK2_COMPASS)
void drawCompassDisk(QPainter& painter, QRectF area);
#else
void drawCompassTape(QPainter& painter, QRectF area, float yaw);
#endif
void drawAltimeter(QPainter& painter, QRectF area, float altitude, float maxAltitude, float vv);
void fillInstrumentBackground(QPainter& painter, QRectF edge);
void fillInstrumentOpagueBackground(QPainter& painter, QRectF edge);
void drawInstrumentBackground(QPainter& painter, QRectF edge);
void drawLinkStatsPanel(QPainter& painter, QRectF area);
......@@ -160,10 +168,7 @@ private:
void drawMissionStatsPanel(QPainter& painter, QRectF area);
void drawSensorsStatsPanel(QPainter& painter, QRectF area);
void makeDummyData();
void doPaint();
void paintAllInOne();
void paintSeparate();
float roll;
float pitch;
......@@ -180,12 +185,15 @@ private:
float groundSpeed;
float airSpeed;
float verticalVelocity;
QString mode;
QString state;
float load;
Layout layout;
Style style;
QPen whitePen;
QPen redPen;
QPen amberPen;
......@@ -194,7 +202,8 @@ private:
QPen instrumentEdgePen;
QBrush instrumentBackground;
QBrush HUDInstrumentBackground;
QBrush instrumentOpagueBackground;
//QBrush HUDInstrumentBackground;
QFont font;
......@@ -204,6 +213,7 @@ private:
//QString energyStatus; ///< Current fuel level / battery voltage
double batteryVoltage;
double batteryCurrent;
double batteryCharge;
static const int tickValues[];
......
......@@ -278,7 +278,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
disconnect(mav, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
disconnect(mav, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
disconnect(mav, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
disconnect(mav, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBatteryRemaining(UASInterface*,double,double,int)));
disconnect(mav, SIGNAL(batteryChanged(UASInterface*, double, double, double,int)), this, SLOT(updateBatteryRemaining(UASInterface*, double, double, double, int)));
disconnect(mav, SIGNAL(armingChanged(bool)), this, SLOT(updateArmingState(bool)));
disconnect(mav, SIGNAL(heartbeatTimeout(bool, unsigned int)), this, SLOT(heartbeatTimeout(bool,unsigned int)));
if (mav->getWaypointManager())
......@@ -295,7 +295,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
connect(active, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
connect(active, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
connect(active, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
connect(active, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBatteryRemaining(UASInterface*,double,double,int)));
connect(active, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBatteryRemaining(UASInterface*, double, double, double, int)));
connect(active, SIGNAL(armingChanged(bool)), this, SLOT(updateArmingState(bool)));
connect(active, SIGNAL(heartbeatTimeout(bool, unsigned int)), this, SLOT(heartbeatTimeout(bool,unsigned int)));
if (active->getWaypointManager())
......@@ -379,7 +379,7 @@ void QGCToolBar::updateCurrentWaypoint(quint16 id)
wpId = id;
}
void QGCToolBar::updateBatteryRemaining(UASInterface* uas, double voltage, double percent, int seconds)
void QGCToolBar::updateBatteryRemaining(UASInterface* uas, double voltage, double current, double percent, int seconds)
{
Q_UNUSED(uas);
Q_UNUSED(seconds);
......
......@@ -63,7 +63,7 @@ public slots:
/** @brief Received system text message */
void receiveTextMessage(int uasid, int componentid, int severity, QString text);
/** @brief Update battery charge state */
void updateBatteryRemaining(UASInterface* uas, double voltage, double percent, int seconds);
void updateBatteryRemaining(UASInterface* uas, double voltage, double current, double percent, int seconds);
/** @brief Update current waypoint */
void updateCurrentWaypoint(quint16 id);
/** @brief Update distance to current waypoint */
......
......@@ -97,7 +97,7 @@ void UASInfoWidget::hideEvent(QHideEvent* event)
void UASInfoWidget::addUAS(UASInterface* uas)
{
if (uas != NULL) {
connect(uas, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBattery(UASInterface*,double,double,int)));
connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int)));
connect(uas, SIGNAL(dropRateChanged(int,float)), this, SLOT(updateReceiveLoss(int,float)));
connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double)));
connect(uas, SIGNAL(errCountChanged(int,QString,QString,int)), this, SLOT(updateErrorCount(int,QString,QString,int)));
......@@ -113,8 +113,9 @@ void UASInfoWidget::setActiveUAS(UASInterface* uas)
activeUAS = uas;
}
void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds)
{
Q_UNUSED(current)
setVoltage(uas, voltage);
setChargeLevel(uas, percent);
setTimeRemaining(uas, seconds);
......
......@@ -55,7 +55,7 @@ public slots:
void setActiveUAS(UASInterface* uas);
void updateBattery(UASInterface* uas, double voltage, double percent, int seconds);
void updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds);
void updateCPULoad(UASInterface* uas, double load);
/**
* @brief Set the loss rate of packets received by the MAV.
......
......@@ -86,7 +86,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
// Setup communication
//connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(receiveValue(int,QString,double,quint64)));
connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int)));
connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double)));
connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
......@@ -462,9 +462,10 @@ void UASView::updateThrust(UASInterface* uas, double thrust)
}
}
void UASView::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
void UASView::updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds)
{
Q_UNUSED(voltage);
Q_UNUSED(current);
if (this->uas == uas)
{
timeRemaining = seconds;
......
......@@ -55,7 +55,7 @@ public slots:
void updateName(const QString& name);
void receiveHeartbeat(UASInterface* uas);
void updateThrust(UASInterface* uas, double thrust);
void updateBattery(UASInterface* uas, double voltage, double percent, int seconds);
void updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds);
void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
void updateGlobalPosition(UASInterface*, double lon, double lat, double alt, quint64 usec);
void updateSpeed(UASInterface*, double x, double y, double z, quint64 usec);
......
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