SITL: add rpm-based motor noise per-motor to gyros and accels

convert multicopter thrust to rpm per motor

libraries/SITL/SIM_Aircraft.cpp

@@ -55,6 +55,7 @@ Aircraft::Aircraft(const char *frame_str) :
     rate_hz(1200.0f),
     autotest_dir(nullptr),
     frame(frame_str),
+    num_motors(1),
 #if defined(__CYGWIN__) || defined(__CYGWIN64__)
     min_sleep_time(20000)
 #else
@@ -341,8 +342,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.airspeed = airspeed_pitot;
     fdm.battery_voltage = battery_voltage;
     fdm.battery_current = battery_current;
-    fdm.rpm1 = rpm1;
-    fdm.rpm2 = rpm2;
+    fdm.num_motors = num_motors;
+    memcpy(fdm.rpm, rpm, num_motors * sizeof(float));
     fdm.rcin_chan_count = rcin_chan_count;
     fdm.range = range;
     memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));

libraries/SITL/SIM_Aircraft.h

@@ -155,8 +155,8 @@ protected:
     float airspeed_pitot;                // m/s, apparent airspeed, as seen by fwd pitot tube
     float battery_voltage = -1.0f;
     float battery_current = 0.0f;
-    float rpm1 = 0;
-    float rpm2 = 0;
+    uint8_t num_motors = 1;
+    float rpm[12];
     uint8_t rcin_chan_count = 0;
     float rcin[8];
     float range = -1.0f;                 // rangefinder detection in m

libraries/SITL/SIM_EFI_MegaSquirt.cpp

@@ -46,7 +46,7 @@ void EFI_MegaSquirt::update()
     if (!connected) {
         return;
     }
-    float rpm = sitl->state.rpm1;
+    float rpm = sitl->state.rpm[0];
 
     table7.rpm = rpm;
     table7.fuelload = 20;

libraries/SITL/SIM_FlightAxis.cpp

@@ -86,6 +86,7 @@ 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;
@@ -473,8 +474,8 @@ void FlightAxis::update(const struct sitl_input &input)
 
     battery_voltage = state.m_batteryVoltage_VOLTS;
     battery_current = state.m_batteryCurrentDraw_AMPS;
-    rpm1 = state.m_heliMainRotorRPM;
-    rpm2 = state.m_propRPM;
+    rpm[0] = state.m_heliMainRotorRPM;
+    rpm[1] = state.m_propRPM;
 
     /*
       the interlink interface supports 8 input channels

libraries/SITL/SIM_Frame.cpp

@@ -206,13 +206,13 @@ static Frame supported_frames[] =
     Frame("firefly",   6, firefly_motors)
 };
 
-void Frame::init(float _mass, float hover_throttle, float _terminal_velocity, float _terminal_rotation_rate)
+void Frame::init(float _mass, float _hover_throttle, float _terminal_velocity, float _terminal_rotation_rate)
 {
     /*
        scaling from total motor power to Newtons. Allows the copter
        to hover against gravity when each motor is at hover_throttle
     */
-    thrust_scale = (_mass * GRAVITY_MSS) / (num_motors * hover_throttle);
+    thrust_scale = (_mass * GRAVITY_MSS) / (num_motors * _hover_throttle);
 
     terminal_velocity = _terminal_velocity;
     terminal_rotation_rate = _terminal_rotation_rate;
@@ -228,6 +228,8 @@ Frame *Frame::find_frame(const char *name)
         if (strncasecmp(name, supported_frames[i].name, strlen(supported_frames[i].name)) == 0) {
             return &supported_frames[i];
         }
+
+
     }
     return nullptr;
 }
@@ -236,7 +238,8 @@ Frame *Frame::find_frame(const char *name)
 void Frame::calculate_forces(const Aircraft &aircraft,
                              const struct sitl_input &input,
                              Vector3f &rot_accel,
-                             Vector3f &body_accel)
+                             Vector3f &body_accel,
+                             float* rpm)
 {
     Vector3f thrust; // newtons
 
@@ -245,6 +248,10 @@ void Frame::calculate_forces(const Aircraft &aircraft,
         motors[i].calculate_forces(input, thrust_scale, motor_offset, mraccel, mthrust);
         rot_accel += mraccel;
         thrust += mthrust;
+        // simulate motor rpm
+        if (!is_zero(AP::sitl()->vibe_motor)) {
+            rpm[i] = sqrtf(mthrust.length() / thrust_scale) * AP::sitl()->vibe_motor * 60.0f;
+        }
     }
 
     body_accel = thrust/aircraft.gross_mass();

libraries/SITL/SIM_Frame.h

@@ -49,7 +49,7 @@ public:
     // calculate rotational and linear accelerations
     void calculate_forces(const Aircraft &aircraft,
                           const struct sitl_input &input,
-                          Vector3f &rot_accel, Vector3f &body_accel);
+                          Vector3f &rot_accel, Vector3f &body_accel, float* rpm);
     
     float terminal_velocity;
     float terminal_rotation_rate;

libraries/SITL/SIM_Helicopter.cpp

