Commit 10840b7c authored by dongfang's avatar dongfang

Multiple altitudes and speeds signals. Simplified PFD

parent 13dcbfae
This diff is collapsed.
This diff is collapsed.
...@@ -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
#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: ...@@ -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: ...@@ -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 */
......
...@@ -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, "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 ...@@ -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 ...@@ -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;
} }
\ No newline at end of file
This diff is collapsed.
...@@ -106,8 +106,12 @@ public slots: ...@@ -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: ...@@ -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: ...@@ -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: ...@@ -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.
......
...@@ -73,6 +73,18 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent) ...@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment