zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Multiple altitudes and speeds signals. Simplified PFD

QGC4.4
dongfang 12 years ago
parent
13dcbfae6d
commit
10840b7c31
7 changed files with 368 additions and 167 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 127
      src/uas/UAS.cc
  2. 139
      src/uas/UAS.h
  3. 49
      src/uas/UASInterface.h
  4. 6
      src/uas/senseSoarMAV.cpp
  5. 147
      src/ui/PrimaryFlightDisplay.cpp
  6. 55
      src/ui/PrimaryFlightDisplay.h
  7. 12
      src/ui/uas/UASQuickView.cc

127
src/uas/UAS.cc
Unescape View File

@ -39,16 +39,37 @@ @@ -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<LinkInterface*>()),
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),
@ -59,10 +80,13 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), @@ -59,10 +80,13 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
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),
@ -71,10 +95,11 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), @@ -71,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),
@ -82,7 +107,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), @@ -82,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),
@ -90,18 +120,17 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), @@ -90,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),
@ -110,7 +139,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), @@ -110,7 +139,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
sensorHil(false),
lastSendTimeGPS(0),
lastSendTimeSensors(0)
{
for (unsigned int i = 0; i<255;++i)
{
@ -124,7 +152,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), @@ -124,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);
@ -646,7 +673,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -646,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;
@ -685,15 +712,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -685,15 +712,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
// dongfang: For APM, this altitude is the mix altitude[m]
emit altitudeChanged(uasId, hud.alt);
// dongfang: For APM, airspeed is airspeed or AHRS estimated airspeed
// dongfang: For APM, climb rate is barometric
// dongfang: The signal has no parameter for groundspeed.
// dongfang: The signal is emitted also from other places,
// such as GPS xyz speeds. This will cause a mix of signals
// from different sensors, which will probably not be so good.
float weAlsoWantGroundSpeedPlease = hud.groundspeed;
emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time);
emit altitudeChanged(this, PRIMARY_ALTITUDE, hud.alt, time);
// This makes the lateral velocity zero when it might really not be, problematic.
// emit speedChanged(this, PRIMARY_SPEED, hud.airspeed, 0.0f, hud.climb, time);
emit speedChanged(this, PRIMARY_SPEED, hud.airspeed, time);
emit speedChanged(this, GROUNDSPEED_BY_UAV, hud.groundspeed, time);
emit climbRateChanged(this, BAROMETRIC_ALTITUDE, hud.groundspeed, time);
}
break;
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
@ -707,7 +732,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -707,7 +732,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;
@ -717,7 +741,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -717,7 +741,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Emit
emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
emit velocityChanged(this, MAV_FRAME_LOCAL_NED, pos.vx, pos.vy, pos.vz, time);
// Set internal state
if (!positionLock) {
@ -745,18 +769,29 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -745,18 +769,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 altitudeChanged(this, GPS_ALTITUDE, getAltitude(), time);
// We had some frame mess here, global and local axes were mixed.
emit velocityChanged(this, MAV_FRAME_GLOBAL, speedX, speedY, speedZ, time);
double groundspeed = qSqrt(speedX*speedX+speedY*speedY);
emit speedChanged(this, GROUNDSPEED_BY_GPS, groundspeed, time);
// Set internal state
if (!positionLock)
@ -797,14 +832,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -797,14 +832,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 altitudeChanged(this, GPS_ALTITUDE, getAltitude(), time);
}
positionLock = true;
@ -814,10 +848,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -814,10 +848,12 @@ 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);
// TODO: Other sources also? Actually this condition does not quite belong here.
emit speedChanged(this, GROUNDSPEED_BY_GPS, vel, time);
}
else
{
@ -1360,6 +1396,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -1360,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:
@ -1491,7 +1532,7 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl @@ -1491,7 +1532,7 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
/**
* Set the home position of the UAS.
* @param lat The latitude fo the home position
* @param lon The longitute of the home position
* @param lon The longitude of the home position
* @param alt The altitude of the home position
*/
void UAS::setHomePosition(double lat, double lon, double alt)

139
src/uas/UAS.h
Unescape View File

@ -56,16 +56,6 @@ public: @@ -56,16 +56,6 @@ public:
UAS(MAVLinkProtocol* protocol, int id = 0);
~UAS();
enum BatteryType
{
NICD = 0,
NIMH = 1,
LIION = 2,
LIPOLY = 3,
LIFE = 4,
AGZN = 5
}; ///< The type of battery used
static const int lipoFull = 4.2f; ///< 100% charged voltage
static const int lipoEmpty = 3.5f; ///< Discharged voltage
@ -105,7 +95,6 @@ public: @@ -105,7 +95,6 @@ public:
Q_PROPERTY(double localZ READ getLocalZ WRITE setLocalZ NOTIFY localZChanged)
Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged)
Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged)
Q_PROPERTY(double altitude READ getAltitude WRITE setAltitude NOTIFY altitudeChanged)
Q_PROPERTY(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged)
Q_PROPERTY(bool isLocalPositionKnown READ localPositionKnown)
Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown)
@ -113,6 +102,11 @@ public: @@ -113,6 +102,11 @@ public:
Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged)
Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged)
Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged)
Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged)
// dongfang: There is not only one altitude; there are at least (APM) GPS altitude, mix altitude and mix-altitude relative to home.
// I have made this property refer to the mix-altitude ASL as this is the one actually used in navigation by APM.
Q_PROPERTY(double altitude READ getAltitude WRITE setAltitude NOTIFY altitudeChanged)
void setLocalX(double val)
{
@ -120,6 +114,7 @@ public: @@ -120,6 +114,7 @@ public:
emit localXChanged(val,"localX");
emit valueChanged(this->uasId,"localX","M",QVariant(val),getUnixTime());
}
double getLocalX() const
{
return localX;
@ -153,6 +148,7 @@ public: @@ -153,6 +148,7 @@ public:
emit latitudeChanged(val,"latitude");
emit valueChanged(this->uasId,"latitude","deg",QVariant(val),getUnixTime());
}
double getLatitude() const
{
return latitude;
@ -164,6 +160,7 @@ public: @@ -164,6 +160,7 @@ public:
emit longitudeChanged(val,"longitude");
emit valueChanged(this->uasId,"longitude","deg",QVariant(val),getUnixTime());
}
double getLongitude() const
{
return longitude;
@ -172,7 +169,7 @@ public: @@ -172,7 +169,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());
}
@ -197,6 +194,7 @@ public: @@ -197,6 +194,7 @@ public:
{
return isLocalPositionKnown;
}
virtual bool globalPositionKnown() const
{
return isGlobalPositionKnown;
@ -214,6 +212,19 @@ public: @@ -214,6 +212,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;
@ -246,6 +257,7 @@ public: @@ -246,6 +257,7 @@ public:
{
return yaw;
}
bool getSelected() const;
QVector3D getNedPosGlobalOffset() const
{
@ -319,31 +331,43 @@ public: @@ -319,31 +331,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<int, QString> components;///< IDs and names of all detected onboard components
QList<LinkInterface*>* links; ///< List of links this UAS can be reached by
QList<int> 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<double> actuatorValues;
QList<QString> actuatorNames;
QList<double> motorValues;
QList<QString> motorNames;
QMap<int, QString> 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
@ -358,12 +382,14 @@ protected: //COMMENTS FOR TEST UNIT @@ -358,12 +382,14 @@ protected: //COMMENTS FOR TEST UNIT
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
@ -373,16 +399,20 @@ protected: //COMMENTS FOR TEST UNIT @@ -373,16 +399,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
@ -391,13 +421,25 @@ protected: //COMMENTS FOR TEST UNIT @@ -391,13 +421,25 @@ 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 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
@ -432,21 +474,13 @@ protected: //COMMENTS FOR TEST UNIT @@ -432,21 +474,13 @@ protected: //COMMENTS FOR TEST UNIT
qreal receivedRGBDImageTimestamp;
#endif
/// PARAMETERS
QMap<int, QMap<QString, QVariant>* > 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 */
@ -647,12 +681,12 @@ public slots: @@ -647,12 +681,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);
/**
@ -811,15 +845,17 @@ signals: @@ -811,15 +845,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 bearingToWaypointChanged(double val,QString name);
void altitudeChanged(UASInterface*, AltitudeMeasurementSource source, double altitude, quint64 usec);
//void speedChanged(UASInterface*, SpeedMeasurementSource source, double speed, quint64 usec);
//void climbRateChanged(UASInterface*, AltitudeMeasurementSource source, double speed, quint64 usec);
void velocityChanged(UASInterface*, MAV_FRAME, double vx, double vy, double vz, quint64 usec);
protected:
/** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
quint64 getUnixTime(quint64 time=0);
@ -827,6 +863,7 @@ protected: @@ -827,6 +863,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

49
src/uas/UASInterface.h
Unescape View File

@ -51,6 +51,40 @@ This file is part of the QGROUNDCONTROL project @@ -51,6 +51,40 @@ 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.
*
@ -477,7 +511,7 @@ signals: @@ -477,7 +511,7 @@ signals:
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 +520,19 @@ signals: @@ -486,10 +520,19 @@ 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 altitudeChanged(UASInterface*, AltitudeMeasurementSource source, 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 speedChanged(UASInterface*, SpeedMeasurementSource source, double speed, quint64 usec);
// The vertical speed (a scalar)
void climbRateChanged(UASInterface*, AltitudeMeasurementSource source, double speed, quint64 usec);
// Consider adding a MAV_FRAME parameter to this; could help specifying what the 3 scalars are.
void velocityChanged(UASInterface*, MAV_FRAME frame, 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 */

6
src/uas/senseSoarMAV.cpp
Unescape View File

@ -41,7 +41,7 @@ void senseSoarMAV::receiveMessage(LinkInterface *link, mavlink_message_t message @@ -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 @@ -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 @@ -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;
}
}