@@ -152,7 +152,7 @@ void Helicopter::update(const struct sitl_input &input)
     Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
 
     // simulate rotor speed
-    rpm1 = thrust * 1300;
+    rpm[0] = thrust * 1300;
 
     // scale thrust to newtons
     thrust *= thrust_scale;

libraries/SITL/SIM_JSBSim.cpp

@@ -62,6 +62,7 @@ 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);
@@ -443,8 +444,8 @@ void JSBSim::recv_fdm(const struct sitl_input &input)
     // update magnetic field
     update_mag_field_bf();
     
-    rpm1 = fdm.rpm[0];
-    rpm2 = fdm.rpm[1];
+    rpm[0] = fdm.rpm[0];
+    rpm[1] = fdm.rpm[1];
     
     time_now_us = fdm.cur_time;
 }

libraries/SITL/SIM_Multicopter.cpp

@@ -42,13 +42,15 @@ MultiCopter::MultiCopter(const char *frame_str) :
         frame->init(gross_mass(), 0.51, 15, 4*radians(360));
     }
     frame_height = 0.1;
+    num_motors = frame->num_motors;
     ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
 }
 
 // calculate rotational and linear accelerations
 void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
 {
-    frame->calculate_forces(*this, input, rot_accel, body_accel);
+    frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);
+
     add_shove_forces(rot_accel, body_accel);
     add_twist_forces(rot_accel);
 }
@@ -78,3 +80,4 @@ void MultiCopter::update(const struct sitl_input &input)
     // update magnetic field
     update_mag_field_bf();
 }
+

libraries/SITL/SIM_Plane.cpp

@@ -34,6 +34,7 @@ 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;
     
@@ -342,7 +343,7 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
     }
     
     // simulate engine RPM
-    rpm1 = thrust * 7000;
+    rpm[0] = thrust * 7000;
     
     // scale thrust to newtons
     thrust *= thrust_scale;

libraries/SITL/SIM_QuadPlane.cpp

@@ -73,6 +73,7 @@ QuadPlane::QuadPlane(const char *frame_str) :
         printf("Failed to find frame '%s'\n", frame_type);
         exit(1);
     }
+    num_motors = 1 + frame->num_motors;
 
     if (strstr(frame_str, "cl84")) {
         // setup retract servos at front
@@ -107,7 +108,7 @@ 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);
+    frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1]);
 
     // estimate voltage and current
     frame->current_and_voltage(input, battery_voltage, battery_current);

libraries/SITL/SIM_Sailboat.cpp

@@ -182,7 +182,7 @@ void Sailboat::update(const struct sitl_input &input)
     const float wind_apparent_dir_bf = wrap_180(wind_apparent_dir_ef - degrees(AP::ahrs().yaw));
 
     // set RPM and airspeed from wind speed, allows to test RPM and Airspeed wind vane back end in SITL
-    rpm1 = wind_apparent_speed;
+    rpm[0] = wind_apparent_speed;
     airspeed_pitot = wind_apparent_speed;
 
     // calculate angle-of-attack from wind to mainsail

libraries/SITL/SIM_Scrimmage.cpp

@@ -190,8 +190,8 @@ void Scrimmage::recv_fdm(const struct sitl_input &input)
 
     battery_voltage = 0;
     battery_current = 0;
-    rpm1 = 0;
-    rpm2 = 0;
+    rpm[0] = 0;
+    rpm[1] = 0;
 
     airspeed = pkt.airspeed;
     airspeed_pitot = pkt.airspeed;

libraries/SITL/SIM_XPlane.cpp

@@ -42,6 +42,7 @@ 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",
@@ -258,11 +259,11 @@ bool XPlane::receive_data(void)
         }
 
         case EngineRPM:
-            rpm1 = data[1];
+            rpm[0] = data[1];
             break;
 
         case PropRPM:
-            rpm2 = data[1];
+            rpm[1] = data[1];
             break;
             
         case Joystick2:

libraries/SITL/SITL.cpp

@@ -204,6 +204,9 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
 
     AP_GROUPINFO("MAG_SCALING",    60, SITL,  mag_scaling, 1),
 
+    // max motor vibration frequency
+    AP_GROUPINFO("VIB_MOT_MAX", 61, SITL,  vibe_motor, 0.0f),
+
     AP_GROUPEND
 
 };

libraries/SITL/SITL.h

@@ -45,8 +45,8 @@ struct sitl_fdm {
     double airspeed; // m/s
     double battery_voltage; // Volts
     double battery_current; // Amps
-    double rpm1;            // main prop RPM
-    double rpm2;            // secondary RPM
+    uint8_t num_motors;
+    float rpm[12];         // RPM of all motors
     uint8_t rcin_chan_count;
     float  rcin[8];         // RC input 0..1
     double range;           // rangefinder value
@@ -248,6 +248,9 @@ public:
     // vibration frequencies in Hz on each axis
     AP_Vector3f vibe_freq;
 
+    // hover frequency to use as baseline for adding motor noise for the gyros and accels
+    AP_Float vibe_motor;
+
     // gyro and accel fail masks
     AP_Int8 gyro_fail_mask;
     AP_Int8 accel_fail_mask;