From 161096d059b38f2c4f625582c566676f4c64fe68 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Wed, 30 May 2018 09:43:40 -0700 Subject: [PATCH] Move mavlink attitude handling to Vehicle --- src/Vehicle/Vehicle.cc | 68 ++++++++++++--------------- src/Vehicle/Vehicle.h | 6 +-- src/uas/UAS.cc | 101 ----------------------------------------- src/uas/UASInterface.h | 2 - 4 files changed, 31 insertions(+), 146 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2fdb1d584..e2bac965b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -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) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 329dc47ce..735280c35 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1041,10 +1041,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 +1085,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 diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 9663774cb..039d373fa 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -275,86 +275,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 +282,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: { diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index af0257e7f..bd162d1c6 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -213,8 +213,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); -- 2.22.0