From 9c141e4310e9e3a8367f38b35bf81e0ec3ad690a Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Sat, 4 May 2024 20:26:57 +0800
Subject: [PATCH] =?UTF-8?q?=E5=8F=96=E6=B6=88=E7=A6=BB=E5=9C=B0=E6=97=B6?=
 =?UTF-8?q?=E5=AF=B9=E9=80=9F=E5=BA=A6=E9=97=AD=E7=8E=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/lqr_calc.h | 42 ++++++++++++++++------------------
 1 file changed, 20 insertions(+), 22 deletions(-)

diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h
index aad3c90..dede5b7 100644
--- a/application/chassis/lqr_calc.h
+++ b/application/chassis/lqr_calc.h
@@ -25,29 +25,27 @@ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
     float l = p->leg_len;
     float lsqr = l * l;
 
-    for (uint8_t i = 0; i < 2; ++i)
-    {
-        uint8_t j = i * 6;
+    uint8_t i, j;
 
-        if(i == 0)      // 离地时仅对速度闭环，保证落地时轮速与机体速度一致
-        {
-            T[i] = (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) + (p->fly_flag ? 0 :
-                   ( (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
-                     (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + 
-                     (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
-                     (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
-                     (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w ));
-        }
-        else if(i == 1)     // 离地时关节输出仅保留 theta 和 theta_dot，保证滞空时腿部竖直
-        {
-            T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
-                   (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + (p->fly_flag ? 0 :
-                   ( (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
-                     (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
-                     (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
-                     (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w ));
-        }
-    }
+    // 离地时轮子输出置0
+    i = 0; j = i * 6;
+    T[i] = p->fly_flag ? 0 :
+           ((k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
+            (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + 
+            (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
+            (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
+            (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
+            (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w);
+    
+    // 离地时关节输出仅保留 theta 和 theta_dot，保证滞空时腿部竖直
+    i = 1; j = i * 6;
+    T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
+           (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + (p->fly_flag ? 0 :
+           ((k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
+            (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
+            (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
+            (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w));
+    
     p->T_wheel = T[0];
     p->T_hip = T[1];
 }
\ No newline at end of file