Merge pull request #421 from toppers/develop

fixed #418, 419, 420
toppers · Nov 18, 2024 · 914a3f2 · 914a3f2
2 parents 5bfff09 + f55bc04
commit 914a3f2
Show file tree

Hide file tree

Showing 9 changed files with 27 additions and 13 deletions.
diff --git a/drone_control/common/include/drone_alt_controller.hpp b/drone_control/common/include/drone_alt_controller.hpp
@@ -103,8 +103,8 @@ class DroneAltController {
             /*
              * thrust
              */
-            //out.thrust = (mass * gravity) + (throttle_gain * throttle_power);
-            out.thrust = (throttle_gain * throttle_power);
+            out.thrust = (mass * gravity) + (throttle_gain * throttle_power);
+            //out.thrust = (throttle_gain * throttle_power);
             spd_prev_out = out;
         }
         spd_simulation_time += delta_time;

diff --git a/drone_control/config/param-api-mixer.txt b/drone_control/config/param-api-mixer.txt
@@ -11,7 +11,7 @@ PID_ALT_MAX_SPD         5.0
 
 ## 高度制御のPIDパラメータ
 PID_ALT_Kp              5.0
-PID_ALT_Ki              0.3
+PID_ALT_Ki              0.0
 PID_ALT_Kd              5.0
 PID_ALT_SPD_Kp          5.0
 PID_ALT_SPD_Ki          0.0

diff --git a/hakoniwa/config/api_sample/drone_config_0.json b/hakoniwa/config/api_sample/drone_config_0.json
@@ -123,7 +123,6 @@
               }
           ],
           "Ct": 8.3E-07,
-          "Cq": 3.0E-8,
           "Jr": 0.0
       },
       "sensors": {

diff --git a/hakoniwa/config/drone_config_0.json b/hakoniwa/config/drone_config_0.json
@@ -123,7 +123,6 @@
               }
           ],
           "Ct": 8.3E-07,
-          "Cq": 3.0E-8,
           "Jr": 0.0
       },
       "sensors": {

diff --git a/hakoniwa/config/rc-kanko/drone_config_0.json b/hakoniwa/config/rc-kanko/drone_config_0.json
@@ -119,7 +119,6 @@
               }
           ],
           "Ct": 8.3E-07,
-          "Cq": 3.0E-8,
           "Jr": 0.0
       },
       "sensors": {

diff --git a/hakoniwa/config/rc/drone_config_0.json b/hakoniwa/config/rc/drone_config_0.json
@@ -67,13 +67,13 @@
       },
       "battery": {
         "vendor": "None",
-        "model": "constant",
+        "model": "linear",
         "BatteryModelCsvFilePath": "./tmp_battery_model.csv",
         "VoltageLevelGreen": 12.1, 
         "VoltageLevelYellow": 11.1,
-        "CapacityLevelYellow": 900, 
+        "CapacityLevelYellow": 3, 
         "NominalVoltage": 14.8,
-        "NominalCapacity": 1000.0,
+        "NominalCapacity": 4.0,
         "EODVoltage": 3.0
       },
       "rotor": {
@@ -123,7 +123,6 @@
               }
           ],
           "Ct": 8.3E-07,
-          "Cq": 3.0E-8,
           "Jr": 0.0
       },
       "sensors": {

diff --git a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp
@@ -50,7 +50,6 @@ using hako::assets::drone::SensorNoise;
 #define MAG_SAMPLE_NUM              drone_config.getCompSensorSampleCount("mag")
 
 #define THRUST_PARAM_Ct              drone_config.getCompThrusterParameter("Ct")
-#define THRUST_PARAM_Cq              drone_config.getCompThrusterParameter("Cq")
 #define THRUST_PARAM_JR             drone_config.getCompThrusterParameter("Jr")
 
 #define LOGPATH(index, name)        drone_config.getSimLogFullPathFromIndex(index, name)
@@ -180,7 +179,7 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr
     auto thrust_vendor = drone_config.getCompThrusterVendor();
     std::cout<< "Thruster vendor: " << thrust_vendor << std::endl;
     double param_Ct = THRUST_PARAM_Ct;
-    double param_Cq = THRUST_PARAM_Cq;
+    double param_Cq = rotor_constants.Cq;
     std::cout << "param_Ct: " << param_Ct << std::endl;
     std::cout << "param_Cq: " << param_Cq << std::endl;
     if (thrust_vendor == "linear") {
@@ -221,6 +220,10 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr
         HAKO_ASSERT(mixer != nullptr);
         bool inv_m = mixer->calculate_M_inv();
         HAKO_ASSERT(inv_m == true);
+        mixer_info.K = rotor_constants.K;
+        mixer_info.R = rotor_constants.R;
+        mixer_info.Cq = rotor_constants.Cq;
+        mixer_info.V_bat = battery_config.NominalVoltage;
         mixer->setMixerInfo(mixer_info);
         drone->set_mixer(mixer);
     }

diff --git a/hakoniwa/src/assets/drone/controller/drone_mixer.hpp b/hakoniwa/src/assets/drone/controller/drone_mixer.hpp
@@ -24,6 +24,14 @@ namespace hako::assets::drone {
         glm::dmat4 A_inv;
         glm::dmat4 MA_inv;
         DroneConfig::MixerInfo mixer_info {};
+
+        double get_duty(double omega) {
+            //e = K * w + (Cq * R / K ) w ^2
+            double e = mixer_info.K * omega + (mixer_info.Cq * mixer_info.R / mixer_info.K) * omega * omega;
+            //d = e / V_bat
+            double d = e / mixer_info.V_bat;
+            return d;
+        }
     public: 
         virtual ~DroneMixer() {}
         DroneMixer(double Kr, double a, double b, RotorConfigType rotor[ROTOR_NUM])
@@ -98,7 +106,8 @@ namespace hako::assets::drone {
                     Omega2[i] = 0;
                 }
                 else {
-                    duty.d[i] = std::sqrt(Omega2[i]) / this->param_Kr;
+                    //duty.d[i] = std::sqrt(Omega2[i]) / this->param_Kr;
+                    duty.d[i] = get_duty(std::sqrt(Omega2[i]));
                 }
                 if (mixer_info.enableDebugLog) {
                     std::cout << "Motor " << i << ": Omega^2 = " << Omega2[i] << ", PWM Duty = " << duty.d[i] << std::endl;

diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp
@@ -425,6 +425,12 @@ class DroneConfig {
         std::string vendor;
         bool enableDebugLog;
         bool enableErrorLog;
+
+        // not config info
+        double K;
+        double R;
+        double Cq;
+        double V_bat;
     };
     bool getControllerMixerInfo(MixerInfo& info) const
     {