147
src/ui/PrimaryFlightDisplay.cpp
Unescape View File

@ -40,16 +40,20 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren @@ -40,16 +40,20 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren
uas(NULL),
altimeterMode(GPS_MAIN),
altimeterFrame(ASL),
speedMode(GROUND_MAIN),
roll(UNKNOWN_ATTITUDE),
pitch(UNKNOWN_ATTITUDE),
// heading(NAN),
heading(UNKNOWN_ATTITUDE),
aboveASLAltitude(UNKNOWN_ALTITUDE),
primaryAltitude(UNKNOWN_ALTITUDE),
GPSAltitude(UNKNOWN_ALTITUDE),
aboveHomeAltitude(UNKNOWN_ALTITUDE),
primarySpeed(UNKNOWN_SPEED),
groundspeed(UNKNOWN_SPEED),
airspeed(UNKNOWN_SPEED),
verticalVelocity(UNKNOWN_SPEED),
verticalVelocity(UNKNOWN_ALTITUDE),
font("Bitstream Vera Sans"),
refreshTimer(new QTimer(this)),
@ -169,18 +173,20 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas) @@ -169,18 +173,20 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas)
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(armingChanged(bool)), this, SLOT(updateArmed(bool)));
disconnect(this->uas, SIGNAL(satelliteCountChanged(double, QString)), this, SLOT(updateSatelliteCount(double, QString)));
//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)));
//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(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
disconnect(this->uas, SIGNAL(speedChanged(UASInterface*, SpeedMeasurementSource, double, quint64)), this, SLOT(updateSpeed(UASInterface*,SpeedMeasurementSource,double,quint64)));
disconnect(this->uas, SIGNAL(climbRateChanged(UASInterface*, AltitudeMeasurementSource, double, quint64)), this, SLOT(updateClimbRate(UASInterface*, AltitudeMeasurementSource, double, quint64)));
disconnect(this->uas, SIGNAL(altitudeChanged(UASInterface*, AltitudeMeasurementSource, double)), this, SLOT(updateAltitude(UASInterface*, AltitudeMeasurementSource, double, quint64)));
}
if (uas) {
@ -189,17 +195,19 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas) @@ -189,17 +195,19 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas)
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(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(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(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(speedChanged(UASInterface*, SpeedMeasurementSource, double, quint64)), this, SLOT(updateSpeed(UASInterface*,SpeedMeasurementSource,double,quint64)));
connect(uas, SIGNAL(climbRateChanged(UASInterface*, AltitudeMeasurementSource, double, quint64)), this, SLOT(updateClimbRate(UASInterface*, AltitudeMeasurementSource, double, quint64)));
connect(uas, SIGNAL(altitudeChanged(UASInterface*, AltitudeMeasurementSource, double, quint64)), this, SLOT(updateAltitude(UASInterface*, AltitudeMeasurementSource, double, quint64)));
// Set new UAS
this->uas = uas;
@ -249,29 +257,59 @@ void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, int component, doub @@ -249,29 +257,59 @@ void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, int component, doub
* 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::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 timestamp)
void PrimaryFlightDisplay::updateSpeed(UASInterface* uas, SpeedMeasurementSource source, double speed, quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(timestamp);
Q_UNUSED(z);
/*
this->xSpeed = x;
this->ySpeed = y;
this->zSpeed = z;
*/
double newTotalSpeed = qSqrt(x*x + y*y/*+ zSpeed*zSpeed */);
// totalAcc = (newTotalSpeed - totalSpeed) / ((double)(lastSpeedUpdate - timestamp)/1000.0);
if (source == PRIMARY_SPEED) {
primarySpeed = speed;
didReceivePrimarySpeed = true;
} else if (source == GROUNDSPEED_BY_GPS || source == GROUNDSPEED_BY_UAV) {
groundspeed = speed;
if (!didReceivePrimarySpeed)
primarySpeed = speed;
}
}
// TODO: Change to real data.
groundspeed = newTotalSpeed;
airspeed = x;
verticalVelocity = z;
void PrimaryFlightDisplay::updateClimbRate(UASInterface* uas, AltitudeMeasurementSource source, double climbRate, quint64 timestamp) {
Q_UNUSED(uas);
Q_UNUSED(timestamp);
verticalVelocity = climbRate;
}
void PrimaryFlightDisplay::updateAltitude(UASInterface* uas, AltitudeMeasurementSource source, double altitude, quint64 timestamp) {
Q_UNUSED(uas);
Q_UNUSED(timestamp);
if (source == PRIMARY_ALTITUDE) {
primaryAltitude = altitude;
didReceivePrimaryAltitude = true;
} else if (source == GPS_ALTITUDE) {
GPSAltitude = altitude;
if (!didReceivePrimaryAltitude)
primaryAltitude = altitude;
}
}
/*
* 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;
}
}
void PrimaryFlightDisplay::drawTextCenter (
QPainter& painter,
QString text,
@ -832,12 +870,10 @@ void PrimaryFlightDisplay::drawAltimeter( @@ -832,12 +870,10 @@ void PrimaryFlightDisplay::drawAltimeter(
QPainter& painter,
QRectF area, // the area where to draw the tape.
float primaryAltitude,
float maxAltitude,
float secondaryAltitude,
float vv
) {
Q_UNUSED(maxAltitude)
painter.resetTransform();
fillInstrumentBackground(painter, area);
@ -849,8 +885,14 @@ void PrimaryFlightDisplay::drawAltimeter( @@ -849,8 +885,14 @@ void PrimaryFlightDisplay::drawAltimeter(
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;
@ -929,26 +971,34 @@ void PrimaryFlightDisplay::drawAltimeter( @@ -929,26 +971,34 @@ void PrimaryFlightDisplay::drawAltimeter(
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)<markerHalfHeight) return;
float vvPixHeight = -vv/ALTIMETER_VVI_SPAN * effectiveHalfHeight;
if (abs (vvPixHeight)<markerHalfHeight) return; // hidden behind marker.
float vvSign = vvPixHeight>0 ? -1 : 1;
float vvSign = vvPixHeight>0 ? 1 : -1; // reverse y sign
// QRectF vvRect(rightEdge - w*ALTIMETER_VVI_WIDTH, markerHalfHeight*vvSign, w*ALTIMETER_VVI_WIDTH, abs(vvPixHeight)*vvSign);
QPointF vvArrowBegin(rightEdge - w*ALTIMETER_VVI_WIDTH/2, markerHalfHeight*vvSign);
QPointF vvArrowEnd(rightEdge - w*ALTIMETER_VVI_WIDTH/2, -vvPixHeight);
QPointF vvArrowEnd(rightEdge - w*ALTIMETER_VVI_WIDTH/2, vvPixHeight);
painter.drawLine(vvArrowBegin, vvArrowEnd);
QPointF vvArrowHead(rightEdge, -vvPixHeight+w*ALTIMETER_VVI_WIDTH);
// Yeah this is a repitition of above code but we are goigd to trash it all anyway, so no fix.
float vvArowHeadSize = abs(vvPixHeight - markerHalfHeight*vvSign);
if (vvArowHeadSize > w*ALTIMETER_VVI_WIDTH/3) vvArowHeadSize = w*ALTIMETER_VVI_WIDTH/3;
float xcenter = rightEdge-w*ALTIMETER_VVI_WIDTH/2;
QPointF vvArrowHead(xcenter+vvArowHeadSize, vvPixHeight - vvSign *vvArowHeadSize);
painter.drawLine(vvArrowHead, vvArrowEnd);
vvArrowHead = QPointF(rightEdge-ALTIMETER_VVI_WIDTH, -vvPixHeight+w*ALTIMETER_VVI_WIDTH);
vvArrowHead = QPointF(xcenter-vvArowHeadSize, vvPixHeight - vvSign * vvArowHeadSize);
painter.drawLine(vvArrowHead, vvArrowEnd);
}
void PrimaryFlightDisplay::drawVelocityMeter(
QPainter& painter,
QRectF area
QRectF area,
float speed,
float secondarySpeed
) {
painter.resetTransform();
@ -969,10 +1019,9 @@ void PrimaryFlightDisplay::drawVelocityMeter( @@ -969,10 +1019,9 @@ void PrimaryFlightDisplay::drawVelocityMeter(
float markerTip = (tickmarkLeftMajor+tickmarkRight*2)/3;
// Select between air and ground speed:
float primarySpeed = airspeed;
float centerScaleSpeed =
primarySpeed == UNKNOWN_SPEED ? 0 : primarySpeed;
speed == UNKNOWN_SPEED ? 0 : speed;
float start = centerScaleSpeed - AIRSPEED_LINEAR_SPAN/2;
float end = centerScaleSpeed + AIRSPEED_LINEAR_SPAN/2;
@ -1020,10 +1069,10 @@ void PrimaryFlightDisplay::drawVelocityMeter( @@ -1020,10 +1069,10 @@ void PrimaryFlightDisplay::drawVelocityMeter(
pen.setColor(Qt::white);
painter.setPen(pen);
QString s_alt;
if (primarySpeed == UNKNOWN_SPEED)
if (speed == UNKNOWN_SPEED)
s_alt.sprintf("---");
else
s_alt.sprintf("%3.1f", primarySpeed);
s_alt.sprintf("%3.1f", speed);
float xCenter = (markerTip+leftEdge)/2;
drawTextCenter(painter, s_alt, /* TAPES_TEXT_SIZE*width()*/ mediumTextSize, xCenter, 0);
}
@ -1360,11 +1409,9 @@ void PrimaryFlightDisplay::doPaint() { @@ -1360,11 +1409,9 @@ void PrimaryFlightDisplay::doPaint() {
painter.setClipping(hadClip);
// X: To the right of AI and with single margin again. That is, 3 single margins plus width of AI.
// Y: 1 single margin below above gadget.
drawAltimeter(painter, altimeterArea, primaryAltitude, GPSAltitude, verticalVelocity);
drawAltimeter(painter, altimeterArea, aboveASLAltitude, 1000, 3);
drawVelocityMeter(painter, velocityMeterArea);
drawVelocityMeter(painter, velocityMeterArea, primarySpeed, groundspeed);
/*
drawSensorsStatsPanel(painter, sensorsStatsArea);

55
src/ui/PrimaryFlightDisplay.h
Unescape View File

@ -106,8 +106,12 @@ public slots: @@ -106,8 +106,12 @@ public slots:
void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
/** @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 updateSpeed(UASInterface*,double,double,double,quint64);
void updateSpeed(UASInterface* uas, SpeedMeasurementSource source, double speed, quint64 timstamp);
void updateClimbRate(UASInterface* uas, AltitudeMeasurementSource source, double altitude, quint64 timestamp);
void updateAltitude(UASInterface* uas, AltitudeMeasurementSource source, double altitude, quint64 timestamp);
/** @brief Set the currently monitored UAS */
virtual void setActiveUAS(UASInterface* uas);
protected:
enum Layout {
@ -136,23 +140,34 @@ protected: @@ -136,23 +140,34 @@ protected:
// dongfang: We have no context menu. Viewonly.
// void contextMenuEvent (QContextMenuEvent* event);
protected:
// dongfang: What is that?
// dongfang: OK it's for UI interaction. Presently, there is none.
void createActions();
public slots:
/** @brief Set the currently monitored UAS */
virtual void setActiveUAS(UASInterface* uas);
protected slots:
signals:
void visibilityChanged(bool visible);
private:
//void prepareTransform(QPainter& painter, qreal width, qreal height);
//void transformToGlobalSystem(QPainter& painter, qreal width, qreal height, float roll, float pitch);
enum AltimeterMode {
PRIMARY_MAIN_GPS_SUB, // Show the primary alt. on tape and GPS as extra info
GPS_MAIN // Show GPS on tape and no extra info
};
enum AltimeterFrame {
ASL, // Show ASL altitudes (plane pilots' normal preference)
RELATIVE_TO_HOME // Show relative-to-home altitude (copter pilots)
};
enum SpeedMode {
PRIMARY_MAIN_GROUND_SUB,// Show primary speed (often airspeed) on tape and groundspeed as extra
GROUND_MAIN // Show groundspeed on tape and no extra info
};
/*
* There are at least these differences between airplane and copter PDF view:
* - Airplane show absolute altutude in altimeter, copter shows relative to home
*/
bool isAirplane();
void drawTextCenter(QPainter& painter, QString text, float fontSize, float x, float y);
void drawTextLeftCenter(QPainter& painter, QString text, float fontSize, float x, float y);
@ -167,8 +182,8 @@ private: @@ -167,8 +182,8 @@ private:
void drawAICompassDisk(QPainter& painter, QRectF area, float halfspan);
void drawSeparateCompassDisk(QPainter& painter, QRectF area);
void drawAltimeter(QPainter& painter, QRectF area, float altitude, float maxAltitude, float vv);
void drawVelocityMeter(QPainter& painter, QRectF area);
void drawAltimeter(QPainter& painter, QRectF area, float altitude, float secondaryAltitude, float vv);
void drawVelocityMeter(QPainter& painter, QRectF area, float speed, float secondarySpeed);
void fillInstrumentBackground(QPainter& painter, QRectF edge);
void fillInstrumentOpagueBackground(QPainter& painter, QRectF edge);
void drawInstrumentBackground(QPainter& painter, QRectF edge);
@ -184,21 +199,27 @@ private: @@ -184,21 +199,27 @@ private:
UASInterface* uas; ///< The uas currently monitored
AltimeterMode altimeterMode;
AltimeterFrame altimeterFrame;
SpeedMode speedMode;
bool didReceivePrimaryAltitude;
bool didReceivePrimarySpeed;
float roll;
float pitch;
float heading;
// APM: GPS and baro mix. From the GLOBAL_POSITION_INT or VFR_HUD messages.
float aboveASLAltitude;
float primaryAltitude;
float GPSAltitude;
// APM: GPS and baro mix above home (GPS) altitude. This value comes from the GLOBAL_POSITION_INT message.
// Do !!!NOT!!! ever do altitude calculations at the ground station. There are enough pitfalls already.
// The MP "set home altitude" button will not be repeated here if it did that.
// If the MP "set home altitude" button is migrated to here, it must set the UAS home altitude, not a GS-local one.
float aboveHomeAltitude;
float primarySpeed;
float groundspeed;
float airspeed;
float verticalVelocity;
Layout layout; // The display layout.

12
src/ui/uas/UASQuickView.cc
Unescape View File

@ -73,6 +73,18 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent) @@ -73,6 +73,18 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent)
uasPropertyToLabelMap["distToWaypoint"] = item;
}
{
QAction *action = new QAction("bearingToWaypoint",this);
action->setCheckable(true);
action->setChecked(true);
connect(action,SIGNAL(toggled(bool)),this,SLOT(actionTriggered(bool)));
this->addAction(action);
UASQuickViewItem *item = new UASQuickViewItem(this);
item->setTitle("bearingToWaypoint");
ui.verticalLayout->addWidget(item);
uasPropertyToLabelMap["bearingToWaypoint"] = item;
}
updateTimer = new QTimer(this);
connect(updateTimer,SIGNAL(timeout()),this,SLOT(updateTimerTick()));
updateTimer->start(1000);

Write Preview
Loading…
Cancel
Save