SITL: fixed quadplane mass and rpm indexing

libraries/SITL/SIM_Aircraft.cpp

@@ -352,6 +352,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.battery_voltage = battery_voltage;
     fdm.battery_current = battery_current;
     fdm.num_motors = num_motors;
+    fdm.vtol_motor_start = vtol_motor_start;
     memcpy(fdm.rpm, rpm, num_motors * sizeof(float));
     fdm.rcin_chan_count = rcin_chan_count;
     fdm.range = range;

libraries/SITL/SIM_Aircraft.h

@@ -171,6 +171,7 @@ protected:
     float battery_voltage = -1.0f;
     float battery_current;
     uint8_t num_motors = 1;
+    uint8_t vtol_motor_start;
     float rpm[12];
     uint8_t rcin_chan_count;
     float rcin[12];

libraries/SITL/SIM_Frame.cpp

@@ -489,8 +489,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
         thrust += mthrust;
         // simulate motor rpm
         if (!is_zero(AP::sitl()->vibe_motor)) {
-            const float mot_thrust_max = thrust_max / num_motors;
-            rpm[i] = sqrtf(mthrust.length() / mot_thrust_max) * AP::sitl()->vibe_motor * 60.0f;
+            rpm[i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f;
         }
     }
 

libraries/SITL/SIM_Frame.h

@@ -65,6 +65,11 @@ public:
         return mass;
     }
 
+    // set mass in kg
+    void set_mass(float new_mass) {
+        mass = new_mass;
+    }
+    
 private:
     /*
       parameters that define the multicopter model. Can be loaded from

libraries/SITL/SIM_Motor.h

@@ -96,6 +96,10 @@ public:
         slew_max = _slew_max;
     }
 
+    float get_command(void) const {
+        return last_command;
+    }
+
     // calculate thrust of motor
     float calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const;
 

libraries/SITL/SIM_QuadPlane.cpp

@@ -74,6 +74,7 @@ QuadPlane::QuadPlane(const char *frame_str) :
         exit(1);
     }
     num_motors = 1 + frame->num_motors;
+    vtol_motor_start = 1;
 
     if (strstr(frame_str, "cl84")) {
         // setup retract servos at front
@@ -89,6 +90,10 @@ QuadPlane::QuadPlane(const char *frame_str) :
     // we use zero terminal velocity to let the plane model handle the drag
     frame->init(frame_str);
 
+    // increase mass for plane components
+    mass = frame->get_mass() * 1.5;
+    frame->set_mass(mass);
+
     ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
 }
 

libraries/SITL/SITL.h

@@ -56,6 +56,7 @@ struct sitl_fdm {
     double battery_voltage; // Volts
     double battery_current; // Amps
     uint8_t num_motors;
+    uint8_t vtol_motor_start;
     float rpm[12];         // RPM of all motors
     uint8_t rcin_chan_count;
     float  rcin[12];         // RC input 0..1