From dfd61045ba5eb86a403ba33a321025df10019215 Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Thu, 31 Oct 2024 13:22:31 +0900 Subject: [PATCH 01/50] removed 'without_Coriolis_for_testing_only' function --- drone_physics/body_physics.cpp | 28 ---------------------------- drone_physics/body_physics.hpp | 8 -------- 2 files changed, 36 deletions(-) diff --git a/drone_physics/body_physics.cpp b/drone_physics/body_physics.cpp index cf62c9e2..996628e2 100644 --- a/drone_physics/body_physics.cpp +++ b/drone_physics/body_physics.cpp @@ -258,34 +258,6 @@ AccelerationType acceleration_in_body_frame( } -/* Obsolete. for testing only. */ -AccelerationType acceleration_in_body_frame_without_Coriolis_for_testing_only( - const VelocityType& body, - const EulerType& angle, - double thrust, double mass /* 0 is not allowed */, double gravity, double drag) -{ - assert(!is_zero(mass)); - using std::sin; using std::cos; - - const auto - c_phi = cos(angle.phi), s_phi = sin(angle.phi), - c_theta = cos(angle.theta), s_theta = sin(angle.theta); - const auto [u, v, w] = body; - const auto g = gravity; - const auto m = mass; - const auto d = drag; - const auto T = thrust; - - /*****************************************************************/ - double dot_u = - g * s_theta - d/m * u; - double dot_v = g * c_theta * s_phi - d/m * v; - double dot_w = -T/m + g * c_theta * c_phi - d/m * w; - /*****************************************************************/ - - return {dot_u, dot_v, dot_w}; -} - - /** * Acceleration in body frame based on eq.(2.46), (2.47) in Nonami's book. * A mistake in the book: (2.46) z-axis should be inverted (and also psi is inverted) in (2.47). diff --git a/drone_physics/body_physics.hpp b/drone_physics/body_physics.hpp index 9846da18..2e8ec392 100644 --- a/drone_physics/body_physics.hpp +++ b/drone_physics/body_physics.hpp @@ -117,14 +117,6 @@ AccelerationType acceleration_in_ground_frame( double drag1, /* air friction of 1-st order(-d1*v) counter to velocity */ double drag2 = 0.0 /* air friction of 2-nd order(-d2*v*v) counter to velocity */); - -/* physics for Force/Mass(F= ma) and Torque/Inertia(I dw/dt = T - w x Iw) */ -AccelerationType acceleration_in_body_frame_without_Coriolis_for_testing_only( - const VelocityType& body, - const EulerType& angle, - double thrust, double mass, /* 0 is not allowed */ - double gravity, double drag); - /* The translation dynamics. drags are vectors in the three directions */ AccelerationType acceleration_in_body_frame( const VelocityType& body_velocity, From f86f7f451abb1208ae7b0e2bd2d4a7e6948e43e4 Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Thu, 31 Oct 2024 13:46:14 +0900 Subject: [PATCH 02/50] 2-nd order wind effect --- drone_physics/body_physics.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/drone_physics/body_physics.cpp b/drone_physics/body_physics.cpp index 996628e2..a31b9f21 100644 --- a/drone_physics/body_physics.cpp +++ b/drone_physics/body_physics.cpp @@ -236,7 +236,7 @@ AccelerationType acceleration_in_body_frame( const auto g = gravity; const auto d1 = drag1; const auto d2 = drag2; - const auto [wx, wy, wz] = wind; + const auto [wind_u, wind_v, wind_w] = wind; /* * See nonami's book eq.(1.136).(2.31) @@ -246,12 +246,19 @@ AccelerationType acceleration_in_body_frame( * (1) 'g' is broken down to x, y, z components. * (2) T is only relavant to z-axis. * (3) Coriolis force(using uvw,pqr) IS needed(because the body frame is rotating!) - + * + * 10/31/2024: added wind effect to 1-st, 2-nd order drag. + * Note: ma = T - colioris - d1 v - d2 sing(v) (v*v) + * the air friction is always counter to the velocity vs wind, in the 1st and sencond order. */ - /*****************************************************************/ - double dot_u = - g * s_theta - (q*w - r*v) - d1.x/m * (u - wx) - d2.x/m * u * u; - double dot_v = + g * c_theta * s_phi - (r*u - p*w) - d1.y/m * (v - wy) - d2.y/m * v * v; - double dot_w = -T/m + g * c_theta * c_phi - (p*v - q*u) - d1.z/m * (w - wz) - d2.z/m * w * w; + /*****************************************************************/ + double air_u = u - wind_u; + double air_v = v - wind_v; + double air_w = w - wind_w; + + double dot_u = - g * s_theta - (q*w - r*v) - d1.x/m * (air_u) - d2.x/m * air_u * std::fabs(air_u); + double dot_v = + g * c_theta * s_phi - (r*u - p*w) - d1.y/m * (air_v) - d2.y/m * air_v * std::fabs(air_v); + double dot_w = -T/m + g * c_theta * c_phi - (p*v - q*u) - d1.z/m * (air_w) - d2.z/m * air_w * std::fabs(air_w); /*****************************************************************/ return {dot_u, dot_v, dot_w}; @@ -261,6 +268,9 @@ AccelerationType acceleration_in_body_frame( /** * Acceleration in body frame based on eq.(2.46), (2.47) in Nonami's book. * A mistake in the book: (2.46) z-axis should be inverted (and also psi is inverted) in (2.47). + * + * This is OBSOLETE because not considering wind, 3-dimentional drag coefficient. + * Used in the old ground coordinate system drone to check the correctness of the new one. */ AccelerationType acceleration_in_ground_frame( const VelocityType& ground, From ea6a108cc02c6d18480aa7854a08e7ffb7018a2d Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Fri, 1 Nov 2024 12:01:19 +0900 Subject: [PATCH 03/50] added back simple version of body acceleration interface only for testing --- drone_physics/body_physics.cpp | 25 ++++++++++++++++++++----- drone_physics/body_physics.hpp | 10 ++++++++++ drone_physics/drone_physics_c.cpp | 2 +- drone_physics/utest.cpp | 20 ++++++++++---------- 4 files changed, 41 insertions(+), 16 deletions(-) diff --git a/drone_physics/body_physics.cpp b/drone_physics/body_physics.cpp index a31b9f21..e0fd7f53 100644 --- a/drone_physics/body_physics.cpp +++ b/drone_physics/body_physics.cpp @@ -236,7 +236,6 @@ AccelerationType acceleration_in_body_frame( const auto g = gravity; const auto d1 = drag1; const auto d2 = drag2; - const auto [wind_u, wind_v, wind_w] = wind; /* * See nonami's book eq.(1.136).(2.31) @@ -248,13 +247,13 @@ AccelerationType acceleration_in_body_frame( * (3) Coriolis force(using uvw,pqr) IS needed(because the body frame is rotating!) * * 10/31/2024: added wind effect to 1-st, 2-nd order drag. - * Note: ma = T - colioris - d1 v - d2 sing(v) (v*v) + * Note: ma = T - colioris - d1 v - d2 sign(v) (v*v), where v = v - wind = air_v. * the air friction is always counter to the velocity vs wind, in the 1st and sencond order. */ /*****************************************************************/ - double air_u = u - wind_u; - double air_v = v - wind_v; - double air_w = w - wind_w; + double air_u = u - wind.x; + double air_v = v - wind.y; + double air_w = w - wind.z; double dot_u = - g * s_theta - (q*w - r*v) - d1.x/m * (air_u) - d2.x/m * air_u * std::fabs(air_u); double dot_v = + g * c_theta * s_phi - (r*u - p*w) - d1.y/m * (air_v) - d2.y/m * air_v * std::fabs(air_v); @@ -264,6 +263,22 @@ AccelerationType acceleration_in_body_frame( return {dot_u, dot_v, dot_w}; } +/* simplified version of the above */ +AccelerationType acceleration_in_body_frame( + const VelocityType& body_velocity, + const EulerType& angle, + const AngularVelocityType& body_angular_velocity, + double thrust, double mass /* 0 is not allowed */, + double gravity, /* usually 9.8 > 0*/ + double drag1, /* air friction of 1-st order(-d1*v) counter to velocity */ + double drag2 /* air friction of 2-nd order(-d2*v*v) counter to velocity */) +{ + return acceleration_in_body_frame( + body_velocity, angle, body_angular_velocity, + thrust, mass, gravity, {0, 0, 0}, {drag1, drag1, drag1}, {drag2, drag2, drag2}); +} + + /** * Acceleration in body frame based on eq.(2.46), (2.47) in Nonami's book. diff --git a/drone_physics/body_physics.hpp b/drone_physics/body_physics.hpp index 2e8ec392..29984ef5 100644 --- a/drone_physics/body_physics.hpp +++ b/drone_physics/body_physics.hpp @@ -128,6 +128,16 @@ AccelerationType acceleration_in_body_frame( const VectorType& drag1, /* air friction of 1-st order(-d1*v) counter to velocity */ const VectorType& drag2 /* air friction of 2-nd order(-d2*v*v) counter to velocity */); +/* simplified version of the above */ +AccelerationType acceleration_in_body_frame( + const VelocityType& body_velocity, + const EulerType& angle, + const AngularVelocityType& body_angular_velocity, + double thrust, double mass /* 0 is not allowed */, + double gravity, /* usually 9.8 > 0*/ + double drag1, /* air friction of 1-st order(-d1*v) counter to velocity */ + double drag2 = 0 /* air friction of 2-nd order(-d2*v*v) counter to velocity */); + /* angular acceleration in body frame based on JW' = W x JW =Tb ...eq.(1.137),(2.31) */ AngularAccelerationType angular_acceleration_in_body_frame( diff --git a/drone_physics/drone_physics_c.cpp b/drone_physics/drone_physics_c.cpp index 27b53ca1..3e992685 100644 --- a/drone_physics/drone_physics_c.cpp +++ b/drone_physics/drone_physics_c.cpp @@ -73,7 +73,7 @@ dp_acceleration_t dp_acceleration_in_body_frame( to_Vector(body_velocity), to_Euler(angle), to_Vector(body_angular_velocity), - thrust, mass, gravity, {0, 0, 0},{drag, drag, drag},{0, 0, 0} + thrust, mass, gravity, drag ) ); } diff --git a/drone_physics/utest.cpp b/drone_physics/utest.cpp index 10e1535a..a2b6d4dc 100644 --- a/drone_physics/utest.cpp +++ b/drone_physics/utest.cpp @@ -183,33 +183,33 @@ void test_body_acceleration() { double trust = 1, mass = 1, gravity = 1, drag = 0; AccelerationType a = acceleration_in_body_frame(v, EulerType{0, 0, 0}, AngularVelocityType{0, 0, 0}, - trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + trust, mass, gravity, drag); assert_almost_equal(a, (AccelerationType{0, 0, 0})); trust = 10, mass = 2, gravity = 1, drag = 0; a = acceleration_in_body_frame(v, EulerType{0, 0, 0}, AngularVelocityType{0, 0, 0}, - trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + trust, mass, gravity, drag); assert_almost_equal(a, (AccelerationType{0, 0, -trust/mass+gravity})); /* change psi angle (doesn't matter) */ a = acceleration_in_body_frame(v, EulerType{0, 0, PI/6}, AngularVelocityType{0, 0, 0}, - trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + trust, mass, gravity, drag); assert_almost_equal(a, (AccelerationType{0, 0, -trust/mass+gravity})); /* change phi */ a = acceleration_in_body_frame(v, EulerType{PI/6, 0, 0}, AngularVelocityType{0, 0, 0}, - trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + trust, mass, gravity, drag); assert_almost_equal(a, (AccelerationType{0, gravity*sin(PI/6), -trust/mass+gravity*cos(PI/6)})); /* change theta */ a = acceleration_in_body_frame(v, EulerType{0, PI/6, 0}, AngularVelocityType{0, 0, 0}, - trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + trust, mass, gravity, drag); assert_almost_equal(a, (AccelerationType{-gravity*sin(PI/6), 0, -trust/mass+gravity*cos(PI/6)})); /* add drag */ trust = 10, mass = 2, gravity = 1, drag = 0.1; a = acceleration_in_body_frame(v, EulerType{0, PI/6, 0}, AngularVelocityType{0, 0, 0}, - trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + trust, mass, gravity, drag); assert_almost_equal(a, (AccelerationType{ -gravity*sin(PI/6)-drag/mass*1, -drag/mass*2, @@ -218,12 +218,12 @@ void test_body_acceleration() { trust = 10, mass = 2, gravity = 1, drag = 0; // setting angle to (0,0,0), drag = 0, same anglular and linear velocity, so Coliori=(0,0,0) a = acceleration_in_body_frame(v, EulerType{0, 0, 0}, AngularVelocityType{1, 2, 3}, - trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + trust, mass, gravity, drag); assert_almost_equal(a, (AccelerationType{0, 0, -trust/mass+gravity})); // now Coliori is (1,1,1)x(1,2,3) = (1,-2,1) a = acceleration_in_body_frame(v, EulerType{0, 0, 0}, AngularVelocityType{1, 1, 1}, - trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + trust, mass, gravity, drag); assert_almost_equal(a, (AccelerationType{-1, 2, -trust/mass+gravity-1})); } @@ -240,7 +240,7 @@ void test_ground_acceleration() { angle = {PI/3, PI/4, PI/6}; v = {0, 0, 0}; a_g = acceleration_in_ground_frame({0,0,0}, angle, trust, mass, gravity, drag); - a_b = acceleration_in_body_frame({0,0,0}, angle, {0, 0, 0}, trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + a_b = acceleration_in_body_frame({0,0,0}, angle, {0, 0, 0}, trust, mass, gravity, drag); a_g2 = ground_vector_from_body(a_b, angle); assert_almost_equal(a_g, a_g2); @@ -251,7 +251,7 @@ void test_ground_acceleration() { v = {1, 2, 3}; a_g = acceleration_in_ground_frame(v, angle, trust, mass, gravity, drag); - a_b = acceleration_in_body_frame(v, angle, {0, 0, 0}, trust, mass, gravity, {0, 0, 0}, {drag, drag, drag}, {0, 0, 0}); + a_b = acceleration_in_body_frame(v, angle, {0, 0, 0}, trust, mass, gravity, drag); a_g2 = ground_vector_from_body(a_b, angle); assert_almost_equal(a_g, a_g2); From 35dc897c42f5e87105800da7af6cba3f12c777fb Mon Sep 17 00:00:00 2001 From: yamati <135952435+yamati-kz@users.noreply.github.com> Date: Sat, 2 Nov 2024 14:41:50 +0900 Subject: [PATCH 04/50] Update ps4-control-lnx.json camera adjust Chsnge camera adjust --- drone_api/sample/rc_config/ps4-control-lnx.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/drone_api/sample/rc_config/ps4-control-lnx.json b/drone_api/sample/rc_config/ps4-control-lnx.json index 129be3cc..28a9c8c9 100644 --- a/drone_api/sample/rc_config/ps4-control-lnx.json +++ b/drone_api/sample/rc_config/ps4-control-lnx.json @@ -67,13 +67,13 @@ "on": "down" }, "CameraMoveUp": { - "index": 11, + "index": 5, "type": "switch", "off": "up", "on": "down" }, "CameraMoveDown": { - "index": 12, + "index": 4, "type": "switch", "off": "up", "on": "down" From 6c3f3413767f8ef368aa509f6d27f08b133793d1 Mon Sep 17 00:00:00 2001 From: yamati <135952435+yamati-kz@users.noreply.github.com> Date: Sat, 2 Nov 2024 14:43:37 +0900 Subject: [PATCH 05/50] Update ps5-control-lnx.json camera adjust Change camera adjust --- drone_api/sample/rc_config/ps5-control-lnx.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/drone_api/sample/rc_config/ps5-control-lnx.json b/drone_api/sample/rc_config/ps5-control-lnx.json index 129be3cc..28a9c8c9 100644 --- a/drone_api/sample/rc_config/ps5-control-lnx.json +++ b/drone_api/sample/rc_config/ps5-control-lnx.json @@ -67,13 +67,13 @@ "on": "down" }, "CameraMoveUp": { - "index": 11, + "index": 5, "type": "switch", "off": "up", "on": "down" }, "CameraMoveDown": { - "index": 12, + "index": 4, "type": "switch", "off": "up", "on": "down" From da0ecb9fa311c5d3437137e31b5849735f162037 Mon Sep 17 00:00:00 2001 From: yamati <135952435+yamati-kz@users.noreply.github.com> Date: Sun, 3 Nov 2024 23:41:03 +0900 Subject: [PATCH 06/50] =?UTF-8?q?Update=20ubuntu22.04=5Finstall&operation?= =?UTF-8?q?=5FpythonAPI=E7=B7=A8.md?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ...tu22.04_install&operation_pythonAPI\347\267\250.md" | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git "a/docs/manual/ubuntu22.04_install&operation_pythonAPI\347\267\250.md" "b/docs/manual/ubuntu22.04_install&operation_pythonAPI\347\267\250.md" index 280dad17..7c069151 100644 --- "a/docs/manual/ubuntu22.04_install&operation_pythonAPI\347\267\250.md" +++ "b/docs/manual/ubuntu22.04_install&operation_pythonAPI\347\267\250.md" @@ -311,9 +311,17 @@ $ python3 sample.py ../../../hakoniwa-unity-drone-model/DroneAppLinux/custom.jso Unityアプリ上のドローンをPS4のコントローラで操作させるためのプロポ用のアプリを起動します。PC本体にPS4のコントローラをUSBに接続します。 -1.4.1 箱庭コア機能の起動、1.4.2 Unityアプリの起動手順を実施後に、以下のプロポ用のアプリを起動します。 +箱庭コア機能、Unityアプリの起動手順、プロポ用のアプリの順番で起動します。 +- 箱庭コア機能の起動(プロポ用のアプリを起動の場合) +``` bash +$ cd ~/work/hakoniwa-px4sim/hakoniwa +$ bash drone-app.bash ../../hakoniwa-unity-drone-model/DroneAppLinux ./config/rc +``` + +- 1.4.2 Unityアプリの起動手順 +- プロポ用のアプリを起動 ```bash $ cd ~/work/hakoniwa-px4sim $ cd drone_api/sample From 2cd11b8c93a52d760fc8633698f69e009c64e4fd Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Tue, 5 Nov 2024 17:26:06 +0900 Subject: [PATCH 07/50] description of Battery Rotor model. --- drone_physics/README-ja.md | 45 ++++++++++++++++++++++++++++++++- drone_physics/README.md | 51 +++++++++++++++++++++++++++++++++++--- 2 files changed, 92 insertions(+), 4 deletions(-) diff --git a/drone_physics/README-ja.md b/drone_physics/README-ja.md index 48434811..9705ca6c 100644 --- a/drone_physics/README-ja.md +++ b/drone_physics/README-ja.md @@ -391,7 +391,7 @@ $$ ### 1つのローターの力学 -#### 1ローターの回転数 +#### 1ローターの回転角速度 1つ1つのモーターの角速度は， $\Omega(t)$ は，デューティー比 $d(t)$ によって1次遅れ系としてモデル化できます． 書籍の式 (2.48) の伝達関数 $G(s)$ によって記述され， @@ -409,6 +409,49 @@ $\dot{\Omega}(t) = (K_r d(t) - \Omega(t))/ T_r$ 関数名は，`rotor_omega_acceleration` ． +#### 1 ローターの回転角速度（バッテリー電圧を使った非線形モデル） + +もう一つのモデルは、ローターの角速度 $\Omega(t)$ がバッテリー電圧 $V_{bat}$ のデューティー比 $d(t)$ で制御されるというものです。 +ローターはモーター(電圧によるトルク発生)とプロペラ(トルクによる推力発生)で構成されています。 + +参照： https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p2 eq (3) + +$$ +\begin{array}{l} +J \dot{\Omega}(t) + D \Omega(t) + C_q \Omega(t)^2 = K i(t) \\ +L \dot{i}(t) + R i(t) + K \Omega(t) = e(t) \\ +e(t) = V_{bat} d(t) +\end{array} +$$ + +ここで、 +- $J$ - プロペラの慣性モーメント。[$kg m^2$] +- $D$ - プロペラの減衰係数。[$N m s/rad$] +- $L$ - モーターのインダクタンス。[$H$] +- $R$ - モーターの抵抗。[$\Omega$] +- $K$ - ローターのトルク定数。[$N m/A$] +- $d(t)$ - モーターのデューティー比。($0.0 \le d(t) \le 1.0$) +- $V_{bat}$ - バッテリー電圧。[$V$] +- $e(t)$ - モーターに印加される電圧。$V_{bat}d(t)$ に等しい。[$V$] +- $i(t)$ - モーターの電流。[$A$] +- $\Omega(t)$ - 得られるプロペラの角速度。[$rad/s$] + +インダクタンス $L$ は非常に小さいため無視しすると、 + +$ i(t) = (e(t) - K \Omega(t))/R $ + +さらに、電流 $i(t)$ を消去すると、 $\Omega(t)$ は以下のような非線形微分方程式になります。 + +$$ +J \dot{\Omega}(t) + (D + K^2/R) \Omega(t) + C_q \Omega(t)^2 = (K/R) V_{bat} d(t) +$$ + +$$ +\dot{\Omega}(t) = (K V_{bat} d(t) - (K^2+DR) \Omega(t) - C_q R \Omega(t)^2) /JR +$$ + +関数名は，`rotor_omega_acceleration` の別バージョンです． + #### 1ローターのトルクと反トルク 1つのロータ推力 $T$ は，ローターの角速度 $\Omega$ の2乗に比例します eq.(2.50)． $A$ はローターのサイズと空気密度に関連するパラメータです． diff --git a/drone_physics/README.md b/drone_physics/README.md index 48c3c2bc..97f9d5ec 100644 --- a/drone_physics/README.md +++ b/drone_physics/README.md @@ -392,7 +392,9 @@ The function name is `euler_rate_from_body_angular_velocity` , and the inverse transformation is `body_angular_velocity_from_euler_rate`. -### One Rotor dynamics +### One Rotor dynamics(1st-order lag system) + +There are two versions of the rotor dynamics in this library. #### One rotor rotation speed Each rotor can be modeled as a first-order lag system, in which the rotor angular rate @@ -413,9 +415,52 @@ where; The function name is `rotor_omega_acceleration`. +#### One rotor rotation speed(non-linear using battery voltage) +Another model is that the rotor angular rate $\Omega(t)$ is controlled by the duty rate $d(t)$ +times the battery voltage $V_{bat}$ . +The rotor is composed of a motor(torque generator by voltage) and a propeller(thrust generator using the torque). + +See https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p2 eq (3) + +$$ +\begin{array}{l} +J \dot{\Omega}(t) + D \Omega(t) + C_q \Omega(t)^2 = K i(t) \\ +L \dot{i}(t) + R i(t) + K \Omega(t) = e(t) \\ +e(t) = V_{bat} d(t) +\end{array} +$$ + +where; +- $J$ - The inertia of the propeller. [$kg m^2$] +- $D$ - The damping coefficient of the propeller. [$N m s/rad$] +- $L$ - The inductance of the motor. [$H$] +- $R$ - The resistance of the motor. [$\Omega$] +- $K$ - The torque constant of the rotor. [$N m/A$] +- $d(t)$ - The duty rate of the motor. ($0.0 \le d(t) \le 1.0$) +- $V_{bat}$ - The battery voltage. [$V$] +- $e(t)$ - The voltage applied to the motor, equals to $V_{bat}d(t)$. [$V$] +- $i(t)$ - The current of the motor. [$A$] +- $\Omega(t)$ - The angular velocity of the propeller. [$rad/s$] + +Neglecting the inductance $L$ which is very small, we have; + +$ i(t) = (e(t) - K \Omega(t))/R $ + +Then the equations are simplified as follows, by erasing the current $i(t)$. + +$$ +J \dot{\Omega}(t) + (D + K^2/R) \Omega(t) + C_q \Omega(t)^2 = (K/R) V_{bat} d(t) +$$ + +$$ +\dot{\Omega}(t) = (K V_{bat} d(t) - (K^2+DR) \Omega(t) - C_q R \Omega(t)^2) /JR +$$ + +The function name is (another version of)`rotor_omega_acceleration`. + #### One rotor thrust and anti-torque The thrust $T$ of the rotor is proportional to the square of the rotor angular velocity -$\Omega$ eq.(2.50). $A$ is a parameter related to the rotor size and the air density. +$\Omega$ eq.(2.50). $A$ is a parameter related to the propeller size and the air density. $T = A \Omega^2 $ @@ -423,7 +468,7 @@ The anti-torque $\tau_i$ of the rotor (2.56). $\tau_i = B \Omega^2 + Jr \dot{\Omega}$ -where $B$, $Jr$ is parameters related to the rotor properties. This makes the drone rotate around the $z$-axis. +where $B(=C_q)$, $Jr$ is parameters related to the rotor properties. This makes the drone rotate around the $z$-axis. The function name is `rotor_thrust` and `rotor_anti_torque`. From eeb7ae4aedde65f8c9b30c27f81fdaae33484971 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 6 Nov 2024 09:31:06 +0900 Subject: [PATCH 08/50] compile warning fixed --- drone_control/common/include/drone_angle_controller.hpp | 2 -- drone_control/common/include/flight_controller_types.hpp | 2 +- .../RadioController/hako_module_drone_controller_impl.cpp | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/drone_control/common/include/drone_angle_controller.hpp b/drone_control/common/include/drone_angle_controller.hpp index 5cbc59ad..b4bd3694 100644 --- a/drone_control/common/include/drone_angle_controller.hpp +++ b/drone_control/common/include/drone_angle_controller.hpp @@ -54,7 +54,6 @@ class DroneAngleController { std::unique_ptr roll_control; std::unique_ptr pitch_control; DroneAngleRateInputType angle_prev_out = {}; - double pos_simulation_time = 0; /* * rate control @@ -68,7 +67,6 @@ class DroneAngleController { std::unique_ptr pitch_rate_control; std::unique_ptr yaw_rate_control; DroneAngleOutputType rate_prev_out = {}; - double spd_simulation_time = 0; DroneAngleRateInputType run_angle(DroneAngleInputType& in) { DroneAngleRateInputType out = angle_prev_out; diff --git a/drone_control/common/include/flight_controller_types.hpp b/drone_control/common/include/flight_controller_types.hpp index 930bae20..d69b2f1b 100644 --- a/drone_control/common/include/flight_controller_types.hpp +++ b/drone_control/common/include/flight_controller_types.hpp @@ -11,7 +11,7 @@ struct PidControlInputType { double current; PidControlInputType() : target(0), current(0) {} PidControlInputType(double current_val, double target_val) - : current(current_val), target(target_val) {} + : target(target_val), current(current_val) {} }; struct FlightControllerInputEulerType diff --git a/drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp b/drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp index 4ee889d7..e98d8144 100644 --- a/drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp +++ b/drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp @@ -14,7 +14,7 @@ void* hako_module_drone_controller_impl_create_context(void*) return (void*)new DroneRadioController(); } -int hako_module_drone_controller_impl_is_operation_doing(void *context) +int hako_module_drone_controller_impl_is_operation_doing(void *) { return true; } From 6af7c3072de9e82ac5cc59e605dab75b8fddca34 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 6 Nov 2024 09:32:39 +0900 Subject: [PATCH 09/50] compile warning fixed --- .../FlightController/hako_module_drone_controller_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drone_control/workspace/FlightController/hako_module_drone_controller_impl.cpp b/drone_control/workspace/FlightController/hako_module_drone_controller_impl.cpp index 12315bcc..0545c893 100644 --- a/drone_control/workspace/FlightController/hako_module_drone_controller_impl.cpp +++ b/drone_control/workspace/FlightController/hako_module_drone_controller_impl.cpp @@ -14,7 +14,7 @@ void* hako_module_drone_controller_impl_create_context(void*) return (void*)new DroneController(); } -int hako_module_drone_controller_impl_is_operation_doing(void *context) +int hako_module_drone_controller_impl_is_operation_doing(void *) { return true; } From 6cb97353d33587eb7e4e1efb77d95e40e06b8fc4 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 6 Nov 2024 12:09:22 +0900 Subject: [PATCH 10/50] default drone name bug fixed --- drone_api/libs/hakosim.py | 22 ++++++++++++++-------- drone_api/sample/sample.py | 2 +- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/drone_api/libs/hakosim.py b/drone_api/libs/hakosim.py index e1a6ed7c..744c7259 100644 --- a/drone_api/libs/hakosim.py +++ b/drone_api/libs/hakosim.py @@ -20,19 +20,25 @@ def __init__(self, name): self.camera_move_cmd_request_id = 1 class MultirotorClient: - def __init__(self, config_path): + def __init__(self, config_path, default_drone_name = None): self.pdu_manager = None self.config_path = config_path self.pdudef = self._load_json(config_path) self.vehicles = {} default_drone_set = False - for entry in self.pdudef['robots']: - entry_name = entry['name'] - if len(entry['shm_pdu_readers']) > 0 or len(entry['shm_pdu_writers']) > 0 or len(entry['rpc_pdu_readers']) > 0 or len(entry['rpc_pdu_writers']) > 0: - self.vehicles[entry_name] = HakoDrone(entry_name) - if not default_drone_set: - self.default_drone_name = entry_name - default_drone_set = True + if default_drone_name is None: + for entry in self.pdudef['robots']: + entry_name = entry['name'] + if len(entry['shm_pdu_readers']) > 0 or len(entry['shm_pdu_writers']) > 0 or len(entry['rpc_pdu_readers']) > 0 or len(entry['rpc_pdu_writers']) > 0: + self.vehicles[entry_name] = HakoDrone(entry_name) + if not default_drone_set: + self.default_drone_name = entry_name + default_drone_set = True + print("default drone name: ", default_drone_name) + else: + print("default drone name: ", default_drone_name) + self.default_drone_name = default_drone_name + self.vehicles[default_drone_name] = HakoDrone(default_drone_name) def _load_json(self, path): try: diff --git a/drone_api/sample/sample.py b/drone_api/sample/sample.py index bf69c324..a906cb90 100644 --- a/drone_api/sample/sample.py +++ b/drone_api/sample/sample.py @@ -41,7 +41,7 @@ def main(): return 1 # connect to the HakoSim simulator - client = hakosim.MultirotorClient(sys.argv[1]) + client = hakosim.MultirotorClient(sys.argv[1], "DroneTransporter") client.confirmConnection() client.enableApiControl(True) client.armDisarm(True) From db433def7bb5a2faadc40a67600d0dadc0d9d83b Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Wed, 6 Nov 2024 14:06:31 +0900 Subject: [PATCH 11/50] added new rotor dynamics to the code and README --- drone_physics/README-ja.md | 17 +++++++++++++++-- drone_physics/README.md | 13 ++++++++++++- drone_physics/rotor_physics.cpp | 15 +++++++++++++-- drone_physics/rotor_physics.hpp | 12 ++++++++++-- 4 files changed, 50 insertions(+), 7 deletions(-) diff --git a/drone_physics/README-ja.md b/drone_physics/README-ja.md index 9705ca6c..5d032edb 100644 --- a/drone_physics/README-ja.md +++ b/drone_physics/README-ja.md @@ -11,7 +11,7 @@ 地上と機体座標系間の変換や，ロータの推力と重力からドローンの速度，加速度などが計算できます． 当初この箱庭 PX4 ドローンシミュレーションプロジェクト用に開発されましたが，その中から汎用的に利用できる部分を切り出し，さらに，インターフェイスをより使いやすくし，以下の参考文献との対応をとって公開することにしました． -すべての関数は，以下の書籍の式の C++ 実装であり，ソースコードコメントに式番号が記載されています． +すべての関数は，以下の書籍および他の参考文献（末尾）の式の C++ 実装であり，ソースコードコメントに式番号が記載されています． 本のすべての式を実装しているわけではありませんが，箱庭プロジェクトのドローンはこのライブラリを使って実際に飛行しています． なお，C 言語の I/F もあります． @@ -23,6 +23,8 @@ [![image](https://github.com/toppers/hakoniwa-px4sim/assets/1093925/c92d3d96-25f9-4b6a-ae4e-25d898b75a28)](https://www.coronasha.co.jp/np/isbn/9784339032307/) +- [ 『マルチコプタの運動と制御の基礎のきそ』 こうへい【著】](https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343) + ## Hello World @@ -452,6 +454,16 @@ $$ 関数名は，`rotor_omega_acceleration` の別バージョンです． +また、流れる電流は、 + +$$ +i(t) = (e(t) - K \Omega(t))/R = (V_{bat} d(t) - K \Omega(t))/R +$$ + +で求まる。関数名は、`rotor_current` ． + +$\Omega(t)$ が何らかの外力で大きくなると、逆起電力(Back EMF)によって電流が逆流（電池に充電）する可能性がある式になっていることに注意してください。 + #### 1ローターのトルクと反トルク 1つのロータ推力 $T$ は，ローターの角速度 $\Omega$ の2乗に比例します eq.(2.50)． $A$ はローターのサイズと空気密度に関連するパラメータです． @@ -495,7 +507,8 @@ $\tau_i = B \Omega^2 + Jr \dot{\Omega}$ - すべて関数として実装され，クラスはありません．すなわち内部状態を持ちません． ## 謝辞 -難しい数学と力学について，非常に丁寧な解説書を書いていただいた，野波先生に感謝します． +難しい数学と力学について，非常に丁寧な解説書を書いていただいた，野波先生に感謝します．また、マルチコプタ制御の基礎について、こうへいさん([@Kohei_Ito](https://www.docswell.com/user/Kouhei_Ito))の解説を参考にしました。質問にも丁寧に答えていただきありがとうございました。 + また，箱庭プロジェクトをリードし，実際に PX4, QGroundControl と接続し，長い時間を掛けて Unity でビジュアル表示しながら 仮想的にドローンの飛行テストを行ってくれた，箱庭ラボの森さん（[@tmori](https://github.com/tmori)）に感謝します！ diff --git a/drone_physics/README.md b/drone_physics/README.md index 97f9d5ec..c736d76d 100644 --- a/drone_physics/README.md +++ b/drone_physics/README.md @@ -456,8 +456,19 @@ $$ \dot{\Omega}(t) = (K V_{bat} d(t) - (K^2+DR) \Omega(t) - C_q R \Omega(t)^2) /JR $$ + The function name is (another version of)`rotor_omega_acceleration`. +And the current $i(t)$ is obtained by the following equation. + +$$ +i(t) = (e(t) - K \Omega(t))/R = (V_{bat} d(t) - K \Omega(t))/R +$$ + +The function name is `rotor_current` ． + +Note that when $\Omega(t)$ gets larger by some external force, the current may flow back to the battery(charging the battery) by the back EMF(back electromotive force) of the motor. + #### One rotor thrust and anti-torque The thrust $T$ of the rotor is proportional to the square of the rotor angular velocity $\Omega$ eq.(2.50). $A$ is a parameter related to the propeller size and the air density. @@ -505,7 +516,7 @@ Mission: - Implemented as functions, not classes. Meaning stateless. ## Acknowledgement -I thank Dr. Nonami for writing the detailed description of the math around the drone development. +I thank Dr. Nonami for writing the detailed description of the math around the drone development. And Kohei Ito([@Kouhei_Ito](https://www.docswell.com/user/Kouhei_Ito)) for the detailed explanation of the drone dynamics in his blog and also kind replies to my questions. And also I thank （[@tmori](https://github.com/tmori)）for connecting Hakoniwa to PX4, QGroundControl, and Unity, and spending a long time testing the drone flight virtually, and also for leading this whole Hakoniwa project. diff --git a/drone_physics/rotor_physics.cpp b/drone_physics/rotor_physics.cpp index 82483bcd..35c330db 100644 --- a/drone_physics/rotor_physics.cpp +++ b/drone_physics/rotor_physics.cpp @@ -70,7 +70,7 @@ double rotor_omega_acceleration( double J, /* propeller inertia in [kg m^2] */ double K, /* back electromotive force coeff in [N m/A] */ double D, /* Kinematic viscosity coefficient [Nms/rad] */ - double omega, /* in radian/sec */ + double omega, /* angular velocity in [rad/sec] */ double duty_rate /* 0.0-1.0 (ratio of PWM) */) { assert(R != 0.0); @@ -81,7 +81,18 @@ double rotor_omega_acceleration( * where, e = duty_rate * Vbat. * Assuming inductance L and Qf is very small(zero). */ - return ( K*Vbat*duty_rate - (Cq*R*omega + K*K + D*R)*omega) / (J*R); + return ( K * Vbat * duty_rate - (Cq*R*omega + K*K + D*R)*omega) / (J*R); +} + +double rotor_current( + double Vbat, /* battery voltage in volt [V]*/ + double R, /* resistance in ohm [V/A] */ + double K, /* back electromotive force coeff in [N m/A] */ + double omega, /* angular velocity in [rad/sec] */ + double duty_rate /* 0.0-1.0 (ratio of PWM) */) +{ + assert(R != 0.0); + return (Vbat * duty_rate - K * omega)/R; } /* thrust from omega in radian/sec eq.(2.50)*/ diff --git a/drone_physics/rotor_physics.hpp b/drone_physics/rotor_physics.hpp index fba47def..11774001 100644 --- a/drone_physics/rotor_physics.hpp +++ b/drone_physics/rotor_physics.hpp @@ -14,7 +14,7 @@ namespace hako::drone_physics { double rotor_omega_acceleration( double Kr, /* gain constant */ double Tr, /* time constant */ - double omega, /* in radian/sec */ + double omega, /* angular velocity in [rad/s] */ double duty_rate /* 0.0-1.0 (ratio of PWM) */ ); /* realistic model for rotor */ @@ -25,7 +25,15 @@ double rotor_omega_acceleration( double J, /* propeller inertia in [kg m^2] */ double K, /* back electromotive force coeff in [N m/A] */ double D, /* Kinematic viscosity coefficient [Nms/rad] */ - double omega, /* in radian/sec */ + double omega, /* angular velocity in [rad/s] */ + double duty_rate /* 0.0-1.0 (ratio of PWM) */); + +/* current in the rotor */ +double rotor_current( + double Vbat, /* battery voltage in volt [V]*/ + double R, /* resistance in ohm [V/A] */ + double K, /* back electromotive force coeff in [N m/A] */ + double omega, /* angular velocity in [rad/sec] */ double duty_rate /* 0.0-1.0 (ratio of PWM) */); /* thrust from omega in radian/sec eq.(2.50)*/ From 31eaa32788bb3ee1271ab866c37ebb63e32a232b Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 6 Nov 2024 15:08:40 +0900 Subject: [PATCH 12/50] add battery sources --- .../drone/aircraft/aircraft_factory.cpp | 8 +++++ .../src/assets/drone/aircraft/aricraft.hpp | 7 ++++- .../assets/drone/include/irotor_dynamics.hpp | 5 ++++ .../drone/physics/rotor/rotor_dynamics.hpp | 29 +++++++++++++++++++ .../physics/rotor/rotor_dynamics_jmavsim.hpp | 14 +++++++++ hakoniwa/src/config/drone_config.hpp | 23 +++++++++++++++ hakoniwa/src/config/drone_config_types.hpp | 7 +++++ 7 files changed, 92 insertions(+), 1 deletion(-) diff --git a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp index 97ecb19e..9a4df95e 100644 --- a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp +++ b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp @@ -129,6 +129,14 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr static_cast(rotor)->set_params(RAD_PER_SEC_MAX, ROTOR_TAU, ROTOR_K); drone->get_logger().add_entry(*static_cast(rotor), LOGPATH(drone->get_index(), logfilename)); } + else if (rotor_vendor == "BatteryModel") { + rotor = new RotorDynamics(DELTA_TIME_SEC); + HAKO_ASSERT(rotor != nullptr); + auto constants = drone_config.getCompDroneDynamicsRotorDynamicsConstants(); + rotor->set_battery_dynamics_constants(constants); + static_cast(rotor)->set_params(RAD_PER_SEC_MAX, 0, ROTOR_K); + drone->get_logger().add_entry(*static_cast(rotor), LOGPATH(drone->get_index(), logfilename)); + } else { rotor = new RotorDynamics(DELTA_TIME_SEC); HAKO_ASSERT(rotor != nullptr); diff --git a/hakoniwa/src/assets/drone/aircraft/aricraft.hpp b/hakoniwa/src/assets/drone/aircraft/aricraft.hpp index 12aff773..de810bef 100644 --- a/hakoniwa/src/assets/drone/aircraft/aricraft.hpp +++ b/hakoniwa/src/assets/drone/aircraft/aricraft.hpp @@ -34,7 +34,12 @@ class AirCraft : public hako::assets::drone::IAirCraft { if (input.no_use_actuator == false) { DroneRotorSpeedType rotor_speed[ROTOR_NUM]; for (int i = 0; i < ROTOR_NUM; i++) { - rotor_dynamics[i]->run(input.controls[i]); + if (rotor_dynamics[i]->has_battery_dynamics()) { + rotor_dynamics[i]->run(input.controls[i], 11.1 /* TODO need battery model */); + } + else { + rotor_dynamics[i]->run(input.controls[i]); + } rotor_speed[i] = rotor_dynamics[i]->get_rotor_speed(); } thrust_dynamis->run(rotor_speed); diff --git a/hakoniwa/src/assets/drone/include/irotor_dynamics.hpp b/hakoniwa/src/assets/drone/include/irotor_dynamics.hpp index 8a6187aa..b1ce4eb0 100644 --- a/hakoniwa/src/assets/drone/include/irotor_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/irotor_dynamics.hpp @@ -2,6 +2,7 @@ #define _IROTOR_DYNAMICS_HPP_ #include "drone_primitive_types.hpp" +#include "config/drone_config_types.hpp" namespace hako::assets::drone { @@ -14,9 +15,13 @@ class IRotorDynamics { virtual DroneRotorSpeedType get_rotor_speed() const = 0; + virtual bool has_battery_dynamics() = 0; + virtual void set_battery_dynamics_constants(const RotorBatteryModelConstants &c) = 0; + virtual double get_rad_per_sec_max() const = 0; virtual void run(double control) = 0; + virtual void run(double control, double vbat) = 0; }; } diff --git a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp index 1efef545..f09aeee2 100644 --- a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp @@ -15,6 +15,8 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public ICsvLog double param_rad_per_sec_max = 6000.0; double param_tr = 1.0; double param_kr = 1.0; + bool battery_dynamics = false; + RotorBatteryModelConstants constants; DroneRotorSpeedType speed; DroneRotorSpeedType next_speed; double delta_time_sec; @@ -28,6 +30,16 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public ICsvLog this->total_time_sec = 0; this->speed.data = 0; } + void set_battery_dynamics_constants(const RotorBatteryModelConstants &c) override + { + battery_dynamics = true; + this->constants = c; + } + bool has_battery_dynamics() override + { + return battery_dynamics; + } + void set_params(double rad_per_sec_max, double tr, double kr) { this->param_rad_per_sec_max = rad_per_sec_max; @@ -66,6 +78,23 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public ICsvLog this->speed.data = this->next_speed.data; this->total_time_sec += this->delta_time_sec; } + void run(double control, double vbat) override + { + this->next_speed.data = ( + drone_physics::rotor_omega_acceleration(vbat, constants.R, constants.Cq, constants.J, constants.K, constants.D, speed.data, control) + ) * this->delta_time_sec + + this->speed.data; + // Cap the next speed at the maximum RPS if it exceeds it + if (this->next_speed.data > this->param_rad_per_sec_max) { + this->next_speed.data = this->param_rad_per_sec_max; + } + // Ensure the next speed does not fall below the minimum RPS (assuming 0 for this example) + else if (this->next_speed.data < 0) { + this->next_speed.data = 0; + } + this->speed.data = this->next_speed.data; + this->total_time_sec += this->delta_time_sec; + } const std::vector log_head() override { return { "timestamp", "RadPerSec" }; diff --git a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics_jmavsim.hpp b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics_jmavsim.hpp index d729cc6e..1e5aea66 100644 --- a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics_jmavsim.hpp +++ b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics_jmavsim.hpp @@ -15,6 +15,7 @@ class RotorDynamicsJmavsim : public hako::assets::drone::IRotorDynamics, public double param_rad_per_sec_max = 6000.0; double param_tr = 1.0; double param_kr = 1.0; + RotorBatteryModelConstants constants; double w; double delta_time_sec; double total_time_sec; @@ -27,6 +28,15 @@ class RotorDynamicsJmavsim : public hako::assets::drone::IRotorDynamics, public this->total_time_sec = 0; this->w = 0; } + void set_battery_dynamics_constants(const RotorBatteryModelConstants &c) override + { + this->constants = c; + } + bool has_battery_dynamics() override + { + return false; + } + void set_params(double rad_per_sec_max, double tr, double kr) { this->param_rad_per_sec_max = rad_per_sec_max; @@ -54,6 +64,10 @@ class RotorDynamicsJmavsim : public hako::assets::drone::IRotorDynamics, public this->w += (control - this->w) * (1.0 - exp(-this->delta_time_sec/ this->param_tr)); this->total_time_sec += this->delta_time_sec; } + void run(double control, double) override + { + return run(control); + } const std::vector log_head() override { return { "timestamp", "RadPerSec" }; diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 3c656b89..5b4be353 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -240,6 +240,29 @@ class DroneConfig { return configJson["components"]["rotor"]["Kr"].get(); } + hako::assets::drone::RotorBatteryModelConstants getCompDroneDynamicsRotorDynamicsConstants() const { + hako::assets::drone::RotorBatteryModelConstants constants = {}; + try { + // 必要な全てのキーが存在するかを確認 + if (configJson.contains("components") && + configJson["components"].contains("rotor") && + configJson["components"]["rotor"].contains("dynamics_constants")) { + + // 各定数を取得し、エラーハンドリングも考慮 + constants.R = configJson["components"]["rotor"]["dynamics_constants"]["R"].get(); + constants.Cq = configJson["components"]["rotor"]["dynamics_constants"]["Cq"].get(); + constants.K = configJson["components"]["rotor"]["dynamics_constants"]["K"].get(); + constants.D = configJson["components"]["rotor"]["dynamics_constants"]["D"].get(); + constants.J = configJson["components"]["rotor"]["dynamics_constants"]["J"].get(); + std::cout << "Rotor battery model is enabled." << std::endl; + return constants; + } + } catch (const std::exception& e) { + std::cerr << "Error retrieving rotor dynamics constants: " << e.what() << std::endl; + } + std::cout << "ERROR: Rotor battery model config is invalid." << std::endl; + return constants; + } int getCompRotorRpmMax() const { return configJson["components"]["rotor"]["rpmMax"].get(); } diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index 5843d41b..ae2366fd 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -10,6 +10,13 @@ namespace hako::assets::drone { std::vector velocity; // X, Y, Z std::vector angular_velocity; // X, Y, Z }; + struct RotorBatteryModelConstants { + double R; + double Cq; + double K; + double D; + double J; + }; } #endif /* _DRONE_CONFIG_TYPES_HPP_ */ \ No newline at end of file From fae7bae40c1f9bf15778ebd10e7a7d52a14d0187 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 6 Nov 2024 15:09:58 +0900 Subject: [PATCH 13/50] add battery configs --- hakoniwa/config/rc-battery/backup.json | 157 ++++++++++++++++++ .../config/rc-battery/drone_config_0.json | 153 +++++++++++++++++ 2 files changed, 310 insertions(+) create mode 100644 hakoniwa/config/rc-battery/backup.json create mode 100644 hakoniwa/config/rc-battery/drone_config_0.json diff --git a/hakoniwa/config/rc-battery/backup.json b/hakoniwa/config/rc-battery/backup.json new file mode 100644 index 00000000..7faf4a3e --- /dev/null +++ b/hakoniwa/config/rc-battery/backup.json @@ -0,0 +1,157 @@ +{ + "name": "DroneTransporter", + "simulation": { + "lockstep": true, + "timeStep": 0.003, + "logOutputDirectory": ".", + "logOutput": { + "sensors": { + "acc": true, + "gyro": true, + "mag": true, + "baro": true, + "gps": true + }, + "mavlink": { + "hil_sensor": true, + "hil_gps": true, + "hil_actuator_controls": true + } + }, + "mavlink_tx_period_msec": { + "hil_sensor": 3, + "hil_gps": 3 + }, + "location": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 121.321, + "magneticField": { + "intensity_nT": 53045.1, + "declination_deg": 15.306, + "inclination_deg": 68.984 + } + } + }, + "components": { + "droneDynamics": { + "physicsEquation": "BodyFrame", + "collision_detection": true, + "enable_disturbance": true, + "manual_control": false, + "airFrictionCoefficient": [ + 0.05, + 0 + ], + "inertia": [ + 0.0061, + 0.00653, + 0.0116 + ], + "mass_kg": 0.71, + "body_size": [ + 0.1, + 0.1, + 0.01 + ], + "position_meter": [ + 0, + 0, + 0 + ], + "angle_degree": [ + 0, + 0, + 0 + ] + }, + "battery": { + "capacity": 11.1 + }, + "rotor": { + "vendor": "BatteryModel", + "dynamics_constants": { + "R": 0.12, + "Cq": 3.0e-8, + "K": 3.28e-3, + "D": 0.0, + "J": 8.12e-6 + }, + "Tr": 0.046, + "Kr": 2896, + "rpmMax": 27654.76291 + }, + "thruster": { + "vendor": "None", + "rotorPositions": [ + { + "position": [ + 0.103, + 0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + -0.103, + -0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + 0.103, + -0.103, + 0 + ], + "rotationDirection": -1.0 + }, + { + "position": [ + -0.103, + 0.103, + 0 + ], + "rotationDirection": -1.0 + } + ], + "HoveringRpm": 13827.38146, + "parameterB": 3.0E-8, + "parameterJr": 0.0 + }, + "sensors": { + "acc": { + "sampleCount": 1, + "noise": 0.03 + }, + "gyro": { + "sampleCount": 1, + "noise": 0.0 + }, + "mag": { + "sampleCount": 1, + "noise": 0.03 + }, + "baro": { + "sampleCount": 1, + "noise": 0.01 + }, + "gps": { + "sampleCount": 1, + "noise": 0 + } + } + }, + "controller": { + "moduleDirectory": "../drone_control/cmake-build/workspace/RadioController", + "moduleName": "RadioController", + "direct_rotor_control": false, + "mixer": { + "vendor": "None", + "enableDebugLog": false, + "enableErrorLog": false + } + } +} diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json new file mode 100644 index 00000000..14c0c767 --- /dev/null +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -0,0 +1,153 @@ +{ + "name": "DroneTransporter", + "simulation": { + "lockstep": true, + "timeStep": 0.001, + "logOutputDirectory": ".", + "logOutput": { + "sensors": { + "acc": true, + "gyro": true, + "mag": true, + "baro": true, + "gps": true + }, + "mavlink": { + "hil_sensor": true, + "hil_gps": true, + "hil_actuator_controls": true + } + }, + "mavlink_tx_period_msec": { + "hil_sensor": 3, + "hil_gps": 3 + }, + "location": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 121.321, + "magneticField": { + "intensity_nT": 53045.1, + "declination_deg": 15.306, + "inclination_deg": 68.984 + } + } + }, + "components": { + "droneDynamics": { + "physicsEquation": "BodyFrame", + "collision_detection": false, + "manual_control": false, + "airFrictionCoefficient": [ + 0.05, + 0 + ], + "inertia": [ + 0.0061, + 0.00653, + 0.0116 + ], + "mass_kg": 0.71, + "body_size": [ + 0.1, + 0.1, + 0.01 + ], + "position_meter": [ + 0, + 0, + 0 + ], + "angle_degree": [ + 0, + 0, + 0 + ] + }, + "rotor": { + "vendor": "BatteryModel", + "dynamics_constants": { + "R": 0.12, + "Cq": 3.0e-8, + "K": 3.28e-3, + "D": 0.0, + "J": 8.12e-6 + }, + "Tr": 0.046, + "Kr": 27654.76291, + "rpmMax": 27654.76291 + }, + "thruster": { + "vendor": "None", + "rotorPositions": [ + { + "position": [ + 0.103, + 0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + -0.103, + -0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + 0.103, + -0.103, + 0 + ], + "rotationDirection": -1.0 + }, + { + "position": [ + -0.103, + 0.103, + 0 + ], + "rotationDirection": -1.0 + } + ], + "HoveringRpm": 13827.38146, + "parameterB": 3.0E-8, + "parameterJr": 0.0 + }, + "sensors": { + "acc": { + "sampleCount": 1, + "noise": 0.03 + }, + "gyro": { + "sampleCount": 1, + "noise": 0.0 + }, + "mag": { + "sampleCount": 1, + "noise": 0.03 + }, + "baro": { + "sampleCount": 1, + "noise": 0.01 + }, + "gps": { + "sampleCount": 1, + "noise": 0 + } + } + }, + "controller": { + "moduleDirectory": "../drone_control/cmake-build/workspace/FlightController", + "moduleName": "FlightController", + "direct_rotor_control": false, + "mixer": { + "vendor": "None", + "enableDebugLog": false, + "enableErrorLog": false + } + } +} \ No newline at end of file From fed12b9fd875286d3fe203c44e1be4c376bcc9c5 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 6 Nov 2024 15:19:43 +0900 Subject: [PATCH 14/50] add mixer config --- drone_control/config/param-api-mixer.txt | 80 ++++++++++++ hakoniwa/config/rc-battery/backup.json | 157 ----------------------- 2 files changed, 80 insertions(+), 157 deletions(-) create mode 100644 drone_control/config/param-api-mixer.txt delete mode 100644 hakoniwa/config/rc-battery/backup.json diff --git a/drone_control/config/param-api-mixer.txt b/drone_control/config/param-api-mixer.txt new file mode 100644 index 00000000..e3fc95a8 --- /dev/null +++ b/drone_control/config/param-api-mixer.txt @@ -0,0 +1,80 @@ +# 基本パラメータ +SIMULATION_DELTA_TIME 0.001 +MASS 0.71 +GRAVITY 9.81 + +# 高度制御 +PID_ALT_CONTROL_CYCLE 0.0 +PID_ALT_MAX_POWER 9.81 +PID_ALT_THROTTLE_GAIN 1.0 +PID_ALT_MAX_SPD 2.0 + +## 高度制御のPIDパラメータ +PID_ALT_Kp 5.0 +PID_ALT_Ki 0.1 +PID_ALT_Kd 1.0 +PID_ALT_SPD_Kp 5.0 +PID_ALT_SPD_Ki 0.0 +PID_ALT_SPD_Kd 1.0 + +# 水平制御 +POS_CONTROL_CYCLE 0.0 +SPD_CONTROL_CYCLE 0.0 +PID_POS_MAX_SPD 10.0 + +## 水平位置制御のPIDパラメータ +PID_POS_X_Kp 5.0 +PID_POS_X_Ki 0.0 +PID_POS_X_Kd 10.0 +PID_POS_Y_Kp 5.0 +PID_POS_Y_Ki 0.0 +PID_POS_Y_Kd 10.0 + +# 水平速度制御のPIDパラメータ +PID_POS_VX_Kp 10.0 +PID_POS_VX_Ki 0.0 +PID_POS_VX_Kd 1.0 +PID_POS_VY_Kp 10.0 +PID_POS_VY_Ki 0.0 +PID_POS_VY_Kd 1.0 + +# 姿勢角制御 +HEAD_CONTROL_CYCLE 0.0 +ANGULAR_CONTROL_CYCLE 0.0 +ANGULAR_RATE_CONTROL_CYCLE 0.0 +PID_POS_MAX_ROLL 10.0 +PID_POS_MAX_PITCH 10.0 +PID_ROLL_RPM_MAX 20.0 +PID_PITCH_RPM_MAX 20.0 +PID_ROLL_TORQUE_MAX 10.0 +PID_PITCH_TORQUE_MAX 10.0 +PID_YAW_TORQUE_MAX 10.0 + +## ロール角度とロール角速度制御のPIDパラメータ +PID_ROLL_Kp 4.0 +PID_ROLL_Ki 0.0 +PID_ROLL_Kd 0.0 +PID_ROLL_RATE_Kp 2.0 +PID_ROLL_RATE_Ki 0.0 +PID_ROLL_RATE_Kd 0.0 + +## ピッチ角度とピッチ角速度制御のPIDパラメータ +PID_PITCH_Kp 4.0 +PID_PITCH_Ki 0.0 +PID_PITCH_Kd 0.0 +PID_PITCH_RATE_Kp 2.0 +PID_PITCH_RATE_Ki 0.0 +PID_PITCH_RATE_Kd 0.0 + +## ヨー角度とヨー角速度制御のPIDパラメータ +PID_YAW_RPM_MAX 10.0 +PID_YAW_Kp 0.1 +PID_YAW_Ki 0.0 +PID_YAW_Kd 0.0 +PID_YAW_RATE_Kp 0.1 +PID_YAW_RATE_Ki 0.0 +PID_YAW_RATE_Kd 0.1 + +# ラジオコントロール +YAW_DELTA_VALUE_DEG 0.2 +ALT_DELTA_VALUE_M 0.006 diff --git a/hakoniwa/config/rc-battery/backup.json b/hakoniwa/config/rc-battery/backup.json deleted file mode 100644 index 7faf4a3e..00000000 --- a/hakoniwa/config/rc-battery/backup.json +++ /dev/null @@ -1,157 +0,0 @@ -{ - "name": "DroneTransporter", - "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true - }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } - }, - "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "enable_disturbance": true, - "manual_control": false, - "airFrictionCoefficient": [ - 0.05, - 0 - ], - "inertia": [ - 0.0061, - 0.00653, - 0.0116 - ], - "mass_kg": 0.71, - "body_size": [ - 0.1, - 0.1, - 0.01 - ], - "position_meter": [ - 0, - 0, - 0 - ], - "angle_degree": [ - 0, - 0, - 0 - ] - }, - "battery": { - "capacity": 11.1 - }, - "rotor": { - "vendor": "BatteryModel", - "dynamics_constants": { - "R": 0.12, - "Cq": 3.0e-8, - "K": 3.28e-3, - "D": 0.0, - "J": 8.12e-6 - }, - "Tr": 0.046, - "Kr": 2896, - "rpmMax": 27654.76291 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - { - "position": [ - 0.103, - 0.103, - 0 - ], - "rotationDirection": 1.0 - }, - { - "position": [ - -0.103, - -0.103, - 0 - ], - "rotationDirection": 1.0 - }, - { - "position": [ - 0.103, - -0.103, - 0 - ], - "rotationDirection": -1.0 - }, - { - "position": [ - -0.103, - 0.103, - 0 - ], - "rotationDirection": -1.0 - } - ], - "HoveringRpm": 13827.38146, - "parameterB": 3.0E-8, - "parameterJr": 0.0 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 - }, - "gyro": { - "sampleCount": 1, - "noise": 0.0 - }, - "mag": { - "sampleCount": 1, - "noise": 0.03 - }, - "baro": { - "sampleCount": 1, - "noise": 0.01 - }, - "gps": { - "sampleCount": 1, - "noise": 0 - } - } - }, - "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/RadioController", - "moduleName": "RadioController", - "direct_rotor_control": false, - "mixer": { - "vendor": "None", - "enableDebugLog": false, - "enableErrorLog": false - } - } -} From bc8828d58bfaaf09cd297296bb94b6a7d5a84d29 Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Wed, 6 Nov 2024 15:20:56 +0900 Subject: [PATCH 15/50] added test wind --- drone_physics/utest.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/drone_physics/utest.cpp b/drone_physics/utest.cpp index a2b6d4dc..ab4218eb 100644 --- a/drone_physics/utest.cpp +++ b/drone_physics/utest.cpp @@ -179,7 +179,7 @@ void test_angular_frame_roundtrip() { void test_body_acceleration() { - const VelocityType v{1, 2, 3}; + VelocityType v{1, 2, 3}; double trust = 1, mass = 1, gravity = 1, drag = 0; AccelerationType a = acceleration_in_body_frame(v, EulerType{0, 0, 0}, AngularVelocityType{0, 0, 0}, @@ -227,6 +227,21 @@ void test_body_acceleration() { assert_almost_equal(a, (AccelerationType{-1, 2, -trust/mass+gravity-1})); } +void test_wind() { + VelocityType v{3, 6, 9}; + VelocityType wind{1, 2, 3}; + double trust = 0, mass = 1, gravity = 0; // no gravity to test wind. + VectorType a = acceleration_in_body_frame(v, {0, 0, 0}, {0, 0, 0}, + trust, mass, gravity, wind, {1, 1, 1}, {0, 0, 0}); + assert_almost_equal(a, (VectorType{-2, -4, -6})); + // should be (1-3, 2-6, 3-9) = (-2, -4, -6) + + a = acceleration_in_body_frame(v, {0, 0, 0}, {0, 0, 0}, + trust, mass, gravity, wind, {0, 0, 0}, {1, 1, 1}); + assert_almost_equal(a, (VectorType{-4, -16, -36})); + // should be (-(1-3)^2, -(2-6)^2, -(3-9)^2) = (-4, -16, -36) +} + void test_ground_acceleration() { VelocityType v{1, 2, 3}; EulerType angle{0, 0, 0}; @@ -494,6 +509,7 @@ int main() { T(test_body_anti_Jr_torque); T(test_collision); T(test_rotor_omega_acceleration); + T(test_wind); std::cerr << "-------all standard test PASSSED!!----\n"; T(test_issue_89_yaw_angle_bug); std::cerr << "-------all bug issue test PASSSED!!----\n"; From 70c142dadd498b8e8b3c2741c7dcd4245192523f Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Wed, 6 Nov 2024 15:27:10 +0900 Subject: [PATCH 16/50] deleted temporary glm test --- drone_physics/CMakeLists.txt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/drone_physics/CMakeLists.txt b/drone_physics/CMakeLists.txt index 21ae5045..5649164f 100644 --- a/drone_physics/CMakeLists.txt +++ b/drone_physics/CMakeLists.txt @@ -40,13 +40,14 @@ target_link_libraries(examples drone_physics) add_executable(cexamples cexamples.c) target_link_libraries(cexamples drone_physics_c) -add_executable(glmtest glmtest.cpp) -target_include_directories(glmtest PRIVATE ../hakoniwa/third-party/glm) enable_testing() add_test(NAME utest COMMAND ./utest) add_test(NAME ctest COMMAND ./ctest) -add_test(NAME glmtest COMMAND ./glmtest) + +#add_executable(glmtest glmtest.cpp) +#target_include_directories(glmtest PRIVATE ../hakoniwa/third-party/glm) +#add_test(NAME glmtest COMMAND ./glmtest) add_custom_target(vtest COMMAND ${CMAKE_CTEST_COMMAND} --verbose) From 4f38b90bd61f667a77f3ecbac75e9fa3ccabe2cc Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Thu, 7 Nov 2024 14:19:26 +0900 Subject: [PATCH 17/50] updated param for rc --- drone_control/config/param-api-mixer.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/drone_control/config/param-api-mixer.txt b/drone_control/config/param-api-mixer.txt index e3fc95a8..546177df 100644 --- a/drone_control/config/param-api-mixer.txt +++ b/drone_control/config/param-api-mixer.txt @@ -7,7 +7,7 @@ GRAVITY 9.81 PID_ALT_CONTROL_CYCLE 0.0 PID_ALT_MAX_POWER 9.81 PID_ALT_THROTTLE_GAIN 1.0 -PID_ALT_MAX_SPD 2.0 +PID_ALT_MAX_SPD 5.0 ## 高度制御のPIDパラメータ PID_ALT_Kp 5.0 @@ -76,5 +76,5 @@ PID_YAW_RATE_Ki 0.0 PID_YAW_RATE_Kd 0.1 # ラジオコントロール -YAW_DELTA_VALUE_DEG 0.2 -ALT_DELTA_VALUE_M 0.006 +YAW_DELTA_VALUE_DEG 0.05 +ALT_DELTA_VALUE_M 0.002 From c4660faa8480298791227d718600e8f7fba4fab2 Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Fri, 8 Nov 2024 09:44:13 +0900 Subject: [PATCH 18/50] added units in source comments for clarity --- drone_physics/rotor_physics.cpp | 44 ++++++++++++++++++++++----------- drone_physics/rotor_physics.hpp | 42 +++++++++++++++++-------------- 2 files changed, 53 insertions(+), 33 deletions(-) diff --git a/drone_physics/rotor_physics.cpp b/drone_physics/rotor_physics.cpp index 35c330db..fca78f49 100644 --- a/drone_physics/rotor_physics.cpp +++ b/drone_physics/rotor_physics.cpp @@ -47,10 +47,10 @@ namespace hako::drone_physics { * * Omega = Kr * (1 - exp(-t/Tr) * (duty rate)) */ -double rotor_omega_acceleration( +double rotor_omega_acceleration( /* omega acceleration in [rad/s^2]*/ double Kr /* gain constant */, double Tr /* time constant */, - double omega, /* in radian/sec */ + double omega, /* in [rad/s] */ double duty_rate /* 0.0-1.0 (ratio of PWM) */) { /** @@ -63,14 +63,14 @@ double rotor_omega_acceleration( /** * Realistic modeling of the rotor, using battery voltage, intertia and etc. */ -double rotor_omega_acceleration( +double rotor_omega_acceleration( /* omega acceleration in [rad/s^2]*/ double Vbat, /* battery voltage in volt [V]*/ double R, /* resistance in ohm [V/A] */ double Cq, /* torque coeff (= B param in anti-torque) [N ms^2/rad^2] */ double J, /* propeller inertia in [kg m^2] */ double K, /* back electromotive force coeff in [N m/A] */ double D, /* Kinematic viscosity coefficient [Nms/rad] */ - double omega, /* angular velocity in [rad/sec] */ + double omega, /* angular velocity in [rad/s] */ double duty_rate /* 0.0-1.0 (ratio of PWM) */) { assert(R != 0.0); @@ -84,7 +84,7 @@ double rotor_omega_acceleration( return ( K * Vbat * duty_rate - (Cq*R*omega + K*K + D*R)*omega) / (J*R); } -double rotor_current( +double rotor_current( /* current in [A] */ double Vbat, /* battery voltage in volt [V]*/ double R, /* resistance in ohm [V/A] */ double K, /* back electromotive force coeff in [N m/A] */ @@ -96,9 +96,9 @@ double rotor_current( } /* thrust from omega in radian/sec eq.(2.50)*/ -double rotor_thrust( +double rotor_thrust( /* thrust in [N] */ double A, /* the A parameter in Trust = A*(Omega)^2 */ - double omega /* in radian/sec */ ) + double omega /* in [rad/s] */ ) { /** * Nonami's book (2.50) @@ -109,8 +109,12 @@ double rotor_thrust( } /* this makes z-axis rotation eq.(2.56) */ -double rotor_anti_torque(double B, - double Jr, double omega, double omega_acceleratoin, double ccw) +double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/ + double B, /* torque coefficient (referred to as Cq in Kohei's doc) in [N m s^2/rad^2] */ + double Jr, /* torque coefficient for 2nd order rotation */ + double omega, /* in [rad/s] */ + double omega_acceleratoin, /* in [rad/s^2] */ + double ccw /* 1 or -1 */ ) { /** * See Nonami's book eq.(2.56) @@ -122,7 +126,10 @@ double rotor_anti_torque(double B, /* the sum of the n trust from the rotors eq.(2.61) */ -double body_thrust(double A, unsigned n, double omega[]) +double body_thrust( /* thrust in [N] */ + double A, /* parameter A in Trust = A*(Omega)^2 in each motor */ + unsigned n, /* number of rotors */ + double omega[] /* in radian/sec */ ) { double thrust = 0; for (unsigned i = 0; i < n; i++) { @@ -133,9 +140,15 @@ double body_thrust(double A, unsigned n, double omega[]) /* the sum of the n torques from the rotors including anti-torque */ /* eq.(2.60)-(2.62)*/ -TorqueType body_torque(double A, double B, double Jr, unsigned n, - VectorType position[], double ccw[], double omega[], - double omega_acceleration[]) +TorqueType body_torque( /* torque in [Nm] */ + double A, /* parameter A in Trust = A*(Omega)^2 */ + double B, /* anti-torque parameter B in Ta = B*(Omega)^2 + Jr* (d(Omega)/dt) */ + double Jr, + unsigned n, /* number of rotors */ + VectorType position[], /* position of each rotor in [m] */ + double ccw[], /* 1 or -1 */ + double omega[], /* in [rad/s] */ + double omega_acceleration[] /* in [rad/s^2] */ ) { TorqueType total_torque = {0, 0, 0}; for (unsigned i = 0; i < n; i++) { @@ -162,8 +175,9 @@ TorqueType body_torque(double A, double B, double Jr, unsigned n, } /** - * jMAVsim implemntation - * thrust from omega linearly + * Linear approximation versions + * used in jMAVsim implemntation. + * Used in comparing with the non-linear(our) model. */ double rotor_thrust_linear( double A2, /* the A parameter in Trust = A*(Omega) */ diff --git a/drone_physics/rotor_physics.hpp b/drone_physics/rotor_physics.hpp index 11774001..604cda4f 100644 --- a/drone_physics/rotor_physics.hpp +++ b/drone_physics/rotor_physics.hpp @@ -11,14 +11,14 @@ namespace hako::drone_physics { */ /* The rotor dynamics modeled as a first-order lag system. eq.(2.48) */ -double rotor_omega_acceleration( - double Kr, /* gain constant */ +double rotor_omega_acceleration( /* omega acceleration in [rad/s^2]*/ + double Kr, /* gain constant in */ double Tr, /* time constant */ double omega, /* angular velocity in [rad/s] */ double duty_rate /* 0.0-1.0 (ratio of PWM) */ ); /* realistic model for rotor */ -double rotor_omega_acceleration( +double rotor_omega_acceleration( /* omega acceleration in [rad/s^2]*/ double Vbat, /* battery voltage in volt [V]*/ double R, /* resistance in ohm [V/A] */ double Cq, /* torque coeff (= B param in anti-torque) [N ms^2/rad^2] */ @@ -29,7 +29,7 @@ double rotor_omega_acceleration( double duty_rate /* 0.0-1.0 (ratio of PWM) */); /* current in the rotor */ -double rotor_current( +double rotor_current( /* current in [A] */ double Vbat, /* battery voltage in volt [V]*/ double R, /* resistance in ohm [V/A] */ double K, /* back electromotive force coeff in [N m/A] */ @@ -37,17 +37,16 @@ double rotor_current( double duty_rate /* 0.0-1.0 (ratio of PWM) */); /* thrust from omega in radian/sec eq.(2.50)*/ -double rotor_thrust( +double rotor_thrust( /* thrust in [N] */ double A, /* parameter A in Trust = A*(Omega)^2 */ - double omega /* in radian/sec */ ); + double omega /* in [rad/s] */ ); /* this makes z-axis rotation eq.(2.56) */ -/* Ta = B*(Omega)^2 + Jr* (d(Omega)/dt) */ -double rotor_anti_torque( - double B, - double Jr, - double omega, /* in radian/sec */ - double omega_acceleratoin, /* in radian/s^2 */ +double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/ + double B, /* torque coefficient (referred to as Cq in Kohei's doc) in [N m s^2/rad^2] */ + double Jr, /* torque coefficient for 2nd order rotation */ + double omega, /* in [rad/s] */ + double omega_acceleratoin, /* in [rad/s^2] */ double ccw /* 1 or -1 */ ); /** @@ -56,30 +55,37 @@ double rotor_anti_torque( */ /* the sum of the n trust from the rotors eq.(2.61) */ -double body_thrust( +double body_thrust( /* thrust in [N] */ double A, /* parameter A in Trust = A*(Omega)^2 in each motor */ unsigned n, /* number of rotors */ double omega[] /* in radian/sec */ ); /* the sum of the n torques from the rotors including anti-torque */ /* eq.(2.60)-(2.62)*/ -TorqueType body_torque( +TorqueType body_torque( /* torque in [Nm] */ double A, /* parameter A in Trust = A*(Omega)^2 */ - double B, /* parameter B in Ta = B*(Omega)^2 + Jr* (d(Omega)/dt) */ + double B, /* anti-torque parameter B in Ta = B*(Omega)^2 + Jr* (d(Omega)/dt) */ double Jr, unsigned n, /* number of rotors */ - VectorType position[], /* position of each rotor */ + VectorType position[], /* position of each rotor in [m] */ double ccw[], /* 1 or -1 */ - double omega[], /* in radian/sec */ - double omega_acceleration[] /* in radian/s^2 */ ); + double omega[], /* in [rad/s] */ + double omega_acceleration[] /* in [rad/s^2] */ ); +/** + * Linear approximation versions + * used in jMAVsim implemntation. + * Used in comparing with the non-linear(our) model. + */ double rotor_thrust_linear( double A, /* the A parameter in Trust = A*(Omega) */ double omega /* in radian/sec */ ); + double rotor_anti_torque_linear(double B2, double omega, double ccw); TorqueType body_torque_linear(double A2, double B2, unsigned n, VectorType position[], double ccw[], double omega[]); + double body_thrust_linear(double A2, unsigned n, double omega[]); From 364e46f25da5837ad384ada33adcf4f26a1e7746 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 11:23:26 +0900 Subject: [PATCH 19/50] add battery config --- hakoniwa/config/rc-battery/drone_config_0.json | 9 +++++++-- hakoniwa/src/config/drone_config.hpp | 17 +++++++++++++++++ hakoniwa/src/config/drone_config_types.hpp | 4 ++++ 3 files changed, 28 insertions(+), 2 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 14c0c767..8440c582 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -64,6 +64,11 @@ 0 ] }, + "battery": { + "vendor": "None", + "NominalCapacity": 11.1, + "ActualCapacity": 11.1 + }, "rotor": { "vendor": "BatteryModel", "dynamics_constants": { @@ -141,8 +146,8 @@ } }, "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/FlightController", - "moduleName": "FlightController", + "moduleDirectory": "../drone_control/cmake-build/workspace/RadioController", + "moduleName": "RadioController", "direct_rotor_control": false, "mixer": { "vendor": "None", diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 5b4be353..ab3bb428 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -239,6 +239,23 @@ class DroneConfig { double getCompRotorKr() const { return configJson["components"]["rotor"]["Kr"].get(); } + hako::assets::drone::BatteryModelParameters getComDroneDynamicsBattery() const { + hako::assets::drone::BatteryModelParameters params; + try { + if (configJson["components"].contains("battery")) { + params.NominalCapacity = configJson["components"]["battery"]["NominalCapacity"].get(); + params.ActualCapacity = configJson["components"]["battery"]["ActualCapacity"].get(); + std::cout << "Battery model is enabled." << std::endl; + return params; + } + else { + std::cout << "ERROR: Battery model config is invalid." << std::endl; + } + } catch (const std::exception& e) { + std::cerr << "Error retrieving Battery Model config: " << e.what() << std::endl; + } + return params; + } hako::assets::drone::RotorBatteryModelConstants getCompDroneDynamicsRotorDynamicsConstants() const { hako::assets::drone::RotorBatteryModelConstants constants = {}; diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index ae2366fd..47b89252 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -17,6 +17,10 @@ namespace hako::assets::drone { double D; double J; }; + struct BatteryModelParameters { + double NominalCapacity; + double ActualCapacity; + }; } #endif /* _DRONE_CONFIG_TYPES_HPP_ */ \ No newline at end of file From c5e88a4e2b40ce6e29840a0af1fbb0fbb3102403 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 11:35:31 +0900 Subject: [PATCH 20/50] add voltage --- hakoniwa/src/config/drone_config.hpp | 2 ++ hakoniwa/src/config/drone_config_types.hpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index ab3bb428..5039a732 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -245,6 +245,8 @@ class DroneConfig { if (configJson["components"].contains("battery")) { params.NominalCapacity = configJson["components"]["battery"]["NominalCapacity"].get(); params.ActualCapacity = configJson["components"]["battery"]["ActualCapacity"].get(); + params.RatedVoltage = configJson["components"]["battery"]["RatedVoltage"].get(); + params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); std::cout << "Battery model is enabled." << std::endl; return params; } diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index 47b89252..64d97683 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -20,6 +20,8 @@ namespace hako::assets::drone { struct BatteryModelParameters { double NominalCapacity; double ActualCapacity; + double RatedVoltage; + double NominalVoltage; }; } From cfa1fb2f7e8c021fc2306f830cacb33df4971dca Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 12:33:53 +0900 Subject: [PATCH 21/50] add discharge model --- .../drone/include/idischarge_dynamics.hpp | 14 ++++++++++ .../drone/physics/rotor/rotor_dynamics.hpp | 27 ++++++++++++++++--- 2 files changed, 37 insertions(+), 4 deletions(-) create mode 100644 hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp diff --git a/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp b/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp new file mode 100644 index 00000000..852e4e15 --- /dev/null +++ b/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp @@ -0,0 +1,14 @@ +#ifndef _IDISCHARGE_DYNAMICS_HPP_ +#define _IDISCHARGE_DYNAMICS_HPP_ + + +namespace hako::assets::drone { + +class IDischargeDynamics { +public: + virtual ~IDischargeDynamics() {}; + virtual double get_discharged() = 0; +}; +} + +#endif /* _IDISCHARGE_DYNAMICS_HPP_ */ diff --git a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp index f09aeee2..396766db 100644 --- a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp @@ -3,6 +3,7 @@ #include "drone_primitive_types.hpp" #include "irotor_dynamics.hpp" +#include "idischarge_dynamics.hpp" #include "utils/icsv_log.hpp" #include "rotor_physics.hpp" #include @@ -10,7 +11,7 @@ namespace hako::assets::drone { -class RotorDynamics : public hako::assets::drone::IRotorDynamics, public ICsvLog { +class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::assets::drone::IDischargeDynamics, public ICsvLog { private: double param_rad_per_sec_max = 6000.0; double param_tr = 1.0; @@ -21,7 +22,19 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public ICsvLog DroneRotorSpeedType next_speed; double delta_time_sec; double total_time_sec; + double discharged_value; + void run_discharge(double vbat, double omega, double duty_rate) + { + double current = drone_physics::rotor_current( + vbat, /* battery voltage in volt [V]*/ + this->constants.R, /* resistance in ohm [V/A] */ + this->constants.K, /* back electromotive force coeff in [N m/A] */ + omega, /* angular velocity in [rad/sec] */ + duty_rate /* 0.0-1.0 (ratio of PWM) */ + ); + this->discharged_value += current * this->delta_time_sec; + } public: virtual ~RotorDynamics() {} RotorDynamics(double dt) @@ -29,6 +42,7 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public ICsvLog this->delta_time_sec = dt; this->total_time_sec = 0; this->speed.data = 0; + this->discharged_value = 0; } void set_battery_dynamics_constants(const RotorBatteryModelConstants &c) override { @@ -80,6 +94,7 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public ICsvLog } void run(double control, double vbat) override { + this->run_discharge(vbat, this->speed.data, control); this->next_speed.data = ( drone_physics::rotor_omega_acceleration(vbat, constants.R, constants.Cq, constants.J, constants.K, constants.D, speed.data, control) ) * this->delta_time_sec @@ -94,15 +109,19 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public ICsvLog } this->speed.data = this->next_speed.data; this->total_time_sec += this->delta_time_sec; - } + } + double get_discharged() override + { + return this->discharged_value; + } const std::vector log_head() override { - return { "timestamp", "RadPerSec" }; + return { "timestamp", "RadPerSec", "DischargedValue" }; } const std::vector log_data() override { DroneRotorSpeedType v = get_rotor_speed(); - return {std::to_string(CsvLogger::get_time_usec()), std::to_string(v.data)}; + return {std::to_string(CsvLogger::get_time_usec()), std::to_string(v.data), std::to_string(this->discharged_value)}; } }; From 965ca074412f6e2698ecd8b21f58f2ca127f05c7 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 12:52:07 +0900 Subject: [PATCH 22/50] add parameter --- hakoniwa/src/config/drone_config.hpp | 1 + hakoniwa/src/config/drone_config_types.hpp | 1 + 2 files changed, 2 insertions(+) diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 5039a732..7dc8034c 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -247,6 +247,7 @@ class DroneConfig { params.ActualCapacity = configJson["components"]["battery"]["ActualCapacity"].get(); params.RatedVoltage = configJson["components"]["battery"]["RatedVoltage"].get(); params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); + params.MaximumDischargeCurrent = configJson["components"]["battery"]["MaximumDischargeCurrent"].get(); std::cout << "Battery model is enabled." << std::endl; return params; } diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index 64d97683..59debb0f 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -22,6 +22,7 @@ namespace hako::assets::drone { double ActualCapacity; double RatedVoltage; double NominalVoltage; + double MaximumDischargeCurrent; }; } From f3c6f7f305e4334ce7d3d997250d4806909823a8 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 13:52:40 +0900 Subject: [PATCH 23/50] add battery --- .../src/assets/drone/include/iaircraft.hpp | 6 ++ .../drone/include/ibattery_dynamics.hpp | 37 +++++++++++ .../drone/include/idischarge_dynamics.hpp | 2 + .../physics/battery/battery_dynamics.hpp | 65 +++++++++++++++++++ hakoniwa/src/config/drone_config.hpp | 1 + hakoniwa/src/config/drone_config_types.hpp | 25 +++++++ 6 files changed, 136 insertions(+) create mode 100644 hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp create mode 100644 hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp diff --git a/hakoniwa/src/assets/drone/include/iaircraft.hpp b/hakoniwa/src/assets/drone/include/iaircraft.hpp index 7eee8e8c..508e4657 100644 --- a/hakoniwa/src/assets/drone/include/iaircraft.hpp +++ b/hakoniwa/src/assets/drone/include/iaircraft.hpp @@ -3,6 +3,7 @@ #include "idrone_dynamics.hpp" #include "irotor_dynamics.hpp" +#include "ibattery_dynamics.hpp" #include "ithrust_dynamics.hpp" #include "isensor_acceleration.hpp" #include "isensor_baro.hpp" @@ -19,6 +20,7 @@ class IAirCraft { IDroneDynamics *drone_dynamics; IRotorDynamics *rotor_dynamics[ROTOR_NUM]; IThrustDynamics *thrust_dynamis; + IBatteryDynamics *battery_dynamics; ISensorAcceleration *acc; ISensorBaro *baro; @@ -83,6 +85,10 @@ class IAirCraft { this->rotor_dynamics[i] = src[i]; } } + void set_battery_dynamics(IBatteryDynamics *src) + { + this->battery_dynamics = src; + } double get_rpm_max(int rotor_index) { if (rotor_index < ROTOR_NUM) { diff --git a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp new file mode 100644 index 00000000..c6d2ab61 --- /dev/null +++ b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp @@ -0,0 +1,37 @@ +#ifndef _IBATTERY_DYNAMICS_HPP_ +#define _IBATTERY_DYNAMICS_HPP_ + +#include "idischarge_dynamics.hpp" +#include "config/drone_config_types.hpp" +#include + +namespace hako::assets::drone { + +class IBatteryDynamics { +protected: + void *vendor_model; + void *context; + BatteryModelParameters params; + std::vector devices; +public: + virtual ~IBatteryDynamics() {} + virtual void set_vendor(void *vendor, void *context) + { + this->vendor_model = vendor; + this->context = context; + } + virtual void add_device(IDischargeDynamics& device) + { + devices.push_back(&device); + } + //calculate battery remained value + virtual double get_vbat() = 0; + virtual void set_params(const BatteryModelParameters &p) + { + this->params = p; + } + +}; +} + +#endif /* _IBATTERY_DYNAMICS_HPP_ */ \ No newline at end of file diff --git a/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp b/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp index 852e4e15..eba633a5 100644 --- a/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp @@ -7,6 +7,8 @@ namespace hako::assets::drone { class IDischargeDynamics { public: virtual ~IDischargeDynamics() {}; + + // return value Unit: [A*s] virtual double get_discharged() = 0; }; } diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp new file mode 100644 index 00000000..4e759c40 --- /dev/null +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -0,0 +1,65 @@ +#ifndef _BATTERY_DYNAMICS_HPP_ +#define _BATTERY_DYNAMICS_HPP_ + +#include "drone_primitive_types.hpp" +#include "ibattery_dynamics.hpp" +#include "idischarge_dynamics.hpp" +#include "utils/icsv_log.hpp" +#include + +namespace hako::assets::drone { + +class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICsvLog { +private: + double current_charge_voltage; + double get_current_charge_value(double discharged_value) + { + if (discharged_value > params.ActualCapacity) { + discharged_value = params.ActualCapacity; + } + double V_initial = params.NominalVoltage; // 初期電圧（公称電圧） [V] + double V_final = params.RatedVoltage; // 放電終了電圧（定格電圧） [V] + double MaxCapacity = params.ActualCapacity; // 実容量（実際に使用可能な容量） [Ah] + + // 放電容量に基づく電圧低下の傾きを計算 + double slope = (V_initial - V_final) / (MaxCapacity * 1000.0); // mAhに変換 + double battery_voltage = V_initial - (slope * discharged_value); + + return battery_voltage; + } + +public: + virtual ~BatteryDynamics() {} + BatteryDynamics() + { + this->current_charge_voltage = 0; + this->remain_charge_voltage = 0; + } + double get_vbat() override + { + double total_discharge_value = 0; + for (auto* entry : devices) { + total_discharge_value += entry->get_discharged(); + } + // Convert total_discharge_value from As (Ampere-seconds) to mAh (milliampere-hours) + // 1 A・s = 1 / 3600 Ah + // 1 Ah = 1000 mAh + // Therefore: mAh = A・s * (1000 / 3600) = A・s * 0.2778 + double discharged_value = total_discharge_value * (1000 / 3600); // Unit conversion from As to mAh + this->current_charge_voltage = this->get_current_charge_value(discharged_value); + return this->current_charge_voltage; + } + const std::vector log_head() override + { + return { "timestamp", "CurrentVoltage" }; + } + + const std::vector log_data() override + { + return {std::to_string(CsvLogger::get_time_usec()), std::to_string(current_charge_voltage)}; + } +}; + +} + +#endif /* _BATTERY_DYNAMICS_HPP_ */ diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 7dc8034c..9fa2d8ca 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -248,6 +248,7 @@ class DroneConfig { params.RatedVoltage = configJson["components"]["battery"]["RatedVoltage"].get(); params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); params.MaximumDischargeCurrent = configJson["components"]["battery"]["MaximumDischargeCurrent"].get(); + params.RatedCapacity = configJson["components"]["battery"]["RatedCapacity"].get(); std::cout << "Battery model is enabled." << std::endl; return params; } diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index 59debb0f..f8f548eb 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -18,11 +18,36 @@ namespace hako::assets::drone { double J; }; struct BatteryModelParameters { + /* + * 公称容量: [Ah] + * バッテリーが供給できる理論上の電気量 + */ double NominalCapacity; + /* + * 実容量: [Ah] + * 実際に使用可能な電気容量 + */ double ActualCapacity; + /* + * 定格電圧: [V] + * バッテリーの公称電圧 + */ double RatedVoltage; + /* + * 公称電圧: [V] + * バッテリーの標準的な動作電圧 + */ double NominalVoltage; + /* + * 最大放電電流: [A] + * 安全に放電できる最大電流値 + */ double MaximumDischargeCurrent; + /* + * 定格容量: [Ah] + * メーカーが保証する容量 + */ + double RatedCapacity; }; } From cc1f5ab224e3efe743404e5e65f57b0ae204332f Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 14:14:38 +0900 Subject: [PATCH 24/50] add runner --- hakoniwa/config/rc-battery/drone_config_0.json | 8 ++++++-- .../assets/drone/aircraft/aircraft_factory.cpp | 16 ++++++++++++++++ hakoniwa/src/assets/drone/aircraft/aricraft.hpp | 6 +++++- .../drone/physics/battery/battery_dynamics.hpp | 1 - hakoniwa/src/config/drone_config.hpp | 1 + hakoniwa/src/config/drone_config_types.hpp | 1 + 6 files changed, 29 insertions(+), 4 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 8440c582..3ee8f509 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -66,8 +66,12 @@ }, "battery": { "vendor": "None", - "NominalCapacity": 11.1, - "ActualCapacity": 11.1 + "NominalCapacity": 3.0, + "ActualCapacity": 3.0, + "RatedCapacity": 3.0, + "NominalVoltage": 11.1, + "RatedVoltage": 3.0, + "MaximumDischargeCurrent": 0.0 }, "rotor": { "vendor": "BatteryModel", diff --git a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp index 9a4df95e..21593c59 100644 --- a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp +++ b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp @@ -5,6 +5,7 @@ #include "assets/drone/physics/body_frame_rk4/drone_dynamics_body_frame_rk4.hpp" #include "assets/drone/physics/ground_frame/drone_dynamics_ground_frame.hpp" #include "assets/drone/physics/rotor/rotor_dynamics.hpp" +#include "assets/drone/physics/battery/battery_dynamics.hpp" #include "assets/drone/physics/rotor/rotor_dynamics_jmavsim.hpp" #include "assets/drone/physics/thruster/thrust_dynamics_linear.hpp" #include "assets/drone/physics/thruster/thrust_dynamics_nonlinear.hpp" @@ -147,6 +148,21 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr } drone->set_rotor_dynamics(rotors); + //battery dynamics + BatteryModelParameters battery_config = drone_config.getComDroneDynamicsBattery(); + //TODO vendor support + if (battery_config.vendor == "None") { + auto battery = new BatteryDynamics(); + HAKO_ASSERT(battery != nullptr); + battery->set_params(battery_config); + drone->set_battery_dynamics(battery); + drone->get_logger().add_entry(*static_cast(battery), LOGPATH(drone->get_index(), "log_battery.csv")); + } + else { + std::cout << "INFO: battery is not enabled." << std::endl; + } + + //thrust dynamics IThrustDynamics *thrust = nullptr; auto thrust_vendor = drone_config.getCompThrusterVendor(); diff --git a/hakoniwa/src/assets/drone/aircraft/aricraft.hpp b/hakoniwa/src/assets/drone/aircraft/aricraft.hpp index de810bef..11952029 100644 --- a/hakoniwa/src/assets/drone/aircraft/aricraft.hpp +++ b/hakoniwa/src/assets/drone/aircraft/aricraft.hpp @@ -30,12 +30,16 @@ class AirCraft : public hako::assets::drone::IAirCraft { } void run(DroneDynamicsInputType& input) override { + double vbat = 0.0; + if (this->battery_dynamics != nullptr) { + vbat = this->battery_dynamics->get_vbat(); + } //actuators if (input.no_use_actuator == false) { DroneRotorSpeedType rotor_speed[ROTOR_NUM]; for (int i = 0; i < ROTOR_NUM; i++) { if (rotor_dynamics[i]->has_battery_dynamics()) { - rotor_dynamics[i]->run(input.controls[i], 11.1 /* TODO need battery model */); + rotor_dynamics[i]->run(input.controls[i], vbat); } else { rotor_dynamics[i]->run(input.controls[i]); diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 4e759c40..a00df665 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -33,7 +33,6 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs BatteryDynamics() { this->current_charge_voltage = 0; - this->remain_charge_voltage = 0; } double get_vbat() override { diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 9fa2d8ca..7d377d9a 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -243,6 +243,7 @@ class DroneConfig { hako::assets::drone::BatteryModelParameters params; try { if (configJson["components"].contains("battery")) { + params.vendor = configJson["components"]["battery"][""].get(); params.NominalCapacity = configJson["components"]["battery"]["NominalCapacity"].get(); params.ActualCapacity = configJson["components"]["battery"]["ActualCapacity"].get(); params.RatedVoltage = configJson["components"]["battery"]["RatedVoltage"].get(); diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index f8f548eb..fb4a2fa3 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -18,6 +18,7 @@ namespace hako::assets::drone { double J; }; struct BatteryModelParameters { + std::string vendor; /* * 公称容量: [Ah] * バッテリーが供給できる理論上の電気量 From e944445b57ef1849c097799d4be20b9ae5bc18a5 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 14:21:18 +0900 Subject: [PATCH 25/50] bug fixed --- hakoniwa/src/config/drone_config.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 7d377d9a..3ae5ca6d 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -243,7 +243,7 @@ class DroneConfig { hako::assets::drone::BatteryModelParameters params; try { if (configJson["components"].contains("battery")) { - params.vendor = configJson["components"]["battery"][""].get(); + params.vendor = configJson["components"]["battery"]["vendor"].get(); params.NominalCapacity = configJson["components"]["battery"]["NominalCapacity"].get(); params.ActualCapacity = configJson["components"]["battery"]["ActualCapacity"].get(); params.RatedVoltage = configJson["components"]["battery"]["RatedVoltage"].get(); From f2319af254768445db285834704ea814a19101ff Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 17:40:24 +0900 Subject: [PATCH 26/50] bug fixed --- .../drone/aircraft/aircraft_factory.cpp | 31 ++++++++++--------- .../drone/include/ibattery_dynamics.hpp | 1 + .../physics/battery/battery_dynamics.hpp | 14 ++++----- 3 files changed, 25 insertions(+), 21 deletions(-) diff --git a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp index 21593c59..108cab56 100644 --- a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp +++ b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp @@ -117,6 +117,21 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr std::cout << "INFO: logpath: " << LOGPATH(drone->get_index(), "drone_dynamics.csv") << std::endl; drone->get_logger().add_entry(*drone_dynamics, LOGPATH(drone->get_index(), "drone_dynamics.csv")); + + //battery dynamics + BatteryModelParameters battery_config = drone_config.getComDroneDynamicsBattery(); + //TODO vendor support + IBatteryDynamics *battery = nullptr; + if (battery_config.vendor == "None") { + battery = new BatteryDynamics(); + HAKO_ASSERT(battery != nullptr); + battery->set_params(battery_config); + drone->set_battery_dynamics(battery); + drone->get_logger().add_entry(*static_cast(battery), LOGPATH(drone->get_index(), "log_battery.csv")); + } + else { + std::cout << "INFO: battery is not enabled." << std::endl; + } //rotor dynamics IRotorDynamics* rotors[hako::assets::drone::ROTOR_NUM]; auto rotor_vendor = drone_config.getCompRotorVendor(); @@ -137,6 +152,8 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr rotor->set_battery_dynamics_constants(constants); static_cast(rotor)->set_params(RAD_PER_SEC_MAX, 0, ROTOR_K); drone->get_logger().add_entry(*static_cast(rotor), LOGPATH(drone->get_index(), logfilename)); + HAKO_ASSERT(battery != nullptr); + battery->add_device(*static_cast(rotor)); } else { rotor = new RotorDynamics(DELTA_TIME_SEC); @@ -148,20 +165,6 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr } drone->set_rotor_dynamics(rotors); - //battery dynamics - BatteryModelParameters battery_config = drone_config.getComDroneDynamicsBattery(); - //TODO vendor support - if (battery_config.vendor == "None") { - auto battery = new BatteryDynamics(); - HAKO_ASSERT(battery != nullptr); - battery->set_params(battery_config); - drone->set_battery_dynamics(battery); - drone->get_logger().add_entry(*static_cast(battery), LOGPATH(drone->get_index(), "log_battery.csv")); - } - else { - std::cout << "INFO: battery is not enabled." << std::endl; - } - //thrust dynamics IThrustDynamics *thrust = nullptr; diff --git a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp index c6d2ab61..e5d3fb05 100644 --- a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp @@ -4,6 +4,7 @@ #include "idischarge_dynamics.hpp" #include "config/drone_config_types.hpp" #include +#include namespace hako::assets::drone { diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index a00df665..a9eff3d1 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -12,6 +12,7 @@ namespace hako::assets::drone { class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICsvLog { private: double current_charge_voltage; + double total_discharged_value; double get_current_charge_value(double discharged_value) { if (discharged_value > params.ActualCapacity) { @@ -40,22 +41,21 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs for (auto* entry : devices) { total_discharge_value += entry->get_discharged(); } - // Convert total_discharge_value from As (Ampere-seconds) to mAh (milliampere-hours) + // Convert total_discharge_value from As (Ampere-seconds) to Ah (Ampere-hours) // 1 A・s = 1 / 3600 Ah - // 1 Ah = 1000 mAh - // Therefore: mAh = A・s * (1000 / 3600) = A・s * 0.2778 - double discharged_value = total_discharge_value * (1000 / 3600); // Unit conversion from As to mAh - this->current_charge_voltage = this->get_current_charge_value(discharged_value); + // Therefore: Ah = A・s * (1 / 3600) + total_discharged_value = total_discharge_value * (1.0 / 3600.0); // Unit conversion from As to Ah + this->current_charge_voltage = this->get_current_charge_value(total_discharged_value); return this->current_charge_voltage; } const std::vector log_head() override { - return { "timestamp", "CurrentVoltage" }; + return { "timestamp", "DischargedValue", "CurrentVoltage" }; } const std::vector log_data() override { - return {std::to_string(CsvLogger::get_time_usec()), std::to_string(current_charge_voltage)}; + return {std::to_string(CsvLogger::get_time_usec()), std::to_string(total_discharged_value), std::to_string(current_charge_voltage)}; } }; From 2c44c8958d633f65cd29d6095c2d6f73326bf5ce Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Mon, 11 Nov 2024 17:55:24 +0900 Subject: [PATCH 27/50] bug fixed --- hakoniwa/config/rc-battery/drone_config_0.json | 2 +- hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 3ee8f509..eedf0a03 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -67,7 +67,7 @@ "battery": { "vendor": "None", "NominalCapacity": 3.0, - "ActualCapacity": 3.0, + "ActualCapacity": 30.0, "RatedCapacity": 3.0, "NominalVoltage": 11.1, "RatedVoltage": 3.0, diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index a9eff3d1..d5334581 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -23,7 +23,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double MaxCapacity = params.ActualCapacity; // 実容量（実際に使用可能な容量） [Ah] // 放電容量に基づく電圧低下の傾きを計算 - double slope = (V_initial - V_final) / (MaxCapacity * 1000.0); // mAhに変換 + double slope = (V_initial - V_final) / (MaxCapacity); double battery_voltage = V_initial - (slope * discharged_value); return battery_voltage; From b7cab15c96656a305c95d976f902b0caa41e7fb1 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Tue, 12 Nov 2024 10:34:25 +0900 Subject: [PATCH 28/50] add info --- .../config/rc-battery/drone_config_0.json | 2 +- .../drone/aircraft/aircraft_factory.cpp | 2 +- .../drone/include/idischarge_dynamics.hpp | 6 ++-- .../physics/battery/battery_dynamics.hpp | 28 +++++++++++++------ .../drone/physics/rotor/rotor_dynamics.hpp | 26 +++++++++++------ 5 files changed, 44 insertions(+), 20 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index eedf0a03..3ee8f509 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -67,7 +67,7 @@ "battery": { "vendor": "None", "NominalCapacity": 3.0, - "ActualCapacity": 30.0, + "ActualCapacity": 3.0, "RatedCapacity": 3.0, "NominalVoltage": 11.1, "RatedVoltage": 3.0, diff --git a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp index 108cab56..c9d7acef 100644 --- a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp +++ b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp @@ -123,7 +123,7 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr //TODO vendor support IBatteryDynamics *battery = nullptr; if (battery_config.vendor == "None") { - battery = new BatteryDynamics(); + battery = new BatteryDynamics(DELTA_TIME_SEC); HAKO_ASSERT(battery != nullptr); battery->set_params(battery_config); drone->set_battery_dynamics(battery); diff --git a/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp b/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp index eba633a5..238d6159 100644 --- a/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp @@ -8,8 +8,10 @@ class IDischargeDynamics { public: virtual ~IDischargeDynamics() {}; - // return value Unit: [A*s] - virtual double get_discharged() = 0; + // return value Unit: [As] + virtual double get_discharged_capacity() = 0; + // return value Unit: [A] + virtual double get_current() = 0; }; } diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index d5334581..9a6086ab 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -12,7 +12,11 @@ namespace hako::assets::drone { class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICsvLog { private: double current_charge_voltage; - double total_discharged_value; + double total_discharged_capacity_sec; + double discharge_capacity_hour; + double discharge_current; + double delta_time_sec; + double get_current_charge_value(double discharged_value) { if (discharged_value > params.ActualCapacity) { @@ -31,31 +35,39 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs public: virtual ~BatteryDynamics() {} - BatteryDynamics() + BatteryDynamics(double dt) { + this->delta_time_sec = dt; this->current_charge_voltage = 0; + this->total_discharged_capacity_sec = 0; + this->discharge_current = 0; + this->discharge_capacity_hour = 0; } double get_vbat() override { - double total_discharge_value = 0; + double discharge_capacity_sec = 0; + this->discharge_current = 0; for (auto* entry : devices) { - total_discharge_value += entry->get_discharged(); + discharge_capacity_sec += entry->get_discharged_capacity(); + this->discharge_current += entry->get_current(); } // Convert total_discharge_value from As (Ampere-seconds) to Ah (Ampere-hours) // 1 A・s = 1 / 3600 Ah // Therefore: Ah = A・s * (1 / 3600) - total_discharged_value = total_discharge_value * (1.0 / 3600.0); // Unit conversion from As to Ah - this->current_charge_voltage = this->get_current_charge_value(total_discharged_value); + this->discharge_capacity_hour = discharge_capacity_sec /3600.0; // Unit conversion from As to Ah + this->current_charge_voltage = this->get_current_charge_value(discharge_capacity_hour); + + this->total_discharged_capacity_sec = discharge_capacity_sec; return this->current_charge_voltage; } const std::vector log_head() override { - return { "timestamp", "DischargedValue", "CurrentVoltage" }; + return { "timestamp", "DischargeCurrent", "CurrentVoltage", "DischargeCapacity" }; } const std::vector log_data() override { - return {std::to_string(CsvLogger::get_time_usec()), std::to_string(total_discharged_value), std::to_string(current_charge_voltage)}; + return {std::to_string(CsvLogger::get_time_usec()), std::to_string(discharge_current), std::to_string(current_charge_voltage), std::to_string(discharge_capacity_hour)}; } }; diff --git a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp index 396766db..a508d2e5 100644 --- a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp @@ -22,18 +22,21 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a DroneRotorSpeedType next_speed; double delta_time_sec; double total_time_sec; - double discharged_value; + double discharged_capacity; /* [As]*/ + double current; /* [A] */ + double duty; void run_discharge(double vbat, double omega, double duty_rate) { - double current = drone_physics::rotor_current( + /* current in [A] */ + this->current = drone_physics::rotor_current( vbat, /* battery voltage in volt [V]*/ this->constants.R, /* resistance in ohm [V/A] */ this->constants.K, /* back electromotive force coeff in [N m/A] */ omega, /* angular velocity in [rad/sec] */ duty_rate /* 0.0-1.0 (ratio of PWM) */ ); - this->discharged_value += current * this->delta_time_sec; + this->discharged_capacity += this->current * this->delta_time_sec; } public: virtual ~RotorDynamics() {} @@ -42,7 +45,8 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a this->delta_time_sec = dt; this->total_time_sec = 0; this->speed.data = 0; - this->discharged_value = 0; + this->discharged_capacity = 0; + this->current = 0; } void set_battery_dynamics_constants(const RotorBatteryModelConstants &c) override { @@ -77,6 +81,7 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a void run(double control) override { + this->duty = control; this->next_speed.data = ( drone_physics::rotor_omega_acceleration(param_kr, param_tr, speed.data, control) ) * this->delta_time_sec @@ -94,6 +99,7 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a } void run(double control, double vbat) override { + this->duty = control; this->run_discharge(vbat, this->speed.data, control); this->next_speed.data = ( drone_physics::rotor_omega_acceleration(vbat, constants.R, constants.Cq, constants.J, constants.K, constants.D, speed.data, control) @@ -110,18 +116,22 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a this->speed.data = this->next_speed.data; this->total_time_sec += this->delta_time_sec; } - double get_discharged() override + double get_discharged_capacity() override { - return this->discharged_value; + return this->discharged_capacity; + } + double get_current() override + { + return this->current; } const std::vector log_head() override { - return { "timestamp", "RadPerSec", "DischargedValue" }; + return { "timestamp", "Duty", "RadPerSec", "DischargedValue", "Current" }; } const std::vector log_data() override { DroneRotorSpeedType v = get_rotor_speed(); - return {std::to_string(CsvLogger::get_time_usec()), std::to_string(v.data), std::to_string(this->discharged_value)}; + return {std::to_string(CsvLogger::get_time_usec()), std::to_string(this->duty), std::to_string(v.data), std::to_string(this->discharged_capacity), std::to_string(this->current)}; } }; From 1b9292f4175453a6dae5f41a76009b5a6be1d51d Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Tue, 12 Nov 2024 11:27:49 +0900 Subject: [PATCH 29/50] parameters are tuned --- hakoniwa/config/rc-battery/drone_config_0.json | 7 ++++--- .../assets/drone/physics/battery/battery_dynamics.hpp | 10 +++++----- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 3ee8f509..1b48918a 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -67,11 +67,12 @@ "battery": { "vendor": "None", "NominalCapacity": 3.0, - "ActualCapacity": 3.0, "RatedCapacity": 3.0, - "NominalVoltage": 11.1, + "MaximumDischargeCurrent": 0.0, + + "NominalVoltage": 14.8, "RatedVoltage": 3.0, - "MaximumDischargeCurrent": 0.0 + "ActualCapacity": 4.0 }, "rotor": { "vendor": "BatteryModel", diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 9a6086ab..e64a96cc 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -17,10 +17,10 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double discharge_current; double delta_time_sec; - double get_current_charge_value(double discharged_value) + double get_current_charge_voltage(double discharged_capacity) { - if (discharged_value > params.ActualCapacity) { - discharged_value = params.ActualCapacity; + if (discharged_capacity > params.ActualCapacity) { + discharged_capacity = params.ActualCapacity; } double V_initial = params.NominalVoltage; // 初期電圧（公称電圧） [V] double V_final = params.RatedVoltage; // 放電終了電圧（定格電圧） [V] @@ -28,7 +28,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs // 放電容量に基づく電圧低下の傾きを計算 double slope = (V_initial - V_final) / (MaxCapacity); - double battery_voltage = V_initial - (slope * discharged_value); + double battery_voltage = V_initial - (slope * discharged_capacity); return battery_voltage; } @@ -55,7 +55,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs // 1 A・s = 1 / 3600 Ah // Therefore: Ah = A・s * (1 / 3600) this->discharge_capacity_hour = discharge_capacity_sec /3600.0; // Unit conversion from As to Ah - this->current_charge_voltage = this->get_current_charge_value(discharge_capacity_hour); + this->current_charge_voltage = this->get_current_charge_voltage(discharge_capacity_hour); this->total_discharged_capacity_sec = discharge_capacity_sec; return this->current_charge_voltage; From e90651898fbb4528ea96ddd53579afce00cbeb5c Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Tue, 12 Nov 2024 13:05:11 +0900 Subject: [PATCH 30/50] add battery status --- drone_api/assets/config/area.json | 4 +- drone_api/assets/config/area_property.json | 2 +- drone_api/libs/pdu_info.py | 15 +++--- .../drone/controller/drone_control_proxy.hpp | 1 + .../src/assets/drone/include/iaircraft.hpp | 4 ++ .../drone/include/ibattery_dynamics.hpp | 8 ++++ .../physics/battery/battery_dynamics.hpp | 9 ++++ hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp | 46 +++++++++++++++---- hakoniwa/src/hako/pdu/hako_pdu_data.hpp | 1 + hakoniwa/src/modules/hako_sim.cpp | 1 + hakoniwa/third-party/hakoniwa-ros2pdu | 2 +- 11 files changed, 73 insertions(+), 20 deletions(-) diff --git a/drone_api/assets/config/area.json b/drone_api/assets/config/area.json index 78a22dca..f73ef4c6 100644 --- a/drone_api/assets/config/area.json +++ b/drone_api/assets/config/area.json @@ -3,8 +3,8 @@ { "area_id": "area_1", "bounds": { - "min": { "x": 10, "y": 10, "z": 0 }, - "max": { "x": 20, "y": 20, "z": 10 } + "min": { "x": 1, "y": -10, "z": 0 }, + "max": { "x": 10, "y": 10, "z": 20 } } } ] diff --git a/drone_api/assets/config/area_property.json b/drone_api/assets/config/area_property.json index 56b07d04..01c68105 100644 --- a/drone_api/assets/config/area_property.json +++ b/drone_api/assets/config/area_property.json @@ -3,7 +3,7 @@ { "area_id": "area_1", "properties": { - "wind_velocity": [1.0, 1.0, 0.0], + "wind_velocity": [0.0, 20.0, 0.0], "temperature": 22.5 } }, diff --git a/drone_api/libs/pdu_info.py b/drone_api/libs/pdu_info.py index c4d941ee..e77e14a0 100644 --- a/drone_api/libs/pdu_info.py +++ b/drone_api/libs/pdu_info.py @@ -8,10 +8,11 @@ HAKO_AVATOR_CHANNEL_ID_CMD_LAND = 7 HAKO_AVATOR_CHANNEL_ID_GAME_CTRL = 8 HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA = 9 -HAKO_AVATOR_CHANNEL_ID_CAMERA_DATA = 10 -HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA_MOVE = 11 -HAKO_AVATOR_CHANNEL_ID_CAMERA_INFO = 12 -HAKO_AVATOR_CHANNEL_ID_CMD_MAG = 13 -HAKO_AVATOR_CHANNEL_ID_STAT_MAG = 14 -HAKO_AVATOR_CHANNEL_ID_LIDAR_DATA = 15 -HAKO_AVATOR_CHANNEL_ID_LIDAR_POS = 16 +HAKO_AVATOR_CHANNEL_ID_BATTERY_STAT= 10 +HAKO_AVATOR_CHANNEL_ID_CAMERA_DATA = 11 +HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA_MOVE = 12 +HAKO_AVATOR_CHANNEL_ID_CAMERA_INFO = 13 +HAKO_AVATOR_CHANNEL_ID_CMD_MAG = 14 +HAKO_AVATOR_CHANNEL_ID_STAT_MAG = 15 +HAKO_AVATOR_CHANNEL_ID_LIDAR_DATA = 16 +HAKO_AVATOR_CHANNEL_ID_LIDAR_POS = 17 diff --git a/hakoniwa/src/assets/drone/controller/drone_control_proxy.hpp b/hakoniwa/src/assets/drone/controller/drone_control_proxy.hpp index b82a766d..68ffacde 100644 --- a/hakoniwa/src/assets/drone/controller/drone_control_proxy.hpp +++ b/hakoniwa/src/assets/drone/controller/drone_control_proxy.hpp @@ -380,6 +380,7 @@ class DroneControlProxyManager { if ((mixer == nullptr) && (module.drone->is_rotor_control_enabled() == false)) { calculate_simple_controls(module, thrust); } + do_io_write_battery_status(module.drone); do_io_write(module.drone, module.controls); index++; } diff --git a/hakoniwa/src/assets/drone/include/iaircraft.hpp b/hakoniwa/src/assets/drone/include/iaircraft.hpp index 508e4657..5213afd8 100644 --- a/hakoniwa/src/assets/drone/include/iaircraft.hpp +++ b/hakoniwa/src/assets/drone/include/iaircraft.hpp @@ -89,6 +89,10 @@ class IAirCraft { { this->battery_dynamics = src; } + IBatteryDynamics *get_battery_dynamics() + { + return this->battery_dynamics; + } double get_rpm_max(int rotor_index) { if (rotor_index < ROTOR_NUM) { diff --git a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp index e5d3fb05..8fdf2f0c 100644 --- a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp @@ -8,6 +8,13 @@ namespace hako::assets::drone { +typedef struct { + double full_voltage; + double curr_voltage; + uint32_t status; + uint32_t cycles; +} BatteryStatusType; + class IBatteryDynamics { protected: void *vendor_model; @@ -27,6 +34,7 @@ class IBatteryDynamics { } //calculate battery remained value virtual double get_vbat() = 0; + virtual BatteryStatusType get_status() = 0; virtual void set_params(const BatteryModelParameters &p) { this->params = p; diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index e64a96cc..9d60284e 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -60,6 +60,15 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs this->total_discharged_capacity_sec = discharge_capacity_sec; return this->current_charge_voltage; } + BatteryStatusType get_status() override + { + BatteryStatusType status; + status.full_voltage = this->params.NominalVoltage; + status.curr_voltage = this->current_charge_voltage; + status.cycles = 0; + status.status = 0; + return status; + } const std::vector log_head() override { return { "timestamp", "DischargeCurrent", "CurrentVoltage", "DischargeCapacity" }; diff --git a/hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp b/hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp index 91795a1f..79a629b3 100644 --- a/hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp +++ b/hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp @@ -11,15 +11,16 @@ #define HAKO_AVATOR_CHANNEL_ID_CMD_MOVE 6 #define HAKO_AVATOR_CHANNEL_ID_CMD_LAND 7 #define HAKO_AVATOR_CHANNEL_ID_GAME_CTRL 8 -#define HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA 9 -#define HAKO_AVATOR_CHANNEL_ID_CAMERA_DATA 10 -#define HAKO_AVATOR_CHANNEL_ID_CAMERA_MOVE 11 -#define HAKO_AVATOR_CHANNEL_ID_CAMERA_INFO 12 -#define HAKO_AVATOR_CHANNEL_ID_CMD_MAG 13 -#define HAKO_AVATOR_CHANNEL_ID_STAT_MAG 14 -#define HAKO_AVATOR_CHANNEL_ID_LIDAR_DATA 15 -#define HAKO_AVATOR_CHANNEL_ID_LIDAR_POS 16 -#define HAKO_AVATOR_CHANNLE_ID_CTRL 17 /* for pid control program only */ +#define HAKO_AVATOR_CHANNEL_ID_BATTERY_STAT 9 +#define HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA 10 +#define HAKO_AVATOR_CHANNEL_ID_CAMERA_DATA 11 +#define HAKO_AVATOR_CHANNEL_ID_CAMERA_MOVE 12 +#define HAKO_AVATOR_CHANNEL_ID_CAMERA_INFO 13 +#define HAKO_AVATOR_CHANNEL_ID_CMD_MAG 14 +#define HAKO_AVATOR_CHANNEL_ID_STAT_MAG 15 +#define HAKO_AVATOR_CHANNEL_ID_LIDAR_DATA 16 +#define HAKO_AVATOR_CHANNEL_ID_LIDAR_POS 17 +#define HAKO_AVATOR_CHANNLE_ID_CTRL 18 /* for pid control program only */ /* { @@ -198,6 +199,33 @@ static inline bool do_io_write_cmd(hako::assets::drone::IAirCraft *drone, int ch } return true; } +template +static inline bool do_io_write_data(hako::assets::drone::IAirCraft *drone, int channel_id, const PacketType& packet) { + char buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(PacketType)]; + if (hako_pdu_put_fixed_data(buffer, reinterpret_cast(&packet), sizeof(PacketType), sizeof(buffer)) < 0) { + std::cerr << "ERROR: can not put pdu data: PacketType" << std::endl; + return false; + } + if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), channel_id, buffer, sizeof(buffer))) { + std::cerr << "ERROR: can not write pdu data: packet channel_id: " << channel_id << " pdu_size: " << sizeof(buffer) << std::endl; + return false; + } + return true; +} +static inline bool do_io_write_battery_status(hako::assets::drone::IAirCraft *drone) +{ + Hako_HakoBatteryStatus packet = {}; + auto battery = drone->get_battery_dynamics(); + if (battery != nullptr) { + auto status = battery->get_status(); + packet.full_voltage = status.full_voltage; + packet.curr_voltage = status.curr_voltage; + packet.cycles = status.cycles; + packet.status = status.status; + return do_io_write_data(drone, HAKO_AVATOR_CHANNEL_ID_BATTERY_STAT, packet); + } + return false; +} static inline void do_io_write(hako::assets::drone::IAirCraft *drone, const double controls[hako::assets::drone::ROTOR_NUM]) { diff --git a/hakoniwa/src/hako/pdu/hako_pdu_data.hpp b/hakoniwa/src/hako/pdu/hako_pdu_data.hpp index 5a88282a..8c11fa1d 100644 --- a/hakoniwa/src/hako/pdu/hako_pdu_data.hpp +++ b/hakoniwa/src/hako/pdu/hako_pdu_data.hpp @@ -13,6 +13,7 @@ #include "hako_msgs/pdu_ctype_HakoDroneCmdMove.h" #include "hako_msgs/pdu_ctype_HakoDroneCmdLand.h" #include "hako_msgs/pdu_ctype_GameControllerOperation.h" +#include "hako_msgs/pdu_ctype_HakoBatteryStatus.h" #include "config/drone_config.hpp" extern bool hako_pdu_data_init(DroneConfigManager& mgr); diff --git a/hakoniwa/src/modules/hako_sim.cpp b/hakoniwa/src/modules/hako_sim.cpp index 1392defb..74f8d5fe 100644 --- a/hakoniwa/src/modules/hako_sim.cpp +++ b/hakoniwa/src/modules/hako_sim.cpp @@ -260,6 +260,7 @@ static void my_task() drone_input.controls[i] = container.controls[i]; } container.drone->run(drone_input); + do_io_write_battery_status(container.drone); do_io_write(container.drone, container.controls); } return; diff --git a/hakoniwa/third-party/hakoniwa-ros2pdu b/hakoniwa/third-party/hakoniwa-ros2pdu index fcd60632..f37a1d74 160000 --- a/hakoniwa/third-party/hakoniwa-ros2pdu +++ b/hakoniwa/third-party/hakoniwa-ros2pdu @@ -1 +1 @@ -Subproject commit fcd60632d2f206389312664751f1a9190f80e4d3 +Subproject commit f37a1d7488d55794ef91d16d032cc1c42bd53b45 From 5ae931ca3699da8a2f4d10020f9f654486beccd2 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Tue, 12 Nov 2024 15:20:48 +0900 Subject: [PATCH 31/50] add status --- hakoniwa/config/rc-battery/drone_config_0.json | 2 ++ .../assets/drone/physics/battery/battery_dynamics.hpp | 10 +++++++++- hakoniwa/src/config/drone_config.hpp | 2 ++ hakoniwa/src/config/drone_config_types.hpp | 9 +++++++++ 4 files changed, 22 insertions(+), 1 deletion(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 1b48918a..eba66d47 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -70,6 +70,8 @@ "RatedCapacity": 3.0, "MaximumDischargeCurrent": 0.0, + "VoltageLevelGreen": 11.1, + "VoltageLevelYellow": 9.5, "NominalVoltage": 14.8, "RatedVoltage": 3.0, "ActualCapacity": 4.0 diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 9d60284e..aad72f1b 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -66,7 +66,15 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs status.full_voltage = this->params.NominalVoltage; status.curr_voltage = this->current_charge_voltage; status.cycles = 0; - status.status = 0; + if (status.curr_voltage > this->params.VoltageLevelGreen) { + status.status = 0; //green + } + else if (status.curr_voltage > this->params.VoltageLevelYellow) { + status.status = 1; //yellow + } + else { + status.status = 2; //red + } return status; } const std::vector log_head() override diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 3ae5ca6d..cd954790 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -250,6 +250,8 @@ class DroneConfig { params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); params.MaximumDischargeCurrent = configJson["components"]["battery"]["MaximumDischargeCurrent"].get(); params.RatedCapacity = configJson["components"]["battery"]["RatedCapacity"].get(); + params.VoltageLevelGreen = configJson["components"]["battery"]["VoltageLevelGreen"].get(); + params.VoltageLevelYellow = configJson["components"]["battery"]["VoltageLevelYellow"].get(); std::cout << "Battery model is enabled." << std::endl; return params; } diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index fb4a2fa3..a6bfbb88 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -49,6 +49,15 @@ namespace hako::assets::drone { * メーカーが保証する容量 */ double RatedCapacity; + + /* + * 電圧 Green レベルのミニマム値： [V] + */ + double VoltageLevelGreen; + /* + * 電圧 Yellow レベルのミニマム値： [V] + */ + double VoltageLevelYellow; }; } From 512118adefd48ceb1601bf8449956c820a4abd3d Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Tue, 12 Nov 2024 15:43:16 +0900 Subject: [PATCH 32/50] add disturbance --- hakoniwa/config/rc-battery/drone_config_0.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index eba66d47..9222e58f 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -36,7 +36,8 @@ "components": { "droneDynamics": { "physicsEquation": "BodyFrame", - "collision_detection": false, + "collision_detection": true, + "enable_disturbance": true, "manual_control": false, "airFrictionCoefficient": [ 0.05, From b48f66b7b8d3ed373cb351f079c8518fd6c4f873 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 13 Nov 2024 11:45:58 +0900 Subject: [PATCH 33/50] renamed classes --- .../config/rc-battery/drone_config_0.json | 11 +++++++---- .../drone/include/ibattery_dynamics.hpp | 18 ++++++++++++++---- .../drone/include/icurrent_dynamics.hpp | 16 ++++++++++++++++ .../drone/include/idischarge_dynamics.hpp | 18 ------------------ .../physics/battery/battery_dynamics.hpp | 11 +++++++---- .../drone/physics/rotor/rotor_dynamics.hpp | 19 ++++++------------- 6 files changed, 50 insertions(+), 43 deletions(-) create mode 100644 hakoniwa/src/assets/drone/include/icurrent_dynamics.hpp delete mode 100644 hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 9222e58f..9e5254cd 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -37,7 +37,7 @@ "droneDynamics": { "physicsEquation": "BodyFrame", "collision_detection": true, - "enable_disturbance": true, + "enable_disturbance": false, "manual_control": false, "airFrictionCoefficient": [ 0.05, @@ -67,15 +67,18 @@ }, "battery": { "vendor": "None", - "NominalCapacity": 3.0, "RatedCapacity": 3.0, "MaximumDischargeCurrent": 0.0, + "ActualCapacity": 4.0, + "RatedVoltage": 3.0, + "TerminalVoltage": 3.0, "VoltageLevelGreen": 11.1, "VoltageLevelYellow": 9.5, "NominalVoltage": 14.8, - "RatedVoltage": 3.0, - "ActualCapacity": 4.0 + "comment": "TODO", + "EODVoltage": 3.0, + "NominalCapacity": 3.0 }, "rotor": { "vendor": "BatteryModel", diff --git a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp index 8fdf2f0c..5d5a7964 100644 --- a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp @@ -1,8 +1,9 @@ #ifndef _IBATTERY_DYNAMICS_HPP_ #define _IBATTERY_DYNAMICS_HPP_ -#include "idischarge_dynamics.hpp" +#include "icurrent_dynamics.hpp" #include "config/drone_config_types.hpp" +#include "utils/hako_utils.hpp" #include #include @@ -15,12 +16,17 @@ typedef struct { uint32_t cycles; } BatteryStatusType; +typedef struct { + ICurrentDynamics *device; + double discharge_capacity_sec; +} DischargeDynamicsType; + class IBatteryDynamics { protected: void *vendor_model; void *context; BatteryModelParameters params; - std::vector devices; + std::vector devices; public: virtual ~IBatteryDynamics() {} virtual void set_vendor(void *vendor, void *context) @@ -28,9 +34,13 @@ class IBatteryDynamics { this->vendor_model = vendor; this->context = context; } - virtual void add_device(IDischargeDynamics& device) + virtual void add_device(ICurrentDynamics& device) { - devices.push_back(&device); + DischargeDynamicsType* entry = new DischargeDynamicsType(); + HAKO_ASSERT(entry != nullptr); + entry->device = &device; + entry->discharge_capacity_sec = 0; + devices.push_back(entry); } //calculate battery remained value virtual double get_vbat() = 0; diff --git a/hakoniwa/src/assets/drone/include/icurrent_dynamics.hpp b/hakoniwa/src/assets/drone/include/icurrent_dynamics.hpp new file mode 100644 index 00000000..c7e9f36e --- /dev/null +++ b/hakoniwa/src/assets/drone/include/icurrent_dynamics.hpp @@ -0,0 +1,16 @@ +#ifndef _ICURRENT_DYNAMICS_HPP_ +#define _ICURRENT_DYNAMICS_HPP_ + + +namespace hako::assets::drone { + +class ICurrentDynamics { +public: + virtual ~ICurrentDynamics() {}; + + // return value Unit: [A] + virtual double get_current() = 0; +}; +} + +#endif /* _ICURRENT_DYNAMICS_HPP_ */ diff --git a/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp b/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp deleted file mode 100644 index 238d6159..00000000 --- a/hakoniwa/src/assets/drone/include/idischarge_dynamics.hpp +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef _IDISCHARGE_DYNAMICS_HPP_ -#define _IDISCHARGE_DYNAMICS_HPP_ - - -namespace hako::assets::drone { - -class IDischargeDynamics { -public: - virtual ~IDischargeDynamics() {}; - - // return value Unit: [As] - virtual double get_discharged_capacity() = 0; - // return value Unit: [A] - virtual double get_current() = 0; -}; -} - -#endif /* _IDISCHARGE_DYNAMICS_HPP_ */ diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index aad72f1b..0cb0cc7a 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -3,7 +3,7 @@ #include "drone_primitive_types.hpp" #include "ibattery_dynamics.hpp" -#include "idischarge_dynamics.hpp" +#include "icurrent_dynamics.hpp" #include "utils/icsv_log.hpp" #include @@ -19,6 +19,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double get_current_charge_voltage(double discharged_capacity) { + //TODO 最終電圧になったら、フラットにする if (discharged_capacity > params.ActualCapacity) { discharged_capacity = params.ActualCapacity; } @@ -30,6 +31,8 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double slope = (V_initial - V_final) / (MaxCapacity); double battery_voltage = V_initial - (slope * discharged_capacity); + // Yellowラインまでは同じで、そこから急降下する + // グラフをCSVで読み込ませる return battery_voltage; } @@ -48,8 +51,8 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double discharge_capacity_sec = 0; this->discharge_current = 0; for (auto* entry : devices) { - discharge_capacity_sec += entry->get_discharged_capacity(); - this->discharge_current += entry->get_current(); + entry->discharge_capacity_sec += (entry->device->get_current() * this->delta_time_sec); + this->discharge_current += entry->device->get_current(); } // Convert total_discharge_value from As (Ampere-seconds) to Ah (Ampere-hours) // 1 A・s = 1 / 3600 Ah @@ -79,7 +82,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs } const std::vector log_head() override { - return { "timestamp", "DischargeCurrent", "CurrentVoltage", "DischargeCapacity" }; + return { "timestamp", "DischargeCurrent", "Voltage", "DischargeCapacity" }; } const std::vector log_data() override diff --git a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp index a508d2e5..74706bba 100644 --- a/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/rotor/rotor_dynamics.hpp @@ -3,7 +3,7 @@ #include "drone_primitive_types.hpp" #include "irotor_dynamics.hpp" -#include "idischarge_dynamics.hpp" +#include "icurrent_dynamics.hpp" #include "utils/icsv_log.hpp" #include "rotor_physics.hpp" #include @@ -11,7 +11,7 @@ namespace hako::assets::drone { -class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::assets::drone::IDischargeDynamics, public ICsvLog { +class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::assets::drone::ICurrentDynamics, public ICsvLog { private: double param_rad_per_sec_max = 6000.0; double param_tr = 1.0; @@ -22,11 +22,10 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a DroneRotorSpeedType next_speed; double delta_time_sec; double total_time_sec; - double discharged_capacity; /* [As]*/ double current; /* [A] */ double duty; - void run_discharge(double vbat, double omega, double duty_rate) + void run_current(double vbat, double omega, double duty_rate) { /* current in [A] */ this->current = drone_physics::rotor_current( @@ -36,7 +35,6 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a omega, /* angular velocity in [rad/sec] */ duty_rate /* 0.0-1.0 (ratio of PWM) */ ); - this->discharged_capacity += this->current * this->delta_time_sec; } public: virtual ~RotorDynamics() {} @@ -45,7 +43,6 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a this->delta_time_sec = dt; this->total_time_sec = 0; this->speed.data = 0; - this->discharged_capacity = 0; this->current = 0; } void set_battery_dynamics_constants(const RotorBatteryModelConstants &c) override @@ -100,7 +97,7 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a void run(double control, double vbat) override { this->duty = control; - this->run_discharge(vbat, this->speed.data, control); + this->run_current(vbat, this->speed.data, control); this->next_speed.data = ( drone_physics::rotor_omega_acceleration(vbat, constants.R, constants.Cq, constants.J, constants.K, constants.D, speed.data, control) ) * this->delta_time_sec @@ -116,22 +113,18 @@ class RotorDynamics : public hako::assets::drone::IRotorDynamics, public hako::a this->speed.data = this->next_speed.data; this->total_time_sec += this->delta_time_sec; } - double get_discharged_capacity() override - { - return this->discharged_capacity; - } double get_current() override { return this->current; } const std::vector log_head() override { - return { "timestamp", "Duty", "RadPerSec", "DischargedValue", "Current" }; + return { "timestamp", "Duty", "RadPerSec", "Current" }; } const std::vector log_data() override { DroneRotorSpeedType v = get_rotor_speed(); - return {std::to_string(CsvLogger::get_time_usec()), std::to_string(this->duty), std::to_string(v.data), std::to_string(this->discharged_capacity), std::to_string(this->current)}; + return {std::to_string(CsvLogger::get_time_usec()), std::to_string(this->duty), std::to_string(v.data), std::to_string(this->current)}; } }; From 2e0dcbe4b59315d5777798bd3135dc6d0cbe7fb4 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 13 Nov 2024 11:57:23 +0900 Subject: [PATCH 34/50] renamed parameters --- .../config/rc-battery/drone_config_0.json | 11 ++-------- .../physics/battery/battery_dynamics.hpp | 11 +++++----- hakoniwa/src/config/drone_config.hpp | 9 +++----- hakoniwa/src/config/drone_config_types.hpp | 21 ++----------------- 4 files changed, 13 insertions(+), 39 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 9e5254cd..b3720b23 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -67,18 +67,11 @@ }, "battery": { "vendor": "None", - "RatedCapacity": 3.0, - "MaximumDischargeCurrent": 0.0, - "ActualCapacity": 4.0, - "RatedVoltage": 3.0, - "TerminalVoltage": 3.0, - "VoltageLevelGreen": 11.1, "VoltageLevelYellow": 9.5, "NominalVoltage": 14.8, - "comment": "TODO", - "EODVoltage": 3.0, - "NominalCapacity": 3.0 + "NominalCapacity": 4.0, + "EODVoltage": 3.0 }, "rotor": { "vendor": "BatteryModel", diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 0cb0cc7a..3cf9c352 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -20,12 +20,12 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double get_current_charge_voltage(double discharged_capacity) { //TODO 最終電圧になったら、フラットにする - if (discharged_capacity > params.ActualCapacity) { - discharged_capacity = params.ActualCapacity; + if (discharged_capacity > params.NominalCapacity) { + discharged_capacity = params.NominalCapacity; } - double V_initial = params.NominalVoltage; // 初期電圧（公称電圧） [V] - double V_final = params.RatedVoltage; // 放電終了電圧（定格電圧） [V] - double MaxCapacity = params.ActualCapacity; // 実容量（実際に使用可能な容量） [Ah] + double V_initial = params.NominalVoltage; // 初期電圧 [V] + double V_final = params.EODVoltage; // 放電終了電圧 [V] + double MaxCapacity = params.NominalCapacity; // 実容量 [Ah] // 放電容量に基づく電圧低下の傾きを計算 double slope = (V_initial - V_final) / (MaxCapacity); @@ -53,6 +53,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs for (auto* entry : devices) { entry->discharge_capacity_sec += (entry->device->get_current() * this->delta_time_sec); this->discharge_current += entry->device->get_current(); + discharge_capacity_sec += entry->discharge_capacity_sec; } // Convert total_discharge_value from As (Ampere-seconds) to Ah (Ampere-hours) // 1 A・s = 1 / 3600 Ah diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index cd954790..69562b77 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -244,14 +244,11 @@ class DroneConfig { try { if (configJson["components"].contains("battery")) { params.vendor = configJson["components"]["battery"]["vendor"].get(); - params.NominalCapacity = configJson["components"]["battery"]["NominalCapacity"].get(); - params.ActualCapacity = configJson["components"]["battery"]["ActualCapacity"].get(); - params.RatedVoltage = configJson["components"]["battery"]["RatedVoltage"].get(); - params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); - params.MaximumDischargeCurrent = configJson["components"]["battery"]["MaximumDischargeCurrent"].get(); - params.RatedCapacity = configJson["components"]["battery"]["RatedCapacity"].get(); params.VoltageLevelGreen = configJson["components"]["battery"]["VoltageLevelGreen"].get(); params.VoltageLevelYellow = configJson["components"]["battery"]["VoltageLevelYellow"].get(); + params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); + params.NominalCapacity = configJson["components"]["battery"]["NominalCapacity"].get(); + params.EODVoltage = configJson["components"]["battery"]["EODVoltage"].get(); std::cout << "Battery model is enabled." << std::endl; return params; } diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index a6bfbb88..34ded4f3 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -25,31 +25,14 @@ namespace hako::assets::drone { */ double NominalCapacity; /* - * 実容量: [Ah] - * 実際に使用可能な電気容量 + * 終了電圧: [V] */ - double ActualCapacity; - /* - * 定格電圧: [V] - * バッテリーの公称電圧 - */ - double RatedVoltage; + double EODVoltage; /* * 公称電圧: [V] * バッテリーの標準的な動作電圧 */ double NominalVoltage; - /* - * 最大放電電流: [A] - * 安全に放電できる最大電流値 - */ - double MaximumDischargeCurrent; - /* - * 定格容量: [Ah] - * メーカーが保証する容量 - */ - double RatedCapacity; - /* * 電圧 Green レベルのミニマム値： [V] */ From f400e59d592563bd47bea9da76c0cb14212b4957 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 13 Nov 2024 12:02:36 +0900 Subject: [PATCH 35/50] add run method for battery --- hakoniwa/src/assets/drone/aircraft/aricraft.hpp | 1 + hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp | 3 ++- .../src/assets/drone/physics/battery/battery_dynamics.hpp | 6 ++++-- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/hakoniwa/src/assets/drone/aircraft/aricraft.hpp b/hakoniwa/src/assets/drone/aircraft/aricraft.hpp index 11952029..d710911b 100644 --- a/hakoniwa/src/assets/drone/aircraft/aricraft.hpp +++ b/hakoniwa/src/assets/drone/aircraft/aricraft.hpp @@ -32,6 +32,7 @@ class AirCraft : public hako::assets::drone::IAirCraft { { double vbat = 0.0; if (this->battery_dynamics != nullptr) { + this->battery_dynamics->run(); vbat = this->battery_dynamics->get_vbat(); } //actuators diff --git a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp index 5d5a7964..918a63cf 100644 --- a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp @@ -42,8 +42,9 @@ class IBatteryDynamics { entry->discharge_capacity_sec = 0; devices.push_back(entry); } - //calculate battery remained value virtual double get_vbat() = 0; + //calculate battery remained value + virtual void run() = 0; virtual BatteryStatusType get_status() = 0; virtual void set_params(const BatteryModelParameters &p) { diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 3cf9c352..7d39ad3b 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -46,7 +46,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs this->discharge_current = 0; this->discharge_capacity_hour = 0; } - double get_vbat() override + void run() override { double discharge_capacity_sec = 0; this->discharge_current = 0; @@ -60,8 +60,10 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs // Therefore: Ah = A・s * (1 / 3600) this->discharge_capacity_hour = discharge_capacity_sec /3600.0; // Unit conversion from As to Ah this->current_charge_voltage = this->get_current_charge_voltage(discharge_capacity_hour); - this->total_discharged_capacity_sec = discharge_capacity_sec; + } + double get_vbat() override + { return this->current_charge_voltage; } BatteryStatusType get_status() override From ee0de6dc782534a99793659194b1a3e5e6d65b28 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 13 Nov 2024 12:51:21 +0900 Subject: [PATCH 36/50] nonlinear voltage model is added --- .../config/rc-battery/drone_config_0.json | 5 ++-- .../physics/battery/battery_dynamics.hpp | 24 ++++++++++++------- hakoniwa/src/config/drone_config.hpp | 1 + hakoniwa/src/config/drone_config_types.hpp | 4 ++++ 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index b3720b23..d5e84bfa 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -68,9 +68,10 @@ "battery": { "vendor": "None", "VoltageLevelGreen": 11.1, - "VoltageLevelYellow": 9.5, + "VoltageLevelYellow": 9.5, + "CapacityLevelYellow": 2.5, "NominalVoltage": 14.8, - "NominalCapacity": 4.0, + "NominalCapacity": 3.0, "EODVoltage": 3.0 }, "rotor": { diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 7d39ad3b..8d92de0b 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -19,20 +19,25 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double get_current_charge_voltage(double discharged_capacity) { - //TODO 最終電圧になったら、フラットにする + // 放電容量は増え続けるため、最大容量を超えたら、 + // 最大容量にすることで、電圧レベルを固定値にする if (discharged_capacity > params.NominalCapacity) { discharged_capacity = params.NominalCapacity; } - double V_initial = params.NominalVoltage; // 初期電圧 [V] - double V_final = params.EODVoltage; // 放電終了電圧 [V] - double MaxCapacity = params.NominalCapacity; // 実容量 [Ah] + double slope = 0; + double battery_voltage = 0; // 放電容量に基づく電圧低下の傾きを計算 - double slope = (V_initial - V_final) / (MaxCapacity); - double battery_voltage = V_initial - (slope * discharged_capacity); - - // Yellowラインまでは同じで、そこから急降下する - // グラフをCSVで読み込ませる + if (discharged_capacity < this->params.CapacityLevelYellow) { + // 放電容量がyellowレベルを下回っていない場合、電圧を緩やかに低下させる + slope = (params.NominalVoltage - params.VoltageLevelGreen) / this->params.CapacityLevelYellow; + battery_voltage = params.NominalVoltage - (slope * discharged_capacity); + } + else { + // 放電容量がyellowレベルを下回ったら、電圧を急降下させる + slope = (params.VoltageLevelGreen - params.EODVoltage) / (params.NominalCapacity - this->params.CapacityLevelYellow); + battery_voltage = params.VoltageLevelGreen - (slope * (discharged_capacity - this->params.CapacityLevelYellow)); + } return battery_voltage; } @@ -46,6 +51,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs this->discharge_current = 0; this->discharge_capacity_hour = 0; } + // TODO グラフをCSVで読み込ませる void run() override { double discharge_capacity_sec = 0; diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 69562b77..e240fe2c 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -249,6 +249,7 @@ class DroneConfig { params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); params.NominalCapacity = configJson["components"]["battery"]["NominalCapacity"].get(); params.EODVoltage = configJson["components"]["battery"]["EODVoltage"].get(); + params.CapacityLevelYellow = configJson["components"]["battery"]["CapacityLevelYellow"].get(); std::cout << "Battery model is enabled." << std::endl; return params; } diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index 34ded4f3..3d4de851 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -41,6 +41,10 @@ namespace hako::assets::drone { * 電圧 Yellow レベルのミニマム値： [V] */ double VoltageLevelYellow; + /* + * 容量 Yellow レベルのミニマム値： [Ah] + */ + double CapacityLevelYellow; }; } From 67f12131eae7ee252fee103dc58527af0dda03f0 Mon Sep 17 00:00:00 2001 From: kenjihiranabe Date: Wed, 13 Nov 2024 15:59:54 +0900 Subject: [PATCH 37/50] naming change A->Ct, B->Cq --- drone_physics/rotor_physics.cpp | 46 ++++++++++++++++----------------- drone_physics/rotor_physics.hpp | 24 +++++++---------- 2 files changed, 33 insertions(+), 37 deletions(-) diff --git a/drone_physics/rotor_physics.cpp b/drone_physics/rotor_physics.cpp index fca78f49..7305ef73 100644 --- a/drone_physics/rotor_physics.cpp +++ b/drone_physics/rotor_physics.cpp @@ -97,20 +97,21 @@ double rotor_current( /* current in [A] */ /* thrust from omega in radian/sec eq.(2.50)*/ double rotor_thrust( /* thrust in [N] */ - double A, /* the A parameter in Trust = A*(Omega)^2 */ + double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2]*/ double omega /* in [rad/s] */ ) { /** * Nonami's book (2.50) - * T = A * (Omega)^2 - * A is referred to as Ct in Kohei's doc. + * T = Ct * (Omega)^2 + * The name 'Ct' is from Kohei's doc and referred to as 'A' in Nonami's eq.(2.50) + * Kohei's doc https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p2 */ - return A * (omega * omega); + return Ct * (omega * omega); } /* this makes z-axis rotation eq.(2.56) */ double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/ - double B, /* torque coefficient (referred to as Cq in Kohei's doc) in [N m s^2/rad^2] */ + double Cq, /* torque coefficient (referred to as B in nonami's book in [N m s^2/rad^2] */ double Jr, /* torque coefficient for 2nd order rotation */ double omega, /* in [rad/s] */ double omega_acceleratoin, /* in [rad/s^2] */ @@ -118,22 +119,23 @@ double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/ { /** * See Nonami's book eq.(2.56) - * Ta = B * (Omega)^2 + Jr * (d(Omega)/dt) - * B is referred to as Cq in Kohei's doc. + * Ta = Cq * (Omega)^2 + Jr * (d(Omega)/dt) + * The name 'Cq' is from Kohei's doc and referred to as 'B' in Nonami's eq.(2.56) + * Kohei's doc https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p2 */ - return ccw * ( B * omega * omega + Jr * omega_acceleratoin ); + return ccw * ( Cq * omega * omega + Jr * omega_acceleratoin ); } /* the sum of the n trust from the rotors eq.(2.61) */ double body_thrust( /* thrust in [N] */ - double A, /* parameter A in Trust = A*(Omega)^2 in each motor */ + double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2] */ unsigned n, /* number of rotors */ double omega[] /* in radian/sec */ ) { double thrust = 0; for (unsigned i = 0; i < n; i++) { - thrust += rotor_thrust(A, omega[i]); + thrust += rotor_thrust(Ct, omega[i]); } return thrust; } @@ -141,8 +143,8 @@ double body_thrust( /* thrust in [N] */ /* the sum of the n torques from the rotors including anti-torque */ /* eq.(2.60)-(2.62)*/ TorqueType body_torque( /* torque in [Nm] */ - double A, /* parameter A in Trust = A*(Omega)^2 */ - double B, /* anti-torque parameter B in Ta = B*(Omega)^2 + Jr* (d(Omega)/dt) */ + double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2] */ + double Cq, /* torque coefficient (referred to as B in nonami's book in [N m s^2/rad^2] */ double Jr, unsigned n, /* number of rotors */ VectorType position[], /* position of each rotor in [m] */ @@ -161,12 +163,12 @@ TorqueType body_torque( /* torque in [Nm] */ */ // (1)thrust(calculated always +) is upper direction. change to alight z-axis. // then the torque is (position vector) x (thrust vector). - const VectorType thrust_vector = { 0, 0, -rotor_thrust(A, omega[i]) }; + const VectorType thrust_vector = { 0, 0, -rotor_thrust(Ct, omega[i]) }; auto thrust_torque = cross(position[i], thrust_vector); // (2) anti-torque around z-axis = yaw. const auto anti_torque = - rotor_anti_torque(B, Jr, omega[i], omega_acceleration[i], ccw[i]); + rotor_anti_torque(Cq, Jr, omega[i], omega_acceleration[i], ccw[i]); total_torque += thrust_torque; total_torque.z += anti_torque; @@ -179,17 +181,15 @@ TorqueType body_torque( /* torque in [Nm] */ * used in jMAVsim implemntation. * Used in comparing with the non-linear(our) model. */ -double rotor_thrust_linear( - double A2, /* the A parameter in Trust = A*(Omega) */ - double omega /* in radian/sec */ ) +double rotor_thrust_linear(double Ct2, double omega) { - return A2 * omega; + return Ct2 * omega; } double rotor_anti_torque_linear(double B2, double omega, double ccw) { return ccw * ( B2 * omega ); } -TorqueType body_torque_linear(double A2, double B2, unsigned n, +TorqueType body_torque_linear(double Ct2, double Cq2, unsigned n, VectorType position[], double ccw[], double omega[]) { TorqueType total_torque = {0, 0, 0}; @@ -203,23 +203,23 @@ TorqueType body_torque_linear(double A2, double B2, unsigned n, */ // (1)thrust(calculated always +) is upper direction. change to alight z-axis. // then the torque is (position vector) x (thrust vector). - const VectorType thrust_vector = { 0, 0, -rotor_thrust_linear(A2, omega[i]) }; + const VectorType thrust_vector = { 0, 0, -rotor_thrust_linear(Ct2, omega[i]) }; auto thrust_torque = cross(position[i], thrust_vector); // (2) anti-torque around z-axis = yaw. const auto anti_torque = - rotor_anti_torque_linear(B2, omega[i], ccw[i]); + rotor_anti_torque_linear(Cq2, omega[i], ccw[i]); total_torque += thrust_torque; total_torque.z += anti_torque; } return total_torque; } -double body_thrust_linear(double A2, unsigned n, double omega[]) +double body_thrust_linear(double Ct2, unsigned n, double omega[]) { double thrust = 0; for (unsigned i = 0; i < n; i++) { - thrust += rotor_thrust_linear(A2, omega[i]); + thrust += rotor_thrust_linear(Ct2, omega[i]); } return thrust; } diff --git a/drone_physics/rotor_physics.hpp b/drone_physics/rotor_physics.hpp index 604cda4f..6f331914 100644 --- a/drone_physics/rotor_physics.hpp +++ b/drone_physics/rotor_physics.hpp @@ -38,12 +38,12 @@ double rotor_current( /* current in [A] */ /* thrust from omega in radian/sec eq.(2.50)*/ double rotor_thrust( /* thrust in [N] */ - double A, /* parameter A in Trust = A*(Omega)^2 */ + double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2]*/ double omega /* in [rad/s] */ ); /* this makes z-axis rotation eq.(2.56) */ double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/ - double B, /* torque coefficient (referred to as Cq in Kohei's doc) in [N m s^2/rad^2] */ + double Cq, /* torque coefficient (referred to as B in nonami's book in [N m s^2/rad^2] */ double Jr, /* torque coefficient for 2nd order rotation */ double omega, /* in [rad/s] */ double omega_acceleratoin, /* in [rad/s^2] */ @@ -56,16 +56,16 @@ double rotor_anti_torque( /* anti torque(z-axis) in [Nm]*/ /* the sum of the n trust from the rotors eq.(2.61) */ double body_thrust( /* thrust in [N] */ - double A, /* parameter A in Trust = A*(Omega)^2 in each motor */ + double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2] */ unsigned n, /* number of rotors */ double omega[] /* in radian/sec */ ); /* the sum of the n torques from the rotors including anti-torque */ /* eq.(2.60)-(2.62)*/ TorqueType body_torque( /* torque in [Nm] */ - double A, /* parameter A in Trust = A*(Omega)^2 */ - double B, /* anti-torque parameter B in Ta = B*(Omega)^2 + Jr* (d(Omega)/dt) */ - double Jr, + double Ct, /* thrust coeff, in Trust = Ct*(Omega)^2 (referred to as 'A' in Nonami's book ) in [N s^2/rad^2] */ + double Cq, /* torque coefficient (referred to as B in nonami's book in [N m s^2/rad^2] */ + double Jr, /* torque coefficient for 2nd order rotation */ unsigned n, /* number of rotors */ VectorType position[], /* position of each rotor in [m] */ double ccw[], /* 1 or -1 */ @@ -78,15 +78,11 @@ TorqueType body_torque( /* torque in [Nm] */ * used in jMAVsim implemntation. * Used in comparing with the non-linear(our) model. */ -double rotor_thrust_linear( - double A, /* the A parameter in Trust = A*(Omega) */ - double omega /* in radian/sec */ ); - -double rotor_anti_torque_linear(double B2, double omega, double ccw); -TorqueType body_torque_linear(double A2, double B2, unsigned n, +double rotor_thrust_linear(double Ct2, double omega); +double rotor_anti_torque_linear(double Cq2, double omega, double ccw); +TorqueType body_torque_linear(double Ct2, double Cq2, unsigned n, VectorType position[], double ccw[], double omega[]); - -double body_thrust_linear(double A2, unsigned n, double omega[]); +double body_thrust_linear(double Ct2, unsigned n, double omega[]); From 1cf9fff0dfc519250a269f7a93eb4741c5f1a767 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 13 Nov 2024 17:06:24 +0900 Subject: [PATCH 38/50] add battery model loader --- .../config/rc-battery/drone_config_0.json | 1 + .../drone/include/ibattery_dynamics.hpp | 5 +- .../physics/battery/battery_dynamics.hpp | 139 +++++++++++++++--- hakoniwa/src/config/drone_config.hpp | 5 +- hakoniwa/src/config/drone_config_types.hpp | 1 + 5 files changed, 128 insertions(+), 23 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index d5e84bfa..bf106101 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -67,6 +67,7 @@ }, "battery": { "vendor": "None", + "BatteryModelCsvFilePath": "./battery_model.csv", "VoltageLevelGreen": 11.1, "VoltageLevelYellow": 9.5, "CapacityLevelYellow": 2.5, diff --git a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp index 918a63cf..1915dc8a 100644 --- a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp @@ -46,10 +46,7 @@ class IBatteryDynamics { //calculate battery remained value virtual void run() = 0; virtual BatteryStatusType get_status() = 0; - virtual void set_params(const BatteryModelParameters &p) - { - this->params = p; - } + virtual void set_params(const BatteryModelParameters &p) = 0; }; } diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 8d92de0b..5ecb93ac 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -6,19 +6,108 @@ #include "icurrent_dynamics.hpp" #include "utils/icsv_log.hpp" #include +#include +#include +#include +#include +#include namespace hako::assets::drone { +struct DischargeData { + double capacity; // 放電容量 (Ah) + double voltage; // 電圧レベル (V) +}; + class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICsvLog { private: double current_charge_voltage; - double total_discharged_capacity_sec; + double accumulated_capacity_sec; double discharge_capacity_hour; double discharge_current; double delta_time_sec; - - double get_current_charge_voltage(double discharged_capacity) + bool is_battery_model_enabled; + std::vector battery_model_data; + + // CSVファイルを読み込み、データをメモリにロードする関数 + std::vector loadCSV(const std::string& filename) { + std::vector data; + std::ifstream file(filename); + std::string line; + + while (std::getline(file, line)) { + std::istringstream ss(line); + std::string capacityStr, voltageStr; + if (std::getline(ss, capacityStr, ',') && std::getline(ss, voltageStr, ',')) { + DischargeData record; + record.capacity = std::stod(capacityStr); + record.voltage = std::stod(voltageStr); + data.push_back(record); + } + } + + // 放電容量で昇順にソートしておく + std::sort(data.begin(), data.end(), [](const DischargeData& a, const DischargeData& b) { + return a.capacity < b.capacity; + }); + return data; + } + void run_discharged_capacity() + { + double discharge_capacity_sec = 0; + this->discharge_current = 0; + for (auto* entry : devices) { + entry->discharge_capacity_sec += (entry->device->get_current() * this->delta_time_sec); + this->discharge_current += entry->device->get_current(); + discharge_capacity_sec += entry->discharge_capacity_sec; + } + // Convert total_discharge_value from As (Ampere-seconds) to Ah (Ampere-hours) + // 1 A・s = 1 / 3600 Ah + // Therefore: Ah = A・s * (1 / 3600) + this->discharge_capacity_hour = discharge_capacity_sec /3600.0; // Unit conversion from As to Ah + this->accumulated_capacity_sec = discharge_capacity_sec; + } + // 2分探索で指定した容量に近い前後のデータを取得する関数（境界条件を考慮） + std::pair findSurroundingData(const std::vector& data, double targetCapacity) { + if (data.size() < 2) { + throw std::runtime_error("Insufficient data in battery model."); + } + auto it = std::lower_bound(data.begin(), data.end(), targetCapacity, [&](const DischargeData& a, double b) { + return a.capacity < b; + }); + // 境界条件の処理 + if (it == data.begin()) { + return { *it, *(it + 1) }; + } else if (it == data.end()) { + return { *(it - 2), *(it - 1) }; + } else { + return { *(it - 1), *it }; + } + } + + // 線形補間で電圧を計算する関数 + double interpolateVoltage(const DischargeData& lower, const DischargeData& upper, double targetCapacity) { + double slope = (upper.voltage - lower.voltage) / (upper.capacity - lower.capacity); + return lower.voltage + slope * (targetCapacity - lower.capacity); + } + void run_battery_model() + { + double discharged_capacity = this->discharge_capacity_hour; + // 放電容量は増え続けるため、最大容量を超えたら、 + // 最大容量にすることで、電圧レベルを固定値にする + if (discharged_capacity > params.NominalCapacity) { + discharged_capacity = params.NominalCapacity; + } + // 前後のデータ取得 + auto [lower, upper] = findSurroundingData(this->battery_model_data, discharged_capacity); + // 線形補間で電圧を計算 + double interpolatedVoltage = interpolateVoltage(lower, upper, discharged_capacity); + this->current_charge_voltage = interpolatedVoltage; + } + void run_linear_model() + { + double discharged_capacity = this->discharge_capacity_hour; // 放電容量は増え続けるため、最大容量を超えたら、 // 最大容量にすることで、電圧レベルを固定値にする if (discharged_capacity > params.NominalCapacity) { @@ -38,35 +127,49 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs slope = (params.VoltageLevelGreen - params.EODVoltage) / (params.NominalCapacity - this->params.CapacityLevelYellow); battery_voltage = params.VoltageLevelGreen - (slope * (discharged_capacity - this->params.CapacityLevelYellow)); } - return battery_voltage; + this->current_charge_voltage = battery_voltage; } - public: virtual ~BatteryDynamics() {} BatteryDynamics(double dt) { this->delta_time_sec = dt; this->current_charge_voltage = 0; - this->total_discharged_capacity_sec = 0; + this->accumulated_capacity_sec = 0; this->discharge_current = 0; this->discharge_capacity_hour = 0; + this->is_battery_model_enabled = false; } + void set_params(const BatteryModelParameters &p) override + { + this->params = p; + if (params.BatteryModelCsvFilePath.empty()) { + std::cout << "BatteryModelCsvFilePath is empty." << std::endl; + } + else { + std::cout << "BatteryModelCsvFilePath: " << params.BatteryModelCsvFilePath << std::endl; + if (std::filesystem::exists(params.BatteryModelCsvFilePath)) { + std::cout << "BatteryModelCsvFilePath exists." << std::endl; + this->battery_model_data = loadCSV(params.BatteryModelCsvFilePath); + this->is_battery_model_enabled = true; + } + else { + std::cout << "BatteryModelCsvFilePath does not exist." << std::endl; + } + } + } + + // TODO グラフをCSVで読み込ませる void run() override { - double discharge_capacity_sec = 0; - this->discharge_current = 0; - for (auto* entry : devices) { - entry->discharge_capacity_sec += (entry->device->get_current() * this->delta_time_sec); - this->discharge_current += entry->device->get_current(); - discharge_capacity_sec += entry->discharge_capacity_sec; + run_discharged_capacity(); + if (!this->is_battery_model_enabled) { + run_linear_model(); + } + else { + run_battery_model(); } - // Convert total_discharge_value from As (Ampere-seconds) to Ah (Ampere-hours) - // 1 A・s = 1 / 3600 Ah - // Therefore: Ah = A・s * (1 / 3600) - this->discharge_capacity_hour = discharge_capacity_sec /3600.0; // Unit conversion from As to Ah - this->current_charge_voltage = this->get_current_charge_voltage(discharge_capacity_hour); - this->total_discharged_capacity_sec = discharge_capacity_sec; } double get_vbat() override { diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index e240fe2c..d0a614b1 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -240,10 +240,13 @@ class DroneConfig { return configJson["components"]["rotor"]["Kr"].get(); } hako::assets::drone::BatteryModelParameters getComDroneDynamicsBattery() const { - hako::assets::drone::BatteryModelParameters params; + hako::assets::drone::BatteryModelParameters params = {}; try { if (configJson["components"].contains("battery")) { params.vendor = configJson["components"]["battery"]["vendor"].get(); + if (configJson["components"]["battery"].contains("BatteryModelCsvFilePath")) { + params.BatteryModelCsvFilePath = configJson["components"]["battery"]["BatteryModelCsvFilePath"].get(); + } params.VoltageLevelGreen = configJson["components"]["battery"]["VoltageLevelGreen"].get(); params.VoltageLevelYellow = configJson["components"]["battery"]["VoltageLevelYellow"].get(); params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index 3d4de851..fa8b15d3 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -19,6 +19,7 @@ namespace hako::assets::drone { }; struct BatteryModelParameters { std::string vendor; + std::string BatteryModelCsvFilePath; /* * 公称容量: [Ah] * バッテリーが供給できる理論上の電気量 From ea0da30e53e700e7b9f2eb575a19197d63b5d8ca Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Wed, 13 Nov 2024 17:33:35 +0900 Subject: [PATCH 39/50] refactored --- hakoniwa/config/rc-battery/drone_config_0.json | 2 +- .../drone/physics/battery/battery_dynamics.hpp | 14 +++++++++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index bf106101..5692f640 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -72,7 +72,7 @@ "VoltageLevelYellow": 9.5, "CapacityLevelYellow": 2.5, "NominalVoltage": 14.8, - "NominalCapacity": 3.0, + "NominalCapacity": 5.0, "EODVoltage": 3.0 }, "rotor": { diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 5ecb93ac..9d246f8e 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -36,6 +36,17 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs std::string line; while (std::getline(file, line)) { + // コメント行（#で始まる行）をスキップ + if (line.empty() || line[0] == '#') { + continue; + } + + // 行内の#以降の文字を削除 + auto comment_pos = line.find('#'); + if (comment_pos != std::string::npos) { + line = line.substr(0, comment_pos); + } + std::istringstream ss(line); std::string capacityStr, voltageStr; if (std::getline(ss, capacityStr, ',') && std::getline(ss, voltageStr, ',')) { @@ -50,8 +61,10 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs std::sort(data.begin(), data.end(), [](const DischargeData& a, const DischargeData& b) { return a.capacity < b.capacity; }); + return data; } + void run_discharged_capacity() { double discharge_capacity_sec = 0; @@ -160,7 +173,6 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs } - // TODO グラフをCSVで読み込ませる void run() override { run_discharged_capacity(); From 54f24d02177f7c06cd8978f4c93499fa894263dc Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Thu, 14 Nov 2024 07:16:06 +0900 Subject: [PATCH 40/50] add temp --- .../drone/include/ibattery_dynamics.hpp | 10 ++ .../physics/battery/battery_dynamics.hpp | 114 +++++++++++++++--- 2 files changed, 104 insertions(+), 20 deletions(-) diff --git a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp index 1915dc8a..6a01a3ac 100644 --- a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp @@ -9,6 +9,11 @@ namespace hako::assets::drone { +// バッテリーの放電特性を決める要因データ +struct BatteryModelFactor { + double temperature; // 温度 (℃) +}; + typedef struct { double full_voltage; double curr_voltage; @@ -26,6 +31,7 @@ class IBatteryDynamics { void *vendor_model; void *context; BatteryModelParameters params; + BatteryModelFactor current_factor = {}; std::vector devices; public: virtual ~IBatteryDynamics() {} @@ -34,6 +40,10 @@ class IBatteryDynamics { this->vendor_model = vendor; this->context = context; } + virtual void set_current_factor(BatteryModelFactor factor) + { + current_factor = factor; + } virtual void add_device(ICurrentDynamics& device) { DischargeDynamicsType* entry = new DischargeDynamicsType(); diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index 9d246f8e..d42684d0 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -11,6 +11,7 @@ #include #include #include +#include namespace hako::assets::drone { @@ -27,44 +28,111 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double discharge_current; double delta_time_sec; bool is_battery_model_enabled; - std::vector battery_model_data; + // バッテリーの放電特性を決める要因データ + // インデックス番号は、要因IDとして扱う + std::vector discharge_factors; + //要因IDと放電データのマップ + std::map> battery_model_map; - // CSVファイルを読み込み、データをメモリにロードする関数 - std::vector loadCSV(const std::string& filename) { - std::vector data; + + // 1段階目：要因データを読み込み、要因IDを確定する関数 + void loadDischargeFactors(const std::string& filename) { std::ifstream file(filename); std::string line; while (std::getline(file, line)) { - // コメント行（#で始まる行）をスキップ if (line.empty() || line[0] == '#') { continue; } - // 行内の#以降の文字を削除 - auto comment_pos = line.find('#'); - if (comment_pos != std::string::npos) { - line = line.substr(0, comment_pos); + std::istringstream ss(line); + std::string temperatureStr, capacityStr, voltageStr; + if (std::getline(ss, temperatureStr, ',')) { + // 要因データを取得 + double temperature = std::stod(temperatureStr); + + // 重複を避けるため、既に存在する温度かどうかを確認 + auto it = std::find_if(discharge_factors.begin(), discharge_factors.end(), + [temperature](const BatteryModelFactor& factor) { + return factor.temperature == temperature; + }); + + // まだ存在しない場合のみ追加 + if (it == discharge_factors.end()) { + BatteryModelFactor factor; + factor.temperature = temperature; + std::cout << "INFO: battery factor: temperature = " << factor.temperature << std::endl; + discharge_factors.push_back(factor); // 要因データを格納 + } + } + } + } + // 2段階目：要因データに基づいて、要因IDをマップに割り当てる + void loadDischargeData(const std::string& filename) { + std::ifstream file(filename); + std::string line; + + while (std::getline(file, line)) { + if (line.empty() || line[0] == '#') { + continue; } std::istringstream ss(line); - std::string capacityStr, voltageStr; - if (std::getline(ss, capacityStr, ',') && std::getline(ss, voltageStr, ',')) { + std::string temperatureStr, capacityStr, voltageStr; + + if (std::getline(ss, temperatureStr, ',') && + std::getline(ss, capacityStr, ',') && + std::getline(ss, voltageStr, ',')) { + + // 要因データから対応する要因IDを取得 + double temperature = std::stod(temperatureStr); + int factor_id = findFactorOrCreateID(temperature); + + // 放電データを生成 DischargeData record; record.capacity = std::stod(capacityStr); record.voltage = std::stod(voltageStr); - data.push_back(record); + + // 要因IDをキーにしてデータをマップに格納 + battery_model_map[factor_id].push_back(record); } } - // 放電容量で昇順にソートしておく - std::sort(data.begin(), data.end(), [](const DischargeData& a, const DischargeData& b) { - return a.capacity < b.capacity; - }); + // 各要因IDに対するデータを容量でソート + for (auto& [factor_id, data] : battery_model_map) { + std::sort(data.begin(), data.end(), [](const DischargeData& a, const DischargeData& b) { + return a.capacity < b.capacity; + }); + } + } - return data; + // 要因データから要因IDを取得する関数 + int findFactorOrCreateID(double temperature) { + for (uint i = 0; i < discharge_factors.size(); ++i) { + if (discharge_factors[i].temperature == temperature) { + return i; // 要因のインデックスをIDとして使用 + } + } + // 該当する要因がない場合、新しい要因を追加しIDを返す + BatteryModelFactor new_factor{temperature}; + discharge_factors.push_back(new_factor); + return discharge_factors.size() - 1; } + // current_factorの温度に最も近い要因IDを検索する関数 + int findClosestFactorID() { + int closest_id = 0; + double min_difference = std::abs(current_factor.temperature - discharge_factors[0].temperature); + for (uint i = 1; i < discharge_factors.size(); ++i) { + double difference = std::abs(current_factor.temperature - discharge_factors[i].temperature); + if (difference < min_difference) { + closest_id = i; + min_difference = difference; + } + } + //std::cout << "INFO: closest factor ID = " << closest_id << " temp: " << discharge_factors[closest_id].temperature << std::endl; + return closest_id; + } void run_discharged_capacity() { double discharge_capacity_sec = 0; @@ -81,8 +149,10 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs this->accumulated_capacity_sec = discharge_capacity_sec; } // 2分探索で指定した容量に近い前後のデータを取得する関数（境界条件を考慮） - std::pair findSurroundingData(const std::vector& data, double targetCapacity) + std::pair findSurroundingData(double targetCapacity) { + int closest_factor_id = findClosestFactorID(); + const auto& data = battery_model_map.at(closest_factor_id); if (data.size() < 2) { throw std::runtime_error("Insufficient data in battery model."); } @@ -113,7 +183,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs discharged_capacity = params.NominalCapacity; } // 前後のデータ取得 - auto [lower, upper] = findSurroundingData(this->battery_model_data, discharged_capacity); + auto [lower, upper] = findSurroundingData(discharged_capacity); // 線形補間で電圧を計算 double interpolatedVoltage = interpolateVoltage(lower, upper, discharged_capacity); this->current_charge_voltage = interpolatedVoltage; @@ -163,7 +233,11 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs std::cout << "BatteryModelCsvFilePath: " << params.BatteryModelCsvFilePath << std::endl; if (std::filesystem::exists(params.BatteryModelCsvFilePath)) { std::cout << "BatteryModelCsvFilePath exists." << std::endl; - this->battery_model_data = loadCSV(params.BatteryModelCsvFilePath); + // 1段階目：要因データの読み込み + loadDischargeFactors(params.BatteryModelCsvFilePath); + + // 2段階目：放電データの読み込みとマップ化 + loadDischargeData(params.BatteryModelCsvFilePath); this->is_battery_model_enabled = true; } else { From 711046e9f4bfd8f91548c3bb5ae3707430f33c3e Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Thu, 14 Nov 2024 07:21:46 +0900 Subject: [PATCH 41/50] add temperature --- drone_api/assets/hako_env_event.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/drone_api/assets/hako_env_event.py b/drone_api/assets/hako_env_event.py index 28c34e38..ffcd3155 100644 --- a/drone_api/assets/hako_env_event.py +++ b/drone_api/assets/hako_env_event.py @@ -51,6 +51,10 @@ def on_manual_timing_control(context): pdu_data['d_wind']['value']['x'] = wind[0] pdu_data['d_wind']['value']['y'] = wind[1] pdu_data['d_wind']['value']['z'] = wind[2] + temperature = property_info.get_temperature() + if temperature is not None: + print(f"{hakopy.simulation_time()} temperature ==> {property_info.get_temperature()}") + pdu_data['d_temp']['value'] = temperature else: print(f"{hakopy.simulation_time()}: No wind") From 785c2afffc7f30dd375a5b0a8e66248247c3c86a Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Thu, 14 Nov 2024 08:20:25 +0900 Subject: [PATCH 42/50] add temp config --- drone_api/assets/config/area.json | 16 +++++++++++++++- drone_api/assets/config/area_property.json | 4 ++-- drone_control/config/param-api-mixer.txt | 8 ++++---- hakoniwa/config/rc-battery/drone_config_0.json | 2 +- hakoniwa/src/assets/drone/aircraft/aricraft.hpp | 2 ++ .../assets/drone/include/ibattery_dynamics.hpp | 1 + .../drone/physics/battery/battery_dynamics.hpp | 1 + hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp | 1 + hakoniwa/third-party/hakoniwa-ros2pdu | 2 +- 9 files changed, 28 insertions(+), 9 deletions(-) diff --git a/drone_api/assets/config/area.json b/drone_api/assets/config/area.json index f73ef4c6..e8ce8875 100644 --- a/drone_api/assets/config/area.json +++ b/drone_api/assets/config/area.json @@ -2,9 +2,23 @@ "space_areas": [ { "area_id": "area_1", + "bounds": { + "min": { "x": 1, "y": -5, "z": 0 }, + "max": { "x": 5, "y": 5, "z": 20 } + } + }, + { + "area_id": "area_2", "bounds": { "min": { "x": 1, "y": -10, "z": 0 }, - "max": { "x": 10, "y": 10, "z": 20 } + "max": { "x": 5, "y": -5, "z": 20 } + } + }, + { + "area_id": "area_3", + "bounds": { + "min": { "x": 1, "y": 5, "z": 0 }, + "max": { "x": 5, "y": 10, "z": 20 } } } ] diff --git a/drone_api/assets/config/area_property.json b/drone_api/assets/config/area_property.json index 01c68105..186c9855 100644 --- a/drone_api/assets/config/area_property.json +++ b/drone_api/assets/config/area_property.json @@ -11,14 +11,14 @@ "area_id": "area_2", "properties": { "wind_velocity": [2.5, 1.0, 0.0], - "temperature": 19.0 + "temperature": -15.0 } }, { "area_id": "area_3", "properties": { "wind_velocity": [0.0, 0.0, 0.0], - "temperature": 25.0 + "temperature": 55.0 } }, { diff --git a/drone_control/config/param-api-mixer.txt b/drone_control/config/param-api-mixer.txt index 546177df..7a7a9223 100644 --- a/drone_control/config/param-api-mixer.txt +++ b/drone_control/config/param-api-mixer.txt @@ -7,7 +7,7 @@ GRAVITY 9.81 PID_ALT_CONTROL_CYCLE 0.0 PID_ALT_MAX_POWER 9.81 PID_ALT_THROTTLE_GAIN 1.0 -PID_ALT_MAX_SPD 5.0 +PID_ALT_MAX_SPD 10.0 ## 高度制御のPIDパラメータ PID_ALT_Kp 5.0 @@ -20,7 +20,7 @@ PID_ALT_SPD_Kd 1.0 # 水平制御 POS_CONTROL_CYCLE 0.0 SPD_CONTROL_CYCLE 0.0 -PID_POS_MAX_SPD 10.0 +PID_POS_MAX_SPD 20.0 ## 水平位置制御のPIDパラメータ PID_POS_X_Kp 5.0 @@ -42,8 +42,8 @@ PID_POS_VY_Kd 1.0 HEAD_CONTROL_CYCLE 0.0 ANGULAR_CONTROL_CYCLE 0.0 ANGULAR_RATE_CONTROL_CYCLE 0.0 -PID_POS_MAX_ROLL 10.0 -PID_POS_MAX_PITCH 10.0 +PID_POS_MAX_ROLL 20.0 +PID_POS_MAX_PITCH 20.0 PID_ROLL_RPM_MAX 20.0 PID_PITCH_RPM_MAX 20.0 PID_ROLL_TORQUE_MAX 10.0 diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 5692f640..5cdc52d0 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -37,7 +37,7 @@ "droneDynamics": { "physicsEquation": "BodyFrame", "collision_detection": true, - "enable_disturbance": false, + "enable_disturbance": true, "manual_control": false, "airFrictionCoefficient": [ 0.05, diff --git a/hakoniwa/src/assets/drone/aircraft/aricraft.hpp b/hakoniwa/src/assets/drone/aircraft/aricraft.hpp index d710911b..ff1fd5f5 100644 --- a/hakoniwa/src/assets/drone/aircraft/aricraft.hpp +++ b/hakoniwa/src/assets/drone/aircraft/aricraft.hpp @@ -32,6 +32,8 @@ class AirCraft : public hako::assets::drone::IAirCraft { { double vbat = 0.0; if (this->battery_dynamics != nullptr) { + BatteryModelFactor factor = { input.disturbance.values.d_temp.value }; //温度 + this->battery_dynamics->set_current_factor(factor); this->battery_dynamics->run(); vbat = this->battery_dynamics->get_vbat(); } diff --git a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp index 6a01a3ac..abdf8d5c 100644 --- a/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/include/ibattery_dynamics.hpp @@ -17,6 +17,7 @@ struct BatteryModelFactor { typedef struct { double full_voltage; double curr_voltage; + double temperature; uint32_t status; uint32_t cycles; } BatteryStatusType; diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index d42684d0..abccd2e6 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -266,6 +266,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs BatteryStatusType status; status.full_voltage = this->params.NominalVoltage; status.curr_voltage = this->current_charge_voltage; + status.temperature = this->current_factor.temperature; status.cycles = 0; if (status.curr_voltage > this->params.VoltageLevelGreen) { status.status = 0; //green diff --git a/hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp b/hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp index 79a629b3..bb4a6d61 100644 --- a/hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp +++ b/hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp @@ -220,6 +220,7 @@ static inline bool do_io_write_battery_status(hako::assets::drone::IAirCraft *dr auto status = battery->get_status(); packet.full_voltage = status.full_voltage; packet.curr_voltage = status.curr_voltage; + packet.curr_temp = status.temperature; packet.cycles = status.cycles; packet.status = status.status; return do_io_write_data(drone, HAKO_AVATOR_CHANNEL_ID_BATTERY_STAT, packet); diff --git a/hakoniwa/third-party/hakoniwa-ros2pdu b/hakoniwa/third-party/hakoniwa-ros2pdu index f37a1d74..dcc9ae0d 160000 --- a/hakoniwa/third-party/hakoniwa-ros2pdu +++ b/hakoniwa/third-party/hakoniwa-ros2pdu @@ -1 +1 @@ -Subproject commit f37a1d7488d55794ef91d16d032cc1c42bd53b45 +Subproject commit dcc9ae0d0cbd1a37fd2c251d64ea2387e6d9bf98 From da1aefd2545c3c3ec4ccd15cc8d2bad64ec06d84 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Thu, 14 Nov 2024 16:15:55 +0900 Subject: [PATCH 43/50] upgrade battery capacity --- hakoniwa/config/rc-battery/drone_config_0.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 5cdc52d0..455c750e 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -37,7 +37,7 @@ "droneDynamics": { "physicsEquation": "BodyFrame", "collision_detection": true, - "enable_disturbance": true, + "enable_disturbance": false, "manual_control": false, "airFrictionCoefficient": [ 0.05, @@ -67,12 +67,12 @@ }, "battery": { "vendor": "None", - "BatteryModelCsvFilePath": "./battery_model.csv", + "BatteryModelCsvFilePath": "./tmp_battery_model.csv", "VoltageLevelGreen": 11.1, "VoltageLevelYellow": 9.5, "CapacityLevelYellow": 2.5, "NominalVoltage": 14.8, - "NominalCapacity": 5.0, + "NominalCapacity": 1000.0, "EODVoltage": 3.0 }, "rotor": { From 27ce36281d57da19072fb96aaea34fb004dffe8a Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Fri, 15 Nov 2024 05:45:05 +0900 Subject: [PATCH 44/50] readme is updated --- hakoniwa/README-ja.md | 8 +++----- hakoniwa/README.md | 8 +++----- hakoniwa/config/rc-battery/drone_config_0.json | 2 +- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/hakoniwa/README-ja.md b/hakoniwa/README-ja.md index 8894df79..823faf81 100644 --- a/hakoniwa/README-ja.md +++ b/hakoniwa/README-ja.md @@ -163,15 +163,13 @@ bash build.bash HAKONIWA_MATLAB_BUILD=true - **angle_degree**: 機体の初期角度。単位は度(`deg`)。 - **rotor**: ローターの設定。 - **vendor**: ベンダ名を指定します。現状は`None`を設定して下さい。 + - **Ct**: 推力係数 - **Tr**: ローターの応答時間。単位は秒(`s`)。 - - **Kr**: ローターの力定数。 - - **rpmMax**: ローターの最大回転数。単位は回転/分(`rpm`)。 - **thruster**: スラスターの設定。 - **vendor**: ベンダ名を指定します。現状は`None`を設定して下さい。 - **rotorPositions**: ローターの位置と回転方向。単位はメートル(`m`)。rotationDirectionはローターの回転方向(CW:-1.0, CCW: 1.0) - - **HoveringRpm**: ホバリング時の回転数。単位は回転/分(`rpm`)。 - - **parameterB**: スラスターのパラメータB。 - - **parameterJr**: スラスターの慣性モーメントパラメータ。 + - **Cq**: トルク係数。 + - **Jr**: スラスターの慣性モーメントパラメータ。 - **sensors**: 各種センサーの設定。 - **sampleCount**: サンプル数 - **noise**:ノイズレベル(標準偏差)。ノイズ未設定の場合は0。 diff --git a/hakoniwa/README.md b/hakoniwa/README.md index e974034f..efc77f6b 100644 --- a/hakoniwa/README.md +++ b/hakoniwa/README.md @@ -154,15 +154,13 @@ The settings for each item are as follows: - **angle_degree**: The initial angle of the aircraft, in degrees (`deg`). - **rotor**: Rotor settings. - **vendor**: Specify the vendor name. Currently, set it to `None`. + - **Ct**: Thrust coefficient. - **Tr**: Rotor response time, in seconds (`s`). - - **Kr**: Rotor force constant. - - **rpmMax**: Maximum rotor speed, in revolutions per minute (`rpm`). - **thruster**: Thruster settings. - **vendor**: Specify the vendor name. Currently, set it to `None`. - **rotorPositions**: The positions and rotation directions of the rotors, in meters (`m`). rotationDirection is the direction of rotor rotation (CW:-1.0, CCW: 1.0) - - **HoveringRpm**: The rotation speed at hovering, in revolutions per minute (`rpm`). - - **parameterB**: Thruster parameter B. - - **parameterJr**: Thruster moment of inertia parameter. + - **Cq**: Torque coefficient. + - **Jr**: Thruster moment of inertia parameter. - **sensors**: Settings for various sensors. - **sampleCount**: The number of samples. - **noise**: Noise level (standard deviation). If noise is not set, it is 0. diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 455c750e..4a192b05 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -126,7 +126,7 @@ ], "HoveringRpm": 13827.38146, "parameterB": 3.0E-8, - "parameterJr": 0.0 + "parameterJr": 0.0 }, "sensors": { "acc": { From 6070df50d75a53e63ccdc497e0c88d017faf71bf Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Fri, 15 Nov 2024 07:33:44 +0900 Subject: [PATCH 45/50] bug fixed --- drone_api/libs/pdu_info.py | 4 +- drone_api/sample/camera.py | 2 +- drone_api/sample/custom.json | 154 +++++++++++++++++++++++++++++------ 3 files changed, 132 insertions(+), 28 deletions(-) diff --git a/drone_api/libs/pdu_info.py b/drone_api/libs/pdu_info.py index e77e14a0..4d7b346f 100644 --- a/drone_api/libs/pdu_info.py +++ b/drone_api/libs/pdu_info.py @@ -7,8 +7,8 @@ HAKO_AVATOR_CHANNEL_ID_CMD_MOVE = 6 HAKO_AVATOR_CHANNEL_ID_CMD_LAND = 7 HAKO_AVATOR_CHANNEL_ID_GAME_CTRL = 8 -HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA = 9 -HAKO_AVATOR_CHANNEL_ID_BATTERY_STAT= 10 +HAKO_AVATOR_CHANNEL_ID_BATTERY_STAT= 0 +HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA = 10 HAKO_AVATOR_CHANNEL_ID_CAMERA_DATA = 11 HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA_MOVE = 12 HAKO_AVATOR_CHANNEL_ID_CAMERA_INFO = 13 diff --git a/drone_api/sample/camera.py b/drone_api/sample/camera.py index 1dc3cb83..185f0284 100644 --- a/drone_api/sample/camera.py +++ b/drone_api/sample/camera.py @@ -41,7 +41,7 @@ def main(): print(f"Usage: {sys.argv[0]} ") return 1 - client = hakosim.MultirotorClient(sys.argv[1]) + client = hakosim.MultirotorClient(sys.argv[1], "DroneTransporter") client.confirmConnection() client.enableApiControl(True) client.armDisarm(True) diff --git a/drone_api/sample/custom.json b/drone_api/sample/custom.json index 00809253..b3e1f1d7 100644 --- a/drone_api/sample/custom.json +++ b/drone_api/sample/custom.json @@ -1,5 +1,43 @@ { "robots": [ + { + "name": "Baggage1", + "rpc_pdu_readers": [], + "rpc_pdu_writers": [], + "shm_pdu_readers": [], + "shm_pdu_writers": [ + { + "type": "geometry_msgs/Twist", + "org_name": "pos", + "name": "Baggage1_pos", + "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", + "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", + "channel_id": 0, + "pdu_size": 112, + "write_cycle": 1, + "method_type": "SHM" + } + ] + }, + { + "name": "Baggage2", + "rpc_pdu_readers": [], + "rpc_pdu_writers": [], + "shm_pdu_readers": [], + "shm_pdu_writers": [ + { + "type": "geometry_msgs/Twist", + "org_name": "pos", + "name": "Baggage2_pos", + "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", + "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", + "channel_id": 0, + "pdu_size": 112, + "write_cycle": 1, + "method_type": "SHM" + } + ] + }, { "name": "DroneTransporter", "rpc_pdu_readers": [], @@ -12,7 +50,7 @@ "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter", "channel_id": 0, - "pdu_size": 88, + "pdu_size": 112, "write_cycle": 1, "method_type": "SHM" }, @@ -23,7 +61,7 @@ "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter", "channel_id": 1, - "pdu_size": 48, + "pdu_size": 72, "write_cycle": 1, "method_type": "SHM" }, @@ -34,7 +72,18 @@ "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", "channel_id": 3, - "pdu_size": 56, + "pdu_size": 80, + "write_cycle": 1, + "method_type": "SHM" + }, + { + "type": "hako_msgs/Disturbance", + "org_name": "drone_disturbance", + "name": "DroneTransporter_drone_disturbance", + "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader", + "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter", + "channel_id": 4, + "pdu_size": 88, "write_cycle": 1, "method_type": "SHM" }, @@ -45,7 +94,7 @@ "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", "channel_id": 5, - "pdu_size": 32, + "pdu_size": 64, "write_cycle": 1, "method_type": "SHM" }, @@ -56,7 +105,7 @@ "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", "channel_id": 6, - "pdu_size": 48, + "pdu_size": 80, "write_cycle": 1, "method_type": "SHM" }, @@ -67,18 +116,29 @@ "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", "channel_id": 7, - "pdu_size": 32, + "pdu_size": 64, "write_cycle": 1, "method_type": "SHM" }, { - "type": "hako_msgs/HakoCmdMagnetHolder", - "org_name": "hako_cmd_magnet_holder", - "name": "DroneTransporter_hako_cmd_magnet_holder", + "type": "hako_msgs/GameControllerOperation", + "org_name": "hako_cmd_game", + "name": "DroneTransporter_hako_cmd_game", "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", "channel_id": 8, - "pdu_size": 16, + "pdu_size": 136, + "write_cycle": 1, + "method_type": "SHM" + }, + { + "type": "hako_msgs/HakoBatteryStatus", + "org_name": "hako_battery", + "name": "DroneTransporter_hako_battery", + "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader", + "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter", + "channel_id": 9, + "pdu_size": 56, "write_cycle": 1, "method_type": "SHM" }, @@ -89,7 +149,29 @@ "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", "channel_id": 10, - "pdu_size": 20, + "pdu_size": 44, + "write_cycle": 1, + "method_type": "SHM" + }, + { + "type": "hako_msgs/HakoCmdCameraMove", + "org_name": "hako_cmd_camera_move", + "name": "DroneTransporter_hako_cmd_camera_move", + "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", + "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", + "channel_id": 12, + "pdu_size": 64, + "write_cycle": 1, + "method_type": "SHM" + }, + { + "type": "hako_msgs/HakoCmdMagnetHolder", + "org_name": "hako_cmd_magnet_holder", + "name": "DroneTransporter_hako_cmd_magnet_holder", + "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", + "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", + "channel_id": 14, + "pdu_size": 40, "write_cycle": 1, "method_type": "SHM" } @@ -102,18 +184,29 @@ "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", "channel_id": 2, - "pdu_size": 280, + "pdu_size": 304, "write_cycle": 1, "method_type": "SHM" }, { - "type": "hako_msgs/Disturbance", - "org_name": "drone_disturbance", - "name": "DroneTransporter_drone_disturbance", + "type": "hako_msgs/HakoCameraData", + "org_name": "hako_camera_data", + "name": "DroneTransporter_hako_camera_data", "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 4, - "pdu_size": 8, + "channel_id": 11, + "pdu_size": 102968, + "write_cycle": 1, + "method_type": "SHM" + }, + { + "type": "hako_msgs/HakoCameraInfo", + "org_name": "hako_cmd_camera_info", + "name": "DroneTransporter_hako_cmd_camera_info", + "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", + "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", + "channel_id": 13, + "pdu_size": 56, "write_cycle": 1, "method_type": "SHM" }, @@ -123,20 +216,31 @@ "name": "DroneTransporter_hako_status_magnet_holder", "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 9, - "pdu_size": 8, + "channel_id": 15, + "pdu_size": 32, "write_cycle": 1, "method_type": "SHM" }, { - "type": "hako_msgs/HakoCameraData", - "org_name": "hako_camera_data", - "name": "DroneTransporter_hako_camera_data", + "type": "sensor_msgs/PointCloud2", + "org_name": "lidar_points", + "name": "DroneTransporter_lidar_points", "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 11, - "pdu_size": 102668, - "write_cycle": 1, + "channel_id": 16, + "pdu_size": 177400, + "write_cycle": 5, + "method_type": "SHM" + }, + { + "type": "geometry_msgs/Twist", + "org_name": "lidar_pos", + "name": "DroneTransporter_lidar_pos", + "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", + "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", + "channel_id": 17, + "pdu_size": 72, + "write_cycle": 5, "method_type": "SHM" } ] From cae262f379ee08953aa1ca821e798a3e3603c7db Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Fri, 15 Nov 2024 07:38:19 +0900 Subject: [PATCH 46/50] config spec is changed --- hakoniwa/README-ja.md | 3 +- hakoniwa/README.md | 3 +- .../config/rc-battery/drone_config_0.json | 11 +-- .../drone/aircraft/aircraft_factory.cpp | 73 +++++++++---------- hakoniwa/src/config/drone_config.hpp | 6 -- 5 files changed, 39 insertions(+), 57 deletions(-) diff --git a/hakoniwa/README-ja.md b/hakoniwa/README-ja.md index 823faf81..c0f39890 100644 --- a/hakoniwa/README-ja.md +++ b/hakoniwa/README-ja.md @@ -163,11 +163,10 @@ bash build.bash HAKONIWA_MATLAB_BUILD=true - **angle_degree**: 機体の初期角度。単位は度(`deg`)。 - **rotor**: ローターの設定。 - **vendor**: ベンダ名を指定します。現状は`None`を設定して下さい。 - - **Ct**: 推力係数 - - **Tr**: ローターの応答時間。単位は秒(`s`)。 - **thruster**: スラスターの設定。 - **vendor**: ベンダ名を指定します。現状は`None`を設定して下さい。 - **rotorPositions**: ローターの位置と回転方向。単位はメートル(`m`)。rotationDirectionはローターの回転方向(CW:-1.0, CCW: 1.0) + - **Ct**: 推力係数 - **Cq**: トルク係数。 - **Jr**: スラスターの慣性モーメントパラメータ。 - **sensors**: 各種センサーの設定。 diff --git a/hakoniwa/README.md b/hakoniwa/README.md index efc77f6b..ebf73237 100644 --- a/hakoniwa/README.md +++ b/hakoniwa/README.md @@ -154,11 +154,10 @@ The settings for each item are as follows: - **angle_degree**: The initial angle of the aircraft, in degrees (`deg`). - **rotor**: Rotor settings. - **vendor**: Specify the vendor name. Currently, set it to `None`. - - **Ct**: Thrust coefficient. - - **Tr**: Rotor response time, in seconds (`s`). - **thruster**: Thruster settings. - **vendor**: Specify the vendor name. Currently, set it to `None`. - **rotorPositions**: The positions and rotation directions of the rotors, in meters (`m`). rotationDirection is the direction of rotor rotation (CW:-1.0, CCW: 1.0) + - **Ct**: Thrust coefficient. - **Cq**: Torque coefficient. - **Jr**: Thruster moment of inertia parameter. - **sensors**: Settings for various sensors. diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json index 4a192b05..0b674fef 100644 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ b/hakoniwa/config/rc-battery/drone_config_0.json @@ -83,10 +83,7 @@ "K": 3.28e-3, "D": 0.0, "J": 8.12e-6 - }, - "Tr": 0.046, - "Kr": 27654.76291, - "rpmMax": 27654.76291 + } }, "thruster": { "vendor": "None", @@ -124,9 +121,9 @@ "rotationDirection": -1.0 } ], - "HoveringRpm": 13827.38146, - "parameterB": 3.0E-8, - "parameterJr": 0.0 + "Ct": 8.3E-07, + "Cq": 3.0E-8, + "Jr": 0.0 }, "sensors": { "acc": { diff --git a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp index c9d7acef..47744171 100644 --- a/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp +++ b/hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp @@ -35,10 +35,6 @@ using hako::assets::drone::ThrustDynamicsLinear; using hako::assets::drone::ThrustDynamicsNonLinear; using hako::assets::drone::SensorNoise; -static inline double rpmToRadPerSec(double rpm) { - return rpm * (2 * M_PI / 60.0); -} - #define DELTA_TIME_SEC drone_config.getSimTimeStep() #define REFERENCE_LATITUDE drone_config.getSimLatitude() #define REFERENCE_LONGTITUDE drone_config.getSimLongitude() @@ -53,12 +49,9 @@ static inline double rpmToRadPerSec(double rpm) { #define GPS_SAMPLE_NUM drone_config.getCompSensorSampleCount("gps") #define MAG_SAMPLE_NUM drone_config.getCompSensorSampleCount("mag") -#define RAD_PER_SEC_MAX rpmToRadPerSec(drone_config.getCompRotorRpmMax()) -#define ROTOR_TAU drone_config.getCompRotorTr() -#define ROTOR_K rpmToRadPerSec(drone_config.getCompRotorKr()) - -#define THRUST_PARAM_B drone_config.getCompThrusterParameter("parameterB") -#define THRUST_PARAM_JR drone_config.getCompThrusterParameter("parameterJr") +#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) @@ -120,7 +113,6 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr //battery dynamics BatteryModelParameters battery_config = drone_config.getComDroneDynamicsBattery(); - //TODO vendor support IBatteryDynamics *battery = nullptr; if (battery_config.vendor == "None") { battery = new BatteryDynamics(DELTA_TIME_SEC); @@ -132,6 +124,24 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr else { std::cout << "INFO: battery is not enabled." << std::endl; } + // calculate hovering rpm and maxrpm + double HoveringRadPerSec = sqrt(drone_dynamics->get_mass() * GRAVITY / (drone_config.getCompThrusterParameter("Ct") * ROTOR_NUM)); + double RadPerSecMax = HoveringRadPerSec * 2.0; + double HoveringRpm = HoveringRadPerSec * 60.0 / (2 * M_PI); + HAKO_ASSERT(HoveringRpm != 0); + std::cout << "HoveringRadPerSec: " << HoveringRadPerSec << std::endl; + std::cout << "HoveringRpm: " << HoveringRpm << std::endl; + //Tr=J*Rm/(Dm*Rm+K*K+2*Rm*Cq*ω_0) + auto rotor_constants = drone_config.getCompDroneDynamicsRotorDynamicsConstants(); + double RotorTau = ( + (rotor_constants.J * rotor_constants.R) / + ( + (rotor_constants.D * rotor_constants.R) + + (rotor_constants.K * rotor_constants.K) + + (2 * rotor_constants.R * rotor_constants.Cq * HoveringRadPerSec) + ) + ); + std::cout << "RotorTau: " << RotorTau << std::endl; //rotor dynamics IRotorDynamics* rotors[hako::assets::drone::ROTOR_NUM]; auto rotor_vendor = drone_config.getCompRotorVendor(); @@ -142,15 +152,14 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr if (rotor_vendor == "jmavsim") { rotor = new RotorDynamicsJmavsim(DELTA_TIME_SEC); HAKO_ASSERT(rotor != nullptr); - static_cast(rotor)->set_params(RAD_PER_SEC_MAX, ROTOR_TAU, ROTOR_K); + static_cast(rotor)->set_params(RadPerSecMax, RotorTau, RadPerSecMax); drone->get_logger().add_entry(*static_cast(rotor), LOGPATH(drone->get_index(), logfilename)); } else if (rotor_vendor == "BatteryModel") { rotor = new RotorDynamics(DELTA_TIME_SEC); HAKO_ASSERT(rotor != nullptr); - auto constants = drone_config.getCompDroneDynamicsRotorDynamicsConstants(); - rotor->set_battery_dynamics_constants(constants); - static_cast(rotor)->set_params(RAD_PER_SEC_MAX, 0, ROTOR_K); + rotor->set_battery_dynamics_constants(rotor_constants); + static_cast(rotor)->set_params(RadPerSecMax, 0, RadPerSecMax); drone->get_logger().add_entry(*static_cast(rotor), LOGPATH(drone->get_index(), logfilename)); HAKO_ASSERT(battery != nullptr); battery->add_device(*static_cast(rotor)); @@ -158,7 +167,7 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr else { rotor = new RotorDynamics(DELTA_TIME_SEC); HAKO_ASSERT(rotor != nullptr); - static_cast(rotor)->set_params(RAD_PER_SEC_MAX, ROTOR_TAU, ROTOR_K); + static_cast(rotor)->set_params(RadPerSecMax, RotorTau, RadPerSecMax); drone->get_logger().add_entry(*static_cast(rotor), LOGPATH(drone->get_index(), logfilename)); } rotors[i] = rotor; @@ -170,40 +179,24 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr IThrustDynamics *thrust = nullptr; auto thrust_vendor = drone_config.getCompThrusterVendor(); std::cout<< "Thruster vendor: " << thrust_vendor << std::endl; - double param_A = 1.0; - double param_B = THRUST_PARAM_B; + double param_Ct = THRUST_PARAM_Ct; + double param_Cq = THRUST_PARAM_Cq; + std::cout << "param_Ct: " << param_Ct << std::endl; + std::cout << "param_Cq: " << param_Cq << std::endl; if (thrust_vendor == "linear") { thrust = new ThrustDynamicsLinear(DELTA_TIME_SEC); HAKO_ASSERT(thrust != nullptr); - double HoveringRpm = drone_config.getCompThrusterParameter("HoveringRpm"); - HAKO_ASSERT(HoveringRpm != 0); - double mass = drone_dynamics->get_mass(); - param_A = (mass * GRAVITY / (rpmToRadPerSec(HoveringRpm) * ROTOR_NUM)); - param_B = drone_config.getCompThrusterParameter("parameterB_linear"); static_cast(thrust)->set_params( - param_A, - param_B + param_Ct, + param_Cq ); - std::cout << "param_A_linear: " << param_A << std::endl; - std::cout << "param_B_linear: " << param_B << std::endl; drone->get_logger().add_entry(*static_cast(thrust), LOGPATH(drone->get_index(), "log_thrust.csv")); } else { thrust = new ThrustDynamicsNonLinear(DELTA_TIME_SEC); HAKO_ASSERT(thrust != nullptr); - double HoveringRpm = drone_config.getCompThrusterParameter("HoveringRpm"); - HAKO_ASSERT(HoveringRpm != 0); - double mass = drone_dynamics->get_mass(); - param_A = ( - mass * GRAVITY / - ( - pow(rpmToRadPerSec(HoveringRpm), 2) * ROTOR_NUM - ) - ); - std::cout << "param_A: " << param_A << std::endl; - std::cout << "param_B: " << THRUST_PARAM_B << std::endl; std::cout << "param_Jr: " << THRUST_PARAM_JR << std::endl; - static_cast(thrust)->set_params(param_A, THRUST_PARAM_B, THRUST_PARAM_JR); + static_cast(thrust)->set_params(param_Ct, param_Cq, THRUST_PARAM_JR); drone->get_logger().add_entry(*static_cast(thrust), LOGPATH(drone->get_index(), "log_thrust.csv")); } drone->set_thrus_dynamics(thrust); @@ -224,7 +217,7 @@ IAirCraft* hako::assets::drone::create_aircraft(int index, const DroneConfig& dr DroneConfig::MixerInfo mixer_info; if (drone_config.getControllerMixerInfo(mixer_info)) { std::cout << "INFO: mixer is enabled" << std::endl; - DroneMixer *mixer = new DroneMixer(ROTOR_K, param_A, param_B, rotor_config); + DroneMixer *mixer = new DroneMixer(RadPerSecMax, param_Ct, param_Cq, rotor_config); HAKO_ASSERT(mixer != nullptr); bool inv_m = mixer->calculate_M_inv(); HAKO_ASSERT(inv_m == true); diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index d0a614b1..79c4d9ec 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -232,13 +232,7 @@ class DroneConfig { return "None"; } } - double getCompRotorTr() const { - return configJson["components"]["rotor"]["Tr"].get(); - } - double getCompRotorKr() const { - return configJson["components"]["rotor"]["Kr"].get(); - } hako::assets::drone::BatteryModelParameters getComDroneDynamicsBattery() const { hako::assets::drone::BatteryModelParameters params = {}; try { From 2b1b5cb851d685a62f88246182f6f51648b0b07b Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Fri, 15 Nov 2024 07:48:46 +0900 Subject: [PATCH 47/50] refactored configs --- .../config/api_sample/drone_config_0.json | 260 +++++++------ hakoniwa/config/api_twin/drone_config_0.json | 93 ----- hakoniwa/config/api_twin/ros/custom.json | 72 ---- hakoniwa/config/api_twin/unity/custom.json | 358 ------------------ hakoniwa/config/drone_config_0.json | 190 ++++++---- .../hard-maker-demo/drone_config_0.json | 101 ----- .../hard-maker-demo/drone_config_1.json | 101 ----- .../hard-maker-demo/drone_config_2.json | 101 ----- .../hard-maker-demo/drone_config_3.json | 101 ----- .../hard-maker-demo/drone_config_4.json | 101 ----- hakoniwa/config/hard-maker-demo/plan0.txt | 3 - hakoniwa/config/hard-maker-demo/plan1.txt | 3 - hakoniwa/config/hard-maker-demo/plan2.txt | 3 - hakoniwa/config/hard-maker-demo/plan3.txt | 3 - hakoniwa/config/hard-maker-demo/plan4.txt | 3 - hakoniwa/config/hard-maker-demo/sensor0.txt | 3 - hakoniwa/config/hard-maker-demo/sensor1.txt | 3 - hakoniwa/config/hard-maker-demo/sensor2.txt | 3 - hakoniwa/config/hard-maker-demo/sensor3.txt | 3 - hakoniwa/config/hard-maker-demo/sensor4.txt | 3 - .../config/rc-battery/drone_config_0.json | 161 -------- hakoniwa/config/rc-kanko/drone_config_0.json | 254 +++++++------ hakoniwa/config/rc/drone_config_0.json | 261 +++++++------ hakoniwa/drone-app.bash | 2 +- 24 files changed, 544 insertions(+), 1642 deletions(-) delete mode 100644 hakoniwa/config/api_twin/drone_config_0.json delete mode 100644 hakoniwa/config/api_twin/ros/custom.json delete mode 100644 hakoniwa/config/api_twin/unity/custom.json delete mode 100644 hakoniwa/config/hard-maker-demo/drone_config_0.json delete mode 100644 hakoniwa/config/hard-maker-demo/drone_config_1.json delete mode 100644 hakoniwa/config/hard-maker-demo/drone_config_2.json delete mode 100644 hakoniwa/config/hard-maker-demo/drone_config_3.json delete mode 100644 hakoniwa/config/hard-maker-demo/drone_config_4.json delete mode 100644 hakoniwa/config/hard-maker-demo/plan0.txt delete mode 100644 hakoniwa/config/hard-maker-demo/plan1.txt delete mode 100644 hakoniwa/config/hard-maker-demo/plan2.txt delete mode 100644 hakoniwa/config/hard-maker-demo/plan3.txt delete mode 100644 hakoniwa/config/hard-maker-demo/plan4.txt delete mode 100644 hakoniwa/config/hard-maker-demo/sensor0.txt delete mode 100644 hakoniwa/config/hard-maker-demo/sensor1.txt delete mode 100644 hakoniwa/config/hard-maker-demo/sensor2.txt delete mode 100644 hakoniwa/config/hard-maker-demo/sensor3.txt delete mode 100644 hakoniwa/config/hard-maker-demo/sensor4.txt delete mode 100644 hakoniwa/config/rc-battery/drone_config_0.json diff --git a/hakoniwa/config/api_sample/drone_config_0.json b/hakoniwa/config/api_sample/drone_config_0.json index d51687ce..571443be 100644 --- a/hakoniwa/config/api_sample/drone_config_0.json +++ b/hakoniwa/config/api_sample/drone_config_0.json @@ -1,141 +1,161 @@ { "name": "DroneTransporter", "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true + "lockstep": true, + "timeStep": 0.001, + "logOutputDirectory": ".", + "logOutput": { + "sensors": { + "acc": true, + "gyro": true, + "mag": true, + "baro": true, + "gps": true + }, + "mavlink": { + "hil_sensor": true, + "hil_gps": true, + "hil_actuator_controls": true + } }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 + "mavlink_tx_period_msec": { + "hil_sensor": 3, + "hil_gps": 3 + }, + "location": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 121.321, + "magneticField": { + "intensity_nT": 53045.1, + "declination_deg": 15.306, + "inclination_deg": 68.984 + } } - } }, "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "manual_control": false, - "airFrictionCoefficient": [ - 0.05, - 0 - ], - "inertia": [ - 0.0061, - 0.00653, - 0.0116 - ], - "mass_kg": 0.71, - "body_size": [ - 0.1, - 0.1, - 0.01 - ], - "position_meter": [ - 0, - 0, - 0 - ], - "angle_degree": [ - 0, - 0, - 0 - ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.0193, - "Kr": 27654.76291, - "rpmMax": 27654.76291 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - { - "position": [ - 0.103, - 0.103, - 0 + "droneDynamics": { + "physicsEquation": "BodyFrame", + "collision_detection": true, + "enable_disturbance": false, + "manual_control": false, + "airFrictionCoefficient": [ + 0.05, + 0 ], - "rotationDirection": 1.0 - }, - { - "position": [ - -0.103, - -0.103, - 0 + "inertia": [ + 0.0061, + 0.00653, + 0.0116 ], - "rotationDirection": 1.0 - }, - { - "position": [ - 0.103, - -0.103, - 0 + "mass_kg": 0.71, + "body_size": [ + 0.1, + 0.1, + 0.01 ], - "rotationDirection": -1.0 - }, - { - "position": [ - -0.103, - 0.103, - 0 + "position_meter": [ + 0, + 0, + 0 ], - "rotationDirection": -1.0 - } - ], - "HoveringRpm": 13827.38146, - "parameterB": 3.0E-8, - "parameterJr": 0.0 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 + "angle_degree": [ + 0, + 0, + 0 + ] }, - "gyro": { - "sampleCount": 1, - "noise": 0.0 + "battery": { + "vendor": "None", + "BatteryModelCsvFilePath": "./tmp_battery_model.csv", + "VoltageLevelGreen": 11.1, + "VoltageLevelYellow": 9.5, + "CapacityLevelYellow": 2.5, + "NominalVoltage": 14.8, + "NominalCapacity": 1000.0, + "EODVoltage": 3.0 }, - "mag": { - "sampleCount": 1, - "noise": 0.03 + "rotor": { + "vendor": "BatteryModel", + "dynamics_constants": { + "R": 0.12, + "Cq": 3.0e-8, + "K": 3.28e-3, + "D": 0.0, + "J": 8.12e-6 + } }, - "baro": { - "sampleCount": 1, - "noise": 0.01 + "thruster": { + "vendor": "None", + "rotorPositions": [ + { + "position": [ + 0.103, + 0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + -0.103, + -0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + 0.103, + -0.103, + 0 + ], + "rotationDirection": -1.0 + }, + { + "position": [ + -0.103, + 0.103, + 0 + ], + "rotationDirection": -1.0 + } + ], + "Ct": 8.3E-07, + "Cq": 3.0E-8, + "Jr": 0.0 }, - "gps": { - "sampleCount": 1, - "noise": 0 + "sensors": { + "acc": { + "sampleCount": 1, + "noise": 0.03 + }, + "gyro": { + "sampleCount": 1, + "noise": 0.0 + }, + "mag": { + "sampleCount": 1, + "noise": 0.03 + }, + "baro": { + "sampleCount": 1, + "noise": 0.01 + }, + "gps": { + "sampleCount": 1, + "noise": 0 + } } - } }, "controller": { "moduleDirectory": "../drone_control/cmake-build/workspace/FlightController", "moduleName": "FlightController", - "direct_rotor_control": false + "direct_rotor_control": false, + "mixer": { + "vendor": "None", + "enableDebugLog": false, + "enableErrorLog": false + } } -} +} \ No newline at end of file diff --git a/hakoniwa/config/api_twin/drone_config_0.json b/hakoniwa/config/api_twin/drone_config_0.json deleted file mode 100644 index a13884b8..00000000 --- a/hakoniwa/config/api_twin/drone_config_0.json +++ /dev/null @@ -1,93 +0,0 @@ -{ - "name": "DroneTransporter", - "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true - }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } - }, - "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "manual_control": false, - "airFrictionCoefficient": [ 0.05, 0.0 ], - "inertia": [ 0.0000625, 0.00003125, 0.00009375 ], - "mass_kg": 0.1, - "body_size": [ 0.1, 0.1, 0.01 ], - "position_meter": [ 0, 1, 0 ], - "angle_degree": [ 0, 0, 0 ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.2, - "Kr": 8000, - "rpmMax": 8000 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - {"position": [ 0.05, 0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ -0.05, -0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ 0.05, -0.05, 0 ], "rotationDirection": -1.0 }, - {"position": [ -0.05, 0.05, 0 ], "rotationDirection": -1.0 } - ], - "HoveringRpm": 4000, - "parameterB": 1.3e-10, - "parameterJr": 1.0e-10 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 - }, - "gyro": { - "sampleCount": 1, - "noise": 0.0 - }, - "mag": { - "sampleCount": 1, - "noise": 0.03 - }, - "baro": { - "sampleCount": 1, - "noise": 0.01 - }, - "gps": { - "sampleCount": 1, - "noise": 0 - } - } - }, - "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/FlightController", - "moduleName": "FlightController" - } -} diff --git a/hakoniwa/config/api_twin/ros/custom.json b/hakoniwa/config/api_twin/ros/custom.json deleted file mode 100644 index 3ebe052a..00000000 --- a/hakoniwa/config/api_twin/ros/custom.json +++ /dev/null @@ -1,72 +0,0 @@ -{ - "robots": [ - { - "name": "TB3RoboAvatar", - "rpc_pdu_readers": [], - "rpc_pdu_writers": [], - "shm_pdu_readers": [ - { - "type": "geometry_msgs/Twist", - "org_name": "cmd_pos", - "name": "TB3RoboAvatar_cmd_pos", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 0, - "pdu_size": 72, - "write_cycle": 1, - "method_type": "SHM" - } - ], - "shm_pdu_writers": [ - { - "type": "std_msgs/Bool", - "org_name": "baggage_sensor", - "name": "TB3RoboAvatar_baggage_sensor", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 1, - "pdu_size": 28, - "write_cycle": 1, - "method_type": "SHM" - } - ] - }, - { - "name": "LiDARSensor2D", - "rpc_pdu_readers": [], - "rpc_pdu_writers": [], - "shm_pdu_readers": [], - "shm_pdu_writers": [ - { - "type": "sensor_msgs/LaserScan", - "org_name": "scan", - "name": "LiDARSensor2D_scan", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 0, - "pdu_size": 6088, - "write_cycle": 5, - "method_type": "SHM" - } - ] - }, - { - "name": "TB3RoboModel", - "rpc_pdu_readers": [], - "rpc_pdu_writers": [], - "shm_pdu_readers": [ - { - "type": "geometry_msgs/Twist", - "org_name": "cmd_vel", - "name": "TB3RoboModel_cmd_vel", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter", - "channel_id": 1, - "pdu_size": 72, - "write_cycle": 10, - "method_type": "SHM" - } - ] - } - ] -} \ No newline at end of file diff --git a/hakoniwa/config/api_twin/unity/custom.json b/hakoniwa/config/api_twin/unity/custom.json deleted file mode 100644 index 9de67938..00000000 --- a/hakoniwa/config/api_twin/unity/custom.json +++ /dev/null @@ -1,358 +0,0 @@ -{ - "robots": [ - { - "name": "TB3RoboAvatar", - "rpc_pdu_readers": [], - "rpc_pdu_writers": [], - "shm_pdu_readers": [ - { - "type": "geometry_msgs/Twist", - "org_name": "cmd_pos", - "name": "TB3RoboAvatar_cmd_pos", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 0, - "pdu_size": 72, - "write_cycle": 1, - "method_type": "SHM" - } - ], - "shm_pdu_writers": [ - { - "type": "std_msgs/Bool", - "org_name": "baggage_sensor", - "name": "TB3RoboAvatar_baggage_sensor", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 1, - "pdu_size": 28, - "write_cycle": 1, - "method_type": "SHM" - } - ] - }, - { - "name": "Drone", - "rpc_pdu_readers": [], - "rpc_pdu_writers": [], - "shm_pdu_readers": [ - { - "type": "hako_mavlink_msgs/HakoHilActuatorControls", - "org_name": "drone_motor", - "name": "Drone_drone_motor", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter", - "channel_id": 0, - "pdu_size": 112, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "geometry_msgs/Twist", - "org_name": "drone_pos", - "name": "Drone_drone_pos", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter", - "channel_id": 1, - "pdu_size": 72, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/ManualPosAttControl", - "org_name": "drone_manual_pos_att_control", - "name": "Drone_drone_manual_pos_att_control", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 3, - "pdu_size": 80, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoDroneCmdTakeoff", - "org_name": "drone_cmd_takeoff", - "name": "Drone_drone_cmd_takeoff", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 5, - "pdu_size": 64, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoDroneCmdMove", - "org_name": "drone_cmd_move", - "name": "Drone_drone_cmd_move", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 6, - "pdu_size": 80, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoDroneCmdLand", - "org_name": "drone_cmd_land", - "name": "Drone_drone_cmd_land", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 7, - "pdu_size": 64, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/GameControllerOperation", - "org_name": "hako_cmd_game", - "name": "Drone_hako_cmd_game", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 8, - "pdu_size": 136, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoCmdCamera", - "org_name": "hako_cmd_camera", - "name": "Drone_hako_cmd_camera", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 9, - "pdu_size": 44, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoCmdCameraMove", - "org_name": "hako_cmd_camera_move", - "name": "Drone_hako_cmd_camera_move", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 11, - "pdu_size": 64, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoCmdMagnetHolder", - "org_name": "hako_cmd_magnet_holder", - "name": "Drone_hako_cmd_magnet_holder", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 13, - "pdu_size": 40, - "write_cycle": 1, - "method_type": "SHM" - } - ], - "shm_pdu_writers": [ - { - "type": "hako_msgs/Collision", - "org_name": "drone_collision", - "name": "Drone_drone_collision", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 2, - "pdu_size": 304, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/Disturbance", - "org_name": "drone_disturbance", - "name": "Drone_drone_disturbance", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 4, - "pdu_size": 32, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoCameraData", - "org_name": "hako_camera_data", - "name": "Drone_hako_camera_data", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 10, - "pdu_size": 102968, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoCameraInfo", - "org_name": "hako_cmd_camera_info", - "name": "Drone_hako_cmd_camera_info", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 12, - "pdu_size": 56, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "hako_msgs/HakoStatusMagnetHolder", - "org_name": "hako_status_magnet_holder", - "name": "Drone_hako_status_magnet_holder", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 14, - "pdu_size": 32, - "write_cycle": 1, - "method_type": "SHM" - }, - { - "type": "sensor_msgs/PointCloud2", - "org_name": "lidar_points", - "name": "Drone_lidar_points", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 15, - "pdu_size": 177400, - "write_cycle": 5, - "method_type": "SHM" - }, - { - "type": "geometry_msgs/Twist", - "org_name": "lidar_pos", - "name": "Drone_lidar_pos", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 16, - "pdu_size": 72, - "write_cycle": 5, - "method_type": "SHM" - } - ] - }, - { - "name": "LiDARSensor2D", - "rpc_pdu_readers": [], - "rpc_pdu_writers": [], - "shm_pdu_readers": [], - "shm_pdu_writers": [ - { - "type": "sensor_msgs/LaserScan", - "org_name": "scan", - "name": "LiDARSensor2D_scan", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 0, - "pdu_size": 6088, - "write_cycle": 5, - "method_type": "SHM" - } - ] - }, - { - "name": "TB3RoboModel", - "rpc_pdu_readers": [], - "rpc_pdu_writers": [], - "shm_pdu_readers": [ - { - "type": "geometry_msgs/Twist", - "org_name": "cmd_vel", - "name": "TB3RoboModel_cmd_vel", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter", - "channel_id": 1, - "pdu_size": 72, - "write_cycle": 10, - "method_type": "SHM" - } - ], - "shm_pdu_writers": [ - { - "type": "sensor_msgs/JointState", - "org_name": "joint_states", - "name": "TB3RoboModel_joint_states", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 0, - "pdu_size": 1048, - "write_cycle": 10, - "method_type": "SHM" - }, - { - "type": "sensor_msgs/Imu", - "org_name": "imu", - "name": "TB3RoboModel_imu", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 2, - "pdu_size": 456, - "write_cycle": 10, - "method_type": "SHM" - }, - { - "type": "nav_msgs/Odometry", - "org_name": "odom", - "name": "TB3RoboModel_odom", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 3, - "pdu_size": 968, - "write_cycle": 10, - "method_type": "SHM" - }, - { - "type": "tf2_msgs/TFMessage", - "org_name": "tf", - "name": "TB3RoboModel_tf", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 4, - "pdu_size": 536, - "write_cycle": 10, - "method_type": "SHM" - }, - { - "type": "sensor_msgs/Image", - "org_name": "image", - "name": "TB3RoboModel_image", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 5, - "pdu_size": 1229104, - "write_cycle": 100, - "method_type": "SHM" - }, - { - "type": "sensor_msgs/CompressedImage", - "org_name": "image/compressed", - "name": "TB3RoboModel_image/compressed", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 6, - "pdu_size": 1229088, - "write_cycle": 100, - "method_type": "SHM" - }, - { - "type": "sensor_msgs/CameraInfo", - "org_name": "camera_info", - "name": "TB3RoboModel_camera_info", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 7, - "pdu_size": 1048, - "write_cycle": 10, - "method_type": "SHM" - }, - { - "type": "sensor_msgs/LaserScan", - "org_name": "scan", - "name": "TB3RoboModel_scan", - "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter", - "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter", - "channel_id": 8, - "pdu_size": 3160, - "write_cycle": 10, - "method_type": "SHM" - } - ] - } - ] -} \ No newline at end of file diff --git a/hakoniwa/config/drone_config_0.json b/hakoniwa/config/drone_config_0.json index 863d5c03..17e62d8f 100644 --- a/hakoniwa/config/drone_config_0.json +++ b/hakoniwa/config/drone_config_0.json @@ -1,90 +1,152 @@ { "name": "DroneTransporter", "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true + "lockstep": true, + "timeStep": 0.001, + "logOutputDirectory": ".", + "logOutput": { + "sensors": { + "acc": true, + "gyro": true, + "mag": true, + "baro": true, + "gps": true + }, + "mavlink": { + "hil_sensor": true, + "hil_gps": true, + "hil_actuator_controls": true + } + }, + "mavlink_tx_period_msec": { + "hil_sensor": 3, + "hil_gps": 3 }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true + "location": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 121.321, + "magneticField": { + "intensity_nT": 53045.1, + "declination_deg": 15.306, + "inclination_deg": 68.984 + } } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } }, "components": { "droneDynamics": { "physicsEquation": "BodyFrame", "collision_detection": true, + "enable_disturbance": false, "manual_control": false, - "airFrictionCoefficient": [ 0.02, 0.0 ], - "inertia": [ 0.0000625, 0.00003125, 0.00009375 ], - "mass_kg": 0.1, - "body_size": [ 0.1, 0.1, 0.01 ], - "position_meter": [ 0, 0, 0 ], - "angle_degree": [ 0, 0, 0 ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.2, - "Kr": 8000, - "rpmMax": 8000 - }, - "thruster": { + "airFrictionCoefficient": [ + 0.05, + 0 + ], + "inertia": [ + 0.0061, + 0.00653, + 0.0116 + ], + "mass_kg": 0.71, + "body_size": [ + 0.1, + 0.1, + 0.01 + ], + "position_meter": [ + 0, + 0, + 0 + ], + "angle_degree": [ + 0, + 0, + 0 + ] + }, + "battery": { + "vendor": "None", + "BatteryModelCsvFilePath": "./tmp_battery_model.csv", + "VoltageLevelGreen": 11.1, + "VoltageLevelYellow": 9.5, + "CapacityLevelYellow": 2.5, + "NominalVoltage": 14.8, + "NominalCapacity": 1000.0, + "EODVoltage": 3.0 + }, + "rotor": { + "vendor": "BatteryModel", + "dynamics_constants": { + "R": 0.12, + "Cq": 3.0e-8, + "K": 3.28e-3, + "D": 0.0, + "J": 8.12e-6 + } + }, + "thruster": { "vendor": "None", "rotorPositions": [ - {"position": [ 0.05, 0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ -0.05, -0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ 0.05, -0.05, 0 ], "rotationDirection": -1.0 }, - {"position": [ -0.05, 0.05, 0 ], "rotationDirection": -1.0 } - ], - "HoveringRpm": 4000, - "parameterB": 0.3e-10, - "parameterJr": 1.0e-10 - }, - "sensors": { + { + "position": [ + 0.103, + 0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + -0.103, + -0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + 0.103, + -0.103, + 0 + ], + "rotationDirection": -1.0 + }, + { + "position": [ + -0.103, + 0.103, + 0 + ], + "rotationDirection": -1.0 + } + ], + "Ct": 8.3E-07, + "Cq": 3.0E-8, + "Jr": 0.0 + }, + "sensors": { "acc": { - "sampleCount": 1, - "noise": 0.03 + "sampleCount": 1, + "noise": 0.03 }, "gyro": { - "sampleCount": 1, - "noise": 0.0 + "sampleCount": 1, + "noise": 0.0 }, "mag": { - "sampleCount": 1, - "noise": 0.03 + "sampleCount": 1, + "noise": 0.03 }, "baro": { - "sampleCount": 1, - "noise": 0.01 + "sampleCount": 1, + "noise": 0.01 }, "gps": { - "sampleCount": 1, - "noise": 0 + "sampleCount": 1, + "noise": 0 } - } + } }, "controller": { "moduleDirectory": "../drone_control/cmake-build/workspace/RadioController", diff --git a/hakoniwa/config/hard-maker-demo/drone_config_0.json b/hakoniwa/config/hard-maker-demo/drone_config_0.json deleted file mode 100644 index 25429012..00000000 --- a/hakoniwa/config/hard-maker-demo/drone_config_0.json +++ /dev/null @@ -1,101 +0,0 @@ -{ - "name": "DroneFlightController", - "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true - }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } - }, - "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "manual_control": false, - "airFrictionCoefficient": [ 0.02, 0.0 ], - "inertia": [ 0.0000625, 0.00003125, 0.00009375 ], - "mass_kg": 0.1, - "body_size": [ 0.1, 0.1, 0.01 ], - "position_meter": [ 0, -80, 0 ], - "angle_degree": [ 0, 0, 0 ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.2, - "Kr": 8000, - "rpmMax": 8000 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - {"position": [ 0.05, 0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ -0.05, -0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ 0.05, -0.05, 0 ], "rotationDirection": -1.0 }, - {"position": [ -0.05, 0.05, 0 ], "rotationDirection": -1.0 } - ], - "HoveringRpm": 4000, - "parameterB": 1.3e-10, - "parameterJr": 1.0e-10 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 - }, - "gyro": { - "vendor": "../drone_sensors/cmake-build/sensors/gyro/vendor/hakogyro", - "context": { - "moduleName": "hakogyro", - "file": "./config/hard-maker-demo/sensor0.txt" - }, - "sampleCount": 1, - "noise": 0.0 - }, - "mag": { - "sampleCount": 1, - "noise": 0.03 - }, - "baro": { - "sampleCount": 1, - "noise": 0.01 - }, - "gps": { - "sampleCount": 1, - "noise": 0 - } - } - }, - "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/DroneFlightController", - "moduleName": "DroneFlightController", - "context": { - "file": "./config/hard-maker-demo/plan0.txt" - } - } -} diff --git a/hakoniwa/config/hard-maker-demo/drone_config_1.json b/hakoniwa/config/hard-maker-demo/drone_config_1.json deleted file mode 100644 index cab07491..00000000 --- a/hakoniwa/config/hard-maker-demo/drone_config_1.json +++ /dev/null @@ -1,101 +0,0 @@ -{ - "name": "DroneFlightController1", - "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true - }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } - }, - "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "manual_control": false, - "airFrictionCoefficient": [ 0.02, 0.0 ], - "inertia": [ 0.0000625, 0.00003125, 0.00009375 ], - "mass_kg": 0.1, - "body_size": [ 0.1, 0.1, 0.01 ], - "position_meter": [ -5, -80, 0 ], - "angle_degree": [ 0, 0, 0 ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.2, - "Kr": 8000, - "rpmMax": 8000 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - {"position": [ 0.05, 0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ -0.05, -0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ 0.05, -0.05, 0 ], "rotationDirection": -1.0 }, - {"position": [ -0.05, 0.05, 0 ], "rotationDirection": -1.0 } - ], - "HoveringRpm": 4000, - "parameterB": 1.3e-10, - "parameterJr": 1.0e-10 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 - }, - "gyro": { - "vendor": "../drone_sensors/cmake-build/sensors/gyro/vendor/hakogyro", - "context": { - "moduleName": "hakogyro", - "file": "./config/hard-maker-demo/sensor1.txt" - }, - "sampleCount": 1, - "noise": 0.0 - }, - "mag": { - "sampleCount": 1, - "noise": 0.03 - }, - "baro": { - "sampleCount": 1, - "noise": 0.01 - }, - "gps": { - "sampleCount": 1, - "noise": 0 - } - } - }, - "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/DroneFlightController", - "moduleName": "DroneFlightController", - "context": { - "file": "./config/hard-maker-demo/plan1.txt" - } - } -} diff --git a/hakoniwa/config/hard-maker-demo/drone_config_2.json b/hakoniwa/config/hard-maker-demo/drone_config_2.json deleted file mode 100644 index 488ff974..00000000 --- a/hakoniwa/config/hard-maker-demo/drone_config_2.json +++ /dev/null @@ -1,101 +0,0 @@ -{ - "name": "DroneFlightController2", - "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true - }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } - }, - "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "manual_control": false, - "airFrictionCoefficient": [ 0.02, 0.0 ], - "inertia": [ 0.0000625, 0.00003125, 0.00009375 ], - "mass_kg": 0.1, - "body_size": [ 0.1, 0.1, 0.01 ], - "position_meter": [ -10, -80, 0 ], - "angle_degree": [ 0, 0, 0 ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.2, - "Kr": 8000, - "rpmMax": 8000 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - {"position": [ 0.05, 0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ -0.05, -0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ 0.05, -0.05, 0 ], "rotationDirection": -1.0 }, - {"position": [ -0.05, 0.05, 0 ], "rotationDirection": -1.0 } - ], - "HoveringRpm": 4000, - "parameterB": 1.3e-10, - "parameterJr": 1.0e-10 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 - }, - "gyro": { - "vendor": "../drone_sensors/cmake-build/sensors/gyro/vendor/hakogyro", - "context": { - "moduleName": "hakogyro", - "file": "./config/hard-maker-demo/sensor2.txt" - }, - "sampleCount": 1, - "noise": 0.0 - }, - "mag": { - "sampleCount": 1, - "noise": 0.03 - }, - "baro": { - "sampleCount": 1, - "noise": 0.01 - }, - "gps": { - "sampleCount": 1, - "noise": 0 - } - } - }, - "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/DroneFlightController", - "moduleName": "DroneFlightController", - "context": { - "file": "./config/hard-maker-demo/plan2.txt" - } - } -} diff --git a/hakoniwa/config/hard-maker-demo/drone_config_3.json b/hakoniwa/config/hard-maker-demo/drone_config_3.json deleted file mode 100644 index a20f554c..00000000 --- a/hakoniwa/config/hard-maker-demo/drone_config_3.json +++ /dev/null @@ -1,101 +0,0 @@ -{ - "name": "DroneFlightController3", - "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true - }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } - }, - "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "manual_control": false, - "airFrictionCoefficient": [ 0.02, 0.0 ], - "inertia": [ 0.0000625, 0.00003125, 0.00009375 ], - "mass_kg": 0.1, - "body_size": [ 0.1, 0.1, 0.01 ], - "position_meter": [ -15, -80, 0 ], - "angle_degree": [ 0, 0, 0 ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.2, - "Kr": 8000, - "rpmMax": 8000 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - {"position": [ 0.05, 0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ -0.05, -0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ 0.05, -0.05, 0 ], "rotationDirection": -1.0 }, - {"position": [ -0.05, 0.05, 0 ], "rotationDirection": -1.0 } - ], - "HoveringRpm": 4000, - "parameterB": 1.3e-10, - "parameterJr": 1.0e-10 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 - }, - "gyro": { - "vendor": "../drone_sensors/cmake-build/sensors/gyro/vendor/hakogyro", - "context": { - "moduleName": "hakogyro", - "file": "./config/hard-maker-demo/sensor3.txt" - }, - "sampleCount": 1, - "noise": 0.0 - }, - "mag": { - "sampleCount": 1, - "noise": 0.03 - }, - "baro": { - "sampleCount": 1, - "noise": 0.01 - }, - "gps": { - "sampleCount": 1, - "noise": 0 - } - } - }, - "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/DroneFlightController", - "moduleName": "DroneFlightController", - "context": { - "file": "./config/hard-maker-demo/plan3.txt" - } - } -} diff --git a/hakoniwa/config/hard-maker-demo/drone_config_4.json b/hakoniwa/config/hard-maker-demo/drone_config_4.json deleted file mode 100644 index 3334fffd..00000000 --- a/hakoniwa/config/hard-maker-demo/drone_config_4.json +++ /dev/null @@ -1,101 +0,0 @@ -{ - "name": "DroneFlightController4", - "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true - }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } - }, - "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "manual_control": false, - "airFrictionCoefficient": [ 0.02, 0.0 ], - "inertia": [ 0.0000625, 0.00003125, 0.00009375 ], - "mass_kg": 0.1, - "body_size": [ 0.1, 0.1, 0.01 ], - "position_meter": [ -20, -80, 0 ], - "angle_degree": [ 0, 0, 0 ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.2, - "Kr": 8000, - "rpmMax": 8000 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - {"position": [ 0.05, 0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ -0.05, -0.05, 0 ], "rotationDirection": 1.0 }, - {"position": [ 0.05, -0.05, 0 ], "rotationDirection": -1.0 }, - {"position": [ -0.05, 0.05, 0 ], "rotationDirection": -1.0 } - ], - "HoveringRpm": 4000, - "parameterB": 1.3e-10, - "parameterJr": 1.0e-10 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 - }, - "gyro": { - "vendor": "../drone_sensors/cmake-build/sensors/gyro/vendor/hakogyro", - "context": { - "moduleName": "hakogyro", - "file": "./config/hard-maker-demo/sensor4.txt" - }, - "sampleCount": 1, - "noise": 0.0 - }, - "mag": { - "sampleCount": 1, - "noise": 0.03 - }, - "baro": { - "sampleCount": 1, - "noise": 0.01 - }, - "gps": { - "sampleCount": 1, - "noise": 0 - } - } - }, - "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/DroneFlightController", - "moduleName": "DroneFlightController", - "context": { - "file": "./config/hard-maker-demo/plan4.txt" - } - } -} diff --git a/hakoniwa/config/hard-maker-demo/plan0.txt b/hakoniwa/config/hard-maker-demo/plan0.txt deleted file mode 100644 index 8e00ba3d..00000000 --- a/hakoniwa/config/hard-maker-demo/plan0.txt +++ /dev/null @@ -1,3 +0,0 @@ -0 80 -0 -80 - diff --git a/hakoniwa/config/hard-maker-demo/plan1.txt b/hakoniwa/config/hard-maker-demo/plan1.txt deleted file mode 100644 index 995d101c..00000000 --- a/hakoniwa/config/hard-maker-demo/plan1.txt +++ /dev/null @@ -1,3 +0,0 @@ --5 80 --5 -80 - diff --git a/hakoniwa/config/hard-maker-demo/plan2.txt b/hakoniwa/config/hard-maker-demo/plan2.txt deleted file mode 100644 index 25500b02..00000000 --- a/hakoniwa/config/hard-maker-demo/plan2.txt +++ /dev/null @@ -1,3 +0,0 @@ --10 80 --10 -80 - diff --git a/hakoniwa/config/hard-maker-demo/plan3.txt b/hakoniwa/config/hard-maker-demo/plan3.txt deleted file mode 100644 index 25fa3ab5..00000000 --- a/hakoniwa/config/hard-maker-demo/plan3.txt +++ /dev/null @@ -1,3 +0,0 @@ --15 80 --15 -80 - diff --git a/hakoniwa/config/hard-maker-demo/plan4.txt b/hakoniwa/config/hard-maker-demo/plan4.txt deleted file mode 100644 index 9e89e7e2..00000000 --- a/hakoniwa/config/hard-maker-demo/plan4.txt +++ /dev/null @@ -1,3 +0,0 @@ --20 80 --20 -80 - diff --git a/hakoniwa/config/hard-maker-demo/sensor0.txt b/hakoniwa/config/hard-maker-demo/sensor0.txt deleted file mode 100644 index 283bf13a..00000000 --- a/hakoniwa/config/hard-maker-demo/sensor0.txt +++ /dev/null @@ -1,3 +0,0 @@ -TEMP_MIN = 0.0 -TEMP_MAX = 30.0 -TEMP_DRIFT = 0.00001 diff --git a/hakoniwa/config/hard-maker-demo/sensor1.txt b/hakoniwa/config/hard-maker-demo/sensor1.txt deleted file mode 100644 index 19f7f91b..00000000 --- a/hakoniwa/config/hard-maker-demo/sensor1.txt +++ /dev/null @@ -1,3 +0,0 @@ -TEMP_MIN = 0.0 -TEMP_MAX = 30.0 -TEMP_DRIFT = 0.0001 diff --git a/hakoniwa/config/hard-maker-demo/sensor2.txt b/hakoniwa/config/hard-maker-demo/sensor2.txt deleted file mode 100644 index aca5b309..00000000 --- a/hakoniwa/config/hard-maker-demo/sensor2.txt +++ /dev/null @@ -1,3 +0,0 @@ -TEMP_MIN = 0.0 -TEMP_MAX = 30.0 -TEMP_DRIFT = 0.001 diff --git a/hakoniwa/config/hard-maker-demo/sensor3.txt b/hakoniwa/config/hard-maker-demo/sensor3.txt deleted file mode 100644 index be58877a..00000000 --- a/hakoniwa/config/hard-maker-demo/sensor3.txt +++ /dev/null @@ -1,3 +0,0 @@ -TEMP_MIN = 0.0 -TEMP_MAX = 30.0 -TEMP_DRIFT = 0.01 diff --git a/hakoniwa/config/hard-maker-demo/sensor4.txt b/hakoniwa/config/hard-maker-demo/sensor4.txt deleted file mode 100644 index 26631905..00000000 --- a/hakoniwa/config/hard-maker-demo/sensor4.txt +++ /dev/null @@ -1,3 +0,0 @@ -TEMP_MIN = 0.0 -TEMP_MAX = 30.0 -TEMP_DRIFT = 0.1 diff --git a/hakoniwa/config/rc-battery/drone_config_0.json b/hakoniwa/config/rc-battery/drone_config_0.json deleted file mode 100644 index 0b674fef..00000000 --- a/hakoniwa/config/rc-battery/drone_config_0.json +++ /dev/null @@ -1,161 +0,0 @@ -{ - "name": "DroneTransporter", - "simulation": { - "lockstep": true, - "timeStep": 0.001, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true - }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 - } - } - }, - "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "enable_disturbance": false, - "manual_control": false, - "airFrictionCoefficient": [ - 0.05, - 0 - ], - "inertia": [ - 0.0061, - 0.00653, - 0.0116 - ], - "mass_kg": 0.71, - "body_size": [ - 0.1, - 0.1, - 0.01 - ], - "position_meter": [ - 0, - 0, - 0 - ], - "angle_degree": [ - 0, - 0, - 0 - ] - }, - "battery": { - "vendor": "None", - "BatteryModelCsvFilePath": "./tmp_battery_model.csv", - "VoltageLevelGreen": 11.1, - "VoltageLevelYellow": 9.5, - "CapacityLevelYellow": 2.5, - "NominalVoltage": 14.8, - "NominalCapacity": 1000.0, - "EODVoltage": 3.0 - }, - "rotor": { - "vendor": "BatteryModel", - "dynamics_constants": { - "R": 0.12, - "Cq": 3.0e-8, - "K": 3.28e-3, - "D": 0.0, - "J": 8.12e-6 - } - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - { - "position": [ - 0.103, - 0.103, - 0 - ], - "rotationDirection": 1.0 - }, - { - "position": [ - -0.103, - -0.103, - 0 - ], - "rotationDirection": 1.0 - }, - { - "position": [ - 0.103, - -0.103, - 0 - ], - "rotationDirection": -1.0 - }, - { - "position": [ - -0.103, - 0.103, - 0 - ], - "rotationDirection": -1.0 - } - ], - "Ct": 8.3E-07, - "Cq": 3.0E-8, - "Jr": 0.0 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 - }, - "gyro": { - "sampleCount": 1, - "noise": 0.0 - }, - "mag": { - "sampleCount": 1, - "noise": 0.03 - }, - "baro": { - "sampleCount": 1, - "noise": 0.01 - }, - "gps": { - "sampleCount": 1, - "noise": 0 - } - } - }, - "controller": { - "moduleDirectory": "../drone_control/cmake-build/workspace/RadioController", - "moduleName": "RadioController", - "direct_rotor_control": false, - "mixer": { - "vendor": "None", - "enableDebugLog": false, - "enableErrorLog": false - } - } -} \ No newline at end of file diff --git a/hakoniwa/config/rc-kanko/drone_config_0.json b/hakoniwa/config/rc-kanko/drone_config_0.json index 0f52d2fb..20faefea 100644 --- a/hakoniwa/config/rc-kanko/drone_config_0.json +++ b/hakoniwa/config/rc-kanko/drone_config_0.json @@ -1,137 +1,157 @@ { "name": "DroneTransporter", "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true + "lockstep": true, + "timeStep": 0.001, + "logOutputDirectory": ".", + "logOutput": { + "sensors": { + "acc": true, + "gyro": true, + "mag": true, + "baro": true, + "gps": true + }, + "mavlink": { + "hil_sensor": true, + "hil_gps": true, + "hil_actuator_controls": true + } }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 + "mavlink_tx_period_msec": { + "hil_sensor": 3, + "hil_gps": 3 + }, + "location": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 121.321, + "magneticField": { + "intensity_nT": 53045.1, + "declination_deg": 15.306, + "inclination_deg": 68.984 + } } - } }, "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "manual_control": false, - "airFrictionCoefficient": [ - 0.05, - 0 - ], - "inertia": [ - 0.0061, - 0.00653, - 0.0116 - ], - "mass_kg": 0.71, - "body_size": [ - 0.1, - 0.1, - 0.01 - ], - "position_meter": [ 173, -1, -23 ], - "angle_degree": [ - 0, - 0, - 0 - ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.0193, - "Kr": 27654.76291, - "rpmMax": 27654.76291 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - { - "position": [ - 0.103, - 0.103, - 0 + "droneDynamics": { + "physicsEquation": "BodyFrame", + "collision_detection": true, + "enable_disturbance": false, + "manual_control": false, + "airFrictionCoefficient": [ + 0.05, + 0 ], - "rotationDirection": 1.0 - }, - { - "position": [ - -0.103, - -0.103, - 0 + "inertia": [ + 0.0061, + 0.00653, + 0.0116 ], - "rotationDirection": 1.0 - }, - { - "position": [ - 0.103, - -0.103, - 0 + "mass_kg": 0.71, + "body_size": [ + 0.1, + 0.1, + 0.01 ], - "rotationDirection": -1.0 - }, - { - "position": [ - -0.103, - 0.103, - 0 - ], - "rotationDirection": -1.0 - } - ], - "HoveringRpm": 13827.38146, - "parameterB": 3.0E-8, - "parameterJr": 0.0 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 + "position_meter": [ 173, -1, -23 ], + "angle_degree": [ + 0, + 0, + 0 + ] }, - "gyro": { - "sampleCount": 1, - "noise": 0.0 + "battery": { + "vendor": "None", + "BatteryModelCsvFilePath": "./tmp_battery_model.csv", + "VoltageLevelGreen": 11.1, + "VoltageLevelYellow": 9.5, + "CapacityLevelYellow": 2.5, + "NominalVoltage": 14.8, + "NominalCapacity": 1000.0, + "EODVoltage": 3.0 }, - "mag": { - "sampleCount": 1, - "noise": 0.03 + "rotor": { + "vendor": "BatteryModel", + "dynamics_constants": { + "R": 0.12, + "Cq": 3.0e-8, + "K": 3.28e-3, + "D": 0.0, + "J": 8.12e-6 + } }, - "baro": { - "sampleCount": 1, - "noise": 0.01 + "thruster": { + "vendor": "None", + "rotorPositions": [ + { + "position": [ + 0.103, + 0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + -0.103, + -0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + 0.103, + -0.103, + 0 + ], + "rotationDirection": -1.0 + }, + { + "position": [ + -0.103, + 0.103, + 0 + ], + "rotationDirection": -1.0 + } + ], + "Ct": 8.3E-07, + "Cq": 3.0E-8, + "Jr": 0.0 }, - "gps": { - "sampleCount": 1, - "noise": 0 + "sensors": { + "acc": { + "sampleCount": 1, + "noise": 0.03 + }, + "gyro": { + "sampleCount": 1, + "noise": 0.0 + }, + "mag": { + "sampleCount": 1, + "noise": 0.03 + }, + "baro": { + "sampleCount": 1, + "noise": 0.01 + }, + "gps": { + "sampleCount": 1, + "noise": 0 + } } - } }, "controller": { "moduleDirectory": "../drone_control/cmake-build/workspace/RadioController", "moduleName": "RadioController", - "direct_rotor_control": false + "direct_rotor_control": false, + "mixer": { + "vendor": "None", + "enableDebugLog": false, + "enableErrorLog": false + } } -} +} \ No newline at end of file diff --git a/hakoniwa/config/rc/drone_config_0.json b/hakoniwa/config/rc/drone_config_0.json index c26e7f77..0b674fef 100644 --- a/hakoniwa/config/rc/drone_config_0.json +++ b/hakoniwa/config/rc/drone_config_0.json @@ -1,142 +1,161 @@ { "name": "DroneTransporter", "simulation": { - "lockstep": true, - "timeStep": 0.003, - "logOutputDirectory": ".", - "logOutput": { - "sensors": { - "acc": true, - "gyro": true, - "mag": true, - "baro": true, - "gps": true + "lockstep": true, + "timeStep": 0.001, + "logOutputDirectory": ".", + "logOutput": { + "sensors": { + "acc": true, + "gyro": true, + "mag": true, + "baro": true, + "gps": true + }, + "mavlink": { + "hil_sensor": true, + "hil_gps": true, + "hil_actuator_controls": true + } }, - "mavlink": { - "hil_sensor": true, - "hil_gps": true, - "hil_actuator_controls": true - } - }, - "mavlink_tx_period_msec": { - "hil_sensor": 3, - "hil_gps": 3 - }, - "location": { - "latitude": 47.641468, - "longitude": -122.140165, - "altitude": 121.321, - "magneticField": { - "intensity_nT": 53045.1, - "declination_deg": 15.306, - "inclination_deg": 68.984 + "mavlink_tx_period_msec": { + "hil_sensor": 3, + "hil_gps": 3 + }, + "location": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 121.321, + "magneticField": { + "intensity_nT": 53045.1, + "declination_deg": 15.306, + "inclination_deg": 68.984 + } } - } }, "components": { - "droneDynamics": { - "physicsEquation": "BodyFrame", - "collision_detection": true, - "enable_disturbance": false, - "manual_control": false, - "airFrictionCoefficient": [ - 0.05, - 0 - ], - "inertia": [ - 0.0061, - 0.00653, - 0.0116 - ], - "mass_kg": 0.71, - "body_size": [ - 0.1, - 0.1, - 0.01 - ], - "position_meter": [ - 0, - 0, - 0 - ], - "angle_degree": [ - 0, - 0, - 0 - ] - }, - "rotor": { - "vendor": "None", - "Tr": 0.0193, - "Kr": 27654.76291, - "rpmMax": 27654.76291 - }, - "thruster": { - "vendor": "None", - "rotorPositions": [ - { - "position": [ - 0.103, - 0.103, - 0 + "droneDynamics": { + "physicsEquation": "BodyFrame", + "collision_detection": true, + "enable_disturbance": false, + "manual_control": false, + "airFrictionCoefficient": [ + 0.05, + 0 ], - "rotationDirection": 1.0 - }, - { - "position": [ - -0.103, - -0.103, - 0 + "inertia": [ + 0.0061, + 0.00653, + 0.0116 ], - "rotationDirection": 1.0 - }, - { - "position": [ - 0.103, - -0.103, - 0 + "mass_kg": 0.71, + "body_size": [ + 0.1, + 0.1, + 0.01 ], - "rotationDirection": -1.0 - }, - { - "position": [ - -0.103, - 0.103, - 0 + "position_meter": [ + 0, + 0, + 0 ], - "rotationDirection": -1.0 - } - ], - "HoveringRpm": 13827.38146, - "parameterB": 3.0E-8, - "parameterJr": 0.0 - }, - "sensors": { - "acc": { - "sampleCount": 1, - "noise": 0.03 + "angle_degree": [ + 0, + 0, + 0 + ] }, - "gyro": { - "sampleCount": 1, - "noise": 0.0 + "battery": { + "vendor": "None", + "BatteryModelCsvFilePath": "./tmp_battery_model.csv", + "VoltageLevelGreen": 11.1, + "VoltageLevelYellow": 9.5, + "CapacityLevelYellow": 2.5, + "NominalVoltage": 14.8, + "NominalCapacity": 1000.0, + "EODVoltage": 3.0 }, - "mag": { - "sampleCount": 1, - "noise": 0.03 + "rotor": { + "vendor": "BatteryModel", + "dynamics_constants": { + "R": 0.12, + "Cq": 3.0e-8, + "K": 3.28e-3, + "D": 0.0, + "J": 8.12e-6 + } }, - "baro": { - "sampleCount": 1, - "noise": 0.01 + "thruster": { + "vendor": "None", + "rotorPositions": [ + { + "position": [ + 0.103, + 0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + -0.103, + -0.103, + 0 + ], + "rotationDirection": 1.0 + }, + { + "position": [ + 0.103, + -0.103, + 0 + ], + "rotationDirection": -1.0 + }, + { + "position": [ + -0.103, + 0.103, + 0 + ], + "rotationDirection": -1.0 + } + ], + "Ct": 8.3E-07, + "Cq": 3.0E-8, + "Jr": 0.0 }, - "gps": { - "sampleCount": 1, - "noise": 0 + "sensors": { + "acc": { + "sampleCount": 1, + "noise": 0.03 + }, + "gyro": { + "sampleCount": 1, + "noise": 0.0 + }, + "mag": { + "sampleCount": 1, + "noise": 0.03 + }, + "baro": { + "sampleCount": 1, + "noise": 0.01 + }, + "gps": { + "sampleCount": 1, + "noise": 0 + } } - } }, "controller": { "moduleDirectory": "../drone_control/cmake-build/workspace/RadioController", "moduleName": "RadioController", - "direct_rotor_control": false + "direct_rotor_control": false, + "mixer": { + "vendor": "None", + "enableDebugLog": false, + "enableErrorLog": false + } } -} +} \ No newline at end of file diff --git a/hakoniwa/drone-app.bash b/hakoniwa/drone-app.bash index 3e7edb53..70f0acb2 100644 --- a/hakoniwa/drone-app.bash +++ b/hakoniwa/drone-app.bash @@ -24,7 +24,7 @@ fi if [ -z "$HAKO_CONTROLLER_PARAM_FILE" ] then grep moduleDirectory ${DRONE_CONFIG_PATH}/*.json | grep Flight > /dev/null - export HAKO_CONTROLLER_PARAM_FILE=../drone_control/config/param-api.txt + export HAKO_CONTROLLER_PARAM_FILE=../drone_control/config/param-api-mixer.txt fi if [ -z $HAKO_MASTER_DISABLE ] From 73bd30255edc30bc25819b4ee69f7e2364d2be57 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Fri, 15 Nov 2024 07:50:12 +0900 Subject: [PATCH 48/50] removed old config --- drone_control/config/param-api.txt | 80 ------------------------------ 1 file changed, 80 deletions(-) delete mode 100644 drone_control/config/param-api.txt diff --git a/drone_control/config/param-api.txt b/drone_control/config/param-api.txt deleted file mode 100644 index b61eafa5..00000000 --- a/drone_control/config/param-api.txt +++ /dev/null @@ -1,80 +0,0 @@ -# 基本パラメータ -SIMULATION_DELTA_TIME 0.003 -MASS 0.71 -GRAVITY 9.81 - -# 高度制御 -PID_ALT_CONTROL_CYCLE 0.0 -PID_ALT_MAX_POWER 9.81 -PID_ALT_THROTTLE_GAIN 1.0 -PID_ALT_MAX_SPD 10.0 - -## 高度制御のPIDパラメータ -PID_ALT_Kp 10.0 -PID_ALT_Ki 0.1 -PID_ALT_Kd 1.5 -PID_ALT_SPD_Kp 5.0 -PID_ALT_SPD_Ki 0.1 -PID_ALT_SPD_Kd 0.1 - -# 水平制御 -POS_CONTROL_CYCLE 0.0 -SPD_CONTROL_CYCLE 0.0 -PID_POS_MAX_SPD 20.0 - -## 水平位置制御のPIDパラメータ -PID_POS_X_Kp 1.0 -PID_POS_X_Ki 0.000 -PID_POS_X_Kd 0.6 -PID_POS_Y_Kp 1.0 -PID_POS_Y_Ki 0.0 -PID_POS_Y_Kd 0.6 - -# 水平速度制御のPIDパラメータ -PID_POS_VX_Kp 20.0 -PID_POS_VX_Ki 0.0 -PID_POS_VX_Kd 0.1 -PID_POS_VY_Kp 20.0 -PID_POS_VY_Ki 0.0 -PID_POS_VY_Kd 0.1 - -# 姿勢角制御 -HEAD_CONTROL_CYCLE 0.0 -ANGULAR_CONTROL_CYCLE 0.0 -ANGULAR_RATE_CONTROL_CYCLE 0.0 -PID_POS_MAX_ROLL 10.0 -PID_POS_MAX_PITCH 10.0 -PID_ROLL_RPM_MAX 20.0 -PID_PITCH_RPM_MAX 20.0 -PID_ROLL_TORQUE_MAX 10.0 -PID_PITCH_TORQUE_MAX 10.0 -PID_YAW_TORQUE_MAX 10.0 - -## ロール角度とロール角速度制御のPIDパラメータ -PID_ROLL_Kp 4.0 -PID_ROLL_Ki 0.0 -PID_ROLL_Kd 0.0 -PID_ROLL_RATE_Kp 2.0 -PID_ROLL_RATE_Ki 0.0 -PID_ROLL_RATE_Kd 0.0 - -## ピッチ角度とピッチ角速度制御のPIDパラメータ -PID_PITCH_Kp 4.0 -PID_PITCH_Ki 0.0 -PID_PITCH_Kd 0.0 -PID_PITCH_RATE_Kp 2.0 -PID_PITCH_RATE_Ki 0.0 -PID_PITCH_RATE_Kd 0.0 - -## ヨー角度とヨー角速度制御のPIDパラメータ -PID_YAW_RPM_MAX 10.0 -PID_YAW_Kp 1.1 -PID_YAW_Ki 0.0 -PID_YAW_Kd 0.0 -PID_YAW_RATE_Kp 1.1 -PID_YAW_RATE_Ki 0.0 -PID_YAW_RATE_Kd 0.1 - -# ラジオコントロール -YAW_DELTA_VALUE_DEG 0.2 -ALT_DELTA_VALUE_M 0.01 From ebbbec17f13bafd96f67ae32ae05f735bd50efd5 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Fri, 15 Nov 2024 09:14:06 +0900 Subject: [PATCH 49/50] control model is changed for education --- drone_control/common/include/drone_alt_controller.hpp | 3 ++- drone_control/config/param-api-mixer.txt | 8 ++++---- hakoniwa/config/api_sample/drone_config_0.json | 6 +++--- hakoniwa/config/rc-kanko/drone_config_0.json | 6 +++--- hakoniwa/config/rc/drone_config_0.json | 6 +++--- 5 files changed, 15 insertions(+), 14 deletions(-) diff --git a/drone_control/common/include/drone_alt_controller.hpp b/drone_control/common/include/drone_alt_controller.hpp index a330a7ac..5e396dfc 100644 --- a/drone_control/common/include/drone_alt_controller.hpp +++ b/drone_control/common/include/drone_alt_controller.hpp @@ -103,7 +103,8 @@ class DroneAltController { /* * thrust */ - out.thrust = (mass * gravity) + (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 index 7a7a9223..bccc069c 100644 --- a/drone_control/config/param-api-mixer.txt +++ b/drone_control/config/param-api-mixer.txt @@ -7,15 +7,15 @@ GRAVITY 9.81 PID_ALT_CONTROL_CYCLE 0.0 PID_ALT_MAX_POWER 9.81 PID_ALT_THROTTLE_GAIN 1.0 -PID_ALT_MAX_SPD 10.0 +PID_ALT_MAX_SPD 5.0 ## 高度制御のPIDパラメータ PID_ALT_Kp 5.0 -PID_ALT_Ki 0.1 -PID_ALT_Kd 1.0 +PID_ALT_Ki 0.3 +PID_ALT_Kd 5.0 PID_ALT_SPD_Kp 5.0 PID_ALT_SPD_Ki 0.0 -PID_ALT_SPD_Kd 1.0 +PID_ALT_SPD_Kd 5.0 # 水平制御 POS_CONTROL_CYCLE 0.0 diff --git a/hakoniwa/config/api_sample/drone_config_0.json b/hakoniwa/config/api_sample/drone_config_0.json index 571443be..99b5c23f 100644 --- a/hakoniwa/config/api_sample/drone_config_0.json +++ b/hakoniwa/config/api_sample/drone_config_0.json @@ -68,9 +68,9 @@ "battery": { "vendor": "None", "BatteryModelCsvFilePath": "./tmp_battery_model.csv", - "VoltageLevelGreen": 11.1, - "VoltageLevelYellow": 9.5, - "CapacityLevelYellow": 2.5, + "VoltageLevelGreen": 12.1, + "VoltageLevelYellow": 11.1, + "CapacityLevelYellow": 900, "NominalVoltage": 14.8, "NominalCapacity": 1000.0, "EODVoltage": 3.0 diff --git a/hakoniwa/config/rc-kanko/drone_config_0.json b/hakoniwa/config/rc-kanko/drone_config_0.json index 20faefea..f8afb9c7 100644 --- a/hakoniwa/config/rc-kanko/drone_config_0.json +++ b/hakoniwa/config/rc-kanko/drone_config_0.json @@ -64,9 +64,9 @@ "battery": { "vendor": "None", "BatteryModelCsvFilePath": "./tmp_battery_model.csv", - "VoltageLevelGreen": 11.1, - "VoltageLevelYellow": 9.5, - "CapacityLevelYellow": 2.5, + "VoltageLevelGreen": 12.1, + "VoltageLevelYellow": 11.1, + "CapacityLevelYellow": 900, "NominalVoltage": 14.8, "NominalCapacity": 1000.0, "EODVoltage": 3.0 diff --git a/hakoniwa/config/rc/drone_config_0.json b/hakoniwa/config/rc/drone_config_0.json index 0b674fef..c1f61094 100644 --- a/hakoniwa/config/rc/drone_config_0.json +++ b/hakoniwa/config/rc/drone_config_0.json @@ -68,9 +68,9 @@ "battery": { "vendor": "None", "BatteryModelCsvFilePath": "./tmp_battery_model.csv", - "VoltageLevelGreen": 11.1, - "VoltageLevelYellow": 9.5, - "CapacityLevelYellow": 2.5, + "VoltageLevelGreen": 12.1, + "VoltageLevelYellow": 11.1, + "CapacityLevelYellow": 900, "NominalVoltage": 14.8, "NominalCapacity": 1000.0, "EODVoltage": 3.0 From 171a7bdfc38b1f8d3caeec9df7f5a7a3ab919852 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Fri, 15 Nov 2024 11:20:53 +0900 Subject: [PATCH 50/50] fix #416 --- .../config/api_sample/drone_config_0.json | 1 + hakoniwa/config/drone_config_0.json | 1 + hakoniwa/config/rc-kanko/drone_config_0.json | 1 + hakoniwa/config/rc/drone_config_0.json | 1 + .../physics/battery/battery_dynamics.hpp | 26 ++++++++++++++++--- hakoniwa/src/config/drone_config.hpp | 6 +++++ hakoniwa/src/config/drone_config_types.hpp | 6 +++++ 7 files changed, 38 insertions(+), 4 deletions(-) diff --git a/hakoniwa/config/api_sample/drone_config_0.json b/hakoniwa/config/api_sample/drone_config_0.json index 99b5c23f..494672f5 100644 --- a/hakoniwa/config/api_sample/drone_config_0.json +++ b/hakoniwa/config/api_sample/drone_config_0.json @@ -67,6 +67,7 @@ }, "battery": { "vendor": "None", + "model": "constant", "BatteryModelCsvFilePath": "./tmp_battery_model.csv", "VoltageLevelGreen": 12.1, "VoltageLevelYellow": 11.1, diff --git a/hakoniwa/config/drone_config_0.json b/hakoniwa/config/drone_config_0.json index 17e62d8f..e2ded898 100644 --- a/hakoniwa/config/drone_config_0.json +++ b/hakoniwa/config/drone_config_0.json @@ -67,6 +67,7 @@ }, "battery": { "vendor": "None", + "model": "constant", "BatteryModelCsvFilePath": "./tmp_battery_model.csv", "VoltageLevelGreen": 11.1, "VoltageLevelYellow": 9.5, diff --git a/hakoniwa/config/rc-kanko/drone_config_0.json b/hakoniwa/config/rc-kanko/drone_config_0.json index f8afb9c7..0826bdc3 100644 --- a/hakoniwa/config/rc-kanko/drone_config_0.json +++ b/hakoniwa/config/rc-kanko/drone_config_0.json @@ -63,6 +63,7 @@ }, "battery": { "vendor": "None", + "model": "constant", "BatteryModelCsvFilePath": "./tmp_battery_model.csv", "VoltageLevelGreen": 12.1, "VoltageLevelYellow": 11.1, diff --git a/hakoniwa/config/rc/drone_config_0.json b/hakoniwa/config/rc/drone_config_0.json index c1f61094..8db73053 100644 --- a/hakoniwa/config/rc/drone_config_0.json +++ b/hakoniwa/config/rc/drone_config_0.json @@ -67,6 +67,7 @@ }, "battery": { "vendor": "None", + "model": "constant", "BatteryModelCsvFilePath": "./tmp_battery_model.csv", "VoltageLevelGreen": 12.1, "VoltageLevelYellow": 11.1, diff --git a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp index abccd2e6..5d81b09e 100644 --- a/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp +++ b/hakoniwa/src/assets/drone/physics/battery/battery_dynamics.hpp @@ -28,6 +28,7 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double discharge_current; double delta_time_sec; bool is_battery_model_enabled; + bool is_battery_mode_constant; // バッテリーの放電特性を決める要因データ // インデックス番号は、要因IDとして扱う std::vector discharge_factors; @@ -188,6 +189,10 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs double interpolatedVoltage = interpolateVoltage(lower, upper, discharged_capacity); this->current_charge_voltage = interpolatedVoltage; } + void run_constant_model() + { + this->current_charge_voltage = this->params.NominalVoltage; + } void run_linear_model() { double discharged_capacity = this->discharge_capacity_hour; @@ -222,10 +227,18 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs this->discharge_current = 0; this->discharge_capacity_hour = 0; this->is_battery_model_enabled = false; + this->is_battery_mode_constant = false; } void set_params(const BatteryModelParameters &p) override { this->params = p; + if (params.model == "constant") { + this->is_battery_mode_constant = true; + } + else { + this->is_battery_mode_constant = false; + } + if (params.BatteryModelCsvFilePath.empty()) { std::cout << "BatteryModelCsvFilePath is empty." << std::endl; } @@ -249,12 +262,17 @@ class BatteryDynamics : public hako::assets::drone::IBatteryDynamics, public ICs void run() override { - run_discharged_capacity(); - if (!this->is_battery_model_enabled) { - run_linear_model(); + if (this->is_battery_mode_constant) { + run_constant_model(); } else { - run_battery_model(); + run_discharged_capacity(); + if (!this->is_battery_model_enabled) { + run_linear_model(); + } + else { + run_battery_model(); + } } } double get_vbat() override diff --git a/hakoniwa/src/config/drone_config.hpp b/hakoniwa/src/config/drone_config.hpp index 79c4d9ec..2a0f4931 100644 --- a/hakoniwa/src/config/drone_config.hpp +++ b/hakoniwa/src/config/drone_config.hpp @@ -241,6 +241,12 @@ class DroneConfig { if (configJson["components"]["battery"].contains("BatteryModelCsvFilePath")) { params.BatteryModelCsvFilePath = configJson["components"]["battery"]["BatteryModelCsvFilePath"].get(); } + if (configJson["components"]["battery"].contains("model")) { + params.model = configJson["components"]["battery"]["model"].get(); + } + else { + params.model = "constant"; + } params.VoltageLevelGreen = configJson["components"]["battery"]["VoltageLevelGreen"].get(); params.VoltageLevelYellow = configJson["components"]["battery"]["VoltageLevelYellow"].get(); params.NominalVoltage = configJson["components"]["battery"]["NominalVoltage"].get(); diff --git a/hakoniwa/src/config/drone_config_types.hpp b/hakoniwa/src/config/drone_config_types.hpp index fa8b15d3..6457700e 100644 --- a/hakoniwa/src/config/drone_config_types.hpp +++ b/hakoniwa/src/config/drone_config_types.hpp @@ -20,6 +20,12 @@ namespace hako::assets::drone { struct BatteryModelParameters { std::string vendor; std::string BatteryModelCsvFilePath; + /* + * constant: バッテリ電圧が一定のモデル + * linear: リニアモデル + * custom: CSVファイルで指定するモデル + */ + std::string model; /* * 公称容量: [Ah] * バッテリーが供給できる理論上の電気量