diff --git a/README b/README index aad1ba690ea62c443ba63f3a0aeacfa1ce35ca94..eb9b5c4b105e2f4361edc3523a858baa57f89a1a 100644 --- a/README +++ b/README @@ -1,3 +1,4 @@ +maah QGroundControl Open Source Micro Air Vehicle Ground Control Station Project: diff --git a/dongfang_qgroundcontrol_notes.txt b/dongfang_qgroundcontrol_notes.txt new file mode 100644 index 0000000000000000000000000000000000000000..40fcc46d076944deeac1b1ee22c165f4cfa56651 --- /dev/null +++ b/dongfang_qgroundcontrol_notes.txt @@ -0,0 +1,90 @@ +From where is the 3D mouse used? + + +Comm folder: +============ + +LinkInterface is a QThread. Appears to describe an interface on the host system (TCP port, serial port, ....). + Does it suppport multiple connections? +SerialLinkInterface: Extension of above +SerialLink: Implementation + +XBeeLinkInterface: Extension of LinkInterface (but without other serial stuff than baudrate, is the rest assumed?) +XBeeLink: Implementation (with address resetting probably it's Series 1) + +UDPLink: UDP implementation. Port is defaulted. + +MAVLinkSimulationLink: Simulation/dummy implementation of LinkInterface. Does some file IO. +MAVLinkSimulationUAV: Simulation/dummy basic (remote) UAV state (minus mission state) for MAVLinkSimulationLink. +MAVLinkSimulationWaypointPlanner: imulation/dummy basic (remote) UAV mission state for MAVLinkSimulationLink. + + +ProtocolInterface describes a protocol. Major method: + virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0; + +MavlinkProtocol: MAVLink implementation of ProtocolInterface + + +Parameter: Identity of a parameter (the value types are not handled here. Oddly there is no type metainfo either) +ParameterList: Is pretty much what the name impiles. +QGCParamID: Wrapper if parameter text IDs, conversion to byte* and back + +QGCHilLink: Link to a HIL external system + QCGFlightGearLink, QGCJSBSimLink, QGCXPlaneLink : Implementations of QGCHilLink + + +QGCMAVLink: Nothing more than in include to raw mavlink.h + + +input folder: +============= +Some exotic input devices + + + +uas folder: +=========== +UAS.h: Local UAV model. Claims to support some sort of RPC. Uses some properties with notifications. Assumes MAVLink. +Some methods: + int getUASID() const; // the systemId + QList* getLinks(); // it knows its links + + virtual void receiveMessage(LinkInterface* link, mavlink_message_t message); + + float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs) + float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS + +Some fields: + bool positionLock; ///< Status if position information is available or not + double localX; // (what is this?) + double localY; + double localZ; + double latitude; ///< Global latitude as estimated by position estimator + double longitude; ///< Global longitude as estimated by position estimator + double altitude; ///< Global altitude as estimated by position estimator + double satelliteCount; ///< Number of satellites visible to raw GPS + bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position + double latitude_gps; ///< Global latitude as estimated by raw GPS + double longitude_gps; ///< Global longitude as estimated by raw GPS + double altitude_gps; ///< Global altitude as estimated by raw GPS + + and lots ans lots more... + + +*MAV.h: Implementations of UAS + + +QGCUASParamManager.h: A mixture of a widget and a parameter up/download state machine. Has a reference to its associated UAV. + +UASWaypointManager: API to waypoint / mission control. Not a widget. Has a reference to its associated UAV. + +UASManager.h: Singleton interface to all UASs on all interfaces. Maintains a single selected/active UAS. + +Ideas to do +TCPLink +XBee Series2 implementation +Less stress on uplink +Get rid of hardcoded SystemId of 255 +Quick View should print all altitudes (GPS, mix, ground and/or home), letting user see errors. +Console debugging is hopefully removed? + diff --git a/dongfang_qgroundcontrol_wishlist.txt b/dongfang_qgroundcontrol_wishlist.txt new file mode 100644 index 0000000000000000000000000000000000000000..f55ba1dc24dcc2a8f367946a436e72672ac7bec3 --- /dev/null +++ b/dongfang_qgroundcontrol_wishlist.txt @@ -0,0 +1,23 @@ +Major +1) Absolute altitude should absolutely work. Also in face of bad GPS init. + How does the mission planning part work with that, how is alt. stored in waypoints? + When is home altitude stored in the UAV? + +2) Some people have worse datalinks than average - eg. slow or noisy telemetry. GPRS data also easily gets + flooded. All data rates should be configurable and uplinks eased off a little. + +3) Some streams should be sent always even if not in use. For example using RC override and then stopping + using it: If the UAV does not received the final RC override message with zero values for all channels, + it will not get out of RC override. A solution is to make the message non final, repeating it every + now and then. + +Minor +1) I would like to see initial dummy values for eg. battery voltage go away and be replaced by "unknown". + Dummy values confuse, you won't know if your sensors and link are working or not. + +2) Suggestion if the DO_JUMP behavior of APM today (where DO_JUMP requires a waypoint command after it): + Modify APM code so DO_JUMP is regarded a navigation command. Make non navigation commands appear in + planning as sub commands of navigation commands (indent them). Easier to understand. + +3) Old style audio with signals/bells/beeps / Morse may be prefered by some. Maybe add slots'n'signals for + all audio and different implementation types (speech, beep, ...). Or a generic operator messaging thing. diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index e023da0e8e7fc7bbb4ed40214e02885096482d46..3398c6e8ffd2160a85c8180a8518bec61632a912 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -386,7 +386,8 @@ HEADERS += src/MG.h \ src/ui/uas/UASActionsWidget.h \ src/ui/designer/QGCRadioChannelDisplay.h \ src/ui/QGCTabbedInfoView.h \ - src/ui/UASRawStatusView.h + src/ui/UASRawStatusView.h \ + src/ui/PrimaryFlightDisplay.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010|win32-msvc2012::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -560,7 +561,8 @@ SOURCES += src/main.cc \ src/ui/uas/UASActionsWidget.cpp \ src/ui/designer/QGCRadioChannelDisplay.cpp \ src/ui/QGCTabbedInfoView.cpp \ - src/ui/UASRawStatusView.cpp + src/ui/UASRawStatusView.cpp \ + src/ui/PrimaryFlightDisplay.cpp # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010|win32-msvc2012::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc @@ -693,3 +695,7 @@ win32-msvc2008|win32-msvc2010|win32-msvc2012 { } unix:!macx:!symbian: LIBS += -losg + +OTHER_FILES += \ + dongfang_notes.txt \ + src/ui/dongfang-scrapyard.txt diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e0e74a1b904829cca8c2a997041b2709992bd15c..587db0a5ce451f68a9bd38eba6ccd24a92d34f99 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -39,16 +39,37 @@ */ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), uasId(id), - startTime(QGC::groundTimeMilliseconds()), - commStatus(COMM_DISCONNECTED), - name(""), - autopilot(-1), links(new QList()), unknownPackets(), mavlink(protocol), - waypointManager(this), + commStatus(COMM_DISCONNECTED), + receiveDropRate(0), + sendDropRate(0), + statusTimeout(new QTimer(this)), + + name(""), + type(MAV_TYPE_GENERIC), + airframe(QGC_AIRFRAME_GENERIC), + autopilot(-1), + systemIsArmed(false), + mode(-1), + // custom_mode not initialized + navMode(-1), + status(-1), + // shortModeText not initialized + // shortStateText not initialized + + // actuatorValues not initialized + // actuatorNames not initialized + // motorValues not initialized + // motorNames mnot initialized thrustSum(0), thrustMax(10), + + // batteryType not initialized + // cells not initialized + // fullVoltage not initialized + // emptyVoltage not initialized startVoltage(-1.0f), tickVoltage(10.5f), lastTickVoltageValue(13.0f), @@ -57,11 +78,15 @@ 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), - navMode(-1), + // chargeLevel not initialized + // timeRemaining not initialized + lowBattAlarm(false), + + startTime(QGC::groundTimeMilliseconds()), onboardTimeOffset(0), + controlRollManual(true), controlPitchManual(true), controlYawManual(true), @@ -70,10 +95,11 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), manualPitchAngle(0), manualYawAngle(0), manualThrust(0), - receiveDropRate(0), - sendDropRate(0), - lowBattAlarm(false), + positionLock(false), + isLocalPositionKnown(false), + isGlobalPositionKnown(false), + localX(0.0), localY(0.0), localZ(0.0), @@ -81,7 +107,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), latitude_gps(0.0), longitude_gps(0.0), altitude_gps(0.0), - statusTimeout(new QTimer(this)), + nedPosGlobalOffset(0,0,0), + nedAttGlobalOffset(0,0,0), + + + waypointManager(this), + #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) receivedOverlayTimestamp(0.0), receivedObstacleListTimestamp(0.0), @@ -89,18 +120,17 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), receivedPointCloudTimestamp(0.0), receivedRGBDImageTimestamp(0.0), #endif - paramsOnceRequested(false), - airframe(QGC_AIRFRAME_GENERIC), + attitudeKnown(false), - paramManager(NULL), attitudeStamped(false), lastAttitude(0), + + paramsOnceRequested(false), + paramManager(NULL), + simulation(0), - isLocalPositionKnown(false), - isGlobalPositionKnown(false), - systemIsArmed(false), - nedPosGlobalOffset(0,0,0), - nedAttGlobalOffset(0,0,0), + + // The protected members. connectionLost(false), lastVoltageWarning(0), lastNonNullTime(0), @@ -109,7 +139,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), sensorHil(false), lastSendTimeGPS(0), lastSendTimeSensors(0) - { for (unsigned int i = 0; i<255;++i) { @@ -123,7 +152,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); statusTimeout->start(500); readSettings(); - type = MAV_TYPE_GENERIC; // Initial signals emit disarmed(); emit armingChanged(false); @@ -447,7 +475,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) QString audiomodeText = getAudioModeTextFor(static_cast(state.base_mode)); - if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT) { statechanged = true; @@ -539,7 +566,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 +593,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 @@ -644,7 +673,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) attitudeKnown = true; emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); - emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); } } break; @@ -682,8 +711,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); } - emit altitudeChanged(uasId, hud.alt); - emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time); + // The primary altitude is the one that the UAV uses for navigation. + // We assume! that the HUD message reports that as altitude. + emit primaryAltitudeChanged(this, hud.alt, time); + + emit primarySpeedChanged(this, hud.airspeed, time); + emit gpsSpeedChanged(this, hud.groundspeed, time); + emit climbRateChanged(this, hud.climb, time); } break; case MAVLINK_MSG_ID_LOCAL_POSITION_NED: @@ -697,7 +731,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // Emit position always with component ID emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); - if (!wrongComponent) { localX = pos.x; @@ -705,9 +738,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) localZ = pos.z; // Emit - emit localPositionChanged(this, pos.x, pos.y, pos.z, time); - emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); + emit velocityChanged_NED(this, pos.vx, pos.vy, pos.vz, time); // Set internal state if (!positionLock) { @@ -735,18 +767,29 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_global_position_int_t pos; mavlink_msg_global_position_int_decode(&message, &pos); quint64 time = getUnixTime(); - //latitude = pos.lat/(double)1E7; setLatitude(pos.lat/(double)1E7); - //longitude = pos.lon/(double)1E7; setLongitude(pos.lon/(double)1E7); - //altitude = pos.alt/1000.0; + + // dongfang: Beware. There are 2 altitudes in this message; neither is the primary. + // pos.alt is GPS altitude and pos.relative_alt is above-home altitude. + // It would be nice if APM could be modified to present the primary (mix) alt. instead + // of the GPS alt. in this message. setAltitude(pos.alt/1000.0); + globalEstimatorActive = true; + speedX = pos.vx/100.0; speedY = pos.vy/100.0; speedZ = pos.vz/100.0; + emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time); - emit speedChanged(this, speedX, speedY, speedZ, time); + // dongfang: The altitude is GPS altitude. Bugger. It needs to be changed to primary. + emit gpsAltitudeChanged(this, getAltitude(), time); + // We had some frame mess here, global and local axes were mixed. + emit velocityChanged_NED(this, speedX, speedY, speedZ, time); + + double groundspeed = qSqrt(speedX*speedX+speedY*speedY); + emit gpsSpeedChanged(this, groundspeed, time); // Set internal state if (!positionLock) @@ -787,14 +830,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) longitude_gps = pos.lon/(double)1E7; altitude_gps = pos.alt/1000.0; + // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead. if (!globalEstimatorActive) { - //latitude = latitude_gps; setLatitude(latitude_gps); - //longitude = longitude_gps; setLongitude(longitude_gps); - //altitude = altitude_gps; setAltitude(altitude_gps); emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time); + emit gpsAltitudeChanged(this, getAltitude(), time); } positionLock = true; @@ -804,11 +846,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) float vel = pos.vel/100.0f; + // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead. if (!globalEstimatorActive) { if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) { emit speedChanged(this, vel, 0.0, 0.0, time); setGroundSpeed(vel); + // TODO: Other sources also? Actually this condition does not quite belong here. + emit gpsSpeedChanged(this, vel, time); } else { @@ -1351,6 +1396,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_nav_controller_output_t p; mavlink_msg_nav_controller_output_decode(&message,&p); setDistToWaypoint(p.wp_dist); + setBearingToWaypoint(p.nav_bearing); + //setAltitudeError(p.alt_error); + //setSpeedError(p.aspd_error); + //setCrosstrackingError(p.xtrack_error); + emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error); } break; case MAVLINK_MSG_ID_RAW_PRESSURE: @@ -1482,7 +1532,7 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptruasId,"localX","M",QVariant(val),getUnixTime()); } + double getLocalX() const { return localX; @@ -165,6 +160,7 @@ public: emit latitudeChanged(val,"latitude"); emit valueChanged(this->uasId,"latitude","deg",QVariant(val),getUnixTime()); } + double getLatitude() const { return latitude; @@ -176,6 +172,7 @@ public: emit longitudeChanged(val,"longitude"); emit valueChanged(this->uasId,"longitude","deg",QVariant(val),getUnixTime()); } + double getLongitude() const { return longitude; @@ -184,7 +181,7 @@ public: void setAltitude(double val) { altitude = val; - emit altitudeChanged(val,"altitude"); + emit altitudeChanged(val, "altitude"); emit valueChanged(this->uasId,"altitude","M",QVariant(val),getUnixTime()); } @@ -209,6 +206,7 @@ public: { return isLocalPositionKnown; } + virtual bool globalPositionKnown() const { return isGlobalPositionKnown; @@ -226,6 +224,19 @@ public: return distToWaypoint; } + void setBearingToWaypoint(double val) + { + bearingToWaypoint = val; + emit bearingToWaypointChanged(val,"bearingToWaypoint"); + emit valueChanged(this->uasId,"bearingToWaypoint","M",QVariant(val),getUnixTime()); + } + + double getBearingToWaypoint() const + { + return bearingToWaypoint; + } + + void setRoll(double val) { roll = val; @@ -258,6 +269,7 @@ public: { return yaw; } + bool getSelected() const; QVector3D getNedPosGlobalOffset() const { @@ -331,31 +343,43 @@ public: friend class UASWaypointManager; protected: //COMMENTS FOR TEST UNIT + /// LINK ID AND STATUS int uasId; ///< Unique system ID - unsigned char type; ///< UAS type (from type enum) - quint64 startTime; ///< The time the UAS was switched on - CommStatus commStatus; ///< Communication status - QString name; ///< Human-friendly name of the vehicle, e.g. bravo - int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + QMap components;///< IDs and names of all detected onboard components QList* links; ///< List of links this UAS can be reached by QList unknownPackets; ///< Packet IDs which are unknown and have been received MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance - BatteryType batteryType; ///< The battery type - int cells; ///< Number of cells - - UASWaypointManager waypointManager; + CommStatus commStatus; ///< Communication status + float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs) + float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS + quint64 lastHeartbeat; ///< Time of the last heartbeat message + QTimer* statusTimeout; ///< Timer for various status timeouts + /// BASIC UAS TYPE, NAME AND STATE + QString name; ///< Human-friendly name of the vehicle, e.g. bravo + unsigned char type; ///< UAS type (from type enum) + int airframe; ///< The airframe type + int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + bool systemIsArmed; ///< If the system is armed + uint8_t mode; ///< The current mode of the MAV + uint32_t custom_mode; ///< The current mode of the MAV + uint32_t navMode; ///< The current navigation mode of the MAV + int status; ///< The current status of the MAV + QString shortModeText; ///< Short textual mode description + QString shortStateText; ///< Short textual state description + + /// OUTPUT QList actuatorValues; QList actuatorNames; - QList motorValues; QList motorNames; - QMap components; ///< IDs and names of all detected onboard components - double thrustSum; ///< Sum of forward/up thrust of all thrust actuators, in Newtons double thrustMax; ///< Maximum forward/up thrust of this vehicle, in Newtons - // Battery stats + // dongfang: This looks like a candidate for being moved off to a separate class. + /// BATTERY / ENERGY + BatteryType batteryType; ///< The battery type + int cells; ///< Number of cells float fullVoltage; ///< Voltage of the fully charged battery (100%) float emptyVoltage; ///< Voltage of the empty battery (0%) float startVoltage; ///< Voltage at system start @@ -366,15 +390,18 @@ 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 - uint8_t mode; ///< The current mode of the MAV - uint32_t custom_mode; ///< The current mode of the MAV - int status; ///< The current status of the MAV - uint32_t navMode; ///< The current navigation mode of the MAV + bool lowBattAlarm; ///< Switch if battery is low + + + /// TIMEKEEPING + quint64 startTime; ///< The time the UAS was switched on quint64 onboardTimeOffset; + /// MANUAL CONTROL bool controlRollManual; ///< status flag, true if roll is controlled manually bool controlPitchManual; ///< status flag, true if pitch is controlled manually bool controlYawManual; ///< status flag, true if yaw is controlled manually @@ -384,16 +411,20 @@ protected: //COMMENTS FOR TEST UNIT double manualPitchAngle; ///< Pitch angle set by human pilot (radians) double manualYawAngle; ///< Yaw angle set by human pilot (radians) double manualThrust; ///< Thrust set by human pilot (radians) - float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs) - float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS - bool lowBattAlarm; ///< Switch if battery is low + + /// POSITION bool positionLock; ///< Status if position information is available or not + bool isLocalPositionKnown; ///< If the local position has been received for this MAV + bool isGlobalPositionKnown; ///< If the global position has been received for this MAV + double localX; double localY; double localZ; + double latitude; ///< Global latitude as estimated by position estimator double longitude; ///< Global longitude as estimated by position estimator double altitude; ///< Global altitude as estimated by position estimator + double satelliteCount; ///< Number of satellites visible to raw GPS bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position double latitude_gps; ///< Global latitude as estimated by raw GPS @@ -402,14 +433,26 @@ protected: //COMMENTS FOR TEST UNIT double speedX; ///< True speed in X axis double speedY; ///< True speed in Y axis double speedZ; ///< True speed in Z axis + + QVector3D nedPosGlobalOffset; ///< Offset between the system's NED position measurements and the swarm / global 0/0/0 origin + QVector3D nedAttGlobalOffset; ///< Offset between the system's NED position measurements and the swarm / global 0/0/0 origin + + /// WAYPOINT NAVIGATION double distToWaypoint; ///< Distance to next waypoint double groundSpeed; ///< GPS Groundspeed + double bearingToWaypoint; ///< Bearing to next waypoint + UASWaypointManager waypointManager; + + /// ATTITUDE + bool attitudeKnown; ///< True if attitude was received, false else + bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV + quint64 lastAttitude; ///< Timestamp of last attitude measurement double roll; double pitch; double yaw; - quint64 lastHeartbeat; ///< Time of the last heartbeat message - QTimer* statusTimeout; ///< Timer for various status timeouts + // dongfang: This looks like a candidate for being moved off to a separate class. + /// IMAGING int imageSize; ///< Image size being transmitted (bytes) int imagePackets; ///< Number of data packets being sent for this image int imagePacketsArrived; ///< Number of data packets recieved @@ -444,21 +487,13 @@ protected: //COMMENTS FOR TEST UNIT qreal receivedRGBDImageTimestamp; #endif + /// PARAMETERS QMap* > parameters; ///< All parameters bool paramsOnceRequested; ///< If the parameter list has been read at least once - int airframe; ///< The airframe type - bool attitudeKnown; ///< True if attitude was received, false else QGCUASParamManager* paramManager; ///< Parameter manager class - QString shortStateText; ///< Short textual state description - QString shortModeText; ///< Short textual mode description - bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV - quint64 lastAttitude; ///< Timestamp of last attitude measurement + + /// SIMULATION QGCHilLink* simulation; ///< Hardware in the loop simulation link - bool isLocalPositionKnown; ///< If the local position has been received for this MAV - bool isGlobalPositionKnown; ///< If the global position has been received for this MAV - bool systemIsArmed; ///< If the system is armed - QVector3D nedPosGlobalOffset; ///< Offset between the system's NED position measurements and the swarm / global 0/0/0 origin - QVector3D nedAttGlobalOffset; ///< Offset between the system's NED position measurements and the swarm / global 0/0/0 origin public: /** @brief Set the current battery type */ @@ -659,12 +694,12 @@ public slots: void enableHilXPlane(bool enable); /** @brief Send the full HIL state to the MAV */ - void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, double lat, double lon, double alt, + void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, + float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt, float vx, float vy, float vz, float xacc, float yacc, float zacc); /** @brief RAW sensors for sensor HIL */ - void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, + void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollRotationRate, float pitchRotationRate, float yawRotationRate, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed); /** @@ -823,16 +858,17 @@ signals: void longitudeChanged(double val,QString name); void latitudeChanged(double val,QString name); void altitudeChanged(double val,QString name); - void altitudeChanged(int uasid, double altitude); void rollChanged(double val,QString name); void pitchChanged(double val,QString name); void yawChanged(double val,QString name); void satelliteCountChanged(double val,QString name); void distToWaypointChanged(double val,QString name); void groundSpeedChanged(double val, QString name); + void bearingToWaypointChanged(double val,QString name); - - + //void primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec); + //void gpsAltitudeChanged(UASInterface*, double altitude, quint64 usec); + //void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec); protected: /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */ quint64 getUnixTime(quint64 time=0); @@ -840,6 +876,7 @@ protected: quint64 getUnixTimeFromMs(quint64 time); /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ quint64 getUnixReferenceTime(quint64 time); + int componentID[256]; bool componentMulti[256]; bool connectionLost; ///< Flag indicates a timed out connection diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 06a03b70a893929e0a9adc373a429f0f856fca06..83f07f7497dd97faf8e9256048eece0b6ab73fcb 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -51,6 +51,42 @@ This file is part of the QGROUNDCONTROL project #endif #endif +enum BatteryType +{ + NICD = 0, + NIMH = 1, + LIION = 2, + LIPOLY = 3, + LIFE = 4, + AGZN = 5 +}; ///< The type of battery used + +/* +enum SpeedMeasurementSource +{ + PRIMARY_SPEED = 0, // ArduPlane: Measured airspeed or estimated airspeed. ArduCopter: Ground (XY) speed. + GROUNDSPEED_BY_UAV = 1, // Ground speed as reported by UAS + GROUNDSPEED_BY_GPS = 2, // Ground speed as calculated from received GPS velocity data + LOCAL_SPEED = 3 +}; ///< For velocity data, the data source + +enum AltitudeMeasurementSource +{ + PRIMARY_ALTITUDE = 0, // ArduPlane: air and ground speed mix. This is the altitude used for navigastion. + BAROMETRIC_ALTITUDE = 1, // Altitude is pressure altitude. Ardupilot reports no altitude purely by barometer, + // however when ALT_MIX==1, mix-altitude is purely barometric. + GPS_ALTITUDE = 2 // GPS ASL altitude +}; ///< For altitude data, the data source + +// TODO!!! The different frames are probably represented elsewhere. There should really only +// be one set of frames. We also need to keep track of the home alt. somehow. +enum AltitudeMeasurementFrame +{ + ABSOLUTE = 0, // Altitude is pressure altitude + ABOVE_HOME_POSITION = 1 +}; ///< For altitude data, a reference frame +*/ + /** * @brief Interface for all robots. * @@ -470,14 +506,14 @@ 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); void heartbeat(UASInterface* uas); void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec); void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec); - void attitudeSpeedChanged(int uas, double rollspeed, double pitchspeed, double yawspeed, quint64 usec); + void attitudeRotationRatesChanged(int uas, double rollrate, double pitchrate, double yawrate, quint64 usec); void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); /** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */ void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec); @@ -486,10 +522,21 @@ signals: void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); void localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec); void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec); - void altitudeChanged(int uasid, double altitude); + void primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec); + void gpsAltitudeChanged(UASInterface*, double altitude, quint64 usec); /** @brief Update the status of one satellite used for localization */ void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used); - void speedChanged(UASInterface*, double x, double y, double z, quint64 usec); + + // The horizontal speed (a scalar) + void primarySpeedChanged(UASInterface*, double speed, quint64 usec); + void gpsSpeedChanged(UASInterface*, double speed, quint64 usec); + // The vertical speed (a scalar) + void climbRateChanged(UASInterface*, double climb, quint64 usec); + // Consider adding a MAV_FRAME parameter to this; could help specifying what the 3 scalars are. + void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec); + + void navigationControllerErrorsChanged(UASInterface*, double altitudeError, double speedError, double xtrackError); + void imageStarted(int imgid, int width, int height, int depth, int channels); void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex); /** @brief Emit the new system type */ diff --git a/src/uas/senseSoarMAV.cpp b/src/uas/senseSoarMAV.cpp index 031eeb3e9c6b204c202335103818fdf1520eab73..fc1ad320310d8d65e609c08e9c8b6f9a8ee9edc7 100644 --- a/src/uas/senseSoarMAV.cpp +++ b/src/uas/senseSoarMAV.cpp @@ -41,7 +41,7 @@ void senseSoarMAV::receiveMessage(LinkInterface *link, mavlink_message_t message emit valueChanged(uasId, "rollspeed", "rad/s", this->m_rotVel[0], time); emit valueChanged(uasId, "pitchspeed", "rad/s", this->m_rotVel[1], time); emit valueChanged(uasId, "yawspeed", "rad/s", this->m_rotVel[2], time); - emit attitudeSpeedChanged(uasId, this->m_rotVel[0], this->m_rotVel[1], this->m_rotVel[2], time); + emit attitudeRotationRatesChanged(uasId, this->m_rotVel[0], this->m_rotVel[1], this->m_rotVel[2], time); break; } case MAVLINK_MSG_ID_LLC_OUT: // low level controller output @@ -62,7 +62,7 @@ void senseSoarMAV::receiveMessage(LinkInterface *link, mavlink_message_t message mavlink_obs_air_temp_t airTMsg; mavlink_msg_obs_air_temp_decode(&message,&airTMsg); quint64 time = getUnixTime(); - emit valueChanged(uasId, "Air Temp", "°", airTMsg.airT, time); + emit valueChanged(uasId, "Air Temp", "°", airTMsg.airT, time); break; } case MAVLINK_MSG_ID_OBS_AIR_VELOCITY: @@ -211,4 +211,4 @@ void senseSoarMAV::quat2euler(const double *quat, double &roll, double &pitch, d pitch = std::asin(qMax(-1.0,qMin(1.0,2*(quat[0]*quat[2] - quat[1]*quat[3])))); yaw = std::atan2(2*(quat[1]*quat[2] + quat[0]*quat[3]),quat[0]*quat[0] + quat[1]*quat[1] - quat[2]*quat[2] - quat[3]*quat[3]); return; -} \ No newline at end of file +} diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 506561a2606ea841a69a30ab1babe785951d6462..d18ebc038f79757c4f4a449e93b4cce69fe9e51e 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -178,7 +178,10 @@ HSIDisplay::HSIDisplay(QWidget *parent) : connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage())); statusClearTimer.start(3000); - setActiveUAS(UASManager::instance()->getActiveUAS()); + if (UASManager::instance()->getActiveUAS()) + { + setActiveUAS(UASManager::instance()->getActiveUAS()); + } setFocusPolicy(Qt::StrongFocus); } @@ -864,7 +867,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float))); - disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool))); @@ -895,7 +898,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float))); - connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool))); diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 9ddd90027dfc31057c08ce203e287df365d6744e..e4556ff62afa3929e9df3730f47d75c09c6f08d0 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -119,7 +119,7 @@ HUD::HUD(int width, int height, QWidget* parent) load(0.0f), offlineDirectory(""), nextOfflineImage(""), - hudInstrumentsEnabled(true), + HUDInstrumentsEnabled(true), videoEnabled(false), xImageFactor(1.0), yImageFactor(1.0), @@ -218,7 +218,7 @@ void HUD::contextMenuEvent (QContextMenuEvent* event) { QMenu menu(this); // Update actions - enableHUDAction->setChecked(hudInstrumentsEnabled); + enableHUDAction->setChecked(HUDInstrumentsEnabled); enableVideoAction->setChecked(videoEnabled); menu.addAction(enableHUDAction); @@ -234,7 +234,7 @@ void HUD::createActions() enableHUDAction = new QAction(tr("Enable HUD"), this); enableHUDAction->setStatusTip(tr("Show the HUD instruments in this window")); enableHUDAction->setCheckable(true); - enableHUDAction->setChecked(hudInstrumentsEnabled); + enableHUDAction->setChecked(HUDInstrumentsEnabled); connect(enableHUDAction, SIGNAL(triggered(bool)), this, SLOT(enableHUDInstruments(bool))); enableVideoAction = new QAction(tr("Enable Video Live feed"), this); @@ -261,16 +261,16 @@ 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*))); disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(velocityChanged_NEDspeedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); // Try to disconnect the image link @@ -286,14 +286,14 @@ 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*))); connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); // Try to connect the image link @@ -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; @@ -711,7 +712,7 @@ void HUD::paintHUD() // END OF OPENGL PAINTING - if (hudInstrumentsEnabled) { + if (HUDInstrumentsEnabled) { //glEnable(GL_MULTISAMPLE); @@ -1459,7 +1460,7 @@ void HUD::selectOfflineDirectory() void HUD::enableHUDInstruments(bool enabled) { - hudInstrumentsEnabled = enabled; + HUDInstrumentsEnabled = enabled; } void HUD::enableVideo(bool enabled) @@ -1509,7 +1510,7 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s void HUD::copyImage() { - if (isVisible() && hudInstrumentsEnabled) + if (isVisible() && HUDInstrumentsEnabled) { //qDebug() << "HUD::copyImage()"; UAS* u = dynamic_cast(this->uas); diff --git a/src/ui/HUD.h b/src/ui/HUD.h index 92c3d30e7e89f82479ea94929ca953b5ab201fbb..e900d8d7901221364d857d549457b27d3cbc5f2e 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -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); @@ -184,7 +184,7 @@ protected: int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate QTimer* refreshTimer; ///< The main timer, controls the update rate - QPainter* hudPainter; + QPainter* HUDPainter; QFont font; ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system) bool noCamera; ///< No camera images available, draw the ground/sky box to indicate the horizon @@ -218,7 +218,7 @@ protected: float load; QString offlineDirectory; QString nextOfflineImage; - bool hudInstrumentsEnabled; + bool HUDInstrumentsEnabled; bool videoEnabled; bool dataStreamEnabled; bool imageLoggingEnabled; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index d05ab14bbf7613f4e7bcb3b0f9ca00cb7128e84e..bcf55e1bbdd4ae187af43f1a22e578e95ca2a1b2 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -62,6 +62,8 @@ This file is part of the QGROUNDCONTROL project #include "UASActionsWidget.h" #include "QGCTabbedInfoView.h" #include "UASRawStatusView.h" +#include "PrimaryFlightDisplay.h" + #ifdef QGC_OSG_ENABLED #include "Q3DWidgetFactory.h" #endif @@ -357,7 +359,6 @@ QString MainWindow::getWindowStateKey() return QString::number(currentView)+"_windowstate_" + UASManager::instance()->getActiveUAS()->getAutopilotTypeName(); } else - return QString::number(currentView)+"_windowstate"; } @@ -589,11 +590,11 @@ void MainWindow::buildCommonWidgets() connect(tempAction,SIGNAL(triggered(bool)),this, SLOT(showTool(bool))); } - - createDockWidget(simView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea,this->width()/1.5); + // createDockWidget(simView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea,this->width()/1.5); + createDockWidget(simView,new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea,this->width()/1.5); createDockWidget(pilotView,new UASListWidget(this),tr("Unmanned Systems"),"UNMANNED_SYSTEM_LIST_DOCKWIDGET",VIEW_FLIGHT,Qt::RightDockWidgetArea); - createDockWidget(pilotView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea,this->width()/1.8); +// createDockWidget(pilotView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea,this->width()/1.8); //createDockWidget(pilotView,new UASQuickView(this),tr("Quick View"),"UAS_INFO_QUICKVIEW_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea); //UASQuickView *quickview = new UASQuickView(this); @@ -607,6 +608,14 @@ void MainWindow::buildCommonWidgets() createDockWidget(pilotView,infoview,tr("Info View"),"UAS_INFO_INFOVIEW_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea); + //createDockWidget(pilotView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea,this->width()/1.8); + createDockWidget(pilotView,new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea,this->width()/1.8); + +// createDockWidget(pilotView,new UASQuickView(this),tr("Quick View"),"UAS_INFO_QUICKVIEW_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea); +// createDockWidget(pilotView,new HSIDisplay(this),tr("Horizontal Situation"),"HORIZONTAL_SITUATION_INDICATOR_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea); +// pilotView->setTabPosition(Qt::LeftDockWidgetArea,QTabWidget::North); +// pilotView->tabifyDockWidget((QDockWidget*)centralWidgetToDockWidgetsMap[VIEW_FLIGHT]["HORIZONTAL_SITUATION_INDICATOR_DOCKWIDGET"],(QDockWidget*)centralWidgetToDockWidgetsMap[VIEW_FLIGHT]["UAS_INFO_QUICKVIEW_DOCKWIDGET"]); + //UASRawStatusView *view = new UASRawStatusView(); //view->setDecoder(mavlinkDecoder); //view->show(); @@ -764,7 +773,12 @@ void MainWindow::loadDockWidget(QString name) { return; } - if (name == "UNMANNED_SYSTEM_CONTROL_DOCKWIDGET") + if (name.startsWith("HIL_CONFIG")) + { + //It's a HIL widget. + showHILConfigurationWidget(UASManager::instance()->getActiveUAS()); + } + else if (name == "UNMANNED_SYSTEM_CONTROL_DOCKWIDGET") { createDockWidget(centerStack->currentWidget(),new UASControlWidget(this),tr("Control"),"UNMANNED_SYSTEM_CONTROL_DOCKWIDGET",currentView,Qt::LeftDockWidgetArea); } @@ -823,9 +837,10 @@ void MainWindow::loadDockWidget(QString name) qDebug() << "Error loading window:" << name << "Unknown window type"; //createDockWidget(centerStack->currentWidget(),hddisplay,tr("Actuator Status"),"HEADS_DOWN_DISPLAY_2_DOCKWIDGET",currentView,Qt::RightDockWidgetArea); } - else if (name == "HEAD_UP_DISPLAY_DOCKWIDGET") + else if (name == "PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET") { - createDockWidget(centerStack->currentWidget(),new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea); + // createDockWidget(centerStack->currentWidget(),new HUD(320,240,this),tr("Head Up Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea); + createDockWidget(centerStack->currentWidget(),new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea); } else if (name == "UAS_INFO_QUICKVIEW_DOCKWIDGET") { @@ -921,8 +936,11 @@ void MainWindow::showHILConfigurationWidget(UASInterface* uas) if (mav && !hilDocks.contains(mav->getUASID())) { + //QGCToolWidget* tool = new QGCToolWidget("Unnamed Tool " + QString::number(ui.menuTools->actions().size())); + //createDockWidget(centerStack->currentWidget(),tool,"Unnamed Tool " + QString::number(ui.menuTools->actions().size()),"UNNAMED_TOOL_" + QString::number(ui.menuTools->actions().size())+"DOCK",currentView,Qt::BottomDockWidgetArea); + QGCHilConfiguration* hconf = new QGCHilConfiguration(mav, this); - QString hilDockName = tr("HIL Config %1").arg(uas->getUASName()); + QString hilDockName = tr("HIL Config %1").arg(uas->getUASName()); QDockWidget* hilDock = createDockWidget(simView, hconf,hilDockName, hilDockName.toUpper().replace(" ", "_"),VIEW_SIMULATION,Qt::LeftDockWidgetArea); hilDocks.insert(mav->getUASID(), hilDock); @@ -936,8 +954,8 @@ void MainWindow::showHILConfigurationWidget(UASInterface* uas) void MainWindow::closeEvent(QCloseEvent *event) { if (isVisible()) storeViewState(); - storeSettings(); aboutToCloseFlag = true; + storeSettings(); mavlink->storeSettings(); UASManager::instance()->storeSettings(); QMainWindow::closeEvent(event); diff --git a/src/ui/PrimaryFlightDisplay.cpp b/src/ui/PrimaryFlightDisplay.cpp new file mode 100644 index 0000000000000000000000000000000000000000..de82c6e674c7f954b05f7231665407f9fcf11d8a --- /dev/null +++ b/src/ui/PrimaryFlightDisplay.cpp @@ -0,0 +1,1488 @@ +#include "PrimaryFlightDisplay.h" +#include "UASManager.h" + +//#include "ui_primaryflightdisplay.h" +#include +#include +#include +#include +#include +#include +#include +#include +//#include + +/* + *@TODO: + * global fixed pens (and painters too?) + * repaint on demand multiple canvases + * multi implementation with shared model class + */ +double PrimaryFlightDisplay_round(double value, int digits=0) +{ + return floor(value * pow(10, digits) + 0.5) / pow(10, digits); +} + +const int PrimaryFlightDisplay::tickValues[] = {10, 20, 30, 45, 60}; +const QString PrimaryFlightDisplay::compassWindNames[] = { + QString("N"), + QString("NE"), + QString("E"), + QString("SE"), + QString("S"), + QString("SW"), + QString("W"), + QString("NW") +}; + +PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *parent) : + QWidget(parent), + + uas(NULL), + + altimeterMode(GPS_MAIN), + altimeterFrame(ASL), + speedMode(GROUND_MAIN), + + roll(UNKNOWN_ATTITUDE), + pitch(UNKNOWN_ATTITUDE), + // heading(NAN), + heading(UNKNOWN_ATTITUDE), + primaryAltitude(UNKNOWN_ALTITUDE), + GPSAltitude(UNKNOWN_ALTITUDE), + aboveHomeAltitude(UNKNOWN_ALTITUDE), + primarySpeed(UNKNOWN_SPEED), + groundspeed(UNKNOWN_SPEED), + verticalVelocity(UNKNOWN_ALTITUDE), + + font("Bitstream Vera Sans"), + refreshTimer(new QTimer(this)), + + navigationCrosstrackError(INFINITY), + navigationTargetBearing(UNKNOWN_ATTITUDE), + + layout(COMPASS_INTEGRATED), + style(OVERLAY_HSI), + + redColor(QColor::fromHsvF(0, 0.75, 0.9)), + amberColor(QColor::fromHsvF(0.12, 0.6, 1.0)), + greenColor(QColor::fromHsvF(0.25, 0.8, 0.8)), + + lineWidth(2), + fineLineWidth(1), + + instrumentEdgePen(QColor::fromHsvF(0, 0, 0.65, 0.5)), + // AIEdgePen(QColor::fromHsvF(0, 0, 0.65, 0.5)), + + instrumentBackground(QColor::fromHsvF(0, 0, 0.3, 0.3)), + instrumentOpagueBackground(QColor::fromHsvF(0, 0, 0.3, 1.0)) +{ + setMinimumSize(120, 80); + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + // Connect with UAS + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + if (UASManager::instance()->getActiveUAS() != NULL) setActiveUAS(UASManager::instance()->getActiveUAS()); + + // Refresh timer + refreshTimer->setInterval(updateInterval); + // connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD())); + connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update())); +} + +PrimaryFlightDisplay::~PrimaryFlightDisplay() +{ + refreshTimer->stop(); +} + + +QSize PrimaryFlightDisplay::sizeHint() const +{ + return QSize(width(), (width()*3.0f)/4); +} + +void PrimaryFlightDisplay::showEvent(QShowEvent* event) +{ + // React only to internal (pre-display) + // events + QWidget::showEvent(event); + refreshTimer->start(updateInterval); + emit visibilityChanged(true); +} + +void PrimaryFlightDisplay::hideEvent(QHideEvent* event) +{ + // React only to internal (pre-display) + // events + refreshTimer->stop(); + QWidget::hideEvent(event); + emit visibilityChanged(false); +} + +qreal constrain(qreal value, qreal min, qreal max) { + if (valuemax) value=max; + return value; +} + +void PrimaryFlightDisplay::resizeEvent(QResizeEvent *e) { + QWidget::resizeEvent(e); + + qreal size = e->size().width(); + //if(e->size().height()size().height(); + + lineWidth = constrain(size*LINEWIDTH, 1, 6); + fineLineWidth = constrain(size*LINEWIDTH*2/3, 1, 2); + + instrumentEdgePen.setWidthF(fineLineWidth); + //AIEdgePen.setWidthF(fineLineWidth); + + smallTestSize = size * SMALL_TEXT_SIZE; + mediumTextSize = size * MEDIUM_TEXT_SIZE; + largeTextSize = size * LARGE_TEXT_SIZE; + + /* + * Try without layout Change-O-Matic. It was too complicated. + qreal aspect = e->size().width() / e->size().height(); + if (aspect <= SEPARATE_COMPASS_ASPECTRATIO) + layout = COMPASS_SEPARATED; + else + layout = COMPASS_INTEGRATED; + */ + // qDebug("Width %d height %d decision %d", e->size().width(), e->size().height(), layout); +} + +void PrimaryFlightDisplay::paintEvent(QPaintEvent *event) +{ + // Event is not needed + // the event is ignored as this widget + // is refreshed automatically + Q_UNUSED(event); + //makeDummyData(); + doPaint(); +} + +/* + * Interface towards qgroundcontrol + */ +/** + * + * @param uas the UAS/MAV to monitor/display with the HUD + */ +void PrimaryFlightDisplay::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(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); + disconnect(this->uas, SIGNAL(primarySpeedChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimarySpeed(UASInterface*,double,quint64))); + disconnect(this->uas, SIGNAL(gpsSpeedChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSSpeed(UASInterface*,double,quint64))); + disconnect(this->uas, SIGNAL(climbRateChanged(UASInterface*, double, quint64)), this, SLOT(updateClimbRate(UASInterface*, AltitudeMeasurementSource, double, quint64))); + disconnect(this->uas, SIGNAL(primaryAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimaryAltitude(UASInterface*, double, quint64))); + disconnect(this->uas, SIGNAL(gpsAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSAltitude(UASInterface*, double, quint64))); + disconnect(this->uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double))); + + //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*))); + //disconnect(this->uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool))); + //disconnect(this->uas, SIGNAL(satelliteCountChanged(double, QString)), this, SLOT(updateSatelliteCount(double, QString))); + //disconnect(this->uas, SIGNAL(localizationChanged(UASInterface* uas, int fix)), this, SLOT(updateGPSFixType(UASInterface*,int))); + } + + if (uas) { + // Now connect the new 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, 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*))); + //connect(uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool))); + //connect(uas, SIGNAL(satelliteCountChanged(double, QString)), this, SLOT(updateSatelliteCount(double, QString))); + + //connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + //connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); + connect(uas, SIGNAL(primarySpeedChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimarySpeed(UASInterface*,double,quint64))); + connect(uas, SIGNAL(gpsSpeedChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSSpeed(UASInterface*,double,quint64))); + connect(uas, SIGNAL(climbRateChanged(UASInterface*, double, quint64)), this, SLOT(updateClimbRate(UASInterface*, AltitudeMeasurementSource, double, quint64))); + connect(uas, SIGNAL(primaryAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimaryAltitude(UASInterface*, double, quint64))); + connect(uas, SIGNAL(gpsAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSAltitude(UASInterface*, double, quint64))); + connect(uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double))); + + // Set new UAS + this->uas = uas; + } +} + +void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp) +{ + Q_UNUSED(uas); + Q_UNUSED(timestamp); + if (!isnan(roll) && !isinf(roll) && !isnan(pitch) && !isinf(pitch) && !isnan(yaw) && !isinf(yaw)) + { + // TODO: Units conversion? // Called from UAS.cc l. 646 + this->roll = roll * (180.0 / M_PI); + this->pitch = pitch * (180.0 / M_PI); + yaw = yaw * (180.0 / M_PI); + if (yaw<0) yaw+=360; + this->heading = yaw; + } + // TODO: Else-part. We really should have an "attitude bad or unknown" indication instead of just freezing. + + //qDebug("r,p,y: %f,%f,%f", roll, pitch, yaw); +} + +/* + * TODO! Implementation or removal of this. + * Currently a dummy. + */ +void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp) +{ + Q_UNUSED(uas); + Q_UNUSED(component); + Q_UNUSED(timestamp); + // Called from UAS.cc l. 616 + if (!isnan(roll) && !isinf(roll) && !isnan(pitch) && !isinf(pitch) && !isnan(yaw) && !isinf(yaw)) { + this->roll = roll * (180.0 / M_PI); + this->pitch = pitch * (180.0 / M_PI); + yaw = yaw * (180.0 / M_PI); + if (yaw<0) yaw+=360; + this->heading = yaw; + } + // qDebug("(2) r,p,y: %f,%f,%f", roll, pitch, yaw); + +} + +/* + * TODO! Examine what data comes with this call, should we consider it airspeed, ground speed or + * should we not consider it at all? + */ +void PrimaryFlightDisplay::updatePrimarySpeed(UASInterface* uas, double speed, quint64 timestamp) +{ + Q_UNUSED(uas); + Q_UNUSED(timestamp); + + primarySpeed = speed; + didReceivePrimarySpeed = true; +} + +void PrimaryFlightDisplay::updateGPSSpeed(UASInterface* uas, double speed, quint64 timestamp) +{ + Q_UNUSED(uas); + Q_UNUSED(timestamp); + + groundspeed = speed; + if (!didReceivePrimarySpeed) + primarySpeed = speed; +} + +void PrimaryFlightDisplay::updateClimbRate(UASInterface* uas, double climbRate, quint64 timestamp) { + Q_UNUSED(uas); + Q_UNUSED(timestamp); + verticalVelocity = climbRate; +} + +void PrimaryFlightDisplay::updatePrimaryAltitude(UASInterface* uas, double altitude, quint64 timestamp) { + Q_UNUSED(uas); + Q_UNUSED(timestamp); + primaryAltitude = altitude; + didReceivePrimaryAltitude = true; +} + +void PrimaryFlightDisplay::updateGPSAltitude(UASInterface* uas, double altitude, quint64 timestamp) { + Q_UNUSED(uas); + Q_UNUSED(timestamp); + GPSAltitude = altitude; + if (!didReceivePrimaryAltitude) + primaryAltitude = altitude; +} + +void PrimaryFlightDisplay::updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError) { + Q_UNUSED(uas); + this->navigationAltitudeError = altitudeError; + this->navigationSpeedError = speedError; + this->navigationCrosstrackError = xtrackError; +} + + +/* + * Private and such + */ + +// TODO: Move to UAS. Real working implementation. +bool PrimaryFlightDisplay::isAirplane() { + if (!this->uas) + return false; + switch(this->uas->getSystemType()) { + case MAV_TYPE_GENERIC: + case MAV_TYPE_FIXED_WING: + case MAV_TYPE_AIRSHIP: + case MAV_TYPE_FLAPPING_WING: + return true; + default: + return false; + } +} + +// TODO: Implement. Should return true when navigating. +// That would be (APM) in AUTO and RTL modes. +// This could forward to a virtual on UAS bool isNavigatingAutonomusly() or whatever. +bool PrimaryFlightDisplay::shouldDisplayNavigationData() { + return true; +} + +void PrimaryFlightDisplay::drawTextCenter ( + QPainter& painter, + QString text, + float pixelSize, + float x, + float y) +{ + font.setPixelSize(pixelSize); + painter.setFont(font); + + QFontMetrics metrics = QFontMetrics(font); + QRect bounds = metrics.boundingRect(text); + int flags = Qt::AlignCenter | Qt::TextDontClip; // For some reason the bounds rect is too small! + painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text); +} + +void PrimaryFlightDisplay::drawTextLeftCenter ( + QPainter& painter, + QString text, + float pixelSize, + float x, + float y) +{ + font.setPixelSize(pixelSize); + painter.setFont(font); + + QFontMetrics metrics = QFontMetrics(font); + QRect bounds = metrics.boundingRect(text); + int flags = Qt::AlignLeft | Qt::TextDontClip; // For some reason the bounds rect is too small! + painter.drawText(x /*+bounds.x()*/, y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text); +} + +void PrimaryFlightDisplay::drawTextRightCenter ( + QPainter& painter, + QString text, + float pixelSize, + float x, + float y) +{ + font.setPixelSize(pixelSize); + painter.setFont(font); + + QFontMetrics metrics = QFontMetrics(font); + QRect bounds = metrics.boundingRect(text); + int flags = Qt::AlignRight | Qt::TextDontClip; // For some reason the bounds rect is too small! + painter.drawText(x /*+bounds.x()*/ -bounds.width(), y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text); +} + +void PrimaryFlightDisplay::drawTextCenterTop ( + QPainter& painter, + QString text, + float pixelSize, + float x, + float y) +{ + font.setPixelSize(pixelSize); + painter.setFont(font); + + QFontMetrics metrics = QFontMetrics(font); + QRect bounds = metrics.boundingRect(text); + int flags = Qt::AlignCenter | Qt::TextDontClip; // For some reason the bounds rect is too small! + painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y+bounds.height() /*+bounds.y()*/, bounds.width(), bounds.height(), flags, text); +} + +void PrimaryFlightDisplay::drawTextCenterBottom ( + QPainter& painter, + QString text, + float pixelSize, + float x, + float y) +{ + font.setPixelSize(pixelSize); + painter.setFont(font); + + QFontMetrics metrics = QFontMetrics(font); + QRect bounds = metrics.boundingRect(text); + int flags = Qt::AlignCenter; + painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y /*+bounds.y()*/, bounds.width(), bounds.height(), flags, text); +} + +void PrimaryFlightDisplay::drawInstrumentBackground(QPainter& painter, QRectF edge) { + painter.setPen(instrumentEdgePen); + painter.drawRect(edge); +} + +void PrimaryFlightDisplay::fillInstrumentBackground(QPainter& painter, QRectF edge) { + painter.setPen(instrumentEdgePen); + painter.setBrush(instrumentBackground); + painter.drawRect(edge); + painter.setBrush(Qt::NoBrush); +} + +void PrimaryFlightDisplay::fillInstrumentOpagueBackground(QPainter& painter, QRectF edge) { + painter.setPen(instrumentEdgePen); + painter.setBrush(instrumentOpagueBackground); + painter.drawRect(edge); + painter.setBrush(Qt::NoBrush); +} + +qreal pitchAngleToTranslation(qreal viewHeight, float pitch) { + return pitch * viewHeight / PITCHTRANSLATION; +} + +void PrimaryFlightDisplay::drawAIAirframeFixedFeatures(QPainter& painter, QRectF area) { + // red line from -7/10 to -5/10 half-width + // red line from 7/10 to 5/10 half-width + // red slanted line from -2/10 half-width to 0 + // red slanted line from 2/10 half-width to 0 + // red arrow thing under roll scale + // prepareTransform(painter, width, height); + painter.resetTransform(); + painter.translate(area.center()); + + qreal w = area.width(); + qreal h = area.height(); + + QPen pen; + pen.setWidthF(lineWidth * 1.5); + pen.setColor(redColor); + painter.setPen(pen); + + float length = 0.15; + float side = 0.5; + // The 2 lines at sides. + painter.drawLine(QPointF(-side*w, 0), QPointF(-(side-length)*w, 0)); + painter.drawLine(QPointF(side*w, 0), QPointF((side-length)*w, 0)); + + float rel = length/qSqrt(2); + // The gull + painter.drawLine(QPointF(rel*w, rel*w/2), QPoint(0, 0)); + painter.drawLine(QPointF(-rel*w, rel*w/2), QPoint(0, 0)); + + // The roll scale marker. + QPainterPath markerPath(QPointF(0, -w*ROLL_SCALE_RADIUS+1)); + markerPath.lineTo(-h*ROLL_SCALE_MARKERWIDTH/2, -w*(ROLL_SCALE_RADIUS-ROLL_SCALE_MARKERHEIGHT)+1); + markerPath.lineTo(h*ROLL_SCALE_MARKERWIDTH/2, -w*(ROLL_SCALE_RADIUS-ROLL_SCALE_MARKERHEIGHT)+1); + markerPath.closeSubpath(); + painter.drawPath(markerPath); +} + +inline qreal min4(qreal a, qreal b, qreal c, qreal d) { + if(ba) a=b; + if(c>a) a=c; + if(d>a) a=d; + return a; +} + +void PrimaryFlightDisplay::drawAIGlobalFeatures( + QPainter& painter, + QRectF mainArea, + QRectF paintArea) { + + painter.resetTransform(); + painter.translate(mainArea.center()); + + qreal pitchPixels = pitchAngleToTranslation(mainArea.height(), pitch); + qreal gradientEnd = pitchAngleToTranslation(mainArea.height(), 60); + + //painter.rotate(-roll); + //painter.translate(0, pitchPixels); + + // QTransform forwardTransform; + //forwardTransform.translate(mainArea.center().x(), mainArea.center().y()); + painter.rotate(-roll); + painter.translate(0, pitchPixels); + + // Calculate the radius of area we need to paint to cover all. + QTransform rtx = painter.transform().inverted(); + + QPointF topLeft = rtx.map(paintArea.topLeft()); + QPointF topRight = rtx.map(paintArea.topRight()); + QPointF bottomLeft = rtx.map(paintArea.bottomLeft()); + QPointF bottomRight = rtx.map(paintArea.bottomRight()); + // Just KISS... make a rectangluar basis. + qreal minx = min4(topLeft.x(), topRight.x(), bottomLeft.x(), bottomRight.x()); + qreal maxx = max4(topLeft.x(), topRight.x(), bottomLeft.x(), bottomRight.x()); + qreal miny = min4(topLeft.y(), topRight.y(), bottomLeft.y(), bottomRight.y()); + qreal maxy = max4(topLeft.y(), topRight.y(), bottomLeft.y(), bottomRight.y()); + + QPointF hzonLeft = QPoint(minx, 0); + QPointF hzonRight = QPoint(maxx, 0); + + QPainterPath skyPath(hzonLeft); + skyPath.lineTo(QPointF(minx, miny)); + skyPath.lineTo(QPointF(maxx, miny)); + skyPath.lineTo(hzonRight); + skyPath.closeSubpath(); + + // TODO: The gradient is wrong now. + QLinearGradient skyGradient(0, -gradientEnd, 0, 0); + skyGradient.setColorAt(0, QColor::fromHsvF(0.6, 1.0, 0.7)); + skyGradient.setColorAt(1, QColor::fromHsvF(0.6, 0.25, 0.9)); + QBrush skyBrush(skyGradient); + painter.fillPath(skyPath, skyBrush); + + QPainterPath groundPath(hzonRight); + groundPath.lineTo(maxx, maxy); + groundPath.lineTo(minx, maxy); + groundPath.lineTo(hzonLeft); + groundPath.closeSubpath(); + + QLinearGradient groundGradient(0, gradientEnd, 0, 0); + groundGradient.setColorAt(0, QColor::fromHsvF(0.25, 1, 0.5)); + groundGradient.setColorAt(1, QColor::fromHsvF(0.25, 0.25, 0.5)); + QBrush groundBrush(groundGradient); + painter.fillPath(groundPath, groundBrush); + + QPen pen; + pen.setWidthF(lineWidth); + pen.setColor(greenColor); + painter.setPen(pen); + + QPointF start(-mainArea.width(), 0); + QPoint end(mainArea.width(), 0); + painter.drawLine(start, end); +} + +void PrimaryFlightDisplay::drawPitchScale( + QPainter& painter, + QRectF area, + float intrusion, + bool drawNumbersLeft, + bool drawNumbersRight + ) { + + // The area should be quadratic but if not width is the major size. + qreal w = area.width(); + if (w PITCH_SCALE_WIDTHREDUCTION_FROM) { + // we want: 1 at PITCH_SCALE_WIDTHREDUCTION_FROM and PITCH_SCALE_WIDTHREDUCTION at 90. + // That is PITCH_SCALE_WIDTHREDUCTION + (1-PITCH_SCALE_WIDTHREDUCTION) * f(pitch) + // where f(90)=0 and f(PITCH_SCALE_WIDTHREDUCTION_FROM)=1 + // f(p) = (90-p) * 1/(90-PITCH_SCALE_WIDTHREDUCTION_FROM) + // or PITCH_SCALE_WIDTHREDUCTION + f(pitch) - f(pitch) * PITCH_SCALE_WIDTHREDUCTION + // or PITCH_SCALE_WIDTHREDUCTION (1-f(pitch)) + f(pitch) + int fromVertical = abs(pitch>=0 ? 90-pitch : -90-pitch); + float temp = fromVertical * 1/(90.0f-PITCH_SCALE_WIDTHREDUCTION_FROM); + linewidth *= (PITCH_SCALE_WIDTHREDUCTION * (1-temp) + temp); + } + + float shift = pitchAngleToTranslation(w, pitch-degrees); + + // TODO: Intrusion detection and evasion. That is, don't draw + // where the compass has intruded. + + painter.translate(0, shift); + QPointF start(-linewidth*w, 0); + QPointF end(linewidth*w, 0); + painter.drawLine(start, end); + + if (isMajor && (drawNumbersLeft||drawNumbersRight)) { + int displayDegrees = degrees; + if(displayDegrees>90) displayDegrees = 180-displayDegrees; + else if (displayDegrees<-90) displayDegrees = -180 - displayDegrees; + if (SHOW_ZERO_ON_SCALES || degrees) { + QString s_number; //= QString("%d").arg(degrees); + s_number.sprintf("%d", displayDegrees); + if (drawNumbersLeft) drawTextRightCenter(painter, s_number, mediumTextSize, -PITCH_SCALE_MAJORWIDTH * w-10, 0); + if (drawNumbersRight) drawTextLeftCenter(painter, s_number, mediumTextSize, PITCH_SCALE_MAJORWIDTH * w+10, 0); + } + } + + painter.setTransform(savedTransform); + } +} + +void PrimaryFlightDisplay::drawRollScale( + QPainter& painter, + QRectF area, + bool drawTicks, + bool drawNumbers) { + + qreal w = area.width(); + if (wlength) ?-tickValues[i-length-1] : tickValues[i]; + //degrees = 180 - degrees; + painter.rotate(degrees - previousRotation); + previousRotation = degrees; + + QPointF start(0, -_size/2); + QPointF end(0, -(1.0+ROLL_SCALE_TICKMARKLENGTH)*_size/2); + + painter.drawLine(start, end); + + QString s_number; //= QString("%d").arg(degrees); + if (SHOW_ZERO_ON_SCALES || degrees) + s_number.sprintf("%d", abs(degrees)); + + if (drawNumbers) { + drawTextCenterBottom(painter, s_number, mediumTextSize, 0, -(ROLL_SCALE_RADIUS+ROLL_SCALE_TICKMARKLENGTH*1.7)*w); + } + } + } +} + +void PrimaryFlightDisplay::drawAIAttitudeScales( + QPainter& painter, + QRectF area, + float intrusion + ) { + // To save computations, we do these transformations once for both scales: + painter.resetTransform(); + painter.translate(area.center()); + painter.rotate(-roll); + QTransform saved = painter.transform(); + + drawRollScale(painter, area, true, true); + painter.setTransform(saved); + drawPitchScale(painter, area, intrusion, true, true); +} + +void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, float halfspan) { + float start = heading - halfspan; + float end = heading + halfspan; + + int firstTick = ceil(start / COMPASS_DISK_RESOLUTION) * COMPASS_DISK_RESOLUTION; + int lastTick = floor(end / COMPASS_DISK_RESOLUTION) * COMPASS_DISK_RESOLUTION; + + float radius = area.width()/2; + float innerRadius = radius * 0.96; + painter.resetTransform(); + painter.setBrush(instrumentBackground); + painter.setPen(instrumentEdgePen); + painter.drawEllipse(area); + painter.setBrush(Qt::NoBrush); + + QPen scalePen(Qt::black); + scalePen.setWidthF(fineLineWidth); + + for (int tickYaw = firstTick; tickYaw <= lastTick; tickYaw += COMPASS_DISK_RESOLUTION) { + int displayTick = tickYaw; + if (displayTick < 0) displayTick+=360; + else if (displayTick>=360) displayTick-=360; + + // yaw is in center. + float off = tickYaw - heading; + // wrap that to ]-180..180] + if (off<=-180) off+= 360; else if (off>180) off -= 360; + + painter.translate(area.center()); + painter.rotate(off); + bool drewArrow = false; + bool isMajor = displayTick % COMPASS_DISK_MAJORTICK == 0; + + if (displayTick==30 || displayTick==60 || + displayTick==120 || displayTick==150 || + displayTick==210 || displayTick==240 || + displayTick==300 || displayTick==330) { + // draw a number + QString s_number; + s_number.sprintf("%d", displayTick/10); + painter.setPen(scalePen); + drawTextCenter(painter, s_number, /*COMPASS_SCALE_TEXT_SIZE*radius*/ smallTestSize, 0, -innerRadius*0.75); + } else { + if (displayTick % COMPASS_DISK_ARROWTICK == 0) { + if (displayTick!=0) { + QPainterPath markerPath(QPointF(0, -innerRadius*(1-COMPASS_DISK_MARKERHEIGHT/2))); + markerPath.lineTo(innerRadius*COMPASS_DISK_MARKERWIDTH/4, -innerRadius); + markerPath.lineTo(-innerRadius*COMPASS_DISK_MARKERWIDTH/4, -innerRadius); + markerPath.closeSubpath(); + painter.setPen(scalePen); + painter.setBrush(Qt::SolidPattern); + painter.drawPath(markerPath); + painter.setBrush(Qt::NoBrush); + drewArrow = true; + } + if (displayTick%90 == 0) { + // Also draw a label + QString name = compassWindNames[displayTick / 45]; + painter.setPen(scalePen); + drawTextCenter(painter, name, mediumTextSize, 0, -innerRadius*0.75); + } + } + } + // draw the scale lines. If an arrow was drawn, stay off from it. + + QPointF p_start = drewArrow ? QPoint(0, -innerRadius*0.94) : QPoint(0, -innerRadius); + QPoint p_end = isMajor ? QPoint(0, -innerRadius*0.86) : QPoint(0, -innerRadius*0.90); + + painter.setPen(scalePen); + painter.drawLine(p_start, p_end); + painter.resetTransform(); + } + + painter.setPen(scalePen); + //painter.setBrush(Qt::SolidPattern); + painter.translate(area.center()); + QPainterPath markerPath(QPointF(0, -radius-2)); + markerPath.lineTo(radius*COMPASS_DISK_MARKERWIDTH/2, -radius-radius*COMPASS_DISK_MARKERHEIGHT-2); + markerPath.lineTo(-radius*COMPASS_DISK_MARKERWIDTH/2, -radius-radius*COMPASS_DISK_MARKERHEIGHT-2); + markerPath.closeSubpath(); + painter.drawPath(markerPath); + + qreal digitalCompassYCenter = -radius*0.52; + qreal digitalCompassHeight = radius*0.28; + + QPointF digitalCompassBottom(0, digitalCompassYCenter+digitalCompassHeight); + QPointF digitalCompassAbsoluteBottom = painter.transform().map(digitalCompassBottom); + + qreal digitalCompassUpshift = digitalCompassAbsoluteBottom.y()>height() ? digitalCompassAbsoluteBottom.y()-height() : 0; + + QRectF digitalCompassRect(-radius/3, -radius*0.52-digitalCompassUpshift, radius*2/3, radius*0.28); + painter.setPen(instrumentEdgePen); + painter.drawRoundedRect(digitalCompassRect, instrumentEdgePen.widthF()*2/3, instrumentEdgePen.widthF()*2/3); + + /* final safeguard for really stupid systems */ + int digitalCompassValue = static_cast(round(heading)) % 360; + + QString s_digitalCompass; + s_digitalCompass.sprintf("%03d", digitalCompassValue); + + QPen pen; + pen.setWidthF(lineWidth); + pen.setColor(Qt::white); + painter.setPen(pen); + + drawTextCenter(painter, s_digitalCompass, largeTextSize, 0, -radius*0.38-digitalCompassUpshift); + +// dummy +// navigationTargetBearing = 10; +// navigationCrosstrackError = 500; + + // The CDI + if (shouldDisplayNavigationData() && navigationTargetBearing != UNKNOWN_ATTITUDE && !isinf(navigationCrosstrackError)) { + painter.resetTransform(); + painter.translate(area.center()); + // TODO : Sign might be wrong? + // TODO : The case where error exceeds max. Truncate to max. and make that visible somehow. + bool errorBeyondRadius = false; + if (abs(navigationCrosstrackError) > CROSSTRACK_MAX) { + errorBeyondRadius = true; + navigationCrosstrackError = navigationCrosstrackError>0 ? CROSSTRACK_MAX : -CROSSTRACK_MAX; + } + + float r = radius * CROSSTRACK_RADIUS; + float x = navigationCrosstrackError / CROSSTRACK_MAX * r; + float y = qSqrt(r*r - x*x); // the positive y, there is also a negative. + + float sillyHeading = 0; + float angle = sillyHeading - navigationTargetBearing; // TODO: sign. + painter.rotate(-angle); + + QPen pen; + pen.setWidthF(lineWidth); + pen.setColor(Qt::black); + painter.setPen(pen); + + painter.drawLine(QPointF(x, y), QPointF(x, -y)); + } +} + +/* +void PrimaryFlightDisplay::drawSeparateCompassDisk(QPainter& painter, QRectF area) { + float radius = area.width()/2; + float innerRadius = radius * 0.96; + painter.resetTransform(); + + painter.setBrush(instrumentOpagueBackground); + painter.setPen(instrumentEdgePen); + painter.drawEllipse(area); + painter.setBrush(Qt::NoBrush); + + QPen scalePen(Qt::white); + scalePen.setWidthF(fineLineWidth); + + for (int displayTick=0; displayTick<360; displayTick+=COMPASS_SEPARATE_DISK_RESOLUTION) { + // yaw is in center. + float off = displayTick - heading; + // wrap that to ]-180..180] + if (off<=-180) off+= 360; else if (off>180) off -= 360; + + painter.translate(area.center()); + painter.rotate(off); + bool drewArrow = false; + bool isMajor = displayTick % COMPASS_DISK_MAJORTICK == 0; + + if (displayTick==30 || displayTick==60 || + displayTick==120 || displayTick==150 || + displayTick==210 || displayTick==240 || + displayTick==300 || displayTick==330) { + // draw a number + QString s_number; + s_number.sprintf("%d", displayTick/10); + painter.setPen(scalePen); + drawTextCenter(painter, s_number, mediumTextSize, 0, -innerRadius*0.75); + } else { + if (displayTick % COMPASS_DISK_ARROWTICK == 0) { + if (displayTick!=0) { + QPainterPath markerPath(QPointF(0, -innerRadius*(1-COMPASS_DISK_MARKERHEIGHT/2))); + markerPath.lineTo(innerRadius*COMPASS_DISK_MARKERWIDTH/4, -innerRadius); + markerPath.lineTo(-innerRadius*COMPASS_DISK_MARKERWIDTH/4, -innerRadius); + markerPath.closeSubpath(); + painter.setPen(scalePen); + painter.setBrush(Qt::SolidPattern); + painter.drawPath(markerPath); + painter.setBrush(Qt::NoBrush); + drewArrow = true; + } + if (displayTick%90 == 0) { + // Also draw a label + QString name = compassWindNames[displayTick / 45]; + painter.setPen(scalePen); + drawTextCenter(painter, name, mediumTextSize, 0, -innerRadius*0.75); + } + } + } + // draw the scale lines. If an arrow was drawn, stay off from it. + + QPointF p_start = drewArrow ? QPoint(0, -innerRadius*0.94) : QPoint(0, -innerRadius); + QPoint p_end = isMajor ? QPoint(0, -innerRadius*0.86) : QPoint(0, -innerRadius*0.90); + + painter.setPen(scalePen); + painter.drawLine(p_start, p_end); + painter.resetTransform(); + } + + painter.setPen(scalePen); + //painter.setBrush(Qt::SolidPattern); + painter.translate(area.center()); + QPainterPath markerPath(QPointF(0, -radius-2)); + markerPath.lineTo(radius*COMPASS_DISK_MARKERWIDTH/2, -radius-radius*COMPASS_DISK_MARKERHEIGHT-2); + markerPath.lineTo(-radius*COMPASS_DISK_MARKERWIDTH/2, -radius-radius*COMPASS_DISK_MARKERHEIGHT-2); + markerPath.closeSubpath(); + painter.drawPath(markerPath); + + QRectF headingNumberRect(-radius/3, radius*0.12, radius*2/3, radius*0.28); + painter.setPen(instrumentEdgePen); + painter.drawRoundedRect(headingNumberRect, instrumentEdgePen.widthF()*2/3, instrumentEdgePen.widthF()*2/3); + + // if (heading < 0) heading += 360; + // else if (heading >= 360) heading -= 360; + int yawCompass = static_cast(heading) % 360; + + QString yawAngle; + yawAngle.sprintf("%03d", yawCompass); + + QPen pen; + pen.setWidthF(lineWidth); + pen.setColor(Qt::white); + painter.setPen(pen); + + drawTextCenter(painter, yawAngle, largeTextSize, 0, radius/4); +} +*/ + +void PrimaryFlightDisplay::drawAltimeter( + QPainter& painter, + QRectF area, // the area where to draw the tape. + float primaryAltitude, + float secondaryAltitude, + float vv + ) { + + painter.resetTransform(); + fillInstrumentBackground(painter, area); + + QPen pen; + pen.setWidthF(lineWidth); + + pen.setColor(Qt::white); + painter.setPen(pen); + + float h = area.height(); + float w = area.width(); + float secondaryAltitudeBoxHeight = mediumTextSize * 2; + // The height where we being with new tickmarks. + float effectiveHalfHeight = h*0.45; + + // not yet implemented: Display of secondary altitude. + // if (isAirplane()) + // effectiveHalfHeight-= secondaryAltitudeBoxHeight; + + float markerHalfHeight = mediumTextSize*0.8; + float leftEdge = instrumentEdgePen.widthF()*2; + float rightEdge = w-leftEdge; + float tickmarkLeft = leftEdge; + float tickmarkRightMajor = tickmarkLeft+TAPE_GAUGES_TICKWIDTH_MAJOR*w; + float tickmarkRightMinor = tickmarkLeft+TAPE_GAUGES_TICKWIDTH_MINOR*w; + float numbersLeft = 0.42*w; + float markerTip = (tickmarkLeft*2+tickmarkRightMajor)/3; + float scaleCenterAltitude = primaryAltitude == UNKNOWN_ALTITUDE ? 0 : primaryAltitude; + + // altitude scale +#ifdef ALTIMETER_PROJECTED + float range = 1.2; + float start = scaleCenterAltitude - ALTIMETER_PROJECTED_SPAN/2; + float end = scaleCenterAltitude + ALTIMETER_PROJECTED_SPAN/2; + int firstTick = ceil(start / ALTIMETER_PROJECTED_RESOLUTION) * ALTIMETER_PROJECTED_RESOLUTION; + int lastTick = floor(end / ALTIMETER_PROJECTED_RESOLUTION) * ALTIMETER_PROJECTED_RESOLUTION; + for (int tickAlt = firstTick; tickAlt <= lastTick; tickAlt += ALTIMETER_PROJECTED_RESOLUTION) { + // a number between 0 and 1. Use as radians directly. + float r = range*(tickAlt-altitude)/(ALTIMETER_PROJECTED_SPAN/2); + float y = effectiveHalfHeight * sin(r); + scale = cos(r); + if (scale<0) scale = -scale; + bool hasText = tickAlt % ALTIMETER_PROJECTED_MAJOR_RESOLUTION == 0; +#else + float start = scaleCenterAltitude - ALTIMETER_LINEAR_SPAN/2; + float end = scaleCenterAltitude + ALTIMETER_LINEAR_SPAN/2; + int firstTick = ceil(start / ALTIMETER_LINEAR_RESOLUTION) * ALTIMETER_LINEAR_RESOLUTION; + int lastTick = floor(end / ALTIMETER_LINEAR_RESOLUTION) * ALTIMETER_LINEAR_RESOLUTION; + for (int tickAlt = firstTick; tickAlt <= lastTick; tickAlt += ALTIMETER_LINEAR_RESOLUTION) { + float y = (tickAlt-scaleCenterAltitude)*effectiveHalfHeight/(ALTIMETER_LINEAR_SPAN/2); + bool isMajor = tickAlt % ALTIMETER_LINEAR_MAJOR_RESOLUTION == 0; +#endif + painter.resetTransform(); + painter.translate(area.left(), area.center().y() - y); + pen.setColor(tickAlt<0 ? redColor : Qt::white); + painter.setPen(pen); + if (isMajor) { + painter.drawLine(tickmarkLeft, 0, tickmarkRightMajor, 0); + QString s_alt; + s_alt.sprintf("%d", abs(tickAlt)); + drawTextLeftCenter(painter, s_alt, mediumTextSize, numbersLeft, 0); + } else { + painter.drawLine(tickmarkLeft, 0, tickmarkRightMinor, 0); + } + } + + QPainterPath markerPath(QPoint(markerTip, 0)); + markerPath.lineTo(markerTip+markerHalfHeight, markerHalfHeight); + markerPath.lineTo(rightEdge, markerHalfHeight); + markerPath.lineTo(rightEdge, -markerHalfHeight); + markerPath.lineTo(markerTip+markerHalfHeight, -markerHalfHeight); + markerPath.closeSubpath(); + + painter.resetTransform(); + painter.translate(area.left(), area.center().y()); + + pen.setWidthF(lineWidth); + pen.setColor(Qt::white); + painter.setPen(pen); + + painter.setBrush(Qt::SolidPattern); + painter.drawPath(markerPath); + painter.setBrush(Qt::NoBrush); + + pen.setColor(Qt::white); + painter.setPen(pen); + + QString s_alt; + if(primaryAltitude == UNKNOWN_ALTITUDE) + s_alt.sprintf("---"); + else + s_alt.sprintf("%3.0f", primaryAltitude); + + float xCenter = (markerTip+rightEdge)/2; + drawTextCenter(painter, s_alt, /* TAPES_TEXT_SIZE*width()*/ mediumTextSize, xCenter, 0); + + if (vv == UNKNOWN_ALTITUDE) return; + float vvPixHeight = -vv/ALTIMETER_VVI_SPAN * effectiveHalfHeight; + if (abs (vvPixHeight)