From 6272dc33dd96364c34f2205af028313dc1f063f1 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Wed, 13 Apr 2022 15:03:53 +0100
Subject: [PATCH] SITL: move moments inertia to frame property

---
 libraries/SITL/SIM_Frame.cpp | 19 +++++++++++++++----
 libraries/SITL/SIM_Frame.h   |  3 +++
 libraries/SITL/SIM_Motor.cpp | 19 ++++---------------
 libraries/SITL/SIM_Motor.h   |  4 +---
 4 files changed, 23 insertions(+), 22 deletions(-)

diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp
index d7bac9fe7c..fc84513f87 100644
--- a/libraries/SITL/SIM_Frame.cpp
+++ b/libraries/SITL/SIM_Frame.cpp
@@ -476,9 +476,14 @@ void Frame::init(const char *frame_str, Battery *_battery)
 
     for (uint8_t i=0; i<num_motors; i++) {
         motors[i].setup_params(model.pwmMin, model.pwmMax, model.spin_min, model.spin_max, model.propExpo, model.slew_max,
-                               model.mass, model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max);
+                               model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max);
     }
 
+    // assume 50% of mass on ring around center
+    model.moment_of_inertia.x = model.mass * 0.25 * sq(model.diagonal_size*0.5);
+    model.moment_of_inertia.y = model.moment_of_inertia.x;
+    model.moment_of_inertia.z = model.mass * 0.5 * sq(model.diagonal_size*0.5);
+
 
 #if 0
     // useful debug code for thrust curve
@@ -530,6 +535,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
                              bool use_drag)
 {
     Vector3f thrust; // newtons
+    Vector3f torque;
 
     const float air_density = get_air_density(aircraft.get_location().alt*0.01);
 
@@ -537,10 +543,10 @@ void Frame::calculate_forces(const Aircraft &aircraft,
 
     float current = 0;
     for (uint8_t i=0; i<num_motors; i++) {
-        Vector3f mraccel, mthrust;
-        motors[i].calculate_forces(input, motor_offset, mraccel, mthrust, vel_air_bf, air_density, battery->get_voltage());
+        Vector3f mtorque, mthrust;
+        motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, air_density, battery->get_voltage());
         current += motors[i].get_current();
-        rot_accel += mraccel;
+        torque += mtorque;
         thrust += mthrust;
         // simulate motor rpm
         if (!is_zero(AP::sitl()->vibe_motor)) {
@@ -548,6 +554,11 @@ void Frame::calculate_forces(const Aircraft &aircraft,
         }
     }
 
+    // calculate total rotational acceleration
+    rot_accel.x = torque.x / model.moment_of_inertia.x;
+    rot_accel.y = torque.y / model.moment_of_inertia.y;
+    rot_accel.z = torque.z / model.moment_of_inertia.z;
+
     body_accel = thrust/aircraft.gross_mass();
 
     if (terminal_rotation_rate > 0) {
diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h
index 72c3b9e895..2f4ee6db38 100644
--- a/libraries/SITL/SIM_Frame.h
+++ b/libraries/SITL/SIM_Frame.h
@@ -127,6 +127,9 @@ private:
         // momentum drag coefficient
         float mdrag_coef = 0.2;
 
+        // if zero value will be estimated from mass
+        Vector3f moment_of_inertia;
+
     } default_model;
 
     struct Model model;
diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp
index 1c784764e3..da41cd0341 100644
--- a/libraries/SITL/SIM_Motor.cpp
+++ b/libraries/SITL/SIM_Motor.cpp
@@ -24,7 +24,7 @@ using namespace SITL;
 // calculate rotational accel and thrust for a motor
 void Motor::calculate_forces(const struct sitl_input &input,
                              uint8_t motor_offset,
-                             Vector3f &rot_accel,
+                             Vector3f &torque,
                              Vector3f &thrust,
                              const Vector3f &velocity_air_bf,
                              float air_density,
@@ -39,7 +39,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
 
     if (voltage_scale < 0.1) {
         // battery is dead
-        rot_accel.zero();
+        torque.zero();
         thrust.zero();
         current = 0;
         return;
@@ -96,7 +96,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
     last_change_usec = now;
 
     // calculate torque in newton-meters
-    Vector3f torque = (arm % thrust) + rotor_torque;
+    torque = (arm % thrust) + rotor_torque;
 
     // possibly rotate the thrust vector and the rotor torque
     if (!is_zero(roll) || !is_zero(pitch)) {
@@ -106,11 +106,6 @@ void Motor::calculate_forces(const struct sitl_input &input,
         torque = rotation * torque;
     }
 
-    // calculate total rotational acceleration
-    rot_accel.x = torque.x / moment_of_inertia.x;
-    rot_accel.y = torque.y / moment_of_inertia.y;
-    rot_accel.z = torque.z / moment_of_inertia.z;
-
     // calculate current
     float power = power_factor * fabsf(motor_thrust);
     current = power / MAX(voltage, 0.1);
@@ -151,7 +146,7 @@ float Motor::get_current(void) const
 
 // setup PWM ranges for this motor
 void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
-                         float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
+                         float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
                          float _velocity_max)
 {
     mot_pwm_min = _pwm_min;
@@ -160,17 +155,11 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min,
     mot_spin_max = _spin_max;
     mot_expo = _expo;
     slew_max = _slew_max;
-    vehicle_mass = _vehicle_mass;
     diagonal_size = _diagonal_size;
     power_factor = _power_factor;
     voltage_max = _voltage_max;
     effective_prop_area = _effective_prop_area;
     max_outflow_velocity = _velocity_max;
-
-    // assume 50% of mass on ring around center
-    moment_of_inertia.x = vehicle_mass * 0.25 * sq(diagonal_size*0.5);
-    moment_of_inertia.y = moment_of_inertia.x;
-    moment_of_inertia.z = vehicle_mass * 0.5 * sq(diagonal_size*0.5);
 }
 
 /*
diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h
index 6dddaa1963..2cbdf8f5f9 100644
--- a/libraries/SITL/SIM_Motor.h
+++ b/libraries/SITL/SIM_Motor.h
@@ -87,7 +87,7 @@ public:
 
     // setup motor key parameters
     void setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
-                      float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
+                      float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
                       float _velocity_max);
 
     // override slew limit
@@ -109,12 +109,10 @@ private:
     float mot_spin_max;
     float mot_expo;
     float slew_max;
-    float vehicle_mass;
     float diagonal_size;
     float current;
     float power_factor;
     float voltage_max;
-    Vector3f moment_of_inertia;
     float effective_prop_area;
     float max_outflow_velocity;