SITL: allow quadplane simulator to simulate other frame types

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;
 }
 
-MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
-    Aircraft(home_str, frame_str),
-    frame(NULL)
+/*
+  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(frame_str, supported_frames[i].name) == 0) {
-            frame = &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)
+{
+    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;

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;
 };
 
 /*

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);
+
 }
 
 /*