From 5efe98a81440530a447d5ac37b6bfd92e9ec6b9e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 2 Feb 2016 11:34:29 +1100
Subject: [PATCH] SITL: allow quadplane simulator to simulate other frame types

---
 libraries/SITL/SIM_Multicopter.cpp | 21 +++++++++++-----
 libraries/SITL/SIM_Multicopter.h   |  5 ++++
 libraries/SITL/SIM_QuadPlane.cpp   | 40 +++++++++++++++++++-----------
 3 files changed, 46 insertions(+), 20 deletions(-)

diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index b95eb3d89d..2dd9a0b9d9 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -112,15 +112,24 @@ void Frame::init(float _mass, float hover_throttle, float _terminal_velocity, fl
     terminal_rotation_rate = _terminal_rotation_rate;
 }
 
+/*
+  find a frame by name
+ */
+Frame *Frame::find_frame(const char *name)
+{
+    for (uint8_t i=0; i < ARRAY_SIZE(supported_frames); i++) {
+        if (strcasecmp(name, supported_frames[i].name) == 0) {
+            return &supported_frames[i];
+        }
+    }
+    return NULL;
+}
+
 MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
     Aircraft(home_str, frame_str),
     frame(NULL)
 {
-    for (uint8_t i=0; i < ARRAY_SIZE(supported_frames); i++) {
-        if (strcasecmp(frame_str, supported_frames[i].name) == 0) {
-            frame = &supported_frames[i];
-        }
-    }
+    frame = Frame::find_frame(frame_str);
     if (frame == NULL) {
         printf("Frame '%s' not found", frame_str);
         exit(1);
@@ -138,7 +147,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
     float motor_speed[num_motors];
 
     for (uint8_t i=0; i<num_motors; i++) {
-        uint16_t servo = input.servos[motors[i].servo-1];
+        uint16_t servo = input.servos[motor_offset+motors[i].servo-1];
         // assume 1000 to 2000 PWM range
         if (servo <= 1000) {
             motor_speed[i] = 0;
diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h
index 397894a67c..b4fc662cc8 100644
--- a/libraries/SITL/SIM_Multicopter.h
+++ b/libraries/SITL/SIM_Multicopter.h
@@ -55,6 +55,10 @@ public:
         num_motors(_num_motors),
         motors(_motors) {}
 
+
+    // find a frame by name
+    static Frame *find_frame(const char *name);
+    
     // initialise frame
     void init(float mass, float hover_throttle, float terminal_velocity, float terminal_rotation_rate);
 
@@ -67,6 +71,7 @@ public:
     float terminal_rotation_rate;
     float thrust_scale;
     float mass;
+    uint8_t motor_offset;
 };
 
 /*
diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp
index 165ddb4cf1..aec495b2e1 100644
--- a/libraries/SITL/SIM_QuadPlane.cpp
+++ b/libraries/SITL/SIM_QuadPlane.cpp
@@ -23,27 +23,39 @@
 
 using namespace SITL;
 
-static const Motor quad_motors[4] =
-{
-    Motor(45,  false,  5),
-    Motor(225, false,  6),
-    Motor(315, true,   7),
-    Motor(135, true,   8)
-};
-
-static Frame quad_frame("x", 4, quad_motors);
-
 QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
     Plane(home_str, frame_str)
 {
-    frame = &quad_frame;
-    // we use zero terminal velocity to let the plane model handle the drag
-    frame->init(mass, 0.51, 0, 0);
-
     // see if we want a tiltrotor
     if (strstr(frame_str, "-tilt")) {
         tiltrotors = true;
     }
+    // default to X frame
+    const char *frame_type = "x";
+
+    if (strstr(frame_str, "-octa-quad")) {
+        frame_type = "octa-quad";
+    } else if (strstr(frame_str, "-octa")) {
+        frame_type = "octa";
+    } else if (strstr(frame_str, "-hexax")) {
+        frame_type = "hexax";
+    } else if (strstr(frame_str, "-hexa")) {
+        frame_type = "hexa";
+    } else if (strstr(frame_str, "-plus")) {
+        frame_type = "+";
+    }
+    frame = Frame::find_frame(frame_type);
+    if (frame == nullptr) {
+        printf("Failed to find frame '%s'\n", frame_type);
+        exit(1);
+    }
+
+    // leave first 4 servos free for plane
+    frame->motor_offset = 4;
+
+    // we use zero terminal velocity to let the plane model handle the drag
+    frame->init(mass, 0.51, 0, 0);
+
 }
 
 /*