SITL switched to motor_mask for which actuators are motors

this allows for any output to be an ESC, which allows for proper
simulation of quadplanes with ESCs on outputs 5-8 or 9-12, for testing
notch filtering

libraries/SITL/SIM_Aircraft.cpp

@@ -390,9 +390,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.velocity_air_bf = velocity_air_bf;
     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.motor_mask = motor_mask;
+    memcpy(fdm.rpm, rpm, sizeof(fdm.rpm));
     fdm.rcin_chan_count = rcin_chan_count;
     fdm.range = rangefinder_range();
     memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));

libraries/SITL/SIM_Aircraft.h

@@ -91,16 +91,6 @@ public:
     // get frame rate of model in Hz
     float get_rate_hz(void) const { return rate_hz; }
 
-    // get number of motors for model
-    uint16_t get_num_motors() const {
-        return num_motors;
-    }
-
-    // get motor offset for model
-    virtual uint16_t get_motors_offset() const {
-        return 0;
-    }
-
     const Vector3f &get_gyro(void) const {
         return gyro;
     }
@@ -189,9 +179,8 @@ protected:
     // battery model
     Battery battery;
 
-    uint8_t num_motors = 1;
-    uint8_t vtol_motor_start;
-    float rpm[12];
+    uint32_t motor_mask;
+    float rpm[32];
     uint8_t rcin_chan_count;
     float rcin[12];
 

libraries/SITL/SIM_FlightAxis.cpp

@@ -109,7 +109,6 @@ FlightAxis::FlightAxis(const char *frame_str) :
     Aircraft(frame_str)
 {
     use_time_sync = false;
-    num_motors = 2;
     rate_hz = 250 / target_speedup;
     heli_demix = strstr(frame_str, "helidemix") != nullptr;
     rev4_servos = strstr(frame_str, "rev4") != nullptr;

libraries/SITL/SIM_Frame.cpp

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

libraries/SITL/SIM_JSBSim.cpp

@@ -64,7 +64,6 @@ JSBSim::JSBSim(const char *frame_str) :
     }
     control_port = 5505 + instance*10;
     fdm_port = 5504 + instance*10;
-    num_motors = 2;
 
     printf("JSBSim backend started: control_port=%u fdm_port=%u\n",
            control_port, fdm_port);

libraries/SITL/SIM_Multicopter.cpp

@@ -36,7 +36,6 @@ MultiCopter::MultiCopter(const char *frame_str) :
 
     mass = frame->get_mass();
     frame_height = 0.1;
-    num_motors = frame->num_motors;
     ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
     lock_step_scheduled = true;
 }
@@ -44,6 +43,7 @@ MultiCopter::MultiCopter(const char *frame_str) :
 // calculate rotational and linear accelerations
 void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
 {
+    motor_mask |= ((1U<<frame->num_motors)-1U) << frame->motor_offset;
     frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);
 
     add_shove_forces(rot_accel, body_accel);

libraries/SITL/SIM_Multicopter.h

@@ -34,11 +34,6 @@ public:
     /* update model by one time step */
     void update(const struct sitl_input &input) override;
 
-    // get motor offset for model
-    virtual uint16_t get_motors_offset() const override {
-        return frame->motor_offset;
-    }
-
     /* static object creator */
     static Aircraft *create(const char *frame_str) {
         return new MultiCopter(frame_str);

libraries/SITL/SIM_Plane.cpp

@@ -36,7 +36,6 @@ Plane::Plane(const char *frame_str) :
     */
     thrust_scale = (mass * GRAVITY_MSS) / hover_throttle;
     frame_height = 0.1f;
-    num_motors = 1;
 
     ground_behavior = GROUND_BEHAVIOR_FWD_ONLY;
     lock_step_scheduled = true;
@@ -354,7 +353,8 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
     }
     
     // simulate engine RPM
-    rpm[0] = thrust * 7000;
+    motor_mask |= (1U<<2);
+    rpm[2] = thrust * 7000;
     
     // scale thrust to newtons
     thrust *= thrust_scale;

libraries/SITL/SIM_QuadPlane.cpp

@@ -80,8 +80,6 @@ QuadPlane::QuadPlane(const char *frame_str) :
         printf("Failed to find frame '%s'\n", frame_type);
         exit(1);
     }
-    num_motors = 1 + frame->num_motors;
-    vtol_motor_start = 1;
 
     if (strstr(frame_str, "cl84")) {
         // setup retract servos at front
@@ -120,7 +118,8 @@ void QuadPlane::update(const struct sitl_input &input)
     Vector3f quad_rot_accel;
     Vector3f quad_accel_body;
 
-    frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1], false);
+    motor_mask |= ((1U<<frame->num_motors)-1U) << frame->motor_offset;
+    frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, rpm, false);
 
     // rotate frames for copter tailsitters
     if (copter_tailsitter) {

libraries/SITL/SIM_QuadPlane.h

@@ -39,12 +39,6 @@ public:
         return new QuadPlane(frame_str);
     }
 
-    // get motor offset for model
-    virtual uint16_t get_motors_offset() const override {
-        return frame->motor_offset;
-    }
-
-
 private:
     Frame *frame;
 };

libraries/SITL/SIM_XPlane.cpp

@@ -47,7 +47,6 @@ XPlane::XPlane(const char *frame_str) :
     }
 
     heli_frame = (strstr(frame_str, "-heli") != nullptr);
-    num_motors = 2;
 
     socket_in.bind("0.0.0.0", bind_port);
     printf("Waiting for XPlane data on UDP port %u and sending to port %u\n",

libraries/SITL/SITL.h

@@ -64,8 +64,8 @@ struct sitl_fdm {
     double battery_current; // Amps
     double battery_remaining; // Ah, if non-zero capacity
     uint8_t num_motors;
-    uint8_t vtol_motor_start;
-    float rpm[12];         // RPM of all motors
+    uint32_t motor_mask;
+    float rpm[32];         // RPM of all motors
     uint8_t rcin_chan_count;
     float  rcin[12];         // RC input 0..1
     double range;           // rangefinder value