Merge pull request #385 from limhyon/master

Quadrotor HILS Update

src/comm/QGCXPlaneLink.cc

@@ -292,15 +292,6 @@ void QGCXPlaneLink::updateActuators(uint64_t time, float act1, float act2, float
 
 void QGCXPlaneLink::updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode)
 {
-    // Do not update this control type for
-    // all multirotors
-    if (mav->getSystemType() == MAV_TYPE_QUADROTOR ||
-            mav->getSystemType() == MAV_TYPE_HEXAROTOR ||
-            mav->getSystemType() == MAV_TYPE_OCTOROTOR)
-    {
-        return;
-    }
-
     #pragma pack(push, 1)
     struct payload {
         char b[5];
@@ -315,7 +306,11 @@ void QGCXPlaneLink::updateControls(uint64_t time, float rollAilerons, float pitc
     p.b[3] = 'A';
     p.b[4] = '\0';
 
-    p.index = 12;
+    Q_UNUSED(time);
+    Q_UNUSED(systemMode);
+    Q_UNUSED(navMode);
+
+    bool isFixedWing = true;
 
     if (mav->getAirframe() == UASInterface::QGC_AIRFRAME_X8 ||
             mav->getAirframe() == UASInterface::QGC_AIRFRAME_VIPER_2_0 ||
@@ -329,31 +324,52 @@ void QGCXPlaneLink::updateControls(uint64_t time, float rollAilerons, float pitc
         // yaw
         p.f[2] = 0.0f;
     }
+    else if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
+    {
+        qDebug() << "MAV_TYPE_QUADROTOR";
+
+        // Individual effort will be provided directly to the actuators on Xplane quadrotor.
+        p.f[0] = yawRudder;
+        p.f[1] = rollAilerons;
+        p.f[2] = throttle;
+        p.f[3] = pitchElevator;
+
+        isFixedWing = false;
+    }
     else
     {
-        // direct pass-through
+        // direct pass-through, normal fixed-wing.
         p.f[0] = -pitchElevator;
         p.f[1] = rollAilerons;
         p.f[2] = yawRudder;
     }
 
-    Q_UNUSED(time);
-    Q_UNUSED(systemMode);
-    Q_UNUSED(navMode);
+    if(isFixedWing)
+    {
+        // Ail / Elevon / Rudder
+        p.index = 12;   // XPlane, wing sweep
+        writeBytes((const char*)&p, sizeof(p));
+
+        p.index = 8;    // XPlane, joystick? why?
+        writeBytes((const char*)&p, sizeof(p));
+
+        p.index = 25;   // Thrust
+        memset(p.f, 0, sizeof(p.f));
+        p.f[0] = throttle;
+        p.f[1] = throttle;
+        p.f[2] = throttle;
+        p.f[3] = throttle;
+
+        // Throttle
+        writeBytes((const char*)&p, sizeof(p));
+    }
+    else
+    {
+        qDebug() << "Transmitting p.index = 25";
+        p.index = 25;   // XPlane, throttle command.
+        writeBytes((const char*)&p, sizeof(p));
+    }
 
-    // Ail / Elevon / Rudder
-    writeBytes((const char*)&p, sizeof(p));
-    p.index = 8;
-    writeBytes((const char*)&p, sizeof(p));
-
-    p.index = 25;
-    memset(p.f, 0, sizeof(p.f));
-    p.f[0] = throttle;
-    p.f[1] = throttle;
-    p.f[2] = throttle;
-    p.f[3] = throttle;
-    // Throttle
-    writeBytes((const char*)&p, sizeof(p));
 }
 
 Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {

src/uas/UAS.cc

@@ -1326,6 +1326,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
         }
             break;
+#if 0
         case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
         {
             mavlink_servo_output_raw_t raw;
@@ -1340,6 +1341,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
         }
         break;
+#endif
 #ifdef MAVLINK_ENABLED_PIXHAWK
         case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE:
         {