Merge pull request #6537 from DonLakeFlyer/MavlinkAttitude

Put back support for ATTITUDE since ArduPilot needs it

src/QGC.cc

@@ -30,7 +30,7 @@ qreal groundTimeSeconds()
     return static_cast<qreal>(groundTimeMilliseconds()) / 1000.0f;
 }
 
-float limitAngleToPMPIf(float angle)
+float limitAngleToPMPIf(double angle)
 {
     if (angle > -20*M_PI && angle < 20*M_PI)
     {

src/QGC.h

@@ -33,7 +33,7 @@ quint64 groundTimeMilliseconds();
  */
 qreal groundTimeSeconds();
 /** @brief Returns the angle limited to -pi - pi */
-float limitAngleToPMPIf(float angle);
+float limitAngleToPMPIf(double angle);
 /** @brief Returns the angle limited to -pi - pi */
 double limitAngleToPMPId(double angle);
 

src/Vehicle/Vehicle.cc

@@ -134,6 +134,7 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _vehicleCapabilitiesKnown(false)
     , _capabilityBits(0)
     , _highLatencyLink(false)
+    , _receivingAttitudeQuaternion(false)
     , _cameras(NULL)
     , _connectionLost(false)
     , _connectionLostEnabled(true)
@@ -318,6 +319,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _vehicleCapabilitiesKnown(true)
     , _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
     , _highLatencyLink(false)
+    , _receivingAttitudeQuaternion(false)
     , _cameras(NULL)
     , _connectionLost(false)
     , _connectionLostEnabled(true)
@@ -706,6 +708,9 @@ 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);
+        break;
     case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
         _handleAttitudeQuaternion(message);
         break;
@@ -843,19 +848,13 @@ void Vehicle::_handleAttitudeTarget(mavlink_message_t& message)
     _setpointFactGroup.yawRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_yaw_rate));
 }
 
-void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message)
+void Vehicle::_handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians)
 {
-    mavlink_attitude_quaternion_t attitudeQuaternion;
-
-    mavlink_msg_attitude_quaternion_decode(&message, &attitudeQuaternion);
-
-    float roll, pitch, yaw;
-    float q[] = { attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4 };
-    mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw);
+    double roll, pitch, yaw;
 
-    roll = QGC::limitAngleToPMPIf(roll);
-    pitch = QGC::limitAngleToPMPIf(pitch);
-    yaw = QGC::limitAngleToPMPIf(yaw);
+    roll = QGC::limitAngleToPMPIf(rollRadians);
+    pitch = QGC::limitAngleToPMPIf(pitchRadians);
+    yaw = QGC::limitAngleToPMPIf(yawRadians);
 
     roll = qRadiansToDegrees(roll);
     pitch = qRadiansToDegrees(pitch);
@@ -870,6 +869,32 @@ void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message)
     _rollFact.setRawValue(roll);
     _pitchFact.setRawValue(pitch);
     _headingFact.setRawValue(yaw);
+}
+
+void Vehicle::_handleAttitude(mavlink_message_t& message)
+{
+    if (_receivingAttitudeQuaternion) {
+        return;
+    }
+
+    mavlink_attitude_t attitude;
+    mavlink_msg_attitude_decode(&message, &attitude);
+
+    _handleAttitudeWorker(attitude.roll, attitude.pitch, attitude.yaw);
+}
+
+void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message)
+{
+    _receivingAttitudeQuaternion = true;
+
+    mavlink_attitude_quaternion_t attitudeQuaternion;
+    mavlink_msg_attitude_quaternion_decode(&message, &attitudeQuaternion);
+
+    float roll, pitch, yaw;
+    float q[] = { attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4 };
+    mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw);
+
+    _handleAttitudeWorker(roll, pitch, yaw);
 
     rollRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.rollspeed));
     pitchRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.pitchspeed));

src/Vehicle/Vehicle.h

@@ -1083,6 +1083,8 @@ private:
     void _handleScaledPressure2(mavlink_message_t& message);
     void _handleScaledPressure3(mavlink_message_t& message);
     void _handleHighLatency2(mavlink_message_t& message);
+    void _handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians);
+    void _handleAttitude(mavlink_message_t& message);
     void _handleAttitudeQuaternion(mavlink_message_t& message);
     void _handleAttitudeTarget(mavlink_message_t& message);
     void _handleDistanceSensor(mavlink_message_t& message);
@@ -1173,6 +1175,7 @@ private:
     bool            _vehicleCapabilitiesKnown;
     uint64_t        _capabilityBits;
     bool            _highLatencyLink;
+    bool            _receivingAttitudeQuaternion;
 
     QGCCameraManager* _cameras;