Commit 04ae69ba authored by DonLakeFlyer's avatar DonLakeFlyer

parent 6e280eeb
......@@ -695,6 +695,7 @@ HEADERS += \
src/Vehicle/MAVLinkLogManager.h \
src/Vehicle/MultiVehicleManager.h \
src/Vehicle/StateMachine.h \
src/Vehicle/SysStatusSensorInfo.h \
src/Vehicle/TerrainFactGroup.h \
src/Vehicle/TerrainProtocolHandler.h \
src/Vehicle/TrajectoryPoints.h \
......@@ -914,6 +915,7 @@ SOURCES += \
src/Vehicle/MAVLinkLogManager.cc \
src/Vehicle/MultiVehicleManager.cc \
src/Vehicle/StateMachine.cc \
src/Vehicle/SysStatusSensorInfo.cc \
src/Vehicle/TerrainFactGroup.cc \
src/Vehicle/TerrainProtocolHandler.cc \
src/Vehicle/TrajectoryPoints.cc \
......
......@@ -49,7 +49,6 @@
<file alias="FWLandingPatternEditor.qml">src/PlanView/FWLandingPatternEditor.qml</file>
<file alias="GeneralSettings.qml">src/ui/preferences/GeneralSettings.qml</file>
<file alias="GeoTagPage.qml">src/AnalyzeView/GeoTagPage.qml</file>
<file alias="HealthPageWidget.qml">src/FlightMap/Widgets/HealthPageWidget.qml</file>
<file alias="HelpSettings.qml">src/ui/preferences/HelpSettings.qml</file>
<file alias="JoystickConfig.qml">src/VehicleSetup/JoystickConfig.qml</file>
<file alias="JoystickConfigAdvanced.qml">src/VehicleSetup/JoystickConfigAdvanced.qml</file>
......@@ -106,6 +105,7 @@
<file alias="QGroundControl/Controls/JoystickThumbPad.qml">src/QmlControls/JoystickThumbPad.qml</file>
<file alias="QGroundControl/Controls/KMLOrSHPFileDialog.qml">src/QmlControls/KMLOrSHPFileDialog.qml</file>
<file alias="QGroundControl/Controls/LogReplayStatusBar.qml">src/QmlControls/LogReplayStatusBar.qml</file>
<file alias="QGroundControl/Controls/MainStatusIndicator.qml">src/ui/toolbar/MainStatusIndicator.qml</file>
<file alias="QGroundControl/Controls/MainToolBar.qml">src/ui/toolbar/MainToolBar.qml</file>
<file alias="QGroundControl/Controls/MainWindowSavedState.qml">src/QmlControls/MainWindowSavedState.qml</file>
<file alias="QGroundControl/Controls/MAVLinkChart.qml">src/QmlControls/MAVLinkChart.qml</file>
......
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
import QtQuick 2.3
import QtQuick.Layouts 1.2
import QGroundControl 1.0
import QGroundControl.Controls 1.0
import QGroundControl.ScreenTools 1.0
/// Health page for Instrument Panel PageWidget
Column {
width: pageWidth
property bool showSettingsIcon: false
property var _unhealthySensors: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle.unhealthySensors : [ ]
QGCLabel {
width: parent.width
horizontalAlignment: Text.AlignHCenter
text: qsTr("All systems healthy")
visible: healthRepeater.count == 0
}
Repeater {
id: healthRepeater
model: _unhealthySensors
Row {
Image {
source: "/qmlimages/Yield.svg"
height: ScreenTools.defaultFontPixelHeight
sourceSize.height: height
fillMode: Image.PreserveAspectFit
}
QGCLabel {
text: modelData
}
}
}
}
......@@ -31,6 +31,7 @@ InstrumentValueEditDialog 1.0 InstrumentValueEditDialog.qml
JoystickThumbPad 1.0 JoystickThumbPad.qml
KMLOrSHPFileDialog 1.0 KMLOrSHPFileDialog.qml
LogReplayStatusBar 1.0 LogReplayStatusBar.qml
MainStatusIndicator 1.0 MainStatusIndicator.qml
MainToolBar 1.0 MainToolBar.qml
MainWindowSavedState 1.0 MainWindowSavedState.qml
MAVLinkMessageButton 1.0 MAVLinkMessageButton.qml
......
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "SysStatusSensorInfo.h"
#include <QDebug>
SysStatusSensorInfo::SysStatusSensorInfo(QObject* parent)
: QObject(parent)
{
}
void SysStatusSensorInfo::update(const mavlink_sys_status_t& sysStatus)
{
bool dirty = false;
// Walk the bits
for (int bitPosition=0; bitPosition<32; bitPosition++) {
MAV_SYS_STATUS_SENSOR sensorBitMask = static_cast<MAV_SYS_STATUS_SENSOR>(1 << bitPosition);
if (sysStatus.onboard_control_sensors_present & sensorBitMask) {
if (_sensorInfoMap.contains(sensorBitMask)) {
SensorInfo_t& sensorInfo = _sensorInfoMap[sensorBitMask];
bool newEnabled = sysStatus.onboard_control_sensors_enabled & sensorBitMask;
if (sensorInfo.enabled != newEnabled) {
dirty = true;
sensorInfo.enabled = newEnabled;
}
bool newHealthy = sysStatus.onboard_control_sensors_health & sensorBitMask;
if (sensorInfo.healthy != newHealthy) {
dirty = true;
sensorInfo.healthy = newHealthy;
}
} else {
dirty = true;
SensorInfo_t sensorInfo = { !!(sysStatus.onboard_control_sensors_enabled & sensorBitMask), !!(sysStatus.onboard_control_sensors_health & sensorBitMask) };
_sensorInfoMap[sensorBitMask] = sensorInfo;
}
} else {
if (_sensorInfoMap.contains(sensorBitMask)) {
dirty = true;
_sensorInfoMap.remove(sensorBitMask);
}
}
}
if (dirty) {
emit sensorInfoChanged();
}
}
QStringList SysStatusSensorInfo::sensorNames (void) const
{
QStringList rgNames;
// List ordering is unhealthy, healthy, disabled
for (int i=0; i<_sensorInfoMap.keys().count(); i++) {
const MAV_SYS_STATUS_SENSOR sensorBitMask = _sensorInfoMap.keys()[i];
const SensorInfo_t& sensorInfo = _sensorInfoMap[sensorBitMask];
if (sensorInfo.enabled && !sensorInfo.healthy) {
rgNames.append(QGCMAVLink::mavSysStatusSensorToString(sensorBitMask));
}
}
for (int i=0; i<_sensorInfoMap.keys().count(); i++) {
const MAV_SYS_STATUS_SENSOR sensorBitMask = _sensorInfoMap.keys()[i];
const SensorInfo_t& sensorInfo = _sensorInfoMap[sensorBitMask];
if (sensorInfo.enabled && sensorInfo.healthy) {
rgNames.append(QGCMAVLink::mavSysStatusSensorToString(sensorBitMask));
}
}
for (int i=0; i<_sensorInfoMap.keys().count(); i++) {
const MAV_SYS_STATUS_SENSOR sensorBitMask = _sensorInfoMap.keys()[i];
const SensorInfo_t& sensorInfo = _sensorInfoMap[sensorBitMask];
if (!sensorInfo.enabled) {
rgNames.append(QGCMAVLink::mavSysStatusSensorToString(sensorBitMask));
}
}
return rgNames;
}
QStringList SysStatusSensorInfo::sensorStatus(void) const
{
QStringList rgStatus;
// List ordering is unhealthy, healthy, disabled
for (int i=0; i<_sensorInfoMap.keys().count(); i++) {
const MAV_SYS_STATUS_SENSOR sensorBitMask = _sensorInfoMap.keys()[i];
const SensorInfo_t& sensorInfo = _sensorInfoMap[sensorBitMask];
if (sensorInfo.enabled && !sensorInfo.healthy) {
rgStatus.append(tr("Error"));
}
}
for (int i=0; i<_sensorInfoMap.keys().count(); i++) {
const MAV_SYS_STATUS_SENSOR sensorBitMask = _sensorInfoMap.keys()[i];
const SensorInfo_t& sensorInfo = _sensorInfoMap[sensorBitMask];
if (sensorInfo.enabled && sensorInfo.healthy) {
rgStatus.append(tr("Normal"));
}
}
for (int i=0; i<_sensorInfoMap.keys().count(); i++) {
const MAV_SYS_STATUS_SENSOR sensorBitMask = _sensorInfoMap.keys()[i];
const SensorInfo_t& sensorInfo = _sensorInfoMap[sensorBitMask];
if (!sensorInfo.enabled) {
rgStatus.append(tr("Disabled"));
}
}
return rgStatus;
}
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include "QGCMAVLink.h"
#include "QmlObjectListModel.h"
#include <QObject>
/// Class which represents sensor info from the SYS_STATUS mavlink message
class SysStatusSensorInfo : public QObject
{
Q_OBJECT
public:
SysStatusSensorInfo(QObject* parent = nullptr);
Q_PROPERTY(QStringList sensorNames READ sensorNames NOTIFY sensorInfoChanged)
Q_PROPERTY(QStringList sensorStatus READ sensorStatus NOTIFY sensorInfoChanged)
void update (const mavlink_sys_status_t& sysStatus);
QStringList sensorNames (void) const;
QStringList sensorStatus(void) const;
signals:
void sensorInfoChanged(void);
private:
typedef struct {
bool enabled;
bool healthy;
} SensorInfo_t;
QMap<MAV_SYS_STATUS_SENSOR, SensorInfo_t> _sensorInfoMap;
};
......@@ -1429,7 +1429,6 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message)
_onboardControlSensorsEnabled = newOnboardControlSensorsEnabled;
_onboardControlSensorsPresent = newOnboardControlSensorsEnabled;
_onboardControlSensorsUnhealthy = 0;
emit unhealthySensorsChanged();
}
}
......@@ -1652,6 +1651,27 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message)
mavlink_sys_status_t sysStatus;
mavlink_msg_sys_status_decode(&message, &sysStatus);
_sysStatusSensorInfo.update(sysStatus);
if (sysStatus.onboard_control_sensors_enabled & MAV_SYS_STATUS_PREARM_CHECK) {
if (!_readyToFlyAvailable) {
_readyToFlyAvailable = true;
emit readyToFlyAvailableChanged(true);
}
bool newReadyToFly = sysStatus.onboard_control_sensors_health & MAV_SYS_STATUS_PREARM_CHECK;
if (newReadyToFly != _readyToFly) {
_readyToFly = newReadyToFly;
emit readyToFlyChanged(_readyToFly);
}
}
bool newAllSensorsHealthy = (sysStatus.onboard_control_sensors_enabled & sysStatus.onboard_control_sensors_health) == sysStatus.onboard_control_sensors_enabled;
if (newAllSensorsHealthy != _allSensorsHealthy) {
_allSensorsHealthy = newAllSensorsHealthy;
emit allSensorsHealthyChanged(_allSensorsHealthy);
}
if (_onboardControlSensorsPresent != sysStatus.onboard_control_sensors_present) {
_onboardControlSensorsPresent = sysStatus.onboard_control_sensors_present;
emit sensorsPresentBitsChanged(_onboardControlSensorsPresent);
......@@ -1675,7 +1695,6 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message)
uint32_t newSensorsUnhealthy = _onboardControlSensorsEnabled & ~_onboardControlSensorsHealth;
if (newSensorsUnhealthy != _onboardControlSensorsUnhealthy) {
_onboardControlSensorsUnhealthy = newSensorsUnhealthy;
emit unhealthySensorsChanged();
emit sensorsUnhealthyBitsChanged(_onboardControlSensorsUnhealthy);
}
......@@ -3578,58 +3597,6 @@ QString Vehicle::brandImageOutdoor() const
return _firmwarePlugin->brandImageOutdoor(this);
}
QStringList Vehicle::unhealthySensors() const
{
QStringList sensorList;
struct sensorInfo_s {
uint32_t bit;
QString sensorName;
};
static const sensorInfo_s rgSensorInfo[] = {
{ MAV_SYS_STATUS_SENSOR_3D_GYRO, tr("Gyro") },
{ MAV_SYS_STATUS_SENSOR_3D_ACCEL, tr("Accelerometer") },
{ MAV_SYS_STATUS_SENSOR_3D_MAG, tr("Magnetometer") },
{ MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, tr("Absolute pressure") },
{ MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, tr("Differential pressure") },
{ MAV_SYS_STATUS_SENSOR_GPS, tr("GPS") },
{ MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, tr("Optical flow") },
{ MAV_SYS_STATUS_SENSOR_VISION_POSITION, tr("Computer vision position") },
{ MAV_SYS_STATUS_SENSOR_LASER_POSITION, tr("Laser based position") },
{ MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH, tr("External ground truth") },
{ MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL, tr("Angular rate control") },
{ MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, tr("Attitude stabilization") },
{ MAV_SYS_STATUS_SENSOR_YAW_POSITION, tr("Yaw position") },
{ MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL, tr("Z/altitude control") },
{ MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL, tr("X/Y position control") },
{ MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, tr("Motor outputs / control") },
{ MAV_SYS_STATUS_SENSOR_RC_RECEIVER, tr("RC receiver") },
{ MAV_SYS_STATUS_SENSOR_3D_GYRO2, tr("Gyro 2") },
{ MAV_SYS_STATUS_SENSOR_3D_ACCEL2, tr("Accelerometer 2") },
{ MAV_SYS_STATUS_SENSOR_3D_MAG2, tr("Magnetometer 2") },
{ MAV_SYS_STATUS_GEOFENCE, tr("GeoFence") },
{ MAV_SYS_STATUS_AHRS, tr("AHRS") },
{ MAV_SYS_STATUS_TERRAIN, tr("Terrain") },
{ MAV_SYS_STATUS_REVERSE_MOTOR, tr("Motors reversed") },
{ MAV_SYS_STATUS_LOGGING, tr("Logging") },
{ MAV_SYS_STATUS_SENSOR_BATTERY, tr("Battery") },
{ MAV_SYS_STATUS_SENSOR_PROXIMITY, tr("Proximity") },
{ MAV_SYS_STATUS_SENSOR_SATCOM, tr("Satellite Communication") },
{ MAV_SYS_STATUS_PREARM_CHECK, tr("Pre-Arm Check") },
{ MAV_SYS_STATUS_OBSTACLE_AVOIDANCE, tr("Avoidance/collision prevention") },
};
for (size_t i=0; i<sizeof(rgSensorInfo)/sizeof(sensorInfo_s); i++) {
const sensorInfo_s* pSensorInfo = &rgSensorInfo[i];
if ((_onboardControlSensorsEnabled & pSensorInfo->bit) && !(_onboardControlSensorsHealth & pSensorInfo->bit)) {
sensorList << pSensorInfo->sensorName;
}
}
return sensorList;
}
void Vehicle::setOfflineEditingDefaultComponentId(int defaultComponentId)
{
if (_offlineEditingVehicle) {
......
......@@ -25,6 +25,7 @@
#include "SettingsFact.h"
#include "QGCMapCircle.h"
#include "TerrainFactGroup.h"
#include "SysStatusSensorInfo.h"
class UAS;
class UASInterface;
......@@ -596,7 +597,6 @@ public:
Q_PROPERTY(bool isOfflineEditingVehicle READ isOfflineEditingVehicle CONSTANT)
Q_PROPERTY(QString brandImageIndoor READ brandImageIndoor NOTIFY firmwareTypeChanged)
Q_PROPERTY(QString brandImageOutdoor READ brandImageOutdoor NOTIFY firmwareTypeChanged)
Q_PROPERTY(QStringList unhealthySensors READ unhealthySensors NOTIFY unhealthySensorsChanged)
Q_PROPERTY(int sensorsPresentBits READ sensorsPresentBits NOTIFY sensorsPresentBitsChanged)
Q_PROPERTY(int sensorsEnabledBits READ sensorsEnabledBits NOTIFY sensorsEnabledBitsChanged)
Q_PROPERTY(int sensorsHealthBits READ sensorsHealthBits NOTIFY sensorsHealthBitsChanged)
......@@ -643,6 +643,10 @@ public:
Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged)
Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged)
Q_PROPERTY(CheckList checkListState READ checkListState WRITE setCheckListState NOTIFY checkListStateChanged)
Q_PROPERTY(bool readyToFlyAvailable READ readyToFlyAvailable NOTIFY readyToFlyAvailableChanged) ///< true: readyToFly signalling is available on this vehicle
Q_PROPERTY(bool readyToFly READ readyToFly NOTIFY readyToFlyChanged)
Q_PROPERTY(QObject* sysStatusSensorInfo READ sysStatusSensorInfo CONSTANT)
Q_PROPERTY(bool allSensorsHealthy READ allSensorsHealthy NOTIFY allSensorsHealthyChanged) //< true: all sensors in SYS_STATUS reported as healthy
// The following properties relate to Orbit status
Q_PROPERTY(bool orbitActive READ orbitActive NOTIFY orbitActiveChanged)
......@@ -940,7 +944,6 @@ public:
bool isOfflineEditingVehicle () const { return _offlineEditingVehicle; }
QString brandImageIndoor () const;
QString brandImageOutdoor () const;
QStringList unhealthySensors () const;
int sensorsPresentBits () const { return static_cast<int>(_onboardControlSensorsPresent); }
int sensorsEnabledBits () const { return static_cast<int>(_onboardControlSensorsEnabled); }
int sensorsHealthBits () const { return static_cast<int>(_onboardControlSensorsHealth); }
......@@ -968,6 +971,10 @@ public:
bool highLatencyLink () const { return _highLatencyLink; }
bool orbitActive () const { return _orbitActive; }
QGCMapCircle* orbitMapCircle () { return &_orbitMapCircle; }
bool readyToFlyAvailable () { return _readyToFlyAvailable; }
bool readyToFly () { return _readyToFly; }
bool allSensorsHealthy () { return _allSensorsHealthy; }
QObject* sysStatusSensorInfo () { return &_sysStatusSensorInfo; }
/// Get the maximum MAVLink protocol version supported
/// @return the maximum version
......@@ -1178,7 +1185,6 @@ signals:
void vtolInFwdFlightChanged (bool vtolInFwdFlight);
void prearmErrorChanged (const QString& prearmError);
void soloFirmwareChanged (bool soloFirmware);
void unhealthySensorsChanged ();
void defaultCruiseSpeedChanged (double cruiseSpeed);
void defaultHoverSpeedChanged (double hoverSpeed);
void firmwareTypeChanged ();
......@@ -1223,6 +1229,9 @@ signals:
void sensorsHealthBitsChanged (int sensorsHealthBits);
void sensorsUnhealthyBitsChanged (int sensorsUnhealthyBits);
void orbitActiveChanged (bool orbitActive);
void readyToFlyAvailableChanged (bool readyToFlyAvailable);
void readyToFlyChanged (bool readyToFy);
void allSensorsHealthyChanged (bool allSensorsHealthy);
void firmwareVersionChanged ();
void firmwareCustomVersionChanged ();
......@@ -1421,9 +1430,9 @@ private:
uint32_t _onboardControlSensorsEnabled;
uint32_t _onboardControlSensorsHealth;
uint32_t _onboardControlSensorsUnhealthy;
bool _gpsRawIntMessageAvailable = false;
bool _globalPositionIntMessageAvailable = false;
bool _altitudeMessageAvailable = false;
bool _gpsRawIntMessageAvailable = false;
bool _globalPositionIntMessageAvailable = false;
bool _altitudeMessageAvailable = false;
double _defaultCruiseSpeed;
double _defaultHoverSpeed;
int _telemetryRRSSI;
......@@ -1433,14 +1442,19 @@ private:
uint32_t _telemetryTXBuffer;
int _telemetryLNoise;
int _telemetryRNoise;
bool _mavlinkProtocolRequestComplete = false;
unsigned _mavlinkProtocolRequestMaxProtoVersion = 0;
unsigned _maxProtoVersion = 0;
bool _capabilityBitsKnown = false;
bool _mavlinkProtocolRequestComplete = false;
unsigned _mavlinkProtocolRequestMaxProtoVersion = 0;
unsigned _maxProtoVersion = 0;
bool _capabilityBitsKnown = false;
uint64_t _capabilityBits;
bool _highLatencyLink;
bool _receivingAttitudeQuaternion;
CheckList _checkListState = CheckListNotSetup;
CheckList _checkListState = CheckListNotSetup;
bool _readyToFlyAvailable = false;
bool _readyToFly = false;
bool _allSensorsHealthy = true;
SysStatusSensorInfo _sysStatusSensorInfo;
QGCCameraManager* _cameras;
......
......@@ -100,7 +100,6 @@ public:
QmlComponentInfo* valuesPageWidgetInfo = nullptr;
QmlComponentInfo* cameraPageWidgetInfo = nullptr;
QmlComponentInfo* videoPageWidgetInfo = nullptr;
QmlComponentInfo* healthPageWidgetInfo = nullptr;
QmlComponentInfo* vibrationPageWidgetInfo = nullptr;
QGCOptions* defaultOptions = nullptr;
......@@ -206,10 +205,6 @@ void QGCCorePlugin::_resetInstrumentPages()
_p->videoPageWidgetInfo = nullptr;
}
#endif
if(_p->healthPageWidgetInfo) {
_p->healthPageWidgetInfo->deleteLater();
_p->healthPageWidgetInfo = nullptr;
}
if(_p->vibrationPageWidgetInfo) {
_p->vibrationPageWidgetInfo->deleteLater();
_p->vibrationPageWidgetInfo = nullptr;
......@@ -287,7 +282,6 @@ QVariantList& QGCCorePlugin::instrumentPages()
_p->videoPageWidgetInfo = new QmlComponentInfo(tr("Video Stream"), QUrl::fromUserInput("qrc:/qml/VideoPageWidget.qml"));
}
#endif
_p->healthPageWidgetInfo = new QmlComponentInfo(tr("Health"), QUrl::fromUserInput("qrc:/qml/HealthPageWidget.qml"));
_p->vibrationPageWidgetInfo = new QmlComponentInfo(tr("Vibration"), QUrl::fromUserInput("qrc:/qml/VibrationPageWidget.qml"));
_p->instrumentPageWidgetList.append(QVariant::fromValue(_p->valuesPageWidgetInfo));
......@@ -295,7 +289,6 @@ QVariantList& QGCCorePlugin::instrumentPages()
#if defined(QGC_GST_STREAMING)
_p->instrumentPageWidgetList.append(QVariant::fromValue(_p->videoPageWidgetInfo));
#endif
_p->instrumentPageWidgetList.append(QVariant::fromValue(_p->healthPageWidgetInfo));
_p->instrumentPageWidgetList.append(QVariant::fromValue(_p->vibrationPageWidgetInfo));
}
return _p->instrumentPageWidgetList;
......
......@@ -23,12 +23,12 @@ QGCOptions::QGCOptions(QObject* parent)
QColor QGCOptions::toolbarBackgroundLight() const
{
return QColor(255,255,255,204);
return QColor(255,255,255);
}
QColor QGCOptions::toolbarBackgroundDark() const
{
return QColor(0,0,0,192);
return QColor(0,0,0);
}
QGCFlyViewOptions* QGCOptions::flyViewOptions(void)
......
......@@ -10,6 +10,7 @@
#include "QGCMAVLink.h"
#include <QtGlobal>
#include <QDebug>
constexpr QGCMAVLink::FirmwareClass_t QGCMAVLink::FirmwareClassPX4;
constexpr QGCMAVLink::FirmwareClass_t QGCMAVLink::FirmwareClassArduPilot;
......@@ -145,7 +146,7 @@ QString QGCMAVLink::vehicleClassToString(VehicleClass_t vehicleClass)
}
}
QString QGCMAVLink::mavResultToString(MAV_RESULT result)
QString QGCMAVLink::mavResultToString(MAV_RESULT result)
{
switch (result) {
case MAV_RESULT_ACCEPTED:
......@@ -165,6 +166,58 @@ QString QGCMAVLink::mavResultToString(MAV_RESULT result)
}
}
QString QGCMAVLink::mavSysStatusSensorToString(MAV_SYS_STATUS_SENSOR sysStatusSensor)
{
struct sensorInfo_s {
uint32_t bit;
QString sensorName;
};
static const sensorInfo_s rgSensorInfo[] = {
{ MAV_SYS_STATUS_SENSOR_3D_GYRO, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Gyro") },
{ MAV_SYS_STATUS_SENSOR_3D_ACCEL, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Accelerometer") },
{ MAV_SYS_STATUS_SENSOR_3D_MAG, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Magnetometer") },
{ MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Absolute pressure") },
{ MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Differential pressure") },
{ MAV_SYS_STATUS_SENSOR_GPS, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "GPS") },
{ MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Optical flow") },
{ MAV_SYS_STATUS_SENSOR_VISION_POSITION, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Computer vision position") },
{ MAV_SYS_STATUS_SENSOR_LASER_POSITION, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Laser based position") },
{ MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "External ground truth") },
{ MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Angular rate control") },
{ MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Attitude stabilization") },
{ MAV_SYS_STATUS_SENSOR_YAW_POSITION, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Yaw position") },
{ MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Z/altitude control") },
{ MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "X/Y position control") },
{ MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Motor outputs / control") },
{ MAV_SYS_STATUS_SENSOR_RC_RECEIVER, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "RC receiver") },
{ MAV_SYS_STATUS_SENSOR_3D_GYRO2, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Gyro 2") },
{ MAV_SYS_STATUS_SENSOR_3D_ACCEL2, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Accelerometer 2") },
{ MAV_SYS_STATUS_SENSOR_3D_MAG2, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Magnetometer 2") },
{ MAV_SYS_STATUS_GEOFENCE, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "GeoFence") },
{ MAV_SYS_STATUS_AHRS, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "AHRS") },
{ MAV_SYS_STATUS_TERRAIN, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Terrain") },
{ MAV_SYS_STATUS_REVERSE_MOTOR, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Motors reversed") },
{ MAV_SYS_STATUS_LOGGING, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Logging") },
{ MAV_SYS_STATUS_SENSOR_BATTERY, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Battery") },
{ MAV_SYS_STATUS_SENSOR_PROXIMITY, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Proximity") },
{ MAV_SYS_STATUS_SENSOR_SATCOM, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Satellite Communication") },
{ MAV_SYS_STATUS_PREARM_CHECK, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Pre-Arm Check") },
{ MAV_SYS_STATUS_OBSTACLE_AVOIDANCE, QT_TRANSLATE_NOOP("MAVLink SYS_STATUS_SENSOR value", "Avoidance/collision prevention") },
};
for (size_t i=0; i<sizeof(rgSensorInfo)/sizeof(sensorInfo_s); i++) {
const sensorInfo_s* pSensorInfo = &rgSensorInfo[i];
if (sysStatusSensor == pSensorInfo->bit) {
return pSensorInfo->sensorName;
}
}
qWarning() << "QGCMAVLink::mavSysStatusSensorToString: Unknown sensor" << sysStatusSensor;
return QT_TRANSLATE_NOOP("MAVLink unknown SYS_STATUS_SENSOR value", "Unknown sensor");
}
QString MavlinkFTP::opCodeToString(OpCode_t opCode)
{
switch (opCode) {
......@@ -238,3 +291,4 @@ QString MavlinkFTP::errorCodeToString(ErrorCode_t errorCode)
return "Unknown Error";
}
......@@ -63,25 +63,26 @@ public:
static constexpr VehicleClass_t VehicleClassVTOL = MAV_TYPE_VTOL_QUADROTOR;
static constexpr VehicleClass_t VehicleClassGeneric = MAV_TYPE_GENERIC;
static bool isPX4FirmwareClass (MAV_AUTOPILOT autopilot) { return autopilot == MAV_AUTOPILOT_PX4; }
static bool isArduPilotFirmwareClass(MAV_AUTOPILOT autopilot) { return autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA; }
static bool isGenericFirmwareClass (MAV_AUTOPILOT autopilot) { return !isPX4FirmwareClass(autopilot) && ! isArduPilotFirmwareClass(autopilot); }
static FirmwareClass_t firmwareClass (MAV_AUTOPILOT autopilot);
static MAV_AUTOPILOT firmwareClassToAutopilot(FirmwareClass_t firmwareClass) { return static_cast<MAV_AUTOPILOT>(firmwareClass); }
static QString firmwareClassToString (FirmwareClass_t firmwareClass);
static QList<FirmwareClass_t> allFirmwareClasses (void);
static bool isFixedWing (MAV_TYPE mavType);
static bool isRoverBoat (MAV_TYPE mavType);
static bool isSub (MAV_TYPE mavType);
static bool isMultiRotor (MAV_TYPE mavType);
static bool isVTOL (MAV_TYPE mavType);
static VehicleClass_t vehicleClass (MAV_TYPE mavType);
static MAV_TYPE vehicleClassToMavType (VehicleClass_t vehicleClass) { return static_cast<MAV_TYPE>(vehicleClass); }
static QString vehicleClassToString (VehicleClass_t vehicleClass);
static QList<VehicleClass_t> allVehicleClasses (void);
static QString mavResultToString (MAV_RESULT result);
static bool isPX4FirmwareClass (MAV_AUTOPILOT autopilot) { return autopilot == MAV_AUTOPILOT_PX4; }
static bool isArduPilotFirmwareClass (MAV_AUTOPILOT autopilot) { return autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA; }
static bool isGenericFirmwareClass (MAV_AUTOPILOT autopilot) { return !isPX4FirmwareClass(autopilot) && ! isArduPilotFirmwareClass(autopilot); }
static FirmwareClass_t firmwareClass (MAV_AUTOPILOT autopilot);
static MAV_AUTOPILOT firmwareClassToAutopilot (FirmwareClass_t firmwareClass) { return static_cast<MAV_AUTOPILOT>(firmwareClass); }
static QString firmwareClassToString (FirmwareClass_t firmwareClass);
static QList<FirmwareClass_t> allFirmwareClasses (void);
static bool isFixedWing (MAV_TYPE mavType);
static bool isRoverBoat (MAV_TYPE mavType);
static bool isSub (MAV_TYPE mavType);
static bool isMultiRotor (MAV_TYPE mavType);
static bool isVTOL (MAV_TYPE mavType);
static VehicleClass_t vehicleClass (MAV_TYPE mavType);
static MAV_TYPE vehicleClassToMavType (VehicleClass_t vehicleClass) { return static_cast<MAV_TYPE>(vehicleClass); }
static QString vehicleClassToString (VehicleClass_t vehicleClass);
static QList<VehicleClass_t> allVehicleClasses (void);
static QString mavResultToString (MAV_RESULT result);
static QString mavSysStatusSensorToString (MAV_SYS_STATUS_SENSOR sysStatusSensor);
};
class MavlinkFTP {
......
......@@ -526,15 +526,6 @@ ApplicationWindow {
source: "AnalyzeView.qml"
}
//-------------------------------------------------------------------------
// @brief Loader helper for any child, no matter how deep, to display elements
// on top of the main window.
// This is DEPRECATED. Use Popup instead.
Loader {
id: rootLoader
anchors.centerIn: parent
}
//-------------------------------------------------------------------------
//-- Vehicle Messages
......@@ -781,7 +772,7 @@ ApplicationWindow {
Popup {
id: indicatorDropdown
y: ScreenTools.defaultFontPixelHeight
padding: ScreenTools.defaultFontPixelWidth * 0.75
modal: true
focus: true
closePolicy: Popup.CloseOnEscape | Popup.CloseOnPressOutside
......
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
import QtQuick 2.11
import QtQuick.Layouts 1.11
import QGroundControl 1.0
import QGroundControl.Controls 1.0
import QGroundControl.MultiVehicleManager 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Palette 1.0
Item {
id: _root
Layout.preferredWidth: mainStatusLabel.contentWidth + ScreenTools.defaultFontPixelWidth
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property real _margins: ScreenTools.defaultFontPixelWidth
Component {
id: mainStatusInfo
Rectangle {
width: mainLayout.width + (_margins * 2)
height: mainLayout.height + (_margins * 2)
radius: ScreenTools.defaultFontPixelHeight * 0.5
color: qgcPal.window
border.color: qgcPal.text
GridLayout {
id: mainLayout
anchors.margins: _margins
anchors.top: parent.top
anchors.left: parent.left
rowSpacing: ScreenTools.defaultFontPixelWidth / 2
columnSpacing: rowSpacing
rows: _activeVehicle.sysStatusSensorInfo.sensorNames.length
flow: GridLayout.TopToBottom
Repeater {
model: _activeVehicle.sysStatusSensorInfo.sensorNames
QGCLabel {
text: modelData
}
}
Repeater {
model: _activeVehicle.sysStatusSensorInfo.sensorStatus
QGCLabel {
text: modelData
}
}
}
}
}
QGCLabel {
id: mainStatusLabel
text: mainStatusText()
font.pointSize: ScreenTools.largeFontPointSize
anchors.verticalCenter: parent.verticalCenter
property string _commLostText: qsTr("Communication Lost")
property string _readyToFlyText: qsTr("Ready To Fly")
property string _notReadyToFlyText: qsTr("Not Ready")
property string _disconnectedText: qsTr("Disconnected")
property string _armedText: qsTr("Armed")
property string _flyingText: qsTr("Flying")
property string _landingText: qsTr("Landing")
function mainStatusText() {
var statusText
if (_activeVehicle) {
if (_communicationLost) {
_mainStatusBGColor = "red"
return mainStatusLabel._commLostText
}
if (_activeVehicle.armed) {
if (_activeVehicle.flying) {
_mainStatusBGColor = qgcPal.brandingPurple
return mainStatusLabel._flyingText
} else if (_activeVehicle.landing) {
_mainStatusBGColor = qgcPal.brandingPurple
return mainStatusLabel._landingText
} else {
_mainStatusBGColor = qgcPal.brandingPurple
return mainStatusLabel._armedText
}
} else {
if (_activeVehicle.readyToFlyAvailable) {
if (_activeVehicle.readyToFly) {
_mainStatusBGColor = "green"
return mainStatusLabel._readyToFlyText
} else {
_mainStatusBGColor = "yellow"
return mainStatusLabel._notReadyToFlyText
}
} else {
// Best we can do is determine readiness based on AutoPilot component setup and health indicators from SYS_STATUS
if (_activeVehicle.allSensorsHealthy && _activeVehicle.autopilot.setupComplete) {
_mainStatusBGColor = "green"
return mainStatusLabel._readyToFlyText
} else {
_mainStatusBGColor = "yellow"
return mainStatusLabel._notReadyToFlyText
}
}
}
} else {
_mainStatusBGColor = qgcPal.brandingPurple
return mainStatusLabel._disconnectedText
}
}
}
MouseArea {
anchors.fill: parent
enabled: _activeVehicle
onClicked: {
mainWindow.showPopUp(_root, mainStatusInfo)
}
}
}
......@@ -7,7 +7,7 @@
*
****************************************************************************/
import QtQuick 2.11
import QtQuick 2.12
import QtQuick.Controls 2.4
import QtQuick.Layouts 1.11
import QtQuick.Dialogs 1.3
......@@ -31,6 +31,7 @@ Rectangle {
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property bool _communicationLost: _activeVehicle ? _activeVehicle.connectionLost : false
property color _mainStatusBGColor: qgcPal.brandingPurple
Component.onCompleted: toolbar.viewButtonClicked(flyButton)
......@@ -66,6 +67,18 @@ Rectangle {
visible: qgcPal.globalTheme === QGCPalette.Light
}
Rectangle {
anchors.fill: viewButtonRow
visible: currentToolbar === flyViewToolbar
gradient: Gradient {
orientation: Gradient.Horizontal
GradientStop { position: 0; color: _mainStatusBGColor }
GradientStop { position: currentButton.x + currentButton.width; color: _mainStatusBGColor }
GradientStop { position: 1; color: _root.color }
}
}
RowLayout {
id: viewButtonRow
anchors.bottomMargin: 1
......@@ -78,6 +91,19 @@ Rectangle {
Layout.fillHeight: true
onClicked: viewSelectDrawer.visible = true
}
MainStatusIndicator {
Layout.fillHeight: true
visible: currentToolbar === flyViewToolbar
}
QGCButton {
id: disconnectButton
Layout.alignment: Qt.AlignVCenter
text: qsTr("Disconnect")
onClicked: _activeVehicle.disconnectInactiveVehicle()
visible: _activeVehicle && _communicationLost && currentToolbar === flyViewToolbar
}
}
Rectangle {
......@@ -97,7 +123,7 @@ Rectangle {
anchors.bottomMargin: 1
anchors.top: parent.top
anchors.bottom: parent.bottom
anchors.right: connectionStatus.visible ? connectionStatus.left : parent.right
anchors.right: parent.right
contentWidth: indicatorLoader.x + indicatorLoader.width
flickableDirection: Flickable.HorizontalFlick
clip: !valueArea.settingsUnlocked
......@@ -232,47 +258,4 @@ Rectangle {
onClicked: largeProgressBar._userHide = true
}
}
//-------------------------------------------------------------------------
//-- Waiting for a vehicle
QGCLabel {
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
anchors.right: parent.right
anchors.verticalCenter: parent.verticalCenter
text: qsTr("Waiting For Vehicle Connection")
font.pointSize: ScreenTools.mediumFontPointSize
font.family: ScreenTools.demiboldFontFamily
color: qgcPal.colorRed
visible: currentToolbar !== planViewToolbar && !_activeVehicle
}
//-------------------------------------------------------------------------
//-- Connection Status
Row {
id: connectionStatus
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
anchors.top: parent.top
anchors.bottom: parent.bottom
anchors.right: parent.right
layoutDirection: Qt.RightToLeft
spacing: ScreenTools.defaultFontPixelWidth
visible: currentToolbar !== planViewToolbar && _activeVehicle && _communicationLost
QGCButton {
id: disconnectButton
anchors.verticalCenter: parent.verticalCenter
text: qsTr("Disconnect")
primary: true
onClicked: _activeVehicle.disconnectInactiveVehicle()
}
QGCLabel {
id: connectionLost
anchors.verticalCenter: parent.verticalCenter
text: qsTr("COMMUNICATION LOST")
font.pointSize: ScreenTools.largeFontPointSize
font.family: ScreenTools.demiboldFontFamily
color: qgcPal.colorRed
}
}
}
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