Skip to content

Q
qgroundcontrol
Unverified Commit 79f54537 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #6528 from DonLakeFlyer/Attitude 

Remove usage of ATTITUDE in favor of ATTITUDE_QUATERNION
parents 11494b72 82a340f7
Show whitespace changes
Inline Side-by-side
Showing with 35 additions and 263 deletions
src/Vehicle/Vehicle.cc
View file @ 79f54537
......@@ -235,9 +235,6 @@ Vehicle::Vehicle(LinkInterface* link,
// Listen for system messages
connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage);
connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived, this, &Vehicle::_handletextMessageReceived);
// Now connect the new UAS
connect(_mav, SIGNAL(attitudeChanged (UASInterface*,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
if (_highLatencyLink || link->isPX4Flow()) {
// These links don't request information
......@@ -709,8 +706,8 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_HIGH_LATENCY2:
_handleHighLatency2(message);
break;
case MAVLINK_MSG_ID_ATTITUDE:
_handleAttitude(message);
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
_handleAttitudeQuaternion(message);
break;
case MAVLINK_MSG_ID_ATTITUDE_TARGET:
_handleAttitudeTarget(message);
......@@ -846,15 +843,37 @@ void Vehicle::_handleAttitudeTarget(mavlink_message_t& message)
_setpointFactGroup.yawRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_yaw_rate));
}
void Vehicle::_handleAttitude(mavlink_message_t& message)
void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message)
{
mavlink_attitude_t attitude;
mavlink_attitude_quaternion_t attitudeQuaternion;
mavlink_msg_attitude_decode(&message, &attitude);
mavlink_msg_attitude_quaternion_decode(&message, &attitudeQuaternion);
rollRate()->setRawValue(qRadiansToDegrees(attitude.rollspeed));
pitchRate()->setRawValue(qRadiansToDegrees(attitude.pitchspeed));
yawRate()->setRawValue(qRadiansToDegrees(attitude.yawspeed));
float roll, pitch, yaw;
float q[] = { attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4 };
mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw);
roll = QGC::limitAngleToPMPIf(roll);
pitch = QGC::limitAngleToPMPIf(pitch);
yaw = QGC::limitAngleToPMPIf(yaw);
roll = qRadiansToDegrees(roll);
pitch = qRadiansToDegrees(pitch);
yaw = qRadiansToDegrees(yaw);
if (yaw < 0.0) {
yaw += 360.0;
}
// truncate to integer so widget never displays 360
yaw = trunc(yaw);
_rollFact.setRawValue(roll);
_pitchFact.setRawValue(pitch);
_headingFact.setRawValue(yaw);
rollRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.rollspeed));
pitchRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.pitchspeed));
yawRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.yawspeed));
}
void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
......@@ -1788,33 +1807,6 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
}
}
void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
{
if (qIsInf(roll)) {
_rollFact.setRawValue(0);
} else {
_rollFact.setRawValue(roll * (180.0 / M_PI));
}
if (qIsInf(pitch)) {
_pitchFact.setRawValue(0);
} else {
_pitchFact.setRawValue(pitch * (180.0 / M_PI));
}
if (qIsInf(yaw)) {
_headingFact.setRawValue(0);
} else {
yaw = yaw * (180.0 / M_PI);
if (yaw < 0.0) yaw += 360.0;
// truncate to integer so widget never displays 360
_headingFact.setRawValue(trunc(yaw));
}
}
void Vehicle::_updateAttitude(UASInterface* uas, int, double roll, double pitch, double yaw, quint64 timestamp)
{
_updateAttitude(uas, roll, pitch, yaw, timestamp);
}
int Vehicle::motorCount(void)
{
switch (_vehicleType) {
......@@ -1846,19 +1838,6 @@ bool Vehicle::xConfigMotors(void)
return _firmwarePlugin->multiRotorXConfig(this);
}
/*
* Internal
*/
QString Vehicle::getMavIconColor()
{
// TODO: Not using because not only the colors are ghastly, it doesn't respect dark/light palette
if(_mav)
return _mav->getColor().name();
else
return QString("black");
}
QString Vehicle::formatedMessages()
{
QString messages;
......
src/Vehicle/Vehicle.h
View file @ 79f54537
......@@ -556,8 +556,6 @@ public:
/// @return -1: reserver all buttons, >0 number of buttons to reserve
Q_PROPERTY(int manualControlReservedButtonCount READ manualControlReservedButtonCount CONSTANT)
Q_INVOKABLE QString getMavIconColor();
// Called when the message drop-down is invoked to clear current count
Q_INVOKABLE void resetMessages();
......@@ -1041,10 +1039,6 @@ private slots:
void _handleTextMessage (int newCount);
void _handletextMessageReceived (UASMessage* message);
/** @brief Attitude from main autopilot / system state */
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);
/** @brief A new camera image has arrived */
void _imageReady (UASInterface* uas);
void _prearmErrorTimeout(void);
......@@ -1089,7 +1083,7 @@ private:
void _handleScaledPressure2(mavlink_message_t& message);
void _handleScaledPressure3(mavlink_message_t& message);
void _handleHighLatency2(mavlink_message_t& message);
void _handleAttitude(mavlink_message_t& message);
void _handleAttitudeQuaternion(mavlink_message_t& message);
void _handleAttitudeTarget(mavlink_message_t& message);
void _handleDistanceSensor(mavlink_message_t& message);
// ArduPilot dialect messages
......
src/comm/QGCXPlaneLink.cc
View file @ 79f54537
......@@ -1064,7 +1064,7 @@ void QGCXPlaneLink::setRandomPosition()
_vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt,
_vehicle->roll()->rawValue().toDouble(),
_vehicle->pitch()->rawValue().toDouble(),
_vehicle->uas()->getYaw());
_vehicle->heading()->rawValue().toDouble());
}
void QGCXPlaneLink::setRandomAttitude()
......
src/uas/UAS.cc
View file @ 79f54537
......@@ -73,10 +73,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
attitudeStamped(false),
lastAttitude(0),
roll(0.0),
pitch(0.0),
yaw(0.0),
imagePackets(0), // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images
blockHomePositionChanges(false),
......@@ -133,7 +129,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
#ifndef __mobile__
connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage);
color = UASInterface::getNextColor();
#endif
}
......@@ -275,86 +270,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
}
break;
case MAVLINK_MSG_ID_ATTITUDE:
{
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&message, &attitude);
quint64 time = getUnixReferenceTime(attitude.time_boot_ms);
emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time);
if (!wrongComponent)
{
lastAttitude = time;
setRoll(QGC::limitAngleToPMPIf(attitude.roll));
setPitch(QGC::limitAngleToPMPIf(attitude.pitch));
setYaw(QGC::limitAngleToPMPIf(attitude.yaw));
attitudeKnown = true;
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
}
break;
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
{
mavlink_attitude_quaternion_t attitude;
mavlink_msg_attitude_quaternion_decode(&message, &attitude);
quint64 time = getUnixReferenceTime(attitude.time_boot_ms);
double a = attitude.q1;
double b = attitude.q2;
double c = attitude.q3;
double d = attitude.q4;
double aSq = a * a;
double bSq = b * b;
double cSq = c * c;
double dSq = d * d;
float dcm[3][3];
dcm[0][0] = aSq + bSq - cSq - dSq;
dcm[0][1] = 2.0 * (b * c - a * d);
dcm[0][2] = 2.0 * (a * c + b * d);
dcm[1][0] = 2.0 * (b * c + a * d);
dcm[1][1] = aSq - bSq + cSq - dSq;
dcm[1][2] = 2.0 * (c * d - a * b);
dcm[2][0] = 2.0 * (b * d - a * c);
dcm[2][1] = 2.0 * (a * b + c * d);
dcm[2][2] = aSq - bSq - cSq + dSq;
float phi, theta, psi;
theta = asin(-dcm[2][0]);
if (fabs(theta - M_PI_2) < 1.0e-3f) {
phi = 0.0f;
psi = (atan2(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1]) + phi);
} else if (fabs(theta + M_PI_2) < 1.0e-3f) {
phi = 0.0f;
psi = atan2f(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1] - phi);
} else {
phi = atan2f(dcm[2][1], dcm[2][2]);
psi = atan2f(dcm[1][0], dcm[0][0]);
}
emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi),
QGC::limitAngleToPMPIf(theta),
QGC::limitAngleToPMPIf(psi), time);
if (!wrongComponent)
{
lastAttitude = time;
setRoll(QGC::limitAngleToPMPIf(phi));
setPitch(QGC::limitAngleToPMPIf(theta));
setYaw(QGC::limitAngleToPMPIf(psi));
attitudeKnown = true;
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
}
break;
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
mavlink_hil_controls_t hil;
......@@ -362,27 +277,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode);
}
break;
case MAVLINK_MSG_ID_VFR_HUD:
{
mavlink_vfr_hud_t hud;
mavlink_msg_vfr_hud_decode(&message, &hud);
quint64 time = getUnixTime();
if (!attitudeKnown)
{
setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
}
break;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
{
mavlink_global_vision_position_estimate_t pos;
mavlink_msg_global_vision_position_estimate_decode(&message, &pos);
quint64 time = getUnixTime(pos.usec);
emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
}
break;
case MAVLINK_MSG_ID_PARAM_VALUE:
{
......
src/uas/UAS.h
View file @ 79f54537
......@@ -60,46 +60,9 @@ public:
/** @brief The time interval the robot is switched on */
quint64 getUptime() const;
Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged)
Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged)
Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged)
/// Vehicle is about to go away
void shutdownVehicle(void);
void setRoll(double val)
{
roll = val;
emit rollChanged(val,"roll");
}
double getRoll() const
{
return roll;
}
void setPitch(double val)
{
pitch = val;
emit pitchChanged(val,"pitch");
}
double getPitch() const
{
return pitch;
}
void setYaw(double val)
{
yaw = val;
emit yawChanged(val,"yaw");
}
double getYaw() const
{
return yaw;
}
// Setters for HIL noise variance
void setXaccVar(float var){
xacc_var = var;
......@@ -196,9 +159,6 @@ protected: //COMMENTS FOR TEST UNIT
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;
// dongfang: This looks like a candidate for being moved off to a separate class.
/// IMAGING
......
src/uas/UASInterface.h
View file @ 79f54537
......@@ -45,62 +45,12 @@ public:
/** @brief The time interval the robot is switched on **/
virtual quint64 getUptime() const = 0;
virtual double getRoll() const = 0;
virtual double getPitch() const = 0;
virtual double getYaw() const = 0;
#ifndef __mobile__
virtual FileManager* getFileManager() = 0;
#endif
/**
* @brief Get the color for this UAS
*
* This static function holds a color map that allows to draw a new color for each robot
*
* @return The next color in the color map. The map holds 20 colors and starts from the beginning
* if the colors are exceeded.
*/
#if !defined(__mobile__)
static QColor getNextColor() {
/* Create color map */
static QList<QColor> colors = QList<QColor>()
<< QColor(231,72,28)
<< QColor(104,64,240)
<< QColor(203,254,121)
<< QColor(161,252,116)
<< QColor(232,33,47)
<< QColor(116,251,110)
<< QColor(234,38,107)
<< QColor(104,250,138)
<< QColor(235,43,165)
<< QColor(98,248,176)
<< QColor(236,48,221)
<< QColor(92,247,217)
<< QColor(200,54,238)
<< QColor(87,231,246)
<< QColor(151,59,239)
<< QColor(81,183,244)
<< QColor(75,133,243)
<< QColor(242,255,128)
<< QColor(230,126,23);
static int nextColor = -1;
if(nextColor == 18){//if at the end of the list
nextColor = -1;//go back to the beginning
}
nextColor++;
return colors[nextColor];//return the next color
}
#endif
virtual QMap<int, QString> getComponents() = 0;
QColor getColor()
{
return color;
}
enum StartCalibrationType {
StartCalibrationRadio,
StartCalibrationGyro,
......@@ -160,9 +110,6 @@ public slots:
/** @brief Send command to disable all bindings/maps between RC and parameters */
virtual void unsetRCToParameterMap() = 0;
protected:
QColor color;
signals:
/** @brief A text message from the system has been received */
void textMessageReceived(int uasid, int componentid, int severity, QString text);
......@@ -213,8 +160,6 @@ signals:
*/
void batteryChanged(UASInterface* uas, double voltage, double current, double percent, int seconds);
void statusChanged(UASInterface* uas, QString status);
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 imageStarted(int imgid, int width, int height, int depth, int channels);
void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex);
......
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