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
Unified 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 @@
*/ */
UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
uasId(id), uasId(id),
startTime(QGC::groundTimeMilliseconds()),
commStatus(COMM_DISCONNECTED),
name(""),
autopilot(-1),
links(new QList<LinkInterface*>()), links(new QList<LinkInterface*>()),
unknownPackets(), unknownPackets(),
mavlink(protocol), 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), thrustSum(0),
thrustMax(10), thrustMax(10),
// batteryType not initialized
// cells not initialized
// fullVoltage not initialized
// emptyVoltage not initialized
startVoltage(-1.0f), startVoltage(-1.0f),
tickVoltage(10.5f), tickVoltage(10.5f),
lastTickVoltageValue(13.0f), lastTickVoltageValue(13.0f),
@ -59,10 +80,13 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
lpVoltage(12.0f), lpVoltage(12.0f),
currentCurrent(0.4f), currentCurrent(0.4f),
batteryRemainingEstimateEnabled(true), batteryRemainingEstimateEnabled(true),
mode(-1), // chargeLevel not initialized
status(-1), // timeRemaining not initialized
navMode(-1), lowBattAlarm(false),
startTime(QGC::groundTimeMilliseconds()),
onboardTimeOffset(0), onboardTimeOffset(0),
controlRollManual(true), controlRollManual(true),
controlPitchManual(true), controlPitchManual(true),
controlYawManual(true), controlYawManual(true),
@ -71,10 +95,11 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
manualPitchAngle(0), manualPitchAngle(0),
manualYawAngle(0), manualYawAngle(0),
manualThrust(0), manualThrust(0),
receiveDropRate(0),
sendDropRate(0),
lowBattAlarm(false),
positionLock(false), positionLock(false),
isLocalPositionKnown(false),
isGlobalPositionKnown(false),
localX(0.0), localX(0.0),
localY(0.0), localY(0.0),
localZ(0.0), localZ(0.0),
@ -82,7 +107,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
latitude_gps(0.0), latitude_gps(0.0),
longitude_gps(0.0), longitude_gps(0.0),
altitude_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) #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
receivedOverlayTimestamp(0.0), receivedOverlayTimestamp(0.0),
receivedObstacleListTimestamp(0.0), receivedObstacleListTimestamp(0.0),
@ -90,18 +120,17 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
receivedPointCloudTimestamp(0.0), receivedPointCloudTimestamp(0.0),
receivedRGBDImageTimestamp(0.0), receivedRGBDImageTimestamp(0.0),
#endif #endif
paramsOnceRequested(false),
airframe(QGC_AIRFRAME_GENERIC),
attitudeKnown(false), attitudeKnown(false),
paramManager(NULL),
attitudeStamped(false), attitudeStamped(false),
lastAttitude(0), lastAttitude(0),
paramsOnceRequested(false),
paramManager(NULL),
simulation(0), simulation(0),
isLocalPositionKnown(false),
isGlobalPositionKnown(false), // The protected members.
systemIsArmed(false),
nedPosGlobalOffset(0,0,0),
nedAttGlobalOffset(0,0,0),
connectionLost(false), connectionLost(false),
lastVoltageWarning(0), lastVoltageWarning(0),
lastNonNullTime(0), lastNonNullTime(0),
@ -110,7 +139,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
sensorHil(false), sensorHil(false),
lastSendTimeGPS(0), lastSendTimeGPS(0),
lastSendTimeSensors(0) lastSendTimeSensors(0)
{ {
for (unsigned int i = 0; i<255;++i) for (unsigned int i = 0; i<255;++i)
{ {
@ -124,7 +152,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings()));
statusTimeout->start(500); statusTimeout->start(500);
readSettings(); readSettings();
type = MAV_TYPE_GENERIC;
// Initial signals // Initial signals
emit disarmed(); emit disarmed();
emit armingChanged(false); emit armingChanged(false);
@ -646,7 +673,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
attitudeKnown = true; attitudeKnown = true;
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); 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; break;
@ -685,15 +712,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
} }
// dongfang: For APM, this altitude is the mix altitude[m] // dongfang: For APM, this altitude is the mix altitude[m]
emit altitudeChanged(uasId, hud.alt); emit altitudeChanged(this, PRIMARY_ALTITUDE, hud.alt, time);
// dongfang: For APM, airspeed is airspeed or AHRS estimated airspeed
// dongfang: For APM, climb rate is barometric // This makes the lateral velocity zero when it might really not be, problematic.
// dongfang: The signal has no parameter for groundspeed. // emit speedChanged(this, PRIMARY_SPEED, hud.airspeed, 0.0f, hud.climb, time);
// dongfang: The signal is emitted also from other places, emit speedChanged(this, PRIMARY_SPEED, hud.airspeed, time);
// such as GPS xyz speeds. This will cause a mix of signals emit speedChanged(this, GROUNDSPEED_BY_UAV, hud.groundspeed, time);
// from different sensors, which will probably not be so good. emit climbRateChanged(this, BAROMETRIC_ALTITUDE, hud.groundspeed, time);
float weAlsoWantGroundSpeedPlease = hud.groundspeed;
emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time);
} }
break; break;
case MAVLINK_MSG_ID_LOCAL_POSITION_NED: case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
@ -707,7 +732,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Emit position always with component ID // Emit position always with component ID
emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
if (!wrongComponent) if (!wrongComponent)
{ {
localX = pos.x; localX = pos.x;
@ -717,7 +741,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Emit // Emit
emit localPositionChanged(this, pos.x, pos.y, pos.z, time); 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 // Set internal state
if (!positionLock) { if (!positionLock) {
@ -745,18 +769,29 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_global_position_int_t pos; mavlink_global_position_int_t pos;
mavlink_msg_global_position_int_decode(&message, &pos); mavlink_msg_global_position_int_decode(&message, &pos);
quint64 time = getUnixTime(); quint64 time = getUnixTime();
//latitude = pos.lat/(double)1E7;
setLatitude(pos.lat/(double)1E7); setLatitude(pos.lat/(double)1E7);
//longitude = pos.lon/(double)1E7;
setLongitude(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); setAltitude(pos.alt/1000.0);
globalEstimatorActive = true; globalEstimatorActive = true;
speedX = pos.vx/100.0; speedX = pos.vx/100.0;
speedY = pos.vy/100.0; speedY = pos.vy/100.0;
speedZ = pos.vz/100.0; speedZ = pos.vz/100.0;
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time); 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 // Set internal state
if (!positionLock) if (!positionLock)
@ -797,14 +832,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
longitude_gps = pos.lon/(double)1E7; longitude_gps = pos.lon/(double)1E7;
altitude_gps = pos.alt/1000.0; altitude_gps = pos.alt/1000.0;
// If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
if (!globalEstimatorActive) { if (!globalEstimatorActive) {
//latitude = latitude_gps;
setLatitude(latitude_gps); setLatitude(latitude_gps);
//longitude = longitude_gps;
setLongitude(longitude_gps); setLongitude(longitude_gps);
//altitude = altitude_gps;
setAltitude(altitude_gps); setAltitude(altitude_gps);
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time); emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time);
emit altitudeChanged(this, GPS_ALTITUDE, getAltitude(), time);
} }
positionLock = true; positionLock = true;
@ -814,10 +848,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
float vel = pos.vel/100.0f; float vel = pos.vel/100.0f;
// If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
if (!globalEstimatorActive) { if (!globalEstimatorActive) {
if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) 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 else
{ {
@ -1360,6 +1396,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_nav_controller_output_t p; mavlink_nav_controller_output_t p;
mavlink_msg_nav_controller_output_decode(&message,&p); mavlink_msg_nav_controller_output_decode(&message,&p);
setDistToWaypoint(p.wp_dist); 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; break;
case MAVLINK_MSG_ID_RAW_PRESSURE: case MAVLINK_MSG_ID_RAW_PRESSURE:
@ -1491,7 +1532,7 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
/** /**
* Set the home position of the UAS. * Set the home position of the UAS.
* @param lat The latitude fo the home position * @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 * @param alt The altitude of the home position
*/ */
void UAS::setHomePosition(double lat, double lon, double alt) void UAS::setHomePosition(double lat, double lon, double alt)

139
src/uas/UAS.h
Unescape View File

@ -56,16 +56,6 @@ public:
UAS(MAVLinkProtocol* protocol, int id = 0); UAS(MAVLinkProtocol* protocol, int id = 0);
~UAS(); ~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 lipoFull = 4.2f; ///< 100% charged voltage
static const int lipoEmpty = 3.5f; ///< Discharged voltage static const int lipoEmpty = 3.5f; ///< Discharged voltage
@ -105,7 +95,6 @@ public:
Q_PROPERTY(double localZ READ getLocalZ WRITE setLocalZ NOTIFY localZChanged) Q_PROPERTY(double localZ READ getLocalZ WRITE setLocalZ NOTIFY localZChanged)
Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged) Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged)
Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged) 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(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged)
Q_PROPERTY(bool isLocalPositionKnown READ localPositionKnown) Q_PROPERTY(bool isLocalPositionKnown READ localPositionKnown)
Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown) Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown)
@ -113,6 +102,11 @@ public:
Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged) Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged)
Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged) Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged)
Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged) 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) void setLocalX(double val)
{ {
@ -120,6 +114,7 @@ public:
emit localXChanged(val,"localX"); emit localXChanged(val,"localX");
emit valueChanged(this->uasId,"localX","M",QVariant(val),getUnixTime()); emit valueChanged(this->uasId,"localX","M",QVariant(val),getUnixTime());
} }
double getLocalX() const double getLocalX() const
{ {
return localX; return localX;
@ -153,6 +148,7 @@ public:
emit latitudeChanged(val,"latitude"); emit latitudeChanged(val,"latitude");
emit valueChanged(this->uasId,"latitude","deg",QVariant(val),getUnixTime()); emit valueChanged(this->uasId,"latitude","deg",QVariant(val),getUnixTime());
} }
double getLatitude() const double getLatitude() const
{ {
return latitude; return latitude;
@ -164,6 +160,7 @@ public:
emit longitudeChanged(val,"longitude"); emit longitudeChanged(val,"longitude");
emit valueChanged(this->uasId,"longitude","deg",QVariant(val),getUnixTime()); emit valueChanged(this->uasId,"longitude","deg",QVariant(val),getUnixTime());
} }
double getLongitude() const double getLongitude() const
{ {
return longitude; return longitude;
@ -172,7 +169,7 @@ public:
void setAltitude(double val) void setAltitude(double val)
{ {
altitude = val; altitude = val;
emit altitudeChanged(val,"altitude"); emit altitudeChanged(val, "altitude");
emit valueChanged(this->uasId,"altitude","M",QVariant(val),getUnixTime()); emit valueChanged(this->uasId,"altitude","M",QVariant(val),getUnixTime());
} }
@ -197,6 +194,7 @@ public:
{ {
return isLocalPositionKnown; return isLocalPositionKnown;
} }
virtual bool globalPositionKnown() const virtual bool globalPositionKnown() const
{ {
return isGlobalPositionKnown; return isGlobalPositionKnown;
@ -214,6 +212,19 @@ public:
return distToWaypoint; 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) void setRoll(double val)
{ {
roll = val; roll = val;
@ -246,6 +257,7 @@ public:
{ {
return yaw; return yaw;
} }
bool getSelected() const; bool getSelected() const;
QVector3D getNedPosGlobalOffset() const QVector3D getNedPosGlobalOffset() const
{ {
@ -319,31 +331,43 @@ public:
friend class UASWaypointManager; friend class UASWaypointManager;
protected: //COMMENTS FOR TEST UNIT protected: //COMMENTS FOR TEST UNIT
/// LINK ID AND STATUS
int uasId; ///< Unique system ID int uasId; ///< Unique system ID
unsigned char type; ///< UAS type (from type enum) QMap<int, QString> components;///< IDs and names of all detected onboard components
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
QList<LinkInterface*>* links; ///< List of links this UAS can be reached by QList<LinkInterface*>* links; ///< List of links this UAS can be reached by
QList<int> unknownPackets; ///< Packet IDs which are unknown and have been received QList<int> unknownPackets; ///< Packet IDs which are unknown and have been received
MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance
BatteryType batteryType; ///< The battery type CommStatus commStatus; ///< Communication status
int cells; ///< Number of cells 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
UASWaypointManager waypointManager; 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<double> actuatorValues;
QList<QString> actuatorNames; QList<QString> actuatorNames;
QList<double> motorValues; QList<double> motorValues;
QList<QString> motorNames; 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 thrustSum; ///< Sum of forward/up thrust of all thrust actuators, in Newtons
double thrustMax; ///< Maximum forward/up thrust of this vehicle, 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 fullVoltage; ///< Voltage of the fully charged battery (100%)
float emptyVoltage; ///< Voltage of the empty battery (0%) float emptyVoltage; ///< Voltage of the empty battery (0%)
float startVoltage; ///< Voltage at system start float startVoltage; ///< Voltage at system start
@ -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 bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life
float chargeLevel; ///< Charge level of battery, in percent float chargeLevel; ///< Charge level of battery, in percent
int timeRemaining; ///< Remaining time calculated based on previous and current int timeRemaining; ///< Remaining time calculated based on previous and current
uint8_t mode; ///< The current mode of the MAV bool lowBattAlarm; ///< Switch if battery is low
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 /// TIMEKEEPING
quint64 startTime; ///< The time the UAS was switched on
quint64 onboardTimeOffset; quint64 onboardTimeOffset;
/// MANUAL CONTROL
bool controlRollManual; ///< status flag, true if roll is controlled manually bool controlRollManual; ///< status flag, true if roll is controlled manually
bool controlPitchManual; ///< status flag, true if pitch is controlled manually bool controlPitchManual; ///< status flag, true if pitch is controlled manually
bool controlYawManual; ///< status flag, true if yaw is controlled manually bool controlYawManual; ///< status flag, true if yaw is controlled manually
@ -373,16 +399,20 @@ protected: //COMMENTS FOR TEST UNIT
double manualPitchAngle; ///< Pitch angle set by human pilot (radians) double manualPitchAngle; ///< Pitch angle set by human pilot (radians)
double manualYawAngle; ///< Yaw angle set by human pilot (radians) double manualYawAngle; ///< Yaw angle set by human pilot (radians)
double manualThrust; ///< Thrust 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 /// POSITION
bool lowBattAlarm; ///< Switch if battery is low
bool positionLock; ///< Status if position information is available or not 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 localX;
double localY; double localY;
double localZ; double localZ;
double latitude; ///< Global latitude as estimated by position estimator double latitude; ///< Global latitude as estimated by position estimator
double longitude; ///< Global longitude as estimated by position estimator double longitude; ///< Global longitude as estimated by position estimator
double altitude; ///< Global altitude as estimated by position estimator double altitude; ///< Global altitude as estimated by position estimator
double satelliteCount; ///< Number of satellites visible to raw GPS double satelliteCount; ///< Number of satellites visible to raw GPS
bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position 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 latitude_gps; ///< Global latitude as estimated by raw GPS
@ -391,13 +421,25 @@ protected: //COMMENTS FOR TEST UNIT
double speedX; ///< True speed in X axis double speedX; ///< True speed in X axis
double speedY; ///< True speed in Y axis double speedY; ///< True speed in Y axis
double speedZ; ///< True speed in Z 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 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 roll;
double pitch; double pitch;
double yaw; 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 imageSize; ///< Image size being transmitted (bytes)
int imagePackets; ///< Number of data packets being sent for this image int imagePackets; ///< Number of data packets being sent for this image
int imagePacketsArrived; ///< Number of data packets recieved int imagePacketsArrived; ///< Number of data packets recieved
@ -432,21 +474,13 @@ protected: //COMMENTS FOR TEST UNIT
qreal receivedRGBDImageTimestamp; qreal receivedRGBDImageTimestamp;
#endif #endif
/// PARAMETERS
QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
bool paramsOnceRequested; ///< If the parameter list has been read at least once 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 QGCUASParamManager* paramManager; ///< Parameter manager class
QString shortStateText; ///< Short textual state description
QString shortModeText; ///< Short textual mode description /// SIMULATION
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
QGCHilLink* simulation; ///< Hardware in the loop simulation link 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: public:
/** @brief Set the current battery type */ /** @brief Set the current battery type */
@ -647,12 +681,12 @@ public slots:
void enableHilXPlane(bool enable); void enableHilXPlane(bool enable);
/** @brief Send the full HIL state to the MAV */ /** @brief Send the full HIL state to the MAV */
void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
float pitchspeed, float yawspeed, double lat, double lon, double alt, float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
float vx, float vy, float vz, float xacc, float yacc, float zacc); float vx, float vy, float vz, float xacc, float yacc, float zacc);
/** @brief RAW sensors for sensor HIL */ /** @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); 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:
void longitudeChanged(double val,QString name); void longitudeChanged(double val,QString name);
void latitudeChanged(double val,QString name); void latitudeChanged(double val,QString name);
void altitudeChanged(double val,QString name); void altitudeChanged(double val,QString name);
void altitudeChanged(int uasid, double altitude);
void rollChanged(double val,QString name); void rollChanged(double val,QString name);
void pitchChanged(double val,QString name); void pitchChanged(double val,QString name);
void yawChanged(double val,QString name); void yawChanged(double val,QString name);
void satelliteCountChanged(double val,QString name); void satelliteCountChanged(double val,QString name);
void distToWaypointChanged(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: protected:
/** @brief Get the UNIX timestamp in milliseconds, enter microseconds */ /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
quint64 getUnixTime(quint64 time=0); quint64 getUnixTime(quint64 time=0);
@ -827,6 +863,7 @@ protected:
quint64 getUnixTimeFromMs(quint64 time); quint64 getUnixTimeFromMs(quint64 time);
/** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
quint64 getUnixReferenceTime(quint64 time); quint64 getUnixReferenceTime(quint64 time);
int componentID[256]; int componentID[256];
bool componentMulti[256]; bool componentMulti[256];
bool connectionLost; ///< Flag indicates a timed out connection 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
#endif #endif
#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. * @brief Interface for all robots.
* *
@ -477,7 +511,7 @@ signals:
void heartbeat(UASInterface* uas); void heartbeat(UASInterface* uas);
void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec); 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 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); 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 */ /** @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); void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
@ -486,10 +520,19 @@ signals:
void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); 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 localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec);
void globalPositionChanged(UASInterface*, double lat, double lon, double alt, 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 */ /** @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 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 imageStarted(int imgid, int width, int height, int depth, int channels);
void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex); void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex);
/** @brief Emit the new system type */ /** @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
emit valueChanged(uasId, "rollspeed", "rad/s", this->m_rotVel[0], time); 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, "pitchspeed", "rad/s", this->m_rotVel[1], time);
emit valueChanged(uasId, "yawspeed", "rad/s", this->m_rotVel[2], 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; break;
} }
case MAVLINK_MSG_ID_LLC_OUT: // low level controller output 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_obs_air_temp_t airTMsg;
mavlink_msg_obs_air_temp_decode(&message,&airTMsg); mavlink_msg_obs_air_temp_decode(&message,&airTMsg);
quint64 time = getUnixTime(); quint64 time = getUnixTime();
emit valueChanged(uasId, "Air Temp", "°", airTMsg.airT, time); emit valueChanged(uasId, "Air Temp", "°", airTMsg.airT, time);
break; break;
} }
case MAVLINK_MSG_ID_OBS_AIR_VELOCITY: 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])))); 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]); 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; return;
} }

147
src/ui/PrimaryFlightDisplay.cpp
Unescape View File

@ -40,16 +40,20 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren
uas(NULL), uas(NULL),
altimeterMode(GPS_MAIN),
altimeterFrame(ASL),
speedMode(GROUND_MAIN),
roll(UNKNOWN_ATTITUDE), roll(UNKNOWN_ATTITUDE),
pitch(UNKNOWN_ATTITUDE), pitch(UNKNOWN_ATTITUDE),
// heading(NAN), // heading(NAN),
heading(UNKNOWN_ATTITUDE), heading(UNKNOWN_ATTITUDE),
aboveASLAltitude(UNKNOWN_ALTITUDE), primaryAltitude(UNKNOWN_ALTITUDE),
GPSAltitude(UNKNOWN_ALTITUDE), GPSAltitude(UNKNOWN_ALTITUDE),
aboveHomeAltitude(UNKNOWN_ALTITUDE), aboveHomeAltitude(UNKNOWN_ALTITUDE),
primarySpeed(UNKNOWN_SPEED),
groundspeed(UNKNOWN_SPEED), groundspeed(UNKNOWN_SPEED),
airspeed(UNKNOWN_SPEED), verticalVelocity(UNKNOWN_ALTITUDE),
verticalVelocity(UNKNOWN_SPEED),
font("Bitstream Vera Sans"), font("Bitstream Vera Sans"),
refreshTimer(new QTimer(this)), refreshTimer(new QTimer(this)),
@ -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*,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(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(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(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(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); //disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
disconnect(this->uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool))); //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(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(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(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(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(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); 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) { if (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*,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(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(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(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(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); //connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
connect(uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool))); //connect(uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool)));
connect(uas, SIGNAL(satelliteCountChanged(double, QString)), this, SLOT(updateSatelliteCount(double, QString))); //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(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(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(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); 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 // Set new UAS
this->uas = uas; this->uas = uas;
@ -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 * TODO! Examine what data comes with this call, should we consider it airspeed, ground speed or
* should we not consider it at all? * 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(uas);
Q_UNUSED(timestamp); Q_UNUSED(timestamp);
Q_UNUSED(z);
/*
this->xSpeed = x;
this->ySpeed = y;
this->zSpeed = z;
*/
double newTotalSpeed = qSqrt(x*x + y*y/*+ zSpeed*zSpeed */); if (source == PRIMARY_SPEED) {
// totalAcc = (newTotalSpeed - totalSpeed) / ((double)(lastSpeedUpdate - timestamp)/1000.0); 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. void PrimaryFlightDisplay::updateClimbRate(UASInterface* uas, AltitudeMeasurementSource source, double climbRate, quint64 timestamp) {
groundspeed = newTotalSpeed; Q_UNUSED(uas);
airspeed = x; Q_UNUSED(timestamp);
verticalVelocity = z; 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 * 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 ( void PrimaryFlightDisplay::drawTextCenter (
QPainter& painter, QPainter& painter,
QString text, QString text,
@ -832,12 +870,10 @@ void PrimaryFlightDisplay::drawAltimeter(
QPainter& painter, QPainter& painter,
QRectF area, // the area where to draw the tape. QRectF area, // the area where to draw the tape.
float primaryAltitude, float primaryAltitude,
float maxAltitude, float secondaryAltitude,
float vv float vv
) { ) {
Q_UNUSED(maxAltitude)
painter.resetTransform(); painter.resetTransform();
fillInstrumentBackground(painter, area); fillInstrumentBackground(painter, area);
@ -849,8 +885,14 @@ void PrimaryFlightDisplay::drawAltimeter(
float h = area.height(); float h = area.height();
float w = area.width(); float w = area.width();
float secondaryAltitudeBoxHeight = mediumTextSize * 2;
// The height where we being with new tickmarks. // The height where we being with new tickmarks.
float effectiveHalfHeight = h*0.45; float effectiveHalfHeight = h*0.45;
// not yet implemented: Display of secondary altitude.
// if (isAirplane())
// effectiveHalfHeight-= secondaryAltitudeBoxHeight;
float markerHalfHeight = mediumTextSize*0.8; float markerHalfHeight = mediumTextSize*0.8;
float leftEdge = instrumentEdgePen.widthF()*2; float leftEdge = instrumentEdgePen.widthF()*2;
float rightEdge = w-leftEdge; float rightEdge = w-leftEdge;
@ -929,26 +971,34 @@ void PrimaryFlightDisplay::drawAltimeter(
drawTextCenter(painter, s_alt, /* TAPES_TEXT_SIZE*width()*/ mediumTextSize, xCenter, 0); drawTextCenter(painter, s_alt, /* TAPES_TEXT_SIZE*width()*/ mediumTextSize, xCenter, 0);
if (vv == UNKNOWN_ALTITUDE) return; if (vv == UNKNOWN_ALTITUDE) return;
float vvPixHeight = vv/ALTIMETER_VVI_SPAN * effectiveHalfHeight; float vvPixHeight = -vv/ALTIMETER_VVI_SPAN * effectiveHalfHeight;
if (abs (vvPixHeight)<markerHalfHeight) return; 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); // 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 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); 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); 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); painter.drawLine(vvArrowHead, vvArrowEnd);
} }
void PrimaryFlightDisplay::drawVelocityMeter( void PrimaryFlightDisplay::drawVelocityMeter(
QPainter& painter, QPainter& painter,
QRectF area QRectF area,
float speed,
float secondarySpeed
) { ) {
painter.resetTransform(); painter.resetTransform();
@ -969,10 +1019,9 @@ void PrimaryFlightDisplay::drawVelocityMeter(
float markerTip = (tickmarkLeftMajor+tickmarkRight*2)/3; float markerTip = (tickmarkLeftMajor+tickmarkRight*2)/3;
// Select between air and ground speed: // Select between air and ground speed:
float primarySpeed = airspeed;
float centerScaleSpeed = float centerScaleSpeed =
primarySpeed == UNKNOWN_SPEED ? 0 : primarySpeed; speed == UNKNOWN_SPEED ? 0 : speed;
float start = centerScaleSpeed - AIRSPEED_LINEAR_SPAN/2; float start = centerScaleSpeed - AIRSPEED_LINEAR_SPAN/2;
float end = centerScaleSpeed + AIRSPEED_LINEAR_SPAN/2; float end = centerScaleSpeed + AIRSPEED_LINEAR_SPAN/2;
@ -1020,10 +1069,10 @@ void PrimaryFlightDisplay::drawVelocityMeter(
pen.setColor(Qt::white); pen.setColor(Qt::white);
painter.setPen(pen); painter.setPen(pen);
QString s_alt; QString s_alt;
if (primarySpeed == UNKNOWN_SPEED) if (speed == UNKNOWN_SPEED)
s_alt.sprintf("---"); s_alt.sprintf("---");
else else
s_alt.sprintf("%3.1f", primarySpeed); s_alt.sprintf("%3.1f", speed);
float xCenter = (markerTip+leftEdge)/2; float xCenter = (markerTip+leftEdge)/2;
drawTextCenter(painter, s_alt, /* TAPES_TEXT_SIZE*width()*/ mediumTextSize, xCenter, 0); drawTextCenter(painter, s_alt, /* TAPES_TEXT_SIZE*width()*/ mediumTextSize, xCenter, 0);
} }
@ -1360,11 +1409,9 @@ void PrimaryFlightDisplay::doPaint() {
painter.setClipping(hadClip); painter.setClipping(hadClip);
// X: To the right of AI and with single margin again. That is, 3 single margins plus width of AI. drawAltimeter(painter, altimeterArea, primaryAltitude, GPSAltitude, verticalVelocity);
// Y: 1 single margin below above gadget.
drawAltimeter(painter, altimeterArea, aboveASLAltitude, 1000, 3); drawVelocityMeter(painter, velocityMeterArea, primarySpeed, groundspeed);
drawVelocityMeter(painter, velocityMeterArea);
/* /*
drawSensorsStatsPanel(painter, sensorsStatsArea); drawSensorsStatsPanel(painter, sensorsStatsArea);

55
src/ui/PrimaryFlightDisplay.h
Unescape View File

@ -106,8 +106,12 @@ public slots:
void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
/** @brief Attitude from one specific component / redundant autopilot */ /** @brief Attitude from one specific component / redundant autopilot */
void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp); 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: protected:
enum Layout { enum Layout {
@ -136,23 +140,34 @@ protected:
// dongfang: We have no context menu. Viewonly. // dongfang: We have no context menu. Viewonly.
// void contextMenuEvent (QContextMenuEvent* event); // void contextMenuEvent (QContextMenuEvent* event);
protected:
// dongfang: What is that? // dongfang: What is that?
// dongfang: OK it's for UI interaction. Presently, there is none. // dongfang: OK it's for UI interaction. Presently, there is none.
void createActions(); void createActions();
public slots:
/** @brief Set the currently monitored UAS */
virtual void setActiveUAS(UASInterface* uas);
protected slots:
signals: signals:
void visibilityChanged(bool visible); void visibilityChanged(bool visible);
private: private:
//void prepareTransform(QPainter& painter, qreal width, qreal height); enum AltimeterMode {
//void transformToGlobalSystem(QPainter& painter, qreal width, qreal height, float roll, float pitch); 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 drawTextCenter(QPainter& painter, QString text, float fontSize, float x, float y);
void drawTextLeftCenter(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:
void drawAICompassDisk(QPainter& painter, QRectF area, float halfspan); void drawAICompassDisk(QPainter& painter, QRectF area, float halfspan);
void drawSeparateCompassDisk(QPainter& painter, QRectF area); void drawSeparateCompassDisk(QPainter& painter, QRectF area);
void drawAltimeter(QPainter& painter, QRectF area, float altitude, float maxAltitude, float vv); void drawAltimeter(QPainter& painter, QRectF area, float altitude, float secondaryAltitude, float vv);
void drawVelocityMeter(QPainter& painter, QRectF area); void drawVelocityMeter(QPainter& painter, QRectF area, float speed, float secondarySpeed);
void fillInstrumentBackground(QPainter& painter, QRectF edge); void fillInstrumentBackground(QPainter& painter, QRectF edge);
void fillInstrumentOpagueBackground(QPainter& painter, QRectF edge); void fillInstrumentOpagueBackground(QPainter& painter, QRectF edge);
void drawInstrumentBackground(QPainter& painter, QRectF edge); void drawInstrumentBackground(QPainter& painter, QRectF edge);
@ -184,21 +199,27 @@ private:
UASInterface* uas; ///< The uas currently monitored UASInterface* uas; ///< The uas currently monitored
AltimeterMode altimeterMode;
AltimeterFrame altimeterFrame;
SpeedMode speedMode;
bool didReceivePrimaryAltitude;
bool didReceivePrimarySpeed;
float roll; float roll;
float pitch; float pitch;
float heading; float heading;
// APM: GPS and baro mix. From the GLOBAL_POSITION_INT or VFR_HUD messages. float primaryAltitude;
float aboveASLAltitude;
float GPSAltitude; float GPSAltitude;
// APM: GPS and baro mix above home (GPS) altitude. This value comes from the GLOBAL_POSITION_INT message. // 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. // 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 aboveHomeAltitude;
float primarySpeed;
float groundspeed; float groundspeed;
float airspeed;
float verticalVelocity; float verticalVelocity;
Layout layout; // The display layout. Layout layout; // The display layout.

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

@ -73,6 +73,18 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent)
uasPropertyToLabelMap["distToWaypoint"] = item; 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); updateTimer = new QTimer(this);
connect(updateTimer,SIGNAL(timeout()),this,SLOT(updateTimerTick())); connect(updateTimer,SIGNAL(timeout()),this,SLOT(updateTimerTick()));
updateTimer->start(1000); updateTimer->start(1000);

Write Preview
Loading…
Cancel
Save