From 93ca37e36c4700131d73bcf732514156e7cb3a4c Mon Sep 17 00:00:00 2001
From: cyy_mac <cyy@mac.com>
Date: Wed, 12 Aug 2026 18:51:48 +0800
Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E5=96=84=E9=98=BF=E5=85=8B=E6=9B=BC?=
 =?UTF-8?q?=E6=8E=A7=E5=88=B6=E4=B8=8E=E9=AB=98=E9=80=9F=E4=B8=B2=E5=8F=A3?=
 =?UTF-8?q?=E9=81=A5=E6=B5=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
---
 .gitignore                            |   1 +
 BALANCE/balance.c                     | 646 +++++++++++++++---------
 BALANCE/balance.h                     |   9 +-
 BALANCE/robot_select_init.h           |  96 +++-
 BALANCE/show.c                        |   6 +-
 BALANCE/system.c                      | 124 ++---
 BALANCE/system.h                      |   8 +-
 HARDWARE/MPU6050/MPU6050.c            | 222 ++++----
 HARDWARE/motor.h                      |   2 +-
 HARDWARE/timer.c                      | 380 ++++++++------
 HARDWARE/timer.h                      |   2 +
 HARDWARE/usartx.c                     | 696 ++++++++++++++------------
 HARDWARE/usartx.h                     |  54 +-
 Makefile                              |   3 +-
 doc/ackermann_steering_calibration.md | 374 ++++++++++++++
 doc/uart_protocol_v2_timestamp.md     | 111 ++++
 doc/时间戳.md                         | 349 +++++++++++++
 17 files changed, 2151 insertions(+), 932 deletions(-)
 create mode 100644 doc/ackermann_steering_calibration.md
 create mode 100644 doc/uart_protocol_v2_timestamp.md
 create mode 100644 doc/时间戳.md

diff --git a/.gitignore b/.gitignore
index 880ac59..2269f9f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,3 @@
 build/
+.firecrawl/
 .DS_Store
diff --git a/BALANCE/balance.c b/BALANCE/balance.c
index ce1f598..7985c45 100644
--- a/BALANCE/balance.c
+++ b/BALANCE/balance.c
@@ -1,52 +1,108 @@
 #include "balance.h"
 
-int Time_count=0; //Time variable //��ʱ����
+int Time_count=0; //Time variable //��ʱ����
 
 // Robot mode is wrong to detect flag bits
-//������ģʽ�Ƿ��������־λ
+//������ģʽ�Ƿ��������־λ
 int robot_mode_check_flag=0; 
 
 short test_num;
 
-Encoder OriginalEncoder; //Encoder raw data //������ԭʼ����
+Encoder OriginalEncoder; //Encoder raw data //������ԭʼ����
 
-u8 command_lost_count=0; //���ڡ�CAN�������ʧʱ���������ʧ1���ֹͣ����
+u8 command_lost_count=0; //���ڡ�CAN�������ʧʱ���������ʧ1���ֹͣ����
+
+/* Calibrated Ackermann steering model (manual push-test, motors disabled).
+   Maps signed path curvature kappa = 1/R [1/m, R at the rear-axle center] to
+   servo PWM. Sign convention: kappa > 0 -> right turn, kappa < 0 -> left turn.
+   Quadratic fit of measured (servo, 1/R) points, max residual ~12 PWM:
+       servo = AKM_C0 + AKM_C1*kappa + AKM_C2*kappa*kappa
+   NOTE: kappa is in 1/m. The raw calibration table listed 1/R in 1/cm; these
+   coefficients were fit after converting R from cm to m, so they must be fed
+   SI curvature (kappa = wz/Vx, both SI). Straight-ahead lands near 1656 PWM. */
+#define AKM_SERVO_C0        1656.373f
+#define AKM_SERVO_C1         140.548f
+#define AKM_SERVO_C2          (-7.654f)
+/* Steering range from the calibration table: PWM 1100 (left) .. 2000 (right). */
+#define AKM_SERVO_MIN       1100
+#define AKM_SERVO_MAX       2000
+/* Largest curvature the car can actually track: R_min ~= 0.30 m -> 3.33 /m. */
+#define AKM_KAPPA_MAX       3.331f
+
+/* Remote CH1 neutral pulse width [us]. The servo straight-ahead neutral is
+   SERVO_INIT (motor.h). The two differ, so the CH1 passthrough is shifted by
+   (SERVO_INIT - AKM_REMOTER_CH1_MID) to keep stick-center = wheels-straight. */
+#define AKM_REMOTER_CH1_MID 1500
+
+/* Linear steering map that honors the servo's real straight-ahead neutral
+   (SERVO_INIT) instead of the arithmetic midpoint of [MIN, MAX]. norm > 0 =
+   left -> toward AKM_SERVO_MIN; norm < 0 = right -> toward AKM_SERVO_MAX;
+   norm == 0 -> SERVO_INIT. Each side is scaled to its own end stop so the full
+   mechanical travel is used even though the neutral is off-center. */
+#if AKM_DIRECT_MAP || AKM_YAW_ASSIST
+static int Akm_Norm_To_Servo(float norm)
+{
+	float span, pwm;
+
+	norm = target_limit_float(norm, -1.0f, 1.0f);
+	span = (norm >= 0.0f) ? (float)(SERVO_INIT - AKM_SERVO_MIN)
+	                      : (float)(AKM_SERVO_MAX - SERVO_INIT);
+	pwm = (float)SERVO_INIT - norm * span;
+
+	return (int)(pwm + (pwm >= 0.0f ? 0.5f : -0.5f));
+}
+#endif
+
+/* Needed by Mode 0 (calibrated Ackermann) and Mode 2 (yaw-rate assist), i.e.
+   whenever direct-map is off and CH1 is not overriding the servo. Direct-map
+   (Mode 1) and the CH1 debug override never call it. */
+#if !AKM_DIRECT_MAP && !AKM_SERVO_DEBUG_REMOTE_CH1
+static int Akm_Curvature_To_Servo(float kappa)
+{
+	float pwm;
+
+	kappa = target_limit_float(kappa, -AKM_KAPPA_MAX, AKM_KAPPA_MAX);
+	pwm = AKM_SERVO_C0 + AKM_SERVO_C1 * kappa + AKM_SERVO_C2 * kappa * kappa;
+
+	return (int)(pwm + (pwm >= 0.0f ? 0.5f : -0.5f));
+}
+#endif
 /**************************************************************************
 Function: The inverse kinematics solution is used to calculate the target speed of each wheel according to the target speed of three axes
 Input   : X and Y, Z axis direction of the target movement speed
 Output  : none
-�������ܣ��˶�ѧ��⣬��������Ŀ���ٶȼ��������Ŀ��ת��
-��ڲ�����X��Y��Z�᷽���Ŀ���˶��ٶ�
-����  ֵ����
+�������ܣ��˶�ѧ��⣬��������Ŀ���ٶȼ��������Ŀ��ת��
+��ڲ�����X��Y��Z�᷽���Ŀ���˶��ٶ�
+����  ֵ����
 **************************************************************************/
 void Drive_Motor(float Vx,float Vy,float Vz)
 {
-		float amplitude=3.5; //Wheel target speed limit //����Ŀ���ٶ��޷�
+		float amplitude=3.5; //Wheel target speed limit //����Ŀ���ٶ��޷�
 	
 	  //Speed smoothing is enabled when moving the omnidirectional trolley
-	  //ȫ���ƶ�С���ſ����ٶ�ƽ������
+	  //ȫ���ƶ�С���ſ����ٶ�ƽ������
 	  if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
 		{
-			Smooth_control(Vx,Vy,Vz); //Smoothing the input speed //�������ٶȽ���ƽ������
+			Smooth_control(Vx,Vy,Vz); //Smoothing the input speed //�������ٶȽ���ƽ������
   
       //Get the smoothed data 
-			//��ȡƽ�������������			
+			//��ȡƽ�������������			
 			Vx=smooth_control.VX;     
 			Vy=smooth_control.VY;
 			Vz=smooth_control.VZ;
 		}
 		
 		//Mecanum wheel car
-	  //�����ķ��С��
+	  //�����ķ��С��
 	  if (Car_Mode==Mec_Car) 
     {
-			//Inverse kinematics //�˶�ѧ���
+			//Inverse kinematics //�˶�ѧ���
 			MOTOR_A.Target   = +Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
 			MOTOR_B.Target   = -Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
 			MOTOR_C.Target   = +Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
 			MOTOR_D.Target   = -Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
 		
-			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
+			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
 			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
 			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
 			MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude); 
@@ -54,89 +110,204 @@ void Drive_Motor(float Vx,float Vy,float Vz)
 		} 
 		
 		//Omni car
-		//ȫ����С��
+		//ȫ����С��
 		else if (Car_Mode==Omni_Car) 
 		{
-			//Inverse kinematics //�˶�ѧ���
+			//Inverse kinematics //�˶�ѧ���
 			MOTOR_A.Target   =   Vy + Omni_turn_radiaus*Vz;
 			MOTOR_B.Target   =  -X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz;
 			MOTOR_C.Target   =  +X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz;
 		
-			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
+			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
 			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
 			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
 			MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude); 
-			MOTOR_D.Target=0;	//Out of use //û��ʹ�õ�
+			MOTOR_D.Target=0;	//Out of use //û��ʹ�õ�
 		}
 		
 		//Ackermann structure car
-		//��������
-		else if (Car_Mode==Akm_Car) 
+		//��������
+		else if (Car_Mode==Akm_Car)
 		{
-			//Ackerman car specific related variables //������С��ר����ر���
-			float R, Ratio=636.56, AngleR, Angle_Servo;
-			
-			// For Ackerman small car, Vz represents the front wheel steering Angle
-			//���ڰ�����С��Vz������ǰ��ת��Ƕ�
-			AngleR=Vz;
-			R=Axle_spacing/tan(AngleR)-0.5f*Wheel_spacing;
-			//R=Axle_spacing/tan(AngleR);
-			
-			// Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad
-			//ǰ��ת��Ƕ��޷�(�������ǰ��ת��Ƕ�)����λ��rad
-			AngleR=target_limit_float(AngleR,-0.6f,0.6f);
-			
-			//Inverse kinematics //�˶�ѧ���
-			if(AngleR!=0)
-			{
-				MOTOR_A.Target = Vx*(R-0.5f*Wheel_spacing)/R;
-				MOTOR_B.Target = Vx*(R+0.5f*Wheel_spacing)/R;			
-			}
-			else 
-			{
-				MOTOR_A.Target = Vx;
-				MOTOR_B.Target = Vx;
-			}
-			// The PWM value of the servo controls the steering Angle of the front wheel
-			//���PWMֵ���������ǰ��ת��Ƕ�
+		#if AKM_DIRECT_MAP
+			// Direct passthrough mode (tuning/debug, not physically Ackermann):
+			//   - Vz is linearly mapped across the full servo travel, independent
+			//     of speed. Vz > 0 = left (ROS) -> AKM_SERVO_MIN (left end).
+			//   - Vx is sent to both drive wheels unchanged (no differential).
+			// The AKM_SERVO_DEBUG_REMOTE_CH1 override below still takes priority
+			// over this servo value.
+			// vz_norm > 0 (left) -> AKM_SERVO_MIN; 0 -> SERVO_INIT (straight).
+			float vz_norm = target_limit_float(Vz / AKM_DIRECT_VZ_FULL, -1.0f, 1.0f);
+			Servo = Akm_Norm_To_Servo(vz_norm);
 
-			Angle_Servo    =  -0.628f*pow(AngleR, 3) + 1.269f*pow(AngleR, 2) - 1.772f*AngleR + 1.573f;
-			Servo=SERVO_INIT + (Angle_Servo - 1.572f)*Ratio;
-			//	Servo=SERVO_INIT + (Angle_Servo)*Ratio;
-			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
-			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
-			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
-			MOTOR_C.Target=0; //Out of use //û��ʹ�õ�
-			MOTOR_D.Target=0; //Out of use //û��ʹ�õ�
-			Servo=target_limit_int(Servo,800,2200);	//Servo PWM value limit //���PWMֵ�޷�
+			MOTOR_A.Target = Vx;
+			MOTOR_B.Target = Vx;
+			MOTOR_C.Target = 0;
+			MOTOR_D.Target = 0;
+		#elif AKM_YAW_ASSIST
+			// Mode 2: DECOUPLED steering + IMU yaw-rate differential assist
+			// (simplified torque vectoring). This deliberately breaks the Ackermann
+			// w-v coupling that shrinks the steering angle at speed:
+			//
+			//   * Steering (w): the servo is a DIRECT map of Vz across the full
+			//     curvature range, INDEPENDENT of Vx (like Mode 1). So a large Vz
+			//     always yields a large front-wheel angle, even at high Vx.
+			//   * Drive (v): Vx sets the base wheel speed independently.
+			//   * Ackermann is used only as the FEEDFORWARD reference for the rear
+			//     differential; an IMU yaw-rate PI loop trims on top.
+			//
+			// Sign bookkeeping (fit domain, kappa>0 = right; ROS Vz>0 = left = CCW):
+			//   vz_norm  = clamp(Vz/AKM_DIRECT_VZ_FULL, +-1)
+			//   Servo    = linear FULL-TRAVEL map of vz_norm (SAME as Mode 1 /
+			//              direct map), NOT the calibration fit -- this is what
+			//              gives full steering authority at any speed.
+			//   kappa_cmd = -vz_norm * AKM_KAPPA_MAX is used only to build the
+			//              Ackermann feedforward reference for the rear diff.
+			// The yaw loop works in the ROS/geometric frame (r>0 = left).
+			static float yaw_integral = 0.0f;   // PI integrator state [m/s]
+			static float r_filt = 0.0f;         // low-pass filtered yaw rate [rad/s]
+			float vz_norm, kappa_cmd;
+			float r_ref, r_meas, e_r, dv_ff, dv_fb, dv, dv_max;
+
+			// Decoupled steering command: Vz -> normalized steering, NOT via Vx.
+			vz_norm   = target_limit_float(Vz / AKM_DIRECT_VZ_FULL, -1.0f, 1.0f);
+
+			// Servo = linear full-travel map, identical to Mode 1 (direct map),
+			// centered on SERVO_INIT. vz_norm > 0 (left) -> AKM_SERVO_MIN.
+			Servo = Akm_Norm_To_Servo(vz_norm);
+
+			// Ackermann feedforward reference yaw rate for the commanded steering.
+			// kappa_cmd maps full stick to the car's max trackable curvature.
+			// Back in the ROS/geometric frame: r_ref > 0 = left turn.
+			kappa_cmd = -vz_norm * AKM_KAPPA_MAX;
+			r_ref = -kappa_cmd * Vx;
+
+			// Measured yaw rate from the gyro, de-biased LSB -> rad/s, with an
+			// optional sign flip and a light first-order low-pass.
+			r_meas = AKM_GYRO_Z_SIGN * (float)gyro[2] / AKM_GYRO_Z_TO_RADPS;
+			r_filt += AKM_YAW_IMU_LPF * (r_meas - r_filt);
+
+			if(float_abs(Vx) < AKM_YAW_MIN_SPEED)
+			{
+				// Too slow for a meaningful yaw rate: freeze the loop, no assist.
+				yaw_integral = 0.0f;
+				dv = 0.0f;
+			}
+			else
+			{
+				e_r = r_ref - r_filt;
+
+				// Feedforward: alpha=1 reproduces the Mode-0 geometric differential
+				// exactly (dv = 0.5*track*|kappa|*Vx expressed via r_ref).
+				dv_ff = AKM_YAW_FF_ALPHA * 0.5f * Wheel_spacing * r_ref;
+
+				// PI feedback with rectangular integration at the fixed control
+				// period (Drive_Motor runs at CONTROL_FREQUENCY Hz).
+				yaw_integral += e_r * (1.0f / (float)CONTROL_FREQUENCY);
+				dv_fb = AKM_YAW_KP * e_r + AKM_YAW_KI * yaw_integral;
+
+				dv = dv_ff + dv_fb;
+
+				// Clamp the differential to a fraction of Vx and anti-windup: if the
+				// PI part alone saturates, roll the integrator back.
+				dv_max = AKM_YAW_MAX_DIFF_RATIO * float_abs(Vx);
+				if(dv > dv_max)
+				{
+					if(AKM_YAW_KI > 0.0f)
+						yaw_integral -= (dv - dv_max) / AKM_YAW_KI;
+					dv = dv_max;
+				}
+				else if(dv < -dv_max)
+				{
+					if(AKM_YAW_KI > 0.0f)
+						yaw_integral -= (dv + dv_max) / AKM_YAW_KI;
+					dv = -dv_max;
+				}
+			}
+
+			// r_ref > 0 (left) means the left wheel is inner (slower). This matches
+			// Mode 0's MOTOR_A = Vx*(1 + 0.5*track*kappa_fit) once dv_ff is expanded,
+			// because kappa_fit = -r_ref/Vx.
+			MOTOR_A.Target = Vx - dv; // left
+			MOTOR_B.Target = Vx + dv; // right
+			MOTOR_C.Target = 0;
+			MOTOR_D.Target = 0;
+		#else
+			// Inputs: Vx = rear-axle-center linear speed [m/s],
+			//         Vz = rotation speed wz about the turn center [rad/s].
+			// Both refer to the rear-axle center -- exactly the point the servo
+			// calibration measured R against -- so the geometric curvature is
+			//   kappa_geom = wz / v = Vz / Vx = 1/R_rear   [1/m]
+			// with the ROS sign convention: Vz > 0 = CCW = left turn.
+			// The calibration table / servo fit use the opposite sign (kappa > 0
+			// = right turn), so we negate to get the fit-domain curvature:
+			//   kappa_fit = -kappa_geom  -> Vz > 0 gives kappa_fit < 0 = left.
+			float kappa_geom, kappa_fit;
+
+			if(float_abs(Vx) > 0.001f)
+				/* Vx sign is drive direction, not steering direction. */
+				kappa_geom = Vz / float_abs(Vx);
+			else
+				kappa_geom = 0.0f; // Ackermann geometry cannot steer without forward motion.
+
+			kappa_fit = target_limit_float(-kappa_geom, -AKM_KAPPA_MAX, AKM_KAPPA_MAX);
+
+			// Rear-wheel differential about the rear-axle center, expressed in the
+			// fit-domain curvature. On a right turn (kappa_fit > 0) the turn center
+			// is to the right, so the right wheel is inner (slower) and the left
+			// wheel is outer (faster):
+			//   MOTOR_A (left)  = Vx*(1 + 0.5*track*kappa_fit)
+			//   MOTOR_B (right) = Vx*(1 - 0.5*track*kappa_fit)
+			MOTOR_A.Target = Vx * (1.0f + 0.5f * Wheel_spacing * kappa_fit);
+			MOTOR_B.Target = Vx * (1.0f - 0.5f * Wheel_spacing * kappa_fit);
+
+			// The PWM value of the servo controls the steering Angle of the front wheel
+			//���PWMֵ���������ǰ��ת��Ƕ�
+			Servo=Akm_Curvature_To_Servo(kappa_fit);
+		#endif /* AKM_DIRECT_MAP */
+
+			// Servo source override: when enabled, the remote CH1 drives the servo
+			// directly, taking priority over BOTH control laws above. The remote
+			// neutral (AKM_REMOTER_CH1_MID) is shifted onto the servo straight-ahead
+			// neutral (SERVO_INIT) so stick-center = wheels-straight.
+			#if AKM_SERVO_DEBUG_REMOTE_CH1
+			Servo = Remoter_Ch1 + (SERVO_INIT - AKM_REMOTER_CH1_MID);
+			#endif
+
+			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
+			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
+			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
+			MOTOR_C.Target=0; //Out of use //û��ʹ�õ�
+			MOTOR_D.Target=0; //Out of use //û��ʹ�õ�
+			Servo=target_limit_int(Servo,AKM_SERVO_MIN,AKM_SERVO_MAX);	//Servo PWM value limit //PWMֵ޷
 		}
-		
+
 		//Differential car
-		//������
+		//������
 		else if (Car_Mode==Diff_Car) 
 		{
-			//Inverse kinematics //�˶�ѧ���
-			MOTOR_A.Target  = Vx - Vz * Wheel_spacing / 2.0f; //��������ֵ�Ŀ���ٶ�
-		  MOTOR_B.Target =  Vx + Vz * Wheel_spacing / 2.0f; //��������ֵ�Ŀ���ٶ�
+			//Inverse kinematics //�˶�ѧ���
+			MOTOR_A.Target  = Vx - Vz * Wheel_spacing / 2.0f; //��������ֵ�Ŀ���ٶ�
+		  MOTOR_B.Target =  Vx + Vz * Wheel_spacing / 2.0f; //��������ֵ�Ŀ���ٶ�
 			
-			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
+			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
 		  MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); 
 	    MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); 
-			MOTOR_C.Target=0; //Out of use //û��ʹ�õ�
-			MOTOR_D.Target=0; //Out of use //û��ʹ�õ�
+			MOTOR_C.Target=0; //Out of use //û��ʹ�õ�
+			MOTOR_D.Target=0; //Out of use //û��ʹ�õ�
 		}
 		
 		//FourWheel car
-		//������
+		//������
 		else if(Car_Mode==FourWheel_Car) 
 		{	
-			//Inverse kinematics //�˶�ѧ���
-			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //��������ֵ�Ŀ���ٶ�
-			MOTOR_B.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //��������ֵ�Ŀ���ٶ�
-			MOTOR_C.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //��������ֵ�Ŀ���ٶ�
-			MOTOR_D.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //��������ֵ�Ŀ���ٶ�
+			//Inverse kinematics //�˶�ѧ���
+			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //��������ֵ�Ŀ���ٶ�
+			MOTOR_B.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //��������ֵ�Ŀ���ٶ�
+			MOTOR_C.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //��������ֵ�Ŀ���ٶ�
+			MOTOR_D.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //��������ֵ�Ŀ���ٶ�
 					
-			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
+			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
 			MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); 
 			MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); 
 			MOTOR_C.Target=target_limit_float( MOTOR_C.Target,-amplitude,amplitude); 
@@ -144,95 +315,104 @@ void Drive_Motor(float Vx,float Vy,float Vz)
 		}
 		
 		//Tank Car
-		//�Ĵ���
+		//�Ĵ���
 		else if (Car_Mode==Tank_Car) 
 		{
-			//Inverse kinematics //�˶�ѧ���
-			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing) / 2.0f;    //��������ֵ�Ŀ���ٶ�
-		  MOTOR_B.Target =  Vx + Vz * (Wheel_spacing) / 2.0f;    //��������ֵ�Ŀ���ٶ�
+			//Inverse kinematics //�˶�ѧ���
+			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing) / 2.0f;    //��������ֵ�Ŀ���ٶ�
+		  MOTOR_B.Target =  Vx + Vz * (Wheel_spacing) / 2.0f;    //��������ֵ�Ŀ���ٶ�
 			
-			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
+			//Wheel (motor) target speed limit //����(���)Ŀ���ٶ��޷�
 		  MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); 
 	    MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); 
-			MOTOR_C.Target=0; //Out of use //û��ʹ�õ�
-			MOTOR_D.Target=0; //Out of use //û��ʹ�õ�
+			MOTOR_C.Target=0; //Out of use //û��ʹ�õ�
+			MOTOR_D.Target=0; //Out of use //û��ʹ�õ�
 		}
 }
 /**************************************************************************
 Function: FreerTOS task, core motion control task
 Input   : none
 Output  : none
-�������ܣ�FreeRTOS���񣬺����˶���������
-��ڲ�������
-����  ֵ����
+�������ܣ�FreeRTOS���񣬺����˶���������
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void Balance_task(void *pvParameters)
 { 
 	  u32 lastWakeTime = getSysTickCnt();
+	  u32 lastControlTime = lastWakeTime;
 
     while(1)
-    {	
-			// This task runs at a frequency of 100Hz (10ms control once)
-			//��������100Hz��Ƶ�����У�10ms����һ�Σ�
-			vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ)); 
+    {
+			u32 currentControlTime;
+			float controlDt;
+
+			// Run the wheel-speed control loop at 200 Hz.
+			vTaskDelayUntil(&lastWakeTime, F2T(RATE_200_HZ));
+			currentControlTime = getSysTickCnt();
+			controlDt = (float)(currentControlTime - lastControlTime) /
+				(float)configTICK_RATE_HZ;
+			lastControlTime = currentControlTime;
+			// Avoid a large integral jump if the task is stalled while debugging.
+			controlDt = target_limit_float(controlDt, 0.001f, 0.050f);
 			
 			//Time count is no longer needed after 30 seconds
-			//ʱ�������30�������Ҫ
-			if(Time_count<3000)Time_count++;
+			//ʱ�������30�������Ҫ
+			if(Time_count<(30*CONTROL_FREQUENCY))Time_count++;
 			
 			//Get the encoder data, that is, the real time wheel speed, 
 			//and convert to transposition international units
-			//��ȡ���������ݣ�������ʵʱ�ٶȣ���ת��λ���ʵ�λ
+			//��ȡ���������ݣ�������ʵʱ�ٶȣ���ת��λ���ʵ�λ
 			Get_Velocity_Form_Encoder();   
 			
-			if(Check==0) //If self-check mode is not enabled //���û�������Լ�ģʽ
+			if(Check==0) //If self-check mode is not enabled //���û�������Լ�ģʽ
 			{
-//				command_lost_count++; //���ڡ�CAN�������ʧʱ���������ʧ1���ֹͣ����
-//				if(command_lost_count>RATE_100_HZ && APP_ON_Flag==0 && Remote_ON_Flag==0 && PS2_ON_Flag==0) //����APP��PS2����ģң��ģʽ������CAN������1������3����ģʽ
+//				command_lost_count++; //���ڡ�CAN�������ʧʱ���������ʧ1���ֹͣ����
+//				if(command_lost_count>RATE_100_HZ && APP_ON_Flag==0 && Remote_ON_Flag==0 && PS2_ON_Flag==0) //����APP��PS2����ģң��ģʽ������CAN������1������3����ģʽ
 //					Move_X=0, Move_Y=0, Move_Z=0;
 
-				if      (APP_ON_Flag)      Get_RC();         //Handle the APP remote commands //����APPң������
-				else if (Remote_ON_Flag)   Remote_Control(); //Handle model aircraft remote commands //������ģң������
-				else if (PS2_ON_Flag)      PS2_control();    //Handle PS2 controller commands //����PS2�ֱ���������
+				if      (APP_ON_Flag)      Get_RC();         //Handle the APP remote commands //����APPң������
+				else if (Remote_ON_Flag)   Remote_Control(); //Handle model aircraft remote commands //������ģң������
+				else if (PS2_ON_Flag)      PS2_control();    //Handle PS2 controller commands //����PS2�ֱ���������
 				
 				//CAN, Usart 1, Usart 3, Uart5 control can directly get the three axis target speed, 
 				//without additional processing
-				//CAN������1������3(ROS)������5����ֱ�ӵõ�����Ŀ���ٶȣ�������⴦��
+				//CAN������1������3(ROS)������5����ֱ�ӵõ�����Ŀ���ٶȣ�������⴦��
 				else                      Drive_Motor(Move_X, Move_Y, Move_Z);
 				
 				//Click the user button to update the gyroscope zero
-				//�����û������������������
+				//�����û������������������
 				Key(); 
 				
 				//If there is no abnormity in the battery voltage, and the enable switch is in the ON position,
         //and the software failure flag is 0
-				//�����ص�ѹ�������쳣������ʹ�ܿ�����ON��λ����������ʧ�ܱ�־λΪ0
+				//�����ص�ѹ�������쳣������ʹ�ܿ�����ON��λ����������ʧ�ܱ�־λΪ0
 				if(Turn_Off(Voltage)==0) 
 				 { 			
            //Speed closed-loop control to calculate the PWM value of each motor, 
 					 //PWM represents the actual wheel speed					 
-					 //�ٶȱջ����Ƽ�������PWMֵ��PWM��������ʵ��ת��
-					 MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
-					 MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
-					 MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
-					 MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);
+					 //�ٶȱջ����Ƽ�������PWMֵ��PWM��������ʵ��ת��
+					 MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target, controlDt);
+					 MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target, controlDt);
+					 MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target, controlDt);
+					 MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target, controlDt);
 						 
 					 Limit_Pwm(16700);
 					 
 					 //Set different PWM control polarity according to different car models
-					 //���ݲ�ͬС���ͺ����ò�ͬ��PWM���Ƽ���
+					 //���ݲ�ͬС���ͺ����ò�ͬ��PWM���Ƽ���
 					 switch(Car_Mode)
 					 {
-							case Mec_Car:       Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Mecanum wheel car       //�����ķ��С��
-							case Omni_Car:      Set_Pwm(-MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Omni car                //ȫ����С��
-							case Akm_Car:       Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  16799,-16799 ,                   Servo); break; //Ackermann structure car //��������
-							case Diff_Car:      Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Differential car        //���ֲ���С��
-							case FourWheel_Car: Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //FourWheel car           //������ 
-							case Tank_Car:      Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Tank Car                //�Ĵ���
+							case Mec_Car:       Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Mecanum wheel car       //�����ķ��С��
+							case Omni_Car:      Set_Pwm(-MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Omni car                //ȫ����С��
+							case Akm_Car:       Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  16799,-16799 ,                   Servo); break; //Ackermann structure car //��������
+							case Diff_Car:      Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Differential car        //���ֲ���С��
+							case FourWheel_Car: Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //FourWheel car           //������ 
+							case Tank_Car:      Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Tank Car                //�Ĵ���
 					 }
 				 }
 				 //If Turn_Off(Voltage) returns to 1, the car is not allowed to move, and the PWM value is set to 0
-				 //���Turn_Off(Voltage)����ֵΪ1������������С�������˶���PWMֵ����Ϊ0
+				 //���Turn_Off(Voltage)����ֵΪ1������������С�������˶���PWMֵ����Ϊ0
 				 else	Set_Pwm(0,0,0,0,(Car_Mode == Akm_Car) ? SERVO_INIT : 0);
 			 }	
 		 }  
@@ -241,35 +421,35 @@ void Balance_task(void *pvParameters)
 Function: Assign a value to the PWM register to control wheel speed and direction
 Input   : PWM
 Output  : none
-�������ܣ���ֵ��PWM�Ĵ��������Ƴ���ת���뷽��
-��ڲ�����PWM
-����  ֵ����
+�������ܣ���ֵ��PWM�Ĵ��������Ƴ���ת���뷽��
+��ڲ�����PWM
+����  ֵ����
 **************************************************************************/
 void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo)
 {
 	//Forward and reverse control of motor
-	//�������ת����
+	//�������ת����
 	if(motor_a<0)			PWMA1=16799,PWMA2=16799+motor_a;
 	else 	            PWMA2=16799,PWMA1=16799-motor_a;
 	
 	//Forward and reverse control of motor
-	//�������ת����	
+	//�������ת����	
 	if(motor_b<0)			PWMB1=16799,PWMB2=16799+motor_b;
 	else 	            PWMB2=16799,PWMB1=16799-motor_b;
 //  PWMB1=10000,PWMB2=5000;
 
 	//Forward and reverse control of motor
-	//�������ת����	
+	//�������ת����	
 	if(motor_c<0)			PWMC1=16799,PWMC2=16799+motor_c;
 	else 	            PWMC2=16799,PWMC1=16799-motor_c;
 	
 	//Forward and reverse control of motor
-	//�������ת����
+	//�������ת����
 	if(motor_d<0)			PWMD1=16799,PWMD2=16799+motor_d;
 	else 	            PWMD2=16799,PWMD1=16799-motor_d;
 	
 	//Servo control
-	//�������
+	//�������
 	Servo_PWM =servo;
 }
 
@@ -277,9 +457,9 @@ void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo)
 Function: Limit PWM value
 Input   : Value
 Output  : none
-�������ܣ�����PWMֵ 
-��ڲ�������ֵ
-����  ֵ����
+�������ܣ�����PWMֵ 
+��ڲ�������ֵ
+����  ֵ����
 **************************************************************************/
 void Limit_Pwm(int amplitude)
 {	
@@ -292,9 +472,9 @@ void Limit_Pwm(int amplitude)
 Function: Limiting function
 Input   : Value
 Output  : none
-�������ܣ��޷�����
-��ڲ�������ֵ
-����  ֵ����
+�������ܣ��޷�����
+��ڲ�������ֵ
+����  ֵ����
 **************************************************************************/
 float target_limit_float(float insert,float low,float high)
 {
@@ -318,9 +498,9 @@ int target_limit_int(int insert,int low,int high)
 Function: Check the battery voltage, enable switch status, software failure flag status
 Input   : Voltage
 Output  : Whether control is allowed, 1: not allowed, 0 allowed
-�������ܣ�����ص�ѹ��ʹ�ܿ���״̬������ʧ�ܱ�־λ״̬
-��ڲ�������ѹ
-����  ֵ���Ƿ��������ƣ�1����������0����
+�������ܣ�����ص�ѹ��ʹ�ܿ���״̬������ʧ�ܱ�־λ״̬
+��ڲ�������ѹ
+����  ֵ���Ƿ��������ƣ�1����������0����
 **************************************************************************/
 u8 Turn_Off( int voltage)
 {
@@ -341,9 +521,9 @@ u8 Turn_Off( int voltage)
 Function: Calculate absolute value
 Input   : long int
 Output  : unsigned int
-�������ܣ������ֵ
-��ڲ�����long int
-����  ֵ��unsigned int
+�������ܣ������ֵ
+��ڲ�����long int
+����  ֵ��unsigned int
 **************************************************************************/
 u32 myabs(long int a)
 { 		   
@@ -357,78 +537,78 @@ Function: Incremental PI controller
 Input   : Encoder measured value (actual speed), target speed
 Output  : Motor PWM
 According to the incremental discrete PID formula
-pwm+=Kp[e��k��-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
+pwm+=Kp[e��k��-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
 e(k) represents the current deviation
 e(k-1) is the last deviation and so on
 PWM stands for incremental output
 In our speed control closed loop system, only PI control is used
-pwm+=Kp[e��k��-e(k-1)]+Ki*e(k)
+pwm+=Kp[e��k��-e(k-1)]+Ki*e(k)*dt
 
-�������ܣ�����ʽPI������
-��ڲ���������������ֵ(ʵ���ٶ�)��Ŀ���ٶ�
-����  ֵ�����PWM
-��������ʽ��ɢPID��ʽ 
-pwm+=Kp[e��k��-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
-e(k)��������ƫ�� 
-e(k-1)������һ�ε�ƫ��  �Դ����� 
-pwm�����������
-�����ǵ��ٶȿ��Ʊջ�ϵͳ���棬ֻʹ��PI����
-pwm+=Kp[e��k��-e(k-1)]+Ki*e(k)
+�������ܣ�����ʽPI������
+��ڲ���������������ֵ(ʵ���ٶ�)��Ŀ���ٶ�
+����  ֵ�����PWM
+��������ʽ��ɢPID��ʽ 
+pwm+=Kp[e��k��-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
+e(k)��������ƫ�� 
+e(k-1)������һ�ε�ƫ��  �Դ����� 
+pwm�����������
+�����ǵ��ٶȿ��Ʊջ�ϵͳ���棬ֻʹ��PI����
+pwm+=Kp[e��k��-e(k-1)]+Ki*e(k)*dt
 **************************************************************************/
-int Incremental_PI_A (float Encoder,float Target)
+int Incremental_PI_A (float Encoder,float Target,float dt)
 { 	
 	 static float Bias,Pwm,Last_bias;
-	 Bias=Target-Encoder; //Calculate the deviation //����ƫ��
-	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; 
+	 Bias=Target-Encoder; //Calculate the deviation //����ƫ��
+	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
 	 if(Pwm>16700)Pwm=16700;
 	 if(Pwm<-16700)Pwm=-16700;
-	 Last_bias=Bias; //Save the last deviation //������һ��ƫ�� 
+	 Last_bias=Bias; //Save the last deviation //������һ��ƫ�� 
 	 return Pwm;    
 }
-int Incremental_PI_B (float Encoder,float Target)
+int Incremental_PI_B (float Encoder,float Target,float dt)
 {  
 	 static float Bias,Pwm,Last_bias;
-	 Bias=Target-Encoder; //Calculate the deviation //����ƫ��
-	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;  
+	 Bias=Target-Encoder; //Calculate the deviation //����ƫ��
+	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
 	 if(Pwm>16700)Pwm=16700;
 	 if(Pwm<-16700)Pwm=-16700;
-	 Last_bias=Bias; //Save the last deviation //������һ��ƫ�� 
+	 Last_bias=Bias; //Save the last deviation //������һ��ƫ�� 
 	 return Pwm;
 }
-int Incremental_PI_C (float Encoder,float Target)
+int Incremental_PI_C (float Encoder,float Target,float dt)
 {  
 	 static float Bias,Pwm,Last_bias;
-	 Bias=Target-Encoder; //Calculate the deviation //����ƫ��
-	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; 
+	 Bias=Target-Encoder; //Calculate the deviation //����ƫ��
+	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
 	 if(Pwm>16700)Pwm=16700;
 	 if(Pwm<-16700)Pwm=-16700;
-	 Last_bias=Bias; //Save the last deviation //������һ��ƫ�� 
+	 Last_bias=Bias; //Save the last deviation //������һ��ƫ�� 
 	 return Pwm; 
 }
-int Incremental_PI_D (float Encoder,float Target)
+int Incremental_PI_D (float Encoder,float Target,float dt)
 {  
 	 static float Bias,Pwm,Last_bias;
-	 Bias=Target-Encoder; //Calculate the deviation //����ƫ��
-	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;  
+	 Bias=Target-Encoder; //Calculate the deviation //����ƫ��
+	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
 	 if(Pwm>16700)Pwm=16700;
 	 if(Pwm<-16700)Pwm=-16700;
-	 Last_bias=Bias; //Save the last deviation //������һ��ƫ�� 
+	 Last_bias=Bias; //Save the last deviation //������һ��ƫ�� 
 	 return Pwm; 
 }
 /**************************************************************************
 Function: Processes the command sent by APP through usart 2
 Input   : none
 Output  : none
-�������ܣ���APPͨ������2���͹�����������д���
-��ڲ�������
-����  ֵ����
+�������ܣ���APPͨ������2���͹�����������д���
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void Get_RC(void)
 {
 	u8 Flag_Move=1;
-	if(Car_Mode==Mec_Car||Car_Mode==Omni_Car) //The omnidirectional wheel moving trolley can move laterally //ȫ�����˶�С�����Խ��к����ƶ�
+	if(Car_Mode==Mec_Car||Car_Mode==Omni_Car) //The omnidirectional wheel moving trolley can move laterally //ȫ�����˶�С�����Խ��к����ƶ�
 	{
-	 switch(Flag_Direction)  //Handle direction control commands //���������������
+	 switch(Flag_Direction)  //Handle direction control commands //���������������
 	 { 
 			case 1:      Move_X=RC_Velocity;  	 Move_Y=0;             Flag_Move=1;    break;
 			case 2:      Move_X=RC_Velocity;  	 Move_Y=-RC_Velocity;  Flag_Move=1; 	 break;
@@ -443,15 +623,15 @@ void Get_RC(void)
 	 if(Flag_Move==0)		
 	 {	
 		 //If no direction control instruction is available, check the steering control status
-		 //����޷������ָ����ת�����״̬
-		 if     (Flag_Left ==1)  Move_Z= PI/2*(RC_Velocity/500); //left rotation  //����ת  
-		 else if(Flag_Right==1)  Move_Z=-PI/2*(RC_Velocity/500); //right rotation //����ת
+		 //����޷������ָ����ת�����״̬
+		 if     (Flag_Left ==1)  Move_Z= PI/2*(RC_Velocity/500); //left rotation  //����ת  
+		 else if(Flag_Right==1)  Move_Z=-PI/2*(RC_Velocity/500); //right rotation //����ת
 		 else 		               Move_Z=0;                       //stop           //ֹͣ
 	 }
 	}	
-	else //Non-omnidirectional moving trolley //��ȫ���ƶ�С��
+	else //Non-omnidirectional moving trolley //��ȫ���ƶ�С��
 	{
-	 switch(Flag_Direction) //Handle direction control commands //���������������
+	 switch(Flag_Direction) //Handle direction control commands //���������������
 	 { 
 			case 1:      Move_X=+RC_Velocity;  	 Move_Z=0;         break;
 			case 2:      Move_X=+RC_Velocity;  	 Move_Z=-PI/2;   	 break;
@@ -463,29 +643,29 @@ void Get_RC(void)
 			case 8:      Move_X=+RC_Velocity; 	 Move_Z=+PI/2;     break; 
 			default:     Move_X=0;               Move_Z=0;         break;
 	 }
-	 if     (Flag_Left ==1)  Move_Z= PI/2; //left rotation  //����ת 
-	 else if(Flag_Right==1)  Move_Z=-PI/2; //right rotation //����ת	
+	 if     (Flag_Left ==1)  Move_Z= PI/2; //left rotation  //����ת 
+	 else if(Flag_Right==1)  Move_Z=-PI/2; //right rotation //����ת	
 	}
 	
-	//Z-axis data conversion //Z������ת��
+	//Z-axis data conversion //Z������ת��
 	if(Car_Mode==Akm_Car)
 	{
 		//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
-		//�������ṹС��ת��Ϊǰ��ת��Ƕ�
+		//�������ṹС��ת��Ϊǰ��ת��Ƕ�
 		Move_Z=Move_Z*2/9; 
 	}
 	else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
 	{
-	  if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //���ٿ���ԭ��ϵ����Ҫ�˴���
+	  if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //���ٿ���ԭ��ϵ����Ҫ�˴���
 		Move_Z=Move_Z*RC_Velocity/500;
 	}		
 	
 	//Unit conversion, mm/s -> m/s
-  //��λת����mm/s -> m/s	
+  //��λת����mm/s -> m/s	
 	Move_X=Move_X/1000;       Move_Y=Move_Y/1000;         Move_Z=Move_Z;
 	
 	//Control target value is obtained and kinematics analysis is performed
-	//�õ�����Ŀ��ֵ�������˶�ѧ����
+	//�õ�����Ŀ��ֵ�������˶�ѧ����
 	Drive_Motor(Move_X,Move_Y,Move_Z);
 }
 
@@ -493,38 +673,38 @@ void Get_RC(void)
 Function: Handle PS2 controller control commands
 Input   : none
 Output  : none
-�������ܣ���PS2�ֱ�����������д���
-��ڲ�������
-����  ֵ����
+�������ܣ���PS2�ֱ�����������д���
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void PS2_control(void)
 {
    	int LX,LY,RY;
-		int Threshold=20; //Threshold to ignore small movements of the joystick //��ֵ������ҡ��С���ȶ���
+		int Threshold=20; //Threshold to ignore small movements of the joystick //��ֵ������ҡ��С���ȶ���
 			
 	  //128 is the median.The definition of X and Y in the PS2 coordinate system is different from that in the ROS coordinate system
-	  //128Ϊ��ֵ��PS2����ϵ��ROS����ϵ��X��Y�Ķ��岻һ��
+	  //128Ϊ��ֵ��PS2����ϵ��ROS����ϵ��X��Y�Ķ��岻һ��
 		LY=-(PS2_LX-128);  
 		LX=-(PS2_LY-128); 
 		RY=-(PS2_RX-128); 
 	
-	  //Ignore small movements of the joystick //����ҡ��С���ȶ���
+	  //Ignore small movements of the joystick //����ҡ��С���ȶ���
 		if(LX>-Threshold&&LX<Threshold)LX=0; 
 		if(LY>-Threshold&&LY<Threshold)LY=0; 
 		if(RY>-Threshold&&RY<Threshold)RY=0; 
 	
-	  if (PS2_KEY==11)		RC_Velocity+=5;  //To accelerate//����
-	  else if(PS2_KEY==9)	RC_Velocity-=5;  //To slow down //����	
+	  if (PS2_KEY==11)		RC_Velocity+=5;  //To accelerate//����
+	  else if(PS2_KEY==9)	RC_Velocity-=5;  //To slow down //����	
 	
 		if(RC_Velocity<0)   RC_Velocity=0;
 	
 	  //Handle PS2 controller control commands
-	  //��PS2�ֱ�����������д���
+	  //��PS2�ֱ�����������д���
 		Move_X=LX*RC_Velocity/128; 
 		Move_Y=LY*RC_Velocity/128; 
 	  Move_Z=RY*(PI/2)/128;      
 	
-	  //Z-axis data conversion //Z������ת��
+	  //Z-axis data conversion //Z������ת��
 	  if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
 		{
 			Move_Z=Move_Z*RC_Velocity/500;
@@ -532,23 +712,23 @@ void PS2_control(void)
 		else if(Car_Mode==Akm_Car)
 		{
 			//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
-		  //�������ṹС��ת��Ϊǰ��ת��Ƕ�
+		  //�������ṹС��ת��Ϊǰ��ת��Ƕ�
 			Move_Z=Move_Z*2/9;
 		}
 		else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
 		{
-			if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //���ٿ���ԭ��ϵ����Ҫ�˴���
+			if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //���ٿ���ԭ��ϵ����Ҫ�˴���
 			Move_Z=Move_Z*RC_Velocity/500;
 		}	
 		 
 	  //Unit conversion, mm/s -> m/s
-    //��λת����mm/s -> m/s	
+    //��λת����mm/s -> m/s	
 		Move_X=Move_X/1000;        
 		Move_Y=Move_Y/1000;    
 		Move_Z=Move_Z;
 		
 		//Control target value is obtained and kinematics analysis is performed
-	  //�õ�����Ŀ��ֵ�������˶�ѧ����
+	  //�õ�����Ŀ��ֵ�������˶�ѧ����
 		Drive_Motor(Move_X,Move_Y,Move_Z);		 			
 } 
 
@@ -556,18 +736,18 @@ void PS2_control(void)
 Function: The remote control command of model aircraft is processed
 Input   : none
 Output  : none
-�������ܣ��Ժ�ģң�ؿ���������д���
-��ڲ�������
-����  ֵ����
+�������ܣ��Ժ�ģң�ؿ���������д���
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void Remote_Control(void)
 {
 	  //Data within 1 second after entering the model control mode will not be processed
-	  //�Խ��뺽ģ����ģʽ��1���ڵ����ݲ�����
-    static u8 thrice=100; 
-    int Threshold=100; //Threshold to ignore small movements of the joystick //��ֵ������ҡ��С���ȶ���
+	  //�Խ��뺽ģ����ģʽ��1���ڵ����ݲ�����
+    static u8 thrice=CONTROL_FREQUENCY;
+    int Threshold=100; //Threshold to ignore small movements of the joystick //��ֵ������ҡ��С���ȶ���
 
-	  //limiter //�޷�
+	  //limiter //�޷�
     int LX,LY,RY,RX,Remote_RCvelocity; 
 		Remoter_Ch1=target_limit_int(Remoter_Ch1,1000,2000);
 		Remoter_Ch2=target_limit_int(Remoter_Ch2,1000,2000);
@@ -575,20 +755,20 @@ void Remote_Control(void)
 		Remoter_Ch4=target_limit_int(Remoter_Ch4,1000,2000);
 
 	  // Front and back direction of left rocker. Control forward and backward.
-	  //��ҡ��ǰ���򡣿���ǰ�����ˡ�
+	  //��ҡ��ǰ���򡣿���ǰ�����ˡ�
     LX=Remoter_Ch2-1500; 
 	
 	  //Left joystick left and right.Control left and right movement. Only the wheelie omnidirectional wheelie will use the channel.
 	  //Ackerman trolleys use this channel as a PWM output to control the steering gear
-	  //��ҡ�����ҷ��򡣿��������ƶ�������ȫ���ֲŻ�ʹ�õ���ͨ����������С��ʹ�ø�ͨ����ΪPWM������ƶ��
+	  //��ҡ�����ҷ��򡣿��������ƶ�������ȫ���ֲŻ�ʹ�õ���ͨ����������С��ʹ�ø�ͨ����ΪPWM������ƶ��
     LY=Remoter_Ch4-1500;
 
     //Front and back direction of right rocker. Throttle/acceleration/deceleration.
-		//��ҡ��ǰ��������/�Ӽ��١�
+		//��ҡ��ǰ��������/�Ӽ��١�
 	  RX=Remoter_Ch3-1500;
 
     //Right stick left and right. To control the rotation. 
-		//��ҡ�����ҷ��򡣿�����ת��
+		//��ҡ�����ҷ��򡣿�����ת��
     RY=Remoter_Ch1-1500; 
 
     if(LX>-Threshold&&LX<Threshold)LX=0;
@@ -596,17 +776,17 @@ void Remote_Control(void)
     if(RX>-Threshold&&RX<Threshold)RX=0;
 	  if(RY>-Threshold&&RY<Threshold)RY=0;
 		
-		//Throttle related //�������
+		//Throttle related //�������
 		Remote_RCvelocity=RC_Velocity+RX;
 	  if(Remote_RCvelocity<0)Remote_RCvelocity=0;
 		
 		//The remote control command of model aircraft is processed
-		//�Ժ�ģң�ؿ���������д���
+		//�Ժ�ģң�ؿ���������д���
     Move_X= LX*Remote_RCvelocity/500; 
 		Move_Y=-LY*Remote_RCvelocity/500;
 		Move_Z=-RY*(PI/2)/500;      
 			 
-		//Z������ת��
+		//Z������ת��
 	  if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
 		{
 			Move_Z=Move_Z*Remote_RCvelocity/500;
@@ -614,36 +794,36 @@ void Remote_Control(void)
 		else if(Car_Mode==Akm_Car)
 		{
 			//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
-		  //�������ṹС��ת��Ϊǰ��ת��Ƕ�
+		  //�������ṹС��ת��Ϊǰ��ת��Ƕ�
 			Move_Z=Move_Z*2/9;
 		}
 		else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
 		{
-			if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //���ٿ���ԭ��ϵ����Ҫ�˴���
+			if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //���ٿ���ԭ��ϵ����Ҫ�˴���
 			Move_Z=Move_Z*Remote_RCvelocity/500;
 		}
 		
 	  //Unit conversion, mm/s -> m/s
-    //��λת����mm/s -> m/s	
+    //��λת����mm/s -> m/s	
 		Move_X=Move_X/1000;       
     Move_Y=Move_Y/1000;      
 		Move_Z=Move_Z;
 		
 	  //Data within 1 second after entering the model control mode will not be processed
-	  //�Խ��뺽ģ����ģʽ��1���ڵ����ݲ�����
+	  //�Խ��뺽ģ����ģʽ��1���ڵ����ݲ�����
     if(thrice>0) Move_X=0,Move_Z=0,thrice--;
 				
 		//Control target value is obtained and kinematics analysis is performed
-	  //�õ�����Ŀ��ֵ�������˶�ѧ����
+	  //�õ�����Ŀ��ֵ�������˶�ѧ����
 		Drive_Motor(Move_X,Move_Y,Move_Z);
 }
 /**************************************************************************
 Function: Click the user button to update gyroscope zero
 Input   : none
 Output  : none
-�������ܣ������û������������������
-��ڲ�������
-����  ֵ����
+�������ܣ������û������������������
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void Key(void)
 {	
@@ -655,24 +835,28 @@ void Key(void)
 Function: Read the encoder value and calculate the wheel speed, unit m/s
 Input   : none
 Output  : none
-�������ܣ���ȡ��������ֵ�����㳵���ٶȣ���λm/s
-��ڲ�������
-����  ֵ����
+�������ܣ���ȡ��������ֵ�����㳵���ٶȣ���λm/s
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void Get_Velocity_Form_Encoder(void)
 {
 	  //Retrieves the original data of the encoder
-	  //��ȡ��������ԭʼ����
-		float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr; 
-		OriginalEncoder.A=Read_Encoder(2);	
-		OriginalEncoder.B=Read_Encoder(3);	
-		OriginalEncoder.C=Read_Encoder(4);	
-		OriginalEncoder.D=Read_Encoder(5);	
+	  //��ȡ��������ԭʼ����
+		float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr;
+		//Stamp the moment the encoder counters are latched, as close to the
+		//real sample instant as possible.
+		//�ڶ�ȡ��������������˲���ʱ��������������ʵ�ɼ�ʱ�̡�
+		g_speed_sample_time_us = mcu_time_us();
+		OriginalEncoder.A=Read_Encoder(2);
+		OriginalEncoder.B=Read_Encoder(3);
+		OriginalEncoder.C=Read_Encoder(4);
+		OriginalEncoder.D=Read_Encoder(5);
 
 	//test_num=OriginalEncoder.B;
 	
 	  //Decide the encoder numerical polarity according to different car models
-		//���ݲ�ͬС���ͺž�����������ֵ����
+		//���ݲ�ͬС���ͺž�����������ֵ����
 		switch(Car_Mode)
 		{
 			case Mec_Car:       Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C;  Encoder_D_pr=-OriginalEncoder.D; break; 
@@ -684,7 +868,7 @@ void Get_Velocity_Form_Encoder(void)
 		}
 		
 		//The encoder converts the raw data to wheel speed in m/s
-		//������ԭʼ����ת��Ϊ�����ٶȣ���λm/s
+		//������ԭʼ����ת��Ϊ�����ٶȣ���λm/s
 		MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
 		MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
 		MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; 
@@ -694,9 +878,9 @@ void Get_Velocity_Form_Encoder(void)
 Function: Smoothing the three axis target velocity
 Input   : Three-axis target velocity
 Output  : none
-�������ܣ�������Ŀ���ٶ���ƽ������
-��ڲ���������Ŀ���ٶ�
-����  ֵ����
+�������ܣ�������Ŀ���ٶ���ƽ������
+��ڲ���������Ŀ���ٶ�
+����  ֵ����
 **************************************************************************/
 void Smooth_control(float vx,float vy,float vz)
 {
@@ -722,9 +906,9 @@ void Smooth_control(float vx,float vy,float vz)
 Function: Floating-point data calculates the absolute value
 Input   : float
 Output  : The absolute value of the input number
-�������ܣ����������ݼ������ֵ
-��ڲ�����������
-����  ֵ���������ľ���ֵ
+�������ܣ����������ݼ������ֵ
+��ڲ�����������
+����  ֵ���������ľ���ֵ
 **************************************************************************/
 float float_abs(float insert)
 {
@@ -735,9 +919,9 @@ float float_abs(float insert)
 Function: Prevent the potentiometer to choose the wrong mode, resulting in initialization error caused by the motor spinning.Out of service
 Input   : none
 Output  : none
-�������ܣ���ֹ��λ��ѡ��ģʽ�����³�ʼ���������������ת����ֹͣʹ��
-��ڲ�������
-����  ֵ����
+�������ܣ���ֹ��λ��ѡ��ģʽ�����³�ʼ���������������ת����ֹͣʹ��
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void robot_mode_check(void)
 {
@@ -745,6 +929,6 @@ void robot_mode_check(void)
 
 	if(abs(MOTOR_A.Motor_Pwm)>2500||abs(MOTOR_B.Motor_Pwm)>2500||abs(MOTOR_C.Motor_Pwm)>2500||abs(MOTOR_D.Motor_Pwm)>2500)   error++;
 	//If the output is close to full amplitude for 6 times in a row, it is judged that the motor rotates wildly and makes the motor incapacitated
-	//�������6�νӽ�����������ж�Ϊ�����ת���õ��ʧ��	
+	//�������6�νӽ�����������ж�Ϊ�����ת���õ��ʧ��	
 	if(error>6) EN=0,Flag_Stop=1,robot_mode_check_flag=1;  
 }
diff --git a/BALANCE/balance.h b/BALANCE/balance.h
index eb7c51b..fa9b60b 100644
--- a/BALANCE/balance.h
+++ b/BALANCE/balance.h
@@ -22,10 +22,10 @@ float target_limit_float(float insert,float low,float high);
 int target_limit_int(int insert,int low,int high);
 u8 Turn_Off( int voltage);
 u32 myabs(long int a);
-int Incremental_PI_A (float Encoder,float Target);
-int Incremental_PI_B (float Encoder,float Target);
-int Incremental_PI_C (float Encoder,float Target);
-int Incremental_PI_D (float Encoder,float Target);
+int Incremental_PI_A (float Encoder,float Target,float dt);
+int Incremental_PI_B (float Encoder,float Target,float dt);
+int Incremental_PI_C (float Encoder,float Target,float dt);
+int Incremental_PI_D (float Encoder,float Target,float dt);
 void Get_RC(void);
 void Remote_Control(void);
 void Drive_Motor(float Vx,float Vy,float Vz);
@@ -36,4 +36,3 @@ void PS2_control(void);
 float float_abs(float insert);
 void robot_mode_check(void);
 #endif  
-
diff --git a/BALANCE/robot_select_init.h b/BALANCE/robot_select_init.h
index 4f66e6b..c551eed 100644
--- a/BALANCE/robot_select_init.h
+++ b/BALANCE/robot_select_init.h
@@ -4,19 +4,19 @@
 #include "system.h"
 
 //Parameter structure of robot
-//�����˲����ṹ��
+//�����˲����ṹ��
 typedef struct  
 {
-  float WheelSpacing;      //Wheelspacing, Mec_Car is half wheelspacing //�־� ���ֳ�Ϊ���־�
-  float AxleSpacing;       //Axlespacing, Mec_Car is half axlespacing //��� ���ֳ�Ϊ�����	
-  int GearRatio;           //Motor_gear_ratio //������ٱ�
-  int EncoderAccuracy;     //Number_of_encoder_lines //����������(����������)
-  float WheelDiameter;     //Diameter of driving wheel //������ֱ��	
-  float OmniTurnRadiaus;   //Rotation radius of omnidirectional trolley //ȫ����С����ת�뾶	
+  float WheelSpacing;      //Wheelspacing, Mec_Car is half wheelspacing //�־� ���ֳ�Ϊ���־�
+  float AxleSpacing;       //Axlespacing, Mec_Car is half axlespacing //��� ���ֳ�Ϊ�����	
+  int GearRatio;           //Motor_gear_ratio //������ٱ�
+  int EncoderAccuracy;     //Number_of_encoder_lines //����������(����������)
+  float WheelDiameter;     //Diameter of driving wheel //������ֱ��	
+  float OmniTurnRadiaus;   //Rotation radius of omnidirectional trolley //ȫ����С����ת�뾶	
 }Robot_Parament_InitTypeDef;
 
 // Encoder structure
-//�������ṹ��
+//�������ṹ��
 typedef struct  
 {
   int A;      
@@ -27,28 +27,74 @@ typedef struct
 
 //The minimum turning radius of Ackermann models is determined by the mechanical structure: 
 //the maximum Angle of the wheelbase, wheelbase and front wheels
-//���������͵���Сת��뾶���ɻ�е�ṹ�������־ࡢ��ࡢǰ�����ת��
+//���������͵���Сת��뾶���ɻ�е�ṹ�������־ࡢ��ࡢǰ�����ת��
 #define MINI_AKM_MIN_TURN_RADIUS 0.350f 
 
 //Wheelspacing, Mec_Car is half wheelspacing
-//�־� ������һ��
+//�־� ������һ��
 //#define MEC_wheelspacing         0.109
-#define MEC_wheelspacing         0.0930 //����2021.03.30
-#define Akm_wheelspacing         0.162f
+#define MEC_wheelspacing         0.0930 //����2021.03.30
+#define Akm_wheelspacing         0.160f
 #define Diff_wheelSpacing        0.177f
 #define Four_Mortor_wheelSpacing 0.26f
 #define Tank_wheelSpacing        0.235f
 
 //Axlespacing, Mec_Car is half axlespacing
-//��� ������һ��
+//��� ������һ��
 #define MEC_axlespacing           0.085
-#define Akm_axlespacing           0.158f
+#define Akm_axlespacing           0.160f
+// Set to 1 to drive the Ackermann servo directly from TIM8 channel 1 (bench debug).
+// Set to 0 to use the calibrated curvature->servo model in balance.c.
+#define AKM_SERVO_DEBUG_REMOTE_CH1 0
+
+// Ackermann control law selector (mutually exclusive with the debug switch above):
+//   0 = calibrated kinematic model: kappa = wz/Vx -> quadratic servo fit,
+//       rear wheels get Ackermann differential. Physically correct.
+//   1 = direct passthrough (tuning/debug): Vz is linearly mapped across the
+//       full servo travel, Vx is sent to both drive wheels unchanged (no
+//       differential, no curvature math). Handy for isolating servo/motor.
+#define AKM_DIRECT_MAP 0
+// Direct-map input span: |Vz| >= AKM_DIRECT_VZ_FULL maps to the servo end stop.
+// Vz > 0 = left (ROS), which maps toward AKM_SERVO_MIN (left end).
+// TUNING KNOB (Mode 1 & Mode 2 share it): set this to the MAX angular.z [rad/s]
+// your commander actually sends, so a full stick/command uses the full servo
+// travel. Too high -> steering stays small; too low -> servo saturates (always
+// full lock) and loses proportional control. Vz arrives in rad/s (usartx.c
+// XYZ_Target_Speed_transition: raw/1000).
+#define AKM_DIRECT_VZ_FULL 1.0f
+
+// Ackermann yaw-rate closed-loop assist (Mode 2), mutually exclusive with
+// AKM_DIRECT_MAP (direct-map wins if both are 1). "Front wheel does the main
+// steering, rear wheels add a yaw-rate differential" -- a simplified torque-
+// vectoring / yaw-rate closed loop:
+//   path -> (Vx, kappa_cmd) -> servo main steering (calibrated fit)
+//                            + IMU yaw-rate PI differential on the rear wheels.
+// Degenerates EXACTLY to the Mode-0 Ackermann differential when
+// AKM_YAW_FF_ALPHA = 1 and AKM_YAW_KP = AKM_YAW_KI = 0.
+#define AKM_YAW_ASSIST 1
+// PI gains on the yaw-rate error e_r = r_ref - r_imu [rad/s], output in m/s.
+#define AKM_YAW_KP             0.10f
+#define AKM_YAW_KI             0.00f
+// Feedforward blend: 0 = pure IMU feedback, 1 = full geometric differential.
+#define AKM_YAW_FF_ALPHA       0.00f
+// Below this |Vx| the yaw loop is frozen (integrator reset, no differential).
+#define AKM_YAW_MIN_SPEED      0.10f
+// |dv| clamp as a fraction of |Vx|, so the differential cannot stall a wheel.
+#define AKM_YAW_MAX_DIFF_RATIO 0.35f
+// gyro[2] LSB -> rad/s at FS +-500 dps (see MPU6050.c: FS_500 -> /3754.9).
+#define AKM_GYRO_Z_TO_RADPS    3754.9f
+// Light first-order low-pass on the measured yaw rate (0 = none, 1 = no filter
+// lag). r_f += beta*(r_meas - r_f). ~0.3 gives gentle smoothing at 200 Hz.
+#define AKM_YAW_IMU_LPF        0.30f
+// Flip to -1.0f if the IMU +z spins opposite to the ROS convention (+ = left).
+// MUST be verified on hardware: command a left turn and confirm gyro[2] > 0.
+#define AKM_GYRO_Z_SIGN        (+1.0f)
 #define Diff_axlespacing          0.155f
 #define Four_Mortor__axlespacing  0.28f
 #define Tank_axlespacing          0.222f
 
 //Motor_gear_ratio
-//������ٱ�
+//������ٱ�
 #define   HALL_30F    30
 #define   HALL_60F    60
 #define   MD36N_5_18  5.18
@@ -59,12 +105,12 @@ typedef struct
 #define   MD60N_47    47
 
 //Number_of_encoder_lines
-//����������
+//����������
 #define		Photoelectric_500 500
 #define	  Hall_13           13
 
 //Mecanum wheel tire diameter series
-//������ֱ̥��
+//������ֱ̥��
 #define		Mecanum_60  0.060f
 #define		Mecanum_75  0.075f
 #define		Mecanum_100 0.100f
@@ -72,7 +118,7 @@ typedef struct
 #define		Mecanum_152 0.152f
  
 //Omni wheel tire diameter series
-//�־�ȫ����ֱ��ϵ��
+//�־�ȫ����ֱ��ϵ��
 #define	  FullDirecion_60  0.060
 #define	  FullDirecion_75  0.075
 #define	  FullDirecion_127 0.127
@@ -81,26 +127,26 @@ typedef struct
 #define	  FullDirecion_217 0.217
 
 //Black tire, tank_car wheel diameter
-//��ɫ��̥���Ĵ�����ֱ��
+//��ɫ��̥���Ĵ�����ֱ��
 #define	  Black_WheelDiameter   0.065
 //#define	  Tank_WheelDiameter 0.047
 #define	  Tank_WheelDiameter 0.043
 
 //Rotation radius of omnidirectional trolley
-//ȫ����С����ת�뾶
+//ȫ����С����ת�뾶
 #define   Omni_Turn_Radiaus_109 0.109
 #define   Omni_Turn_Radiaus_164 0.164
 #define   Omni_Turn_Radiaus_180 0.180
 #define   Omni_Turn_Radiaus_290 0.290
 
 //The encoder octave depends on the encoder initialization Settings
-//��������Ƶ����ȡ���ڱ�������ʼ������
+//��������Ƶ����ȡ���ڱ�������ʼ������
 #define   EncoderMultiples  4
-//Encoder data reading frequency
-//���������ݶ�ȡƵ��
-#define   CONTROL_FREQUENCY 100
+//Wheel-speed control and encoder reading frequency
+//���ٿ������������ȡƵ��
+#define   CONTROL_FREQUENCY 200
 
-//#define PI 3.1415f  //PI //Բ����
+//#define PI 3.1415f  //PI //Բ����
 
 void Robot_Select(void);
 void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter);
diff --git a/BALANCE/show.c b/BALANCE/show.c
index 4f130ac..3f41d4c 100644
--- a/BALANCE/show.c
+++ b/BALANCE/show.c
@@ -26,8 +26,8 @@ void show_task(void *pvParameters)
 		
 		//����ʱ���������ݷ�������������
 		//The buzzer will beep briefly when the machine is switched on
-		if(Time_count<50)Buzzer=1; 
-		else if(Time_count>=51 && Time_count<100)Buzzer=0;
+		if(Time_count<(CONTROL_FREQUENCY/2))Buzzer=1;
+		else if(Time_count>=(CONTROL_FREQUENCY/2) && Time_count<CONTROL_FREQUENCY)Buzzer=0;
 		 
 		if(LowVoltage_1==1 || LowVoltage_2==1)Buzzer_count=0;
 		if(Buzzer_count<5)Buzzer_count++;
@@ -340,5 +340,3 @@ void APP_Show(void)
 		 printf("{B%d:%d:%d}$",(int)gyro[0],(int)gyro[1],(int)gyro[2]);
 	 }
 }
-
-
diff --git a/BALANCE/system.c b/BALANCE/system.c
index 743385e..0ac6039 100644
--- a/BALANCE/system.c
+++ b/BALANCE/system.c
@@ -1,197 +1,203 @@
 #include "system.h"
 
 //Robot software fails to flag bits
-//����������ʧ�ܱ�־λ
+//����������ʧ�ܱ�־λ
 u8 Flag_Stop=1;   
 
 //The ADC value is variable in segments, depending on the number of car models. Currently there are 6 car models
-//ADCֵ�ֶα�����ȡ����С���ͺ�������Ŀǰ��6��С���ͺ�
+//ADCֵ�ֶα�����ȡ����С���ͺ�������Ŀǰ��6��С���ͺ�
 int Divisor_Mode;
 
 // Robot type variable
-//�������ͺű���
-//0=Mec_Car��1=Omni_Car��2=Akm_Car��3=Diff_Car��4=FourWheel_Car��5=Tank_Car
+//�������ͺű���
+//0=Mec_Car��1=Omni_Car��2=Akm_Car��3=Diff_Car��4=FourWheel_Car��5=Tank_Car
 u8 Car_Mode=4; 
 
 //Servo control PWM value, Ackerman car special
-//�������PWMֵ��������С��ר��
+//�������PWMֵ��������С��ר��
 int Servo = SERVO_INIT;
 
 //Default speed of remote control car, unit: mm/s
-//ң��С����Ĭ���ٶȣ���λ��mm/s
+//ң��С����Ĭ���ٶȣ���λ��mm/s
 float RC_Velocity=500; 
 
 //Vehicle three-axis target moving speed, unit: m/s
-//С������Ŀ���˶��ٶȣ���λ��m/s
+//С������Ŀ���˶��ٶȣ���λ��m/s
 float Move_X, Move_Y, Move_Z;   
 
-//PID parameters of Speed control
-//�ٶȿ���PID����
-float Velocity_KP=300,Velocity_KI=800; 
+//Speed PI parameters: Ki is in PWM/(m/s*s) and multiplies measured dt.
+//�ٶ�PI������Ki��λΪPWM/(m/s*s)����ʵ��dt���
+float Velocity_KP=300,Velocity_KI=80000;
 
 //Smooth control of intermediate variables, dedicated to omni-directional moving cars
-//ƽ�������м������ȫ���ƶ�С��ר��
+//ƽ�������м������ȫ���ƶ�С��ר��
 Smooth_Control smooth_control;  
 
 //The parameter structure of the motor
-//����IJ����ṹ��
+//����IJ����ṹ��
 Motor_parameter MOTOR_A,MOTOR_B,MOTOR_C,MOTOR_D;  
 
-/************ С���ͺ���ر��� **************************/
+/************ С���ͺ���ر��� **************************/
 /************ Variables related to car model ************/
 //Encoder accuracy
-//����������
+//����������
 float Encoder_precision; 
 //Wheel circumference, unit: m
-//�����ܳ�����λ��m
+//�����ܳ�����λ��m
 float Wheel_perimeter; 
 //Drive wheel base, unit: m
-//�������־࣬��λ��m
+//�������־࣬��λ��m
 float Wheel_spacing; 
 //The wheelbase of the front and rear axles of the trolley, unit: m
-//С��ǰ�������࣬��λ��m
+//С��ǰ�������࣬��λ��m
 float Axle_spacing; 
 //All-directional wheel turning radius, unit: m
-//ȫ����ת��뾶����λ��m
+//ȫ����ת��뾶����λ��m
 float Omni_turn_radiaus; 
-/************ С���ͺ���ر��� **************************/
+/************ С���ͺ���ر��� **************************/
 /************ Variables related to car model ************/
 
 //PS2 controller, Bluetooth APP, aircraft model controller, CAN communication, serial port 1, serial port 5 communication control flag bit.
 //These 6 flag bits are all 0 by default, representing the serial port 3 control mode
-//PS2�ֱ�������APP����ģ�ֱ���CANͨ�š�����1������5ͨ�ſ��Ʊ�־λ����6����־λĬ�϶�Ϊ0����������3����ģʽ
+//PS2�ֱ�������APP����ģ�ֱ���CANͨ�š�����1������5ͨ�ſ��Ʊ�־λ����6����־λĬ�϶�Ϊ0����������3����ģʽ
 u8 PS2_ON_Flag=0, APP_ON_Flag=0, Remote_ON_Flag=0, CAN_ON_Flag=0, Usart1_ON_Flag, Usart5_ON_Flag; 
 
 //Bluetooth remote control associated flag bits
-//����ң����صı�־λ
+//����ң����صı�־λ
 u8 Flag_Left, Flag_Right, Flag_Direction=0, Turn_Flag; 
 
 //Sends the parameter's flag bit to the Bluetooth APP
-//������APP���Ͳ����ı�־λ
+//������APP���Ͳ����ı�־λ
 u8 PID_Send; 
 
 //The PS2 gamepad controls related variables
-//PS2�ֱ�������ر���
+//PS2�ֱ�������ر���
 float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY; 
 
 //Self-check the relevant flag variables
-//�Լ���ر�־����
+//�Լ���ر�־����
 int Check=0, Checking=0, Checked=0, CheckCount=0, CheckPhrase1=0, CheckPhrase2=0; 
 
 //Check the result code
-//�Լ�������
+//�Լ�������
 long int ErrorCode=0; 
 
 void systemInit(void)
 {       
 	
 //	//Interrupt priority group setti  ng
-//	//�ж����ȼ���������
+//	//�ж����ȼ���������
 	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
 //	
 //	//Delay function initialization
-//	//��ʱ������ʼ��
+//	//��ʱ������ʼ��
 	delay_init(168);	
 	
 	//Initialize the hardware interface connected to the LED lamp
-	//��ʼ����LED�����ӵ�Ӳ���ӿ�
+	//��ʼ����LED�����ӵ�Ӳ���ӿ�
 	LED_Init();                     
 	    
   //Initialize the hardware interface connected to the buzzer	
-  //��ʼ������������ӵ�Ӳ���ӿ�
+  //��ʼ������������ӵ�Ӳ���ӿ�
 	Buzzer_Init();  
 	
 	//Initialize the hardware interface connected to the enable switch
-	//��ʼ����ʹ�ܿ������ӵ�Ӳ���ӿ�
+	//��ʼ����ʹ�ܿ������ӵ�Ӳ���ӿ�
 	Enable_Pin();
 
   //Initialize the hardware interface connected to the OLED display
-  //��ʼ����OLED��ʾ�����ӵ�Ӳ���ӿ�	
+  //��ʼ����OLED��ʾ�����ӵ�Ӳ���ӿ�	
 	OLED_Init();     
 	
 	//Initialize the hardware interface connected to the user's key
-	//��ʼ�����û��������ӵ�Ӳ���ӿ�
+	//��ʼ�����û��������ӵ�Ӳ���ӿ�
 	KEY_Init();	
 	
 	//Serial port 1 initialization, communication baud rate 115200, 
 	//can be used to communicate with ROS terminal
-	//����1��ʼ����ͨ�Ų�����115200����������ROS��ͨ��
+	//����1��ʼ����ͨ�Ų�����115200����������ROS��ͨ��
 	uart1_init(115200);	  
 	
 	//Serial port 2 initialization, communication baud rate 9600, 
 	//used to communicate with Bluetooth APP terminal
-	//����2��ʼ����ͨ�Ų�����9600������������APP��ͨ��
+	//����2��ʼ����ͨ�Ų�����9600������������APP��ͨ��
 	uart2_init(9600);  
 	
-	//Serial port 3 is initialized and the baud rate is 115200. 
-	//Serial port 3 is the default port used to communicate with ROS terminal
-	//����3��ʼ����ͨ�Ų�����115200������3ΪĬ��������ROS��ͨ�ŵĴ���
-	uart3_init(115200);
+	//Serial port 3 is initialized and the baud rate is 921600.
+	//Serial port 3 is the default port used to communicate with ROS terminal.
+	//NOTE: the ROS side must open this port at 921600 to match.
+	//����3��ʼ����ͨ�Ų�����921600������3ΪĬ��������ROS��ͨ�ŵĴ��ڡ�
+	//ע�⣺ROS �˴򿪴���ʱ������ͬ���� 921600��������ǰ��벻ƥ�䡣
+	uart3_init(921600);
 	
 	//Serial port 5 initialization, communication baud rate 115200, 
 	//can be used to communicate with ROS terminal
-	//����5��ʼ����ͨ�Ų�����115200����������ROS��ͨ��
+	//����5��ʼ����ͨ�Ų�����115200����������ROS��ͨ��
 	uart5_init(115200);
 
 	//ADC pin initialization, used to read the battery voltage and potentiometer gear, 
 	//potentiometer gear determines the car after the boot of the car model
-	//ADC���ų�ʼ�������ڶ�ȡ��ص�ѹ���λ����λ����λ����λ����С���������С�������ͺ�
+	//ADC���ų�ʼ�������ڶ�ȡ��ص�ѹ���λ����λ����λ����λ����С���������С�������ͺ�
  	Adc_Init();  
 	Adc_POWER_Init();
 	
 	//Initialize the CAN communication interface
-  //CANͨ�Žӿڳ�ʼ��
+  //CANͨ�Žӿڳ�ʼ��
 	CAN1_Mode_Init(1,7,6,3,0); 
 	
   //According to the tap position of the potentiometer, determine which type of car needs to be matched, 
   //and then initialize the corresponding parameters	
-  //���ݵ�λ���ĵ�λ�ж���Ҫ���������һ���ͺŵ�С����Ȼ����ж�Ӧ�IJ�����ʼ��	
+  //���ݵ�λ���ĵ�λ�ж���Ҫ���������һ���ͺŵ�С����Ȼ����ж�Ӧ�IJ�����ʼ��	
 	Robot_Select();                 
 	
 	 //Encoder A is initialized to read the real time speed of motor C  
-  //������A��ʼ�������ڶ�ȡ���C��ʵʱ�ٶ�	
+  //������A��ʼ�������ڶ�ȡ���C��ʵʱ�ٶ�	
 	 Encoder_Init_TIM2();
 	//Encoder B is initialized to read the real time speed of motor D
-  //������B��ʼ�������ڶ�ȡ���D��ʵʱ�ٶ�	
+  //������B��ʼ�������ڶ�ȡ���D��ʵʱ�ٶ�	
 	  Encoder_Init_TIM3();   
 	//Encoder C is initialized to read the real time speed of motor B
-  //������C��ʼ�������ڶ�ȡ���B��ʵʱ�ٶ�	
+  //������C��ʼ�������ڶ�ȡ���B��ʵʱ�ٶ�	
 	  Encoder_Init_TIM4(); 
 	//Encoder D is initialized to read the real time speed of motor A
-	//������D��ʼ�������ڶ�ȡ���A��ʵʱ�ٶ�
+	//������D��ʼ�������ڶ�ȡ���A��ʵʱ�ٶ�
 		Encoder_Init_TIM5(); 
 	
-	//��ʱ��12���������PWM�ӿ�
-		TIM12_SERVO_Init(9999,84-1);  //APB1��ʱ��Ƶ��Ϊ84M , Ƶ��=84M/((9999+1)*(83+1))=100Hz
+	//��ʱ��12���������PWM�ӿ�
+		TIM12_SERVO_Init(9999,84-1);  //APB1��ʱ��Ƶ��Ϊ84M , Ƶ��=84M/((9999+1)*(83+1))=100Hz
 		
-		//��ͨС��Ĭ�϶�ʱ��8������ģ�ӿ�
-   // TIM8_SERVO_Init(9999,168-1);//APB2��ʱ��Ƶ��Ϊ168M , Ƶ��=168M/((9999+1)*(167+1))=100Hz
-	 //Initialize the model remote control interface		
-	 //��ʼ����ģң�ؽӿ�
-	 TIM8_Cap_Init(9999,168-1);  //�߼���ʱ��TIM8��ʱ��Ƶ��Ϊ168M    
+		//��ͨС��Ĭ�϶�ʱ��8������ģ�ӿ�
+   // TIM8_SERVO_Init(9999,168-1);//APB2��ʱ��Ƶ��Ϊ168M , Ƶ��=168M/((9999+1)*(167+1))=100Hz
+	 //Initialize the model remote control interface
+	 //��ʼ����ģң�ؽӿ�
+	 TIM8_Cap_Init(9999,168-1);  //�߼���ʱ��TIM8��ʱ��Ƶ��Ϊ168M
+
+	 //Free-running 1 MHz microsecond time base for sensor sample timestamps.
+	 //��������������ʱ���� 1MHz ΢�뼶����ʱ��׼��
+	 TIM7_Init();
 
   //Initialize motor speed control and, for controlling motor speed, PWM frequency 10kHz
-  //��ʼ������ٶȿ����Լ������ڿ��Ƶ���ٶȣ�PWMƵ��10KHZ
-  //APB2ʱ��Ƶ��Ϊ168M����PWMΪ16799��Ƶ��=168M/((16799+1)*(0+1))=10k
+  //��ʼ������ٶȿ����Լ������ڿ��Ƶ���ٶȣ�PWMƵ��10KHZ
+  //APB2ʱ��Ƶ��Ϊ168M����PWMΪ16799��Ƶ��=168M/((16799+1)*(0+1))=10k
 		TIM1_PWM_Init(16799,0);
 		TIM9_PWM_Init(16799,0);
 		TIM10_PWM_Init(16799,0);
 		TIM11_PWM_Init(16799,0);
 		
   //IIC initialization for MPU6050 
-  //IIC��ʼ��������MPU6050  
+  //IIC��ʼ��������MPU6050  
 	I2C_GPIOInit();
 
   //MPU6050  is initialized to read the vehicle's three-axis attitude, 
 	//three-axis angular velocity and three-axis acceleration information
-  //MPU6050 ��ʼ�������ڶ�ȡС��������̬��������ٶȡ�������ٶ���Ϣ
+  //MPU6050 ��ʼ�������ڶ�ȡС��������̬��������ٶȡ�������ٶ���Ϣ
    MPU6050_initialize();        		
 	
 	//Initialize the hardware interface to the PS2 controller
-	//��ʼ����PS2�ֱ����ӵ�Ӳ���ӿ�
+	//��ʼ����PS2�ֱ����ӵ�Ӳ���ӿ�
 	PS2_Init();
 	
 	//PS2 gamepad configuration is initialized and configured in analog mode
-  //PS2�ֱ����ó�ʼ��,����Ϊģ����ģʽ	
+  //PS2�ֱ����ó�ʼ��,����Ϊģ����ģʽ	
 	PS2_SetInit();		 							
 }
diff --git a/BALANCE/system.h b/BALANCE/system.h
index eda2a55..9da1a9a 100644
--- a/BALANCE/system.h
+++ b/BALANCE/system.h
@@ -94,9 +94,11 @@ extern long int ErrorCode;
 void systemInit(void);
 
 /***Macros define***/ /***�궨��***/
-//After starting the car (1000/100Hz =10) for seconds, it is allowed to control the car to move
-//����(1000/100hz=10)������������С�������˶�
-#define CONTROL_DELAY		1000
+//Collect IMU zero-bias samples for 2 seconds at 100 Hz before enabling control.
+//��������100Hz�ɼ�2��IMU��ƫ������������С���˶�
+#define CONTROL_DELAY		200
+//Balance_task counter needed to represent the same 2 second startup delay.
+#define CONTROL_STARTUP_DELAY_COUNT (2 * CONTROL_FREQUENCY)
 //The number of robot types to determine the value of Divisor_Mode. There are currently 6 car types
 //�������ͺ�����������Divisor_Mode��ֵ��Ŀǰ��6��С������
 #define CAR_NUMBER    6      
diff --git a/HARDWARE/MPU6050/MPU6050.c b/HARDWARE/MPU6050/MPU6050.c
index 9bb2332..2701e57 100644
--- a/HARDWARE/MPU6050/MPU6050.c
+++ b/HARDWARE/MPU6050/MPU6050.c
@@ -13,14 +13,14 @@
 #define FLASH_MEM_START ((void*)0x1800)
 #define q30  1073741824.0f
 short gyro[3], accel[3], sensors;
-//���Ư�Ƽ���
+//���Ư�Ƽ���
 int Deviation_Count;
 
 short sum_gyro[3];
 short sum_accel[3];
 
 // Gyro static error, raw data
-//�����Ǿ��ԭʼ����
+//�����Ǿ��ԭʼ����
 short Deviation_gyro[3],Original_gyro[3];  
 short Deviation_accel[3],Original_accel[3]; 
 float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
@@ -55,11 +55,11 @@ void MPU6050_task(void *pvParameters)
     while(1)
     {	
 			//This task runs at 100Hz
-			//��������100Hz��Ƶ������
+			//��������100Hz��Ƶ������
 			vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));	
 		
 			//Read the gyroscope zero before starting
-      //����ǰ����ȡ���������			
+      //����ǰ����ȡ���������			
 		  if(Deviation_Count<CONTROL_DELAY)
 		  {	 
 		  	Deviation_Count++;
@@ -78,10 +78,14 @@ void MPU6050_task(void *pvParameters)
 				memcpy(Deviation_accel,accel,sizeof(accel));	
 		  }		
 
-     MPU_Get_Gyroscope(); //�õ�����������
-     MPU_Get_Accelscope(); //��ü��ٶȼ�ֵ(ԭʼֵ)
+     //Stamp just before the I2C burst read, as close to the physical IMU
+     //sample as we can get without a data-ready interrupt.
+     //�� I2C ��ȡǰ��㣬�ھ��� DRDY �жϵ�����¾����ӽ���������ʱ�̡�
+     g_imu_sample_time_us = mcu_time_us();
+     MPU_Get_Gyroscope(); //�õ�����������
+     MPU_Get_Accelscope(); //��ü��ٶȼ�ֵ(ԭʼֵ)
     }
-}  
+}
 
 //static  unsigned short inv_orientation_matrix_to_scalar(
 //    const signed char *mtx)
@@ -132,17 +136,17 @@ int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;
 
 /**************************************************************************
 Function: The new ADC data is updated to FIFO array for filtering
-Input   : ax��ay��az��x��y, z-axis acceleration data��gx��gy��gz��x. Y, z-axis angular acceleration data
+Input   : ax��ay��az��x��y, z-axis acceleration data��gx��gy��gz��x. Y, z-axis angular acceleration data
 Output  : none
-�������ܣ����µ�ADC���ݸ��µ� FIFO���飬�����˲�����
-��ڲ�����ax��ay��az��x��y��z����ٶ����ݣ�gx��gy��gz��x��y��z��Ǽ��ٶ�����
-����  ֵ����
+�������ܣ����µ�ADC���ݸ��µ� FIFO���飬�����˲�����
+��ڲ�����ax��ay��az��x��y��z����ٶ����ݣ�gx��gy��gz��x��y��z��Ǽ��ٶ�����
+����  ֵ����
 **************************************************************************/
 void  MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
 {
 unsigned char i ;
 int32_t sum=0;
-for(i=1;i<10;i++){	//FIFO ����
+for(i=1;i<10;i++){	//FIFO ����
 MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
 MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
 MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
@@ -150,7 +154,7 @@ MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
 MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
 MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
 }
-MPU6050_FIFO[0][9]=ax;//���µ����ݷ��õ� ���ݵ������
+MPU6050_FIFO[0][9]=ax;//���µ����ݷ��õ� ���ݵ������
 MPU6050_FIFO[1][9]=ay;
 MPU6050_FIFO[2][9]=az;
 MPU6050_FIFO[3][9]=gx;
@@ -158,7 +162,7 @@ MPU6050_FIFO[4][9]=gy;
 MPU6050_FIFO[5][9]=gz;
 
 sum=0;
-for(i=0;i<10;i++){	//��ǰ����ĺϣ���ȡƽ��ֵ
+for(i=0;i<10;i++){	//��ǰ����ĺϣ���ȡƽ��ֵ
    sum+=MPU6050_FIFO[0][i];
 }
 MPU6050_FIFO[0][10]=sum/10;
@@ -196,11 +200,11 @@ MPU6050_FIFO[5][10]=sum/10;
 
 /**************************************************************************
 Function: Setting the clock source of mpu6050
-Input   : source��Clock source number
+Input   : source��Clock source number
 Output  : none
-�������ܣ�����  MPU6050 ��ʱ��Դ
-��ڲ�����source��ʱ��Դ���
-����  ֵ����
+�������ܣ�����  MPU6050 ��ʱ��Դ
+��ڲ�����source��ʱ��Դ���
+����  ֵ����
  * CLK_SEL | Clock Source
  * --------+--------------------------------------
  * 0       | Internal oscillator
@@ -231,27 +235,27 @@ void MPU6050_setFullScaleGyroRange(uint8_t range) {
 
 /**************************************************************************
 Function: Setting the maximum range of mpu6050 accelerometer
-Input   : range��Acceleration maximum range number
+Input   : range��Acceleration maximum range number
 Output  : none
-�������ܣ����� MPU6050 ���ٶȼƵ��������
-��ڲ�����range�����ٶ�������̱��
-����  ֵ����
+�������ܣ����� MPU6050 ���ٶȼƵ��������
+��ڲ�����range�����ٶ�������̱��
+����  ֵ����
 **************************************************************************/
-//#define MPU6050_ACCEL_FS_2          0x00  		//===�������+-2G
-//#define MPU6050_ACCEL_FS_4          0x01			//===�������+-4G
-//#define MPU6050_ACCEL_FS_8          0x02			//===�������+-8G
-//#define MPU6050_ACCEL_FS_16         0x03			//===�������+-16G
+//#define MPU6050_ACCEL_FS_2          0x00  		//===�������+-2G
+//#define MPU6050_ACCEL_FS_4          0x01			//===�������+-4G
+//#define MPU6050_ACCEL_FS_8          0x02			//===�������+-8G
+//#define MPU6050_ACCEL_FS_16         0x03			//===�������+-16G
 void MPU6050_setFullScaleAccelRange(uint8_t range) {
     I2C_WriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
 }
 
 /**************************************************************************
 Function: Set mpu6050 to sleep mode or not
-Input   : enable��1��sleep��0��work��
+Input   : enable��1��sleep��0��work��
 Output  : none
-�������ܣ����� MPU6050 �Ƿ����˯��ģʽ
-��ڲ�����enable��1��˯����0��������
-����  ֵ����
+�������ܣ����� MPU6050 �Ƿ����˯��ģʽ
+��ڲ�����enable��1��˯����0��������
+����  ֵ����
 **************************************************************************/
 void MPU6050_setSleepEnabled(uint8_t enabled) {
     I2C_WriteOneBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
@@ -261,9 +265,9 @@ void MPU6050_setSleepEnabled(uint8_t enabled) {
 Function: Read identity
 Input   : none
 Output  : 0x68
-�������ܣ���ȡ  MPU6050 WHO_AM_I ��ʶ
-��ڲ�������
-����  ֵ��0x68
+�������ܣ���ȡ  MPU6050 WHO_AM_I ��ʶ
+��ڲ�������
+����  ֵ��0x68
 **************************************************************************/
 uint8_t MPU6050_getDeviceID(void) {
 
@@ -277,10 +281,10 @@ uint8_t MPU6050_getDeviceID(void) {
 /**************************************************************************
 Function: Check whether mpu6050 is connected
 Input   : none
-Output  : 1��Connected��0��Not connected
-�������ܣ����MPU6050 �Ƿ��Ѿ�����
-��ڲ�������
-����  ֵ��1�������ӣ�0��δ����
+Output  : 1��Connected��0��Not connected
+�������ܣ����MPU6050 �Ƿ��Ѿ�����
+��ڲ�������
+����  ֵ��1�������ӣ�0��δ����
 **************************************************************************/
 uint8_t MPU6050_testConnection(void) {
    if(MPU6050_getDeviceID() == 0x68)  //0b01101000;
@@ -290,11 +294,11 @@ uint8_t MPU6050_testConnection(void) {
 
 /**************************************************************************
 Function: Setting whether mpu6050 is the host of aux I2C cable
-Input   : enable��1��yes��0;not
+Input   : enable��1��yes��0;not
 Output  : none
-�������ܣ����� MPU6050 �Ƿ�ΪAUX I2C�ߵ�����
-��ڲ�����enable��1���ǣ�0����
-����  ֵ����
+�������ܣ����� MPU6050 �Ƿ�ΪAUX I2C�ߵ�����
+��ڲ�����enable��1���ǣ�0����
+����  ֵ����
 **************************************************************************/
 void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
     I2C_WriteOneBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
@@ -302,11 +306,11 @@ void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
 
 /**************************************************************************
 Function: Setting whether mpu6050 is the host of aux I2C cable
-Input   : enable��1��yes��0;not
+Input   : enable��1��yes��0;not
 Output  : none
-�������ܣ����� MPU6050 �Ƿ�ΪAUX I2C�ߵ�����
-��ڲ�����enable��1���ǣ�0����
-����  ֵ����
+�������ܣ����� MPU6050 �Ƿ�ΪAUX I2C�ߵ�����
+��ڲ�����enable��1���ǣ�0����
+����  ֵ����
 **************************************************************************/
 void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
     I2C_WriteOneBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
@@ -316,38 +320,38 @@ void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
 Function: initialization Mpu6050 to enter the available state
 Input   : none
 Output  : none
-�������ܣ���ʼ��	MPU6050 �Խ������״̬
-��ڲ�������
-����  ֵ����
+�������ܣ���ʼ��	MPU6050 �Խ������״̬
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 u8 MPU6050_initialize(void) 
 	{
 		u8 res;
-	//IIC_Init();  //Initialize the IIC bus //��ʼ��IIC����
-	I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X80);	//Reset MPUrobot_select_init.h //��λMPUrobot_select_init.h
-  delay_ms(200); //Delay 200 ms //��ʱ200ms
-	I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X00);	//Wake mpurobot_select_init.h //����MPUrobot_select_init.h
+	//IIC_Init();  //Initialize the IIC bus //��ʼ��IIC����
+	I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X80);	//Reset MPUrobot_select_init.h //��λMPUrobot_select_init.h
+  delay_ms(200); //Delay 200 ms //��ʱ200ms
+	I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X00);	//Wake mpurobot_select_init.h //����MPUrobot_select_init.h
 	
-  //MPU6050_Set_Gyro_Fsr(1);  //Gyroscope sensor              //�����Ǵ�����,��500dps=��500��/s ��32768 (gyro/32768*500)*PI/180(rad/s)=gyro/3754.9(rad/s)
+  //MPU6050_Set_Gyro_Fsr(1);  //Gyroscope sensor              //�����Ǵ�����,��500dps=��500��/s ��32768 (gyro/32768*500)*PI/180(rad/s)=gyro/3754.9(rad/s)
 	MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_500);
-	//MPU6050_Set_Accel_Fsr(0);	//Acceleration sensor           //���ٶȴ�����,��2g=��2*9.8m/s^2 ��32768 accel/32768*19.6=accel/1671.84
+	//MPU6050_Set_Accel_Fsr(0);	//Acceleration sensor           //���ٶȴ�����,��2g=��2*9.8m/s^2 ��32768 accel/32768*19.6=accel/1671.84
   MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
-	MPU6050_Set_Rate(50);			//Set the sampling rate to 50Hz //������50Hz
+	MPU6050_Set_Rate(50);			//Set the sampling rate to 50Hz //������50Hz
 	
-	I2C_WriteOneByte(devAddr,MPU6050_RA_INT_ENABLE,0X00);	  //Turn off all interrupts //�ر������ж�
-	I2C_WriteOneByte(devAddr,MPU6050_RA_USER_CTRL,0X00);	//The I2C main mode is off //I2C��ģʽ�ر�
-	I2C_WriteOneByte(devAddr,MPU6050_RA_FIFO_EN,0X00);	  //Close the FIFO //�ر�FIFO
+	I2C_WriteOneByte(devAddr,MPU6050_RA_INT_ENABLE,0X00);	  //Turn off all interrupts //�ر������ж�
+	I2C_WriteOneByte(devAddr,MPU6050_RA_USER_CTRL,0X00);	//The I2C main mode is off //I2C��ģʽ�ر�
+	I2C_WriteOneByte(devAddr,MPU6050_RA_FIFO_EN,0X00);	  //Close the FIFO //�ر�FIFO
 	//The INT pin is low, enabling bypass mode to read the magnetometer directly
-	//INT���ŵ͵�ƽ��Ч������bypassģʽ������ֱ�Ӷ�ȡ������
+	//INT���ŵ͵�ƽ��Ч������bypassģʽ������ֱ�Ӷ�ȡ������
 	I2C_WriteOneByte(devAddr,MPU6050_RA_INT_PIN_CFG,0X80);
 	//Read the ID of MPU6050 
-	//��ȡMPU6050��ID	
+	//��ȡMPU6050��ID	
 	res=I2C_ReadOneByte(devAddr,MPU6050_RA_WHO_AM_I);
-	if(res==MPU6050_DEFAULT_ADDRESS) //The device ID is correct, The correct device ID depends on the AD pin //����ID��ȷ, ����ID����ȷȡ����AD����
+	if(res==MPU6050_DEFAULT_ADDRESS) //The device ID is correct, The correct device ID depends on the AD pin //����ID��ȷ, ����ID����ȷȡ����AD����
 	{
-		I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X01);	//Set CLKSEL,PLL X axis as reference //����CLKSEL,PLL X��Ϊ�ο�
-		I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_2,0X00);	//Acceleration and gyroscope both work //���ٶ��������Ƕ�����
-		MPU6050_Set_Rate(50);	                      //Set the sampling rate to 50Hz //���ò�����Ϊ50Hz   
+		I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X01);	//Set CLKSEL,PLL X axis as reference //����CLKSEL,PLL X��Ϊ�ο�
+		I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_2,0X00);	//Acceleration and gyroscope both work //���ٶ��������Ƕ�����
+		MPU6050_Set_Rate(50);	                      //Set the sampling rate to 50Hz //���ò�����Ϊ50Hz   
  	}else return 1;
 	return 0;
 
@@ -357,9 +361,9 @@ u8 MPU6050_initialize(void)
 Function: Initialization of DMP in mpu6050
 Input   : none
 Output  : none
-�������ܣ�MPU6050����DMP�ij�ʼ��
-��ڲ�������
-����  ֵ����
+�������ܣ�MPU6050����DMP�ij�ʼ��
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 //void DMP_Init(void)
 //{ 
@@ -395,9 +399,9 @@ Output  : none
 Function: Read the attitude information of DMP in mpu6050
 Input   : none
 Output  : none
-�������ܣ���ȡMPU6050����DMP����̬��Ϣ
-��ڲ�������
-����  ֵ����
+�������ܣ���ȡMPU6050����DMP����̬��Ϣ
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 //void Read_DMP(void)
 //{	
@@ -405,16 +409,16 @@ Output  : none
 //		unsigned char more;
 //		long quat[4];
 
-//				dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);		//��ȡDMP����
+//				dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);		//��ȡDMP����
 //				if (sensors & INV_WXYZ_QUAT )
 //				{    
 //					 q0=quat[0] / q30;
 //					 q1=quat[1] / q30;
 //					 q2=quat[2] / q30;
-//					 q3=quat[3] / q30; 		//��Ԫ��
-//					 Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 	//����������
-//					 Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // �����������
-//					 Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	 //�����ƫ����
+//					 q3=quat[3] / q30; 		//��Ԫ��
+//					 Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 	//����������
+//					 Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // �����������
+//					 Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	 //�����ƫ����
 //				}
 
 //}
@@ -422,16 +426,16 @@ Output  : none
 Function: Read mpu6050 built-in temperature sensor data
 Input   : none
 Output  : Centigrade temperature
-�������ܣ���ȡMPU6050�����¶ȴ���������
-��ڲ�������
-����  ֵ�������¶�
+�������ܣ���ȡMPU6050�����¶ȴ���������
+��ڲ�������
+����  ֵ�������¶�
 **************************************************************************/
 int Read_Temperature(void)
 {	   
 	  float Temp;
 	  Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L);
-		if(Temp>32768) Temp-=65536;	//��������ת��
-		Temp=(36.53f+Temp/340)*10;	  //�¶ȷŴ�ʮ�����
+		if(Temp>32768) Temp-=65536;	//��������ת��
+		Temp=(36.53f+Temp/340)*10;	  //�¶ȷŴ�ʮ�����
 	  return (int)Temp;
 }
 
@@ -439,9 +443,9 @@ int Read_Temperature(void)
 Function: Initialize TIM2 as the encoder interface mode
 Input   : LPF: Digital low-pass filtering frequency (Hz)
 Output  : 0: Settings successful, others: Settings failed
-�������ܣ�����MPUrobot_select_init.h�����ֵ�ͨ�˲���
-��ڲ�����lpf:���ֵ�ͨ�˲�Ƶ��(Hz)
-����  ֵ��0:���óɹ�, ����:����ʧ��
+�������ܣ�����MPUrobot_select_init.h�����ֵ�ͨ�˲���
+��ڲ�����lpf:���ֵ�ͨ�˲�Ƶ��(Hz)
+����  ֵ��0:���óɹ�, ����:����ʧ��
 **************************************************************************/
 unsigned char MPU6050_Set_LPF(u16 lpf)
 {
@@ -452,15 +456,15 @@ unsigned char MPU6050_Set_LPF(u16 lpf)
 	else if(lpf>=20)data=4;
 	else if(lpf>=10)data=5;
 	else data=6; 
-	return I2C_WriteOneByte(devAddr,MPU6050_RA_CONFIG,data); //Set the digital lowpass filter//�������ֵ�ͨ�˲���  
+	return I2C_WriteOneByte(devAddr,MPU6050_RA_CONFIG,data); //Set the digital lowpass filter//�������ֵ�ͨ�˲���  
 }
 /**************************************************************************
 Function: Initialize TIM2 as the encoder interface mode
 Input   : rate:4~1000(Hz)
 Output  : 0: Settings successful, others: Settings failed
-�������ܣ�����MPUrobot_select_init.h�IJ�����(�ٶ�Fs=1KHz)
-��ڲ�����rate:4~1000(Hz)
-����  ֵ��0:���óɹ�, ����:����ʧ��
+�������ܣ�����MPUrobot_select_init.h�IJ�����(�ٶ�Fs=1KHz)
+��ڲ�����rate:4~1000(Hz)
+����  ֵ��0:���óɹ�, ����:����ʧ��
 **************************************************************************/
 unsigned char MPU6050_Set_Rate(u16 rate)
 {
@@ -468,42 +472,42 @@ unsigned char MPU6050_Set_Rate(u16 rate)
 	if(rate>1000)rate=1000;
 	if(rate<4)rate=4;
 	data=1000/rate-1;
-	data=I2C_WriteOneByte(devAddr,MPU6050_RA_SMPLRT_DIV,data);	//Set the digital lowpass filter//�������ֵ�ͨ�˲���  
- 	return MPU6050_Set_LPF(rate/2);	//Automatically sets LPF to half of the sampling rate //�Զ�����LPFΪ�����ʵ�һ��
+	data=I2C_WriteOneByte(devAddr,MPU6050_RA_SMPLRT_DIV,data);	//Set the digital lowpass filter//�������ֵ�ͨ�˲���  
+ 	return MPU6050_Set_LPF(rate/2);	//Automatically sets LPF to half of the sampling rate //�Զ�����LPFΪ�����ʵ�һ��
 }
 
 /**************************************************************************
 Function: Initialize TIM2 as the encoder interface mode
 Input   : Gx, Gy, Gz: raw readings (plus or minus) of the x,y, and z axes of the gyroscope
 Output  : 0: success, others: error code
-�������ܣ����������ֵ(ԭʼֵ)
+�������ܣ����������ֵ(ԭʼֵ)
 **************************************************************************/
 void MPU_Get_Gyroscope(void)
 {
-		gyro[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L);    //��ȡX��������
-		gyro[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);    //��ȡY��������
-		gyro[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);    //��ȡZ��������
+		gyro[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L);    //��ȡX��������
+		gyro[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);    //��ȡY��������
+		gyro[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);    //��ȡZ��������
 	
-	if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //����ǰ10��
+	if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //����ǰ10��
 		{
 
-			Led_Count=1; //LED high frequency flashing //LED��Ƶ��˸
-			Flag_Stop=1; //The software fails to flag location 1 //����ʧ�ܱ�־λ��1		
+			Led_Count=1; //LED high frequency flashing //LED��Ƶ��˸
+			Flag_Stop=1; //The software fails to flag location 1 //����ʧ�ܱ�־λ��1		
 		}
-	else //10 seconds after starting //����10���
+	else //10 seconds after starting //����10���
 		{  
 			if(Deviation_Count==CONTROL_DELAY)
-				Flag_Stop=0; //The software fails to flag location 0 //����ʧ�ܱ�־λ��0
-			Led_Count=300; //The LED returns to normal flicker frequency //LED�ָ�������˸Ƶ��	
+				Flag_Stop=0; //The software fails to flag location 0 //����ʧ�ܱ�־λ��0
+			Led_Count=300; //The LED returns to normal flicker frequency //LED�ָ�������˸Ƶ��	
 			
 			//Save the raw data to update zero by clicking the user button
-			//����ԭʼ�������ڵ����û������������
+			//����ԭʼ�������ڵ����û������������
 			Original_gyro[0] =gyro[0];  
 			Original_gyro[1] =gyro[1];  
 			Original_gyro[2]= gyro[2];			
 			
 			//Removes zero drift data
-			//ȥ�����Ư�Ƶ�����
+			//ȥ�����Ư�Ƶ�����
 			gyro[0] =Original_gyro[0]-Deviation_gyro[0];  
 			gyro[1] =Original_gyro[1]-Deviation_gyro[1];  
 			gyro[2]= Original_gyro[2]-Deviation_gyro[2];
@@ -514,28 +518,28 @@ void MPU_Get_Gyroscope(void)
 Function: Initialize TIM2 as the encoder interface mode
 Input   : Gx, Gy, Gz: raw readings (plus or minus) of the x,y, and z axes of the gyroscope
 Output  : 0: success, others: error code
-�������ܣ���ü��ٶȼ�ֵ(ԭʼֵ)
+�������ܣ���ü��ٶȼ�ֵ(ԭʼֵ)
 **************************************************************************/
 void MPU_Get_Accelscope(void)
 {
-		accel[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //��ȡX����ٶȼ�
-		accel[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //��ȡX����ٶȼ�
-		accel[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //��ȡZ����ٶȼ�
+		accel[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //��ȡX����ٶȼ�
+		accel[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //��ȡX����ٶȼ�
+		accel[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //��ȡZ����ٶȼ�
 	
-		if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //����ǰ10��
+		if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //����ǰ10��
 		{
 	
 		}
-		else //10 seconds after starting //����10���
+		else //10 seconds after starting //����10���
 		{  		
 			//Save the raw data to update zero by clicking the user button
-			//����ԭʼ�������ڵ����û������������
+			//����ԭʼ�������ڵ����û������������
 			Original_accel[0] =accel[0];  
 			Original_accel[1] =accel[1];  
 			Original_accel[2]= accel[2];			
 			
 			//Removes zero drift data
-			//ȥ�����Ư�Ƶ�����
+			//ȥ�����Ư�Ƶ�����
 			accel[0] =Original_accel[0]-Deviation_accel[0];  
 			accel[1] =Original_accel[1]-Deviation_accel[1];  
 			accel[2]= Original_accel[2]-Deviation_accel[2]+16384;
diff --git a/HARDWARE/motor.h b/HARDWARE/motor.h
index c8e4f32..c1eeaea 100644
--- a/HARDWARE/motor.h
+++ b/HARDWARE/motor.h
@@ -49,7 +49,7 @@
 #define EN     PDin(3)  
 
 #define Servo_PWM  TIM12->CCR2
-#define SERVO_INIT 1590  //Servo zero point //������
+#define SERVO_INIT 1670  //Servo straight-ahead point (calibrated 1/R~=0) //��ֱ��ǰ��
 
 void Enable_Pin(void);
 void Servo_PWM_Init(u16 arr,u16 psc);
diff --git a/HARDWARE/timer.c b/HARDWARE/timer.c
index 846e553..42dd2b2 100644
--- a/HARDWARE/timer.c
+++ b/HARDWARE/timer.c
@@ -2,28 +2,28 @@
 
 //Input the capture flag for channel 1, 
 //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
-//ͨ��1���벶���־������λ�������־����6λ�������־		
+//ͨ��1���벶���־������λ�������־����6λ�������־		
 u8 TIM8CH1_CAPTURE_STA = 0;	
 u16 TIM8CH1_CAPTURE_UPVAL;
 u16 TIM8CH1_CAPTURE_DOWNVAL;
 
 //Input the capture flag for channel 2, 
 //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
-//ͨ��2���벶���־������λ�������־����6λ�������־	
+//ͨ��2���벶���־������λ�������־����6λ�������־	
 u8 TIM8CH2_CAPTURE_STA = 0;		
 u16 TIM8CH2_CAPTURE_UPVAL;
 u16 TIM8CH2_CAPTURE_DOWNVAL;
 
 //Input the capture flag for channel 3, 
 //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
-//ͨ��3���벶���־������λ�������־����6λ�������־	
+//ͨ��3���벶���־������λ�������־����6λ�������־	
 u8 TIM8CH3_CAPTURE_STA = 0;		
 u16 TIM8CH3_CAPTURE_UPVAL;
 u16 TIM8CH3_CAPTURE_DOWNVAL;
 
 //Input the capture flag for channel 4, 
 //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
-//ͨ��4���벶���־������λ�������־����6λ�������־
+//ͨ��4���벶���־������λ�������־����6λ�������־
 u8 TIM8CH4_CAPTURE_STA = 0;			
 u16 TIM8CH4_CAPTURE_UPVAL;
 u16 TIM8CH4_CAPTURE_DOWNVAL;
@@ -34,19 +34,19 @@ u32 TIM8_T3;
 u32 TIM8_T4;
 
 //Variables related to remote control acquisition of model aircraft
-//��ģң�زɼ���ر���
+//��ģң�زɼ���ر���
 int Remoter_Ch1=1500,Remoter_Ch2=1500,Remoter_Ch3=1500,Remoter_Ch4=1500;
 //Model aircraft remote control receiver variable
-//��ģң�ؽ��ձ���
+//��ģң�ؽ��ձ���
 int L_Remoter_Ch1=1500,L_Remoter_Ch2=1500,L_Remoter_Ch3=1500,L_Remoter_Ch4=1500;  
 
 /**************************************************************************
 Function: Model aircraft remote control initialization function, timer 1 input capture initialization
 Input   : arr: Automatic reload value, psc: clock preset frequency
 Output  : none
-�������ܣ���ģң�س�ʼ����������ʱ��1���벶���ʼ��
-��ڲ�����arr���Զ���װֵ��psc��ʱ��Ԥ��Ƶ�� 
-�� �� ֵ����
+�������ܣ���ģң�س�ʼ����������ʱ��1���벶���ʼ��
+��ڲ�����arr���Զ���װֵ��psc��ʱ��Ԥ��Ƶ�� 
+�� �� ֵ����
 **************************************************************************/ 
 void TIM8_Cap_Init(u16 arr, u16 psc)
 {
@@ -55,14 +55,14 @@ void TIM8_Cap_Init(u16 arr, u16 psc)
 	NVIC_InitTypeDef NVIC_InitStructure;
 	TIM_ICInitTypeDef TIM_ICInitStructure;
 	
-	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);  	//TIM1ʱ��ʹ��    
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); 	//ʹ��PORTEʱ��	
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);  	//TIM1ʱ��ʹ��    
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); 	//ʹ��PORTEʱ��	
 		
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; //GPIOC
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//���ù���
-	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;	//�ٶ�100MHz
-	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //���츴�����
-	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; //����
+	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//���ù���
+	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;	//�ٶ�100MHz
+	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //���츴�����
+	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; //����
 	GPIO_Init(GPIOC,&GPIO_InitStructure); 
 
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); 
@@ -70,104 +70,104 @@ void TIM8_Cap_Init(u16 arr, u16 psc)
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8);
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_TIM8);
 
-	/*** Initialize timer 1 || ��ʼ����ʱ��1 ***/
-	//Set the counter to automatically reload //�趨�������Զ���װֵ 
+	/*** Initialize timer 1 || ��ʼ����ʱ��1 ***/
+	//Set the counter to automatically reload //�趨�������Զ���װֵ 
 	TIM_TimeBaseStructure.TIM_Period = arr; 
-	//Pre-divider //Ԥ��Ƶ�� 
+	//Pre-divider //Ԥ��Ƶ�� 
 	TIM_TimeBaseStructure.TIM_Prescaler = psc; 	
-	//Set the clock split: TDTS = Tck_tim //����ʱ�ӷָ�:TDTS = Tck_tim
+	//Set the clock split: TDTS = Tck_tim //����ʱ�ӷָ�:TDTS = Tck_tim
 	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
-	//TIM up count mode //TIM���ϼ���ģʽ	
+	//TIM up count mode //TIM���ϼ���ģʽ	
 	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 
 	//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
-	//����TIM_TimeBaseInitStruct��ָ���IJ�����ʼ��TIMx��ʱ�������λ
+	//����TIM_TimeBaseInitStruct��ָ���IJ�����ʼ��TIMx��ʱ�������λ
 	TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); 
 
-	/*** ��ʼ��TIM1���벶�������ͨ��1 || Initialize TIM1 for the capture parameter, channel 1 ***/
-	//Select input //ѡ������� 
+	/*** ��ʼ��TIM1���벶�������ͨ��1 || Initialize TIM1 for the capture parameter, channel 1 ***/
+	//Select input //ѡ������� 
 	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; 
-  //Rising edge capture //�����ز���
+  //Rising edge capture //�����ز���
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;	
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 
-	//Configure input frequency division, regardless of frequency //���������Ƶ,����Ƶ 
+	//Configure input frequency division, regardless of frequency //���������Ƶ,����Ƶ 
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	
-  //IC1F=0000 Configure input filter //���������˲���
+  //IC1F=0000 Configure input filter //���������˲���
 	TIM_ICInitStructure.TIM_ICFilter = 0x0F;	  
 	TIM_ICInit(TIM8, &TIM_ICInitStructure);
 
-	/*** ��ʼ��TIM1���벶�������ͨ��2 || Initialize TIM1 for the capture parameter, channel 2 ***/
-	//CC1S=01 Select input //ѡ������� 
+	/*** ��ʼ��TIM1���벶�������ͨ��2 || Initialize TIM1 for the capture parameter, channel 2 ***/
+	//CC1S=01 Select input //ѡ������� 
 	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
-	//Rising edge capture //�����ز���
+	//Rising edge capture //�����ز���
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;	
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 
-	//Configure input frequency division, regardless of frequency //���������Ƶ,����Ƶ 
+	//Configure input frequency division, regardless of frequency //���������Ƶ,����Ƶ 
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	  
-	TIM_ICInitStructure.TIM_ICFilter = 0x00;	  //IC1F=0000 ���������˲���
+	TIM_ICInitStructure.TIM_ICFilter = 0x00;	  //IC1F=0000 ���������˲���
 	TIM_ICInit(TIM8, &TIM_ICInitStructure);
 
-	/*** ��ʼ��TIM1���벶�������ͨ��3 || Initialize TIM1 for the capture parameter, channel 3 ***/
-	//Select input //ѡ������� 
+	/*** ��ʼ��TIM1���벶�������ͨ��3 || Initialize TIM1 for the capture parameter, channel 3 ***/
+	//Select input //ѡ������� 
 	TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;   
-	//Rising edge capture //�����ز���
+	//Rising edge capture //�����ز���
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 
-	//Configure input frequency division, regardless of frequency //���������Ƶ,����Ƶ 
+	//Configure input frequency division, regardless of frequency //���������Ƶ,����Ƶ 
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	  
-	//IC1F=0000 Configure input filter //���������˲��������˲�  
+	//IC1F=0000 Configure input filter //���������˲��������˲�  
 	TIM_ICInitStructure.TIM_ICFilter = 0x00;	  
 	TIM_ICInit(TIM8, &TIM_ICInitStructure);
 
-	/*** ��ʼ��TIM1���벶�������ͨ��4 || Initialize TIM1 for the capture parameter, channel 4 ***/
-	//Select input //ѡ������� 
+	/*** ��ʼ��TIM1���벶�������ͨ��4 || Initialize TIM1 for the capture parameter, channel 4 ***/
+	//Select input //ѡ������� 
 	TIM_ICInitStructure.TIM_Channel = TIM_Channel_4; 
-	//Rising edge capture //�����ز���
+	//Rising edge capture //�����ز���
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;	
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 
-	//Configure input frequency division, regardless of frequency //���������Ƶ,����Ƶ 
+	//Configure input frequency division, regardless of frequency //���������Ƶ,����Ƶ 
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	   
-	//IC1F=0000 Configure input filter //���������˲��������˲�  
+	//IC1F=0000 Configure input filter //���������˲��������˲�  
 	TIM_ICInitStructure.TIM_ICFilter = 0x00;	  
 	TIM_ICInit(TIM8, &TIM_ICInitStructure);
 
-  /*** interrupt packet initialization || �жϷ����ʼ�� ***/
-  //TIM1 interrupts //TIM1�ж�
+  /*** interrupt packet initialization || �жϷ����ʼ�� ***/
+  //TIM1 interrupts //TIM1�ж�
 	NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn; 
-  //Preempt priority 0 //��ռ���ȼ�0��	
+  //Preempt priority 0 //��ռ���ȼ�0��	
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;  
-	//Level 0 from priority //�����ȼ�0��
+	//Level 0 from priority //�����ȼ�0��
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; 
-	//IRQ channels are enabled //IRQͨ����ʹ��
+	//IRQ channels are enabled //IRQͨ����ʹ��
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 
 	//Initializes the peripheral NVIC register according to the parameters specified in NVIC_InitStruct
-	//����NVIC_InitStruct��ָ���IJ�����ʼ������NVIC�Ĵ��� 
+	//����NVIC_InitStruct��ָ���IJ�����ʼ������NVIC�Ĵ��� 
 	NVIC_Init(&NVIC_InitStructure);   
 	
   //Allow CC1IE,CC2IE,CC3IE,CC4IE to catch interrupts, not allowed update_interrupts
-  //�����������жϣ�����CC1IE,CC2IE,CC3IE,CC4IE�����ж�	
+  //�����������жϣ�����CC1IE,CC2IE,CC3IE,CC4IE�����ж�	
 	TIM_ITConfig(TIM8, TIM_IT_CC1|TIM_IT_CC2|TIM_IT_CC3|TIM_IT_CC4,	ENABLE);   
-	//Advanced timer output must be enabled //�߼���ʱ���������ʹ�����	
+	//Advanced timer output must be enabled //�߼���ʱ���������ʹ�����	
 	TIM_CtrlPWMOutputs(TIM8,ENABLE); 	
-	//Enable timer //ʹ�ܶ�ʱ��
+	//Enable timer //ʹ�ܶ�ʱ��
 	TIM_Cmd(TIM8, ENABLE); 		
 }
 /**************************************************************************
 Function: Model aircraft remote control receiving interrupt, namely timer 8 input capture interrupt
 Input   : none
 Output  : none
-�������ܣ���ģң�ؽ����жϣ�����ʱ��8���벶���ж�
-��ڲ�������
-�� �� ֵ����
+�������ܣ���ģң�ؽ����жϣ�����ʱ��8���벶���ж�
+��ڲ�������
+�� �� ֵ����
 **************************************************************************/ 
 void TIM8_CC_IRQHandler(void)
 {
-	//���Ӻ�ģңң��������Ҫ����ǰ���ˣ��ſ�����ʽ��ģ����С��
+	//���Ӻ�ģңң��������Ҫ����ǰ���ˣ��ſ�����ʽ��ģ����С��
 	//After connecting the remote controller of the model aircraft, 
 	//you need to push down the forward lever to officially control the car of the model aircraft
   if(Remoter_Ch2>1600&&Remote_ON_Flag==0&&Deviation_Count>=CONTROL_DELAY)
   {
 		//Model aircraft remote control mark position 1, other marks position 0
-		//��ģң�ر�־λ��1��������־λ��0
+		//��ģң�ر�־λ��1��������־λ��0
 		Remote_ON_Flag=1;
 	  APP_ON_Flag=0;
 		PS2_ON_Flag=0;
@@ -179,131 +179,131 @@ void TIM8_CC_IRQHandler(void)
 	
 
 
-//	//Channel 1 //ͨ��һ
+//	//Channel 1 //ͨ��һ
 	if ((TIM8CH1_CAPTURE_STA & 0X80) == 0) 			
 	{
-		if (TIM_GetITStatus(TIM8, TIM_IT_CC1) != RESET) //A capture event occurred on channel 1 //ͨ��1���������¼�
+		if (TIM_GetITStatus(TIM8, TIM_IT_CC1) != RESET) //A capture event occurred on channel 1 //ͨ��1���������¼�
 		{
-			TIM_ClearITPendingBit(TIM8, TIM_IT_CC1); //Clear the interrupt flag bit //����жϱ�־λ
-			if (TIM8CH1_CAPTURE_STA & 0X40)	//A falling edge is caught //����һ���½���
+			TIM_ClearITPendingBit(TIM8, TIM_IT_CC1); //Clear the interrupt flag bit //����жϱ�־λ
+			if (TIM8CH1_CAPTURE_STA & 0X40)	//A falling edge is caught //����һ���½���
 			{
-				TIM8CH1_CAPTURE_DOWNVAL = TIM_GetCapture1(TIM8); //Record the timer value at this point //��¼�´�ʱ�Ķ�ʱ������ֵ
+				TIM8CH1_CAPTURE_DOWNVAL = TIM_GetCapture1(TIM8); //Record the timer value at this point //��¼�´�ʱ�Ķ�ʱ������ֵ
 				if (TIM8CH1_CAPTURE_DOWNVAL < TIM8CH1_CAPTURE_UPVAL)
 				{
 					TIM8_T1 = 9999;
 				}
 				else
 					TIM8_T1 = 0;
-				Remoter_Ch1 = TIM8CH1_CAPTURE_DOWNVAL - TIM8CH1_CAPTURE_UPVAL + TIM8_T1;	//Time to get the total high level //�õ��ܵĸߵ�ƽ��ʱ��
-				if(abs(Remoter_Ch1-L_Remoter_Ch1)>500) Remoter_Ch1=L_Remoter_Ch1; //Filter //�˲�
+				Remoter_Ch1 = TIM8CH1_CAPTURE_DOWNVAL - TIM8CH1_CAPTURE_UPVAL + TIM8_T1;	//Time to get the total high level //�õ��ܵĸߵ�ƽ��ʱ��
+				if(abs(Remoter_Ch1-L_Remoter_Ch1)>500) Remoter_Ch1=L_Remoter_Ch1; //Filter //�˲�
 					 L_Remoter_Ch1=Remoter_Ch1;
 				
-				TIM8CH1_CAPTURE_STA = 0; //Capture flag bit to zero	//�����־λ����
-				TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //����Ϊ�����ز���		  
+				TIM8CH1_CAPTURE_STA = 0; //Capture flag bit to zero	//�����־λ����
+				TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //����Ϊ�����ز���		  
 			}
 			else 
 			{
 				//When the capture time occurs but not the falling edge, the first time the rising edge is captured, record the timer value at this time
-				//��������ʱ�䵫�����½��أ���һ�β��������أ���¼��ʱ�Ķ�ʱ������ֵ
-				TIM8CH1_CAPTURE_UPVAL = TIM_GetCapture1(TIM8); //Obtain rising edge data //��ȡ����������
-				TIM8CH1_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //����Ѳ���������
-				TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //����Ϊ�½��ز���
+				//��������ʱ�䵫�����½��أ���һ�β��������أ���¼��ʱ�Ķ�ʱ������ֵ
+				TIM8CH1_CAPTURE_UPVAL = TIM_GetCapture1(TIM8); //Obtain rising edge data //��ȡ����������
+				TIM8CH1_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //����Ѳ���������
+				TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //����Ϊ�½��ز���
 			}
 		}
 	}
-  //Channel 2 //ͨ����
+  //Channel 2 //ͨ����
 	if ((TIM8CH2_CAPTURE_STA & 0X80) == 0)		
 	{
-		if (TIM_GetITStatus(TIM8, TIM_IT_CC2) != RESET)	//A capture event occurred on channel 2 //ͨ��2���������¼�
+		if (TIM_GetITStatus(TIM8, TIM_IT_CC2) != RESET)	//A capture event occurred on channel 2 //ͨ��2���������¼�
 		{
-			TIM_ClearITPendingBit(TIM8, TIM_IT_CC2); //Clear the interrupt flag bit //����жϱ�־λ
-			if (TIM8CH2_CAPTURE_STA & 0X40)	//A falling edge is caught //����һ���½���
+			TIM_ClearITPendingBit(TIM8, TIM_IT_CC2); //Clear the interrupt flag bit //����жϱ�־λ
+			if (TIM8CH2_CAPTURE_STA & 0X40)	//A falling edge is caught //����һ���½���
 			{
-				TIM8CH2_CAPTURE_DOWNVAL = TIM_GetCapture2(TIM8); //Record the timer value at this point //��¼�´�ʱ�Ķ�ʱ������ֵ
+				TIM8CH2_CAPTURE_DOWNVAL = TIM_GetCapture2(TIM8); //Record the timer value at this point //��¼�´�ʱ�Ķ�ʱ������ֵ
 				if (TIM8CH2_CAPTURE_DOWNVAL < TIM8CH2_CAPTURE_UPVAL)
 				{
 					TIM8_T2 = 9999;
 				}
 				else
 					TIM8_T2 = 0;
-				Remoter_Ch2 = TIM8CH2_CAPTURE_DOWNVAL - TIM8CH2_CAPTURE_UPVAL + TIM8_T2; //Time to get the total high level //�õ��ܵĸߵ�ƽ��ʱ��
-				if(abs(Remoter_Ch2-L_Remoter_Ch2)>500)Remoter_Ch2=L_Remoter_Ch2; //Filter //�˲�
+				Remoter_Ch2 = TIM8CH2_CAPTURE_DOWNVAL - TIM8CH2_CAPTURE_UPVAL + TIM8_T2; //Time to get the total high level //�õ��ܵĸߵ�ƽ��ʱ��
+				if(abs(Remoter_Ch2-L_Remoter_Ch2)>500)Remoter_Ch2=L_Remoter_Ch2; //Filter //�˲�
 				L_Remoter_Ch2=Remoter_Ch2;
 				
-				TIM8CH2_CAPTURE_STA = 0; //Capture flag bit to zero	//�����־λ����
-				TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //����Ϊ�����ز���		  
+				TIM8CH2_CAPTURE_STA = 0; //Capture flag bit to zero	//�����־λ����
+				TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //����Ϊ�����ز���		  
 			}
 			else 
 			{
 				//When the capture time occurs but not the falling edge, the first time the rising edge is captured, record the timer value at this time
-				//��������ʱ�䵫�����½��أ���һ�β��������أ���¼��ʱ�Ķ�ʱ������ֵ
-				TIM8CH2_CAPTURE_UPVAL = TIM_GetCapture2(TIM8); //Obtain rising edge data //��ȡ����������
-				TIM8CH2_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //����Ѳ���������
-				TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //����Ϊ�½��ز���
+				//��������ʱ�䵫�����½��أ���һ�β��������أ���¼��ʱ�Ķ�ʱ������ֵ
+				TIM8CH2_CAPTURE_UPVAL = TIM_GetCapture2(TIM8); //Obtain rising edge data //��ȡ����������
+				TIM8CH2_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //����Ѳ���������
+				TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //����Ϊ�½��ز���
 			}
 		}
 	}
-  //Channel 3 //ͨ����
+  //Channel 3 //ͨ����
 	if ((TIM8CH3_CAPTURE_STA & 0X80) == 0)			
 	{
-		if (TIM_GetITStatus(TIM8, TIM_IT_CC3) != RESET)	//A capture event occurred on channel 3 //ͨ��3���������¼�
+		if (TIM_GetITStatus(TIM8, TIM_IT_CC3) != RESET)	//A capture event occurred on channel 3 //ͨ��3���������¼�
 		{
-			TIM_ClearITPendingBit(TIM8, TIM_IT_CC3); //Clear the interrupt flag bit //����жϱ�־λ
-			if (TIM8CH3_CAPTURE_STA & 0X40)	//A falling edge is caught //����һ���½���
+			TIM_ClearITPendingBit(TIM8, TIM_IT_CC3); //Clear the interrupt flag bit //����жϱ�־λ
+			if (TIM8CH3_CAPTURE_STA & 0X40)	//A falling edge is caught //����һ���½���
 			{
-				TIM8CH3_CAPTURE_DOWNVAL = TIM_GetCapture3(TIM8); //Record the timer value at this point //��¼�´�ʱ�Ķ�ʱ������ֵ
+				TIM8CH3_CAPTURE_DOWNVAL = TIM_GetCapture3(TIM8); //Record the timer value at this point //��¼�´�ʱ�Ķ�ʱ������ֵ
 				if (TIM8CH3_CAPTURE_DOWNVAL < TIM8CH3_CAPTURE_UPVAL)
 				{
 					TIM8_T3 = 9999;
 				}
 				else
 					TIM8_T3 = 0;
-				Remoter_Ch3 = TIM8CH3_CAPTURE_DOWNVAL - TIM8CH3_CAPTURE_UPVAL + TIM8_T3; //Time to get the total high level //�õ��ܵĸߵ�ƽ��ʱ��
-				if(abs(Remoter_Ch3-L_Remoter_Ch3)>500)Remoter_Ch3=L_Remoter_Ch3; //Filter //�˲�
+				Remoter_Ch3 = TIM8CH3_CAPTURE_DOWNVAL - TIM8CH3_CAPTURE_UPVAL + TIM8_T3; //Time to get the total high level //�õ��ܵĸߵ�ƽ��ʱ��
+				if(abs(Remoter_Ch3-L_Remoter_Ch3)>500)Remoter_Ch3=L_Remoter_Ch3; //Filter //�˲�
 									L_Remoter_Ch3=Remoter_Ch3;
-				TIM8CH3_CAPTURE_STA = 0; //Capture flag bit to zero	//�����־λ����
-				TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //����Ϊ�����ز���		  
+				TIM8CH3_CAPTURE_STA = 0; //Capture flag bit to zero	//�����־λ����
+				TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //����Ϊ�����ز���		  
 			}
 			else 
 			{
 				//When the capture time occurs but not the falling edge, the first time the rising edge is captured, record the timer value at this time
-				//��������ʱ�䵫�����½��أ���һ�β��������أ���¼��ʱ�Ķ�ʱ������ֵ
-				TIM8CH3_CAPTURE_UPVAL = TIM_GetCapture3(TIM8); //Obtain rising edge data //��ȡ����������
-				TIM8CH3_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //����Ѳ���������
-				TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //����Ϊ�½��ز���
+				//��������ʱ�䵫�����½��أ���һ�β��������أ���¼��ʱ�Ķ�ʱ������ֵ
+				TIM8CH3_CAPTURE_UPVAL = TIM_GetCapture3(TIM8); //Obtain rising edge data //��ȡ����������
+				TIM8CH3_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //����Ѳ���������
+				TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //����Ϊ�½��ز���
 			}
 		}
 	}
 //	
 
-		//Channel 4 //ͨ����
+		//Channel 4 //ͨ����
 		if ((TIM8CH4_CAPTURE_STA & 0X80) == 0)		
 		{
-			if (TIM_GetITStatus(TIM8, TIM_IT_CC4) != RESET)	//A capture event occurred on channel 4 //ͨ��4���������¼�
+			if (TIM_GetITStatus(TIM8, TIM_IT_CC4) != RESET)	//A capture event occurred on channel 4 //ͨ��4���������¼�
 			{
-				TIM_ClearITPendingBit(TIM8, TIM_IT_CC4); //Clear the interrupt flag bit //����жϱ�־λ
-				if (TIM8CH4_CAPTURE_STA & 0X40)	//A falling edge is caught //����һ���½���
+				TIM_ClearITPendingBit(TIM8, TIM_IT_CC4); //Clear the interrupt flag bit //����жϱ�־λ
+				if (TIM8CH4_CAPTURE_STA & 0X40)	//A falling edge is caught //����һ���½���
 				{
-					TIM8CH4_CAPTURE_DOWNVAL = TIM_GetCapture4(TIM8); //Record the timer value at this point //��¼�´�ʱ�Ķ�ʱ������ֵ
+					TIM8CH4_CAPTURE_DOWNVAL = TIM_GetCapture4(TIM8); //Record the timer value at this point //��¼�´�ʱ�Ķ�ʱ������ֵ
 					if (TIM8CH4_CAPTURE_DOWNVAL < TIM8CH4_CAPTURE_UPVAL)
 					{
 						TIM8_T4 = 9999;
 					}
 					else
 						TIM8_T4 = 0;
-					Remoter_Ch4 = TIM8CH4_CAPTURE_DOWNVAL - TIM8CH4_CAPTURE_UPVAL + TIM8_T4; //Time to get the total high level //�õ��ܵĸߵ�ƽ��ʱ��
-					if(abs(Remoter_Ch4-L_Remoter_Ch4)>500)Remoter_Ch4=L_Remoter_Ch4; //Filter //�˲�
+					Remoter_Ch4 = TIM8CH4_CAPTURE_DOWNVAL - TIM8CH4_CAPTURE_UPVAL + TIM8_T4; //Time to get the total high level //�õ��ܵĸߵ�ƽ��ʱ��
+					if(abs(Remoter_Ch4-L_Remoter_Ch4)>500)Remoter_Ch4=L_Remoter_Ch4; //Filter //�˲�
 					L_Remoter_Ch4=Remoter_Ch4;				
-					TIM8CH4_CAPTURE_STA = 0; //Capture flag bit to zero	//�����־λ����
-					TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //����Ϊ�����ز���		  
+					TIM8CH4_CAPTURE_STA = 0; //Capture flag bit to zero	//�����־λ����
+					TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //����Ϊ�����ز���		  
 				}
 				else 
 				{
 					//When the capture time occurs but not the falling edge, the first time the rising edge is captured, record the timer value at this time
-				  //��������ʱ�䵫�����½��أ���һ�β��������أ���¼��ʱ�Ķ�ʱ������ֵ
-					TIM8CH4_CAPTURE_UPVAL = TIM_GetCapture4(TIM8); //Obtain rising edge data //��ȡ����������
-					TIM8CH4_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //����Ѳ���������
-					TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //����Ϊ�½��ز���
+				  //��������ʱ�䵫�����½��أ���һ�β��������أ���¼��ʱ�Ķ�ʱ������ֵ
+					TIM8CH4_CAPTURE_UPVAL = TIM_GetCapture4(TIM8); //Obtain rising edge data //��ȡ����������
+					TIM8CH4_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //����Ѳ���������
+					TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //����Ϊ�½��ز���
 				}
 			}
 		}
@@ -312,25 +312,25 @@ void TIM8_CC_IRQHandler(void)
 Function: TIM1 Update Interrupt
 Input   : none
 Output  : none
-�������ܣ���ʱ��8�����ж�
-��ڲ�������
-����  ֵ���� 
+�������ܣ���ʱ��8�����ж�
+��ڲ�������
+����  ֵ���� 
 **************************************************************************/
 void TIM8_UP_TIM13_IRQHandler(void) 
 { 
 	//Clear the interrupt flag bit
-	//����жϱ�־λ 
+	//����жϱ�־λ 
   TIM8->SR&=~(1<<0);	    
 }
 
 void TIM8_SERVO_Init(u16 arr,u16 psc)
 {
 	GPIO_InitTypeDef GPIO_InitStructure;           //IO
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //��ʱ��
-	TIM_OCInitTypeDef  TIM_OCInitStructure;        //PWM���
+	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //��ʱ��
+	TIM_OCInitTypeDef  TIM_OCInitStructure;        //PWM���
 	
-	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);  	//TIM1ʱ��ʹ��    
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); 	//ʹ��PORTEʱ��	
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);  	//TIM1ʱ��ʹ��    
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); 	//ʹ��PORTEʱ��	
 		
 	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF; 
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;
@@ -345,55 +345,55 @@ void TIM8_SERVO_Init(u16 arr,u16 psc)
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_TIM8); 
 
 
-	/*** Initialize timer 1 || ��ʼ����ʱ��1 ***/
-	//Set the counter to automatically reload //�趨�������Զ���װֵ 
+	/*** Initialize timer 1 || ��ʼ����ʱ��1 ***/
+	//Set the counter to automatically reload //�趨�������Զ���װֵ 
 	TIM_TimeBaseStructure.TIM_Period = arr; 
-	//Pre-divider //Ԥ��Ƶ�� 
+	//Pre-divider //Ԥ��Ƶ�� 
 	TIM_TimeBaseStructure.TIM_Prescaler = psc; 	
-	//Set the clock split: TDTS = Tck_tim //����ʱ�ӷָ�:TDTS = Tck_tim
+	//Set the clock split: TDTS = Tck_tim //����ʱ�ӷָ�:TDTS = Tck_tim
 	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
-	//TIM up count mode //TIM���ϼ���ģʽ	
+	//TIM up count mode //TIM���ϼ���ģʽ	
 	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 
 	//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
-	//����TIM_TimeBaseInitStruct��ָ���IJ�����ʼ��TIMx��ʱ�������λ
+	//����TIM_TimeBaseInitStruct��ָ���IJ�����ʼ��TIMx��ʱ�������λ
 	TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); 
 
 
-	//-----------�����ʼ��-----------//
+	//-----------�����ʼ��-----------//
 	//Select Timer mode :TIM Pulse Width Modulation mode 1
-  //ѡ��ʱ��ģʽ:TIM������ȵ���ģʽ1
+  //ѡ��ʱ��ģʽ:TIM������ȵ���ģʽ1
  	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; 
 	//Compare output enablement
-	//�Ƚ����ʹ��
+	//�Ƚ����ʹ��
 	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; 
 	//Set the pulse value of the capture comparison register to be loaded
-	//���ô�װ�벶��ȽϼĴ���������ֵ
+	//���ô�װ�벶��ȽϼĴ���������ֵ
 	TIM_OCInitStructure.TIM_Pulse = 0; 
   //Output polarity :TIM output polarity is higher	
-  //�������:TIM����Ƚϼ��Ը�	
+  //�������:TIM����Ƚϼ��Ը�	
 	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;   
 	TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;   
 	
 	//Initialize the peripheral TIMX based on the parameter specified in TIM_OCINITSTRUCT
-  //����TIM_OCInitStruct��ָ���IJ�����ʼ������TIMx	
+  //����TIM_OCInitStruct��ָ���IJ�����ʼ������TIMx	
 	TIM_OC1Init(TIM8, &TIM_OCInitStructure); 
 	TIM_OC2Init(TIM8, &TIM_OCInitStructure); 
 	TIM_OC3Init(TIM8, &TIM_OCInitStructure); 
 	TIM_OC4Init(TIM8, &TIM_OCInitStructure); 
 	//Channel preload enable
-	//ͨ��Ԥװ��ʹ��	 
+	//ͨ��Ԥװ��ʹ��	 
 	TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
 	TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
 	TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);
 	TIM_OC4PreloadConfig(TIM8, TIM_OCPreload_Enable);
-	//-----------�����ʼ��-----------//
+	//-----------�����ʼ��-----------//
 
 	TIM_CtrlPWMOutputs(TIM8,ENABLE); 	
-	//Enable timer //ʹ�ܶ�ʱ��
+	//Enable timer //ʹ�ܶ�ʱ��
 	TIM_Cmd(TIM8, ENABLE); 		 
 
   //The channel value is initialized to 1500, corresponding to the steering gear zero
-	//ͨ��ֵ��ʼ��Ϊ1500���������Ӧֵ
+	//ͨ��ֵ��ʼ��Ϊ1500���������Ӧֵ
 //	TIM8->CCR1=1500;
 //	TIM8->CCR2=1500;
 //	TIM8->CCR3=1500;
@@ -404,11 +404,11 @@ void TIM8_SERVO_Init(u16 arr,u16 psc)
 void TIM12_SERVO_Init(u16 arr,u16 psc)
 {
 	GPIO_InitTypeDef GPIO_InitStructure;           //IO
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //��ʱ��
-	TIM_OCInitTypeDef  TIM_OCInitStructure;        //PWM���
+	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //��ʱ��
+	TIM_OCInitTypeDef  TIM_OCInitStructure;        //PWM���
 	
-	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE);  	//TIM1ʱ��ʹ��    
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); 	//ʹ��PORTEʱ��	
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE);  	//TIM1ʱ��ʹ��    
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); 	//ʹ��PORTEʱ��	
 		
 	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF; 
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14|GPIO_Pin_15;
@@ -421,48 +421,48 @@ void TIM12_SERVO_Init(u16 arr,u16 psc)
 	GPIO_PinAFConfig(GPIOB,GPIO_PinSource15,GPIO_AF_TIM12); 
 
 
-	/*** Initialize timer 1 || ��ʼ����ʱ��1 ***/
-	//Set the counter to automatically reload //�趨�������Զ���װֵ 
+	/*** Initialize timer 1 || ��ʼ����ʱ��1 ***/
+	//Set the counter to automatically reload //�趨�������Զ���װֵ 
 	TIM_TimeBaseStructure.TIM_Period = arr; 
-	//Pre-divider //Ԥ��Ƶ�� 
+	//Pre-divider //Ԥ��Ƶ�� 
 	TIM_TimeBaseStructure.TIM_Prescaler = psc; 	
-	//Set the clock split: TDTS = Tck_tim //����ʱ�ӷָ�:TDTS = Tck_tim
+	//Set the clock split: TDTS = Tck_tim //����ʱ�ӷָ�:TDTS = Tck_tim
 	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
-	//TIM up count mode //TIM���ϼ���ģʽ	
+	//TIM up count mode //TIM���ϼ���ģʽ	
 	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 
 	//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
-	//����TIM_TimeBaseInitStruct��ָ���IJ�����ʼ��TIMx��ʱ�������λ
+	//����TIM_TimeBaseInitStruct��ָ���IJ�����ʼ��TIMx��ʱ�������λ
 	TIM_TimeBaseInit(TIM12, &TIM_TimeBaseStructure); 
 
 
-	//-----------�����ʼ��-----------//
+	//-----------�����ʼ��-----------//
 	//Select Timer mode :TIM Pulse Width Modulation mode 1
-  //ѡ��ʱ��ģʽ:TIM������ȵ���ģʽ1
+  //ѡ��ʱ��ģʽ:TIM������ȵ���ģʽ1
  	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; 
 	//Compare output enablement
-	//�Ƚ����ʹ��
+	//�Ƚ����ʹ��
 	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; 
 	//Set the pulse value of the capture comparison register to be loaded
-	//���ô�װ�벶��ȽϼĴ���������ֵ
+	//���ô�װ�벶��ȽϼĴ���������ֵ
 	TIM_OCInitStructure.TIM_Pulse = 0; 
   //Output polarity :TIM output polarity is higher	
-  //�������:TIM����Ƚϼ��Ը�	
+  //�������:TIM����Ƚϼ��Ը�	
 	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;   
 	TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;   
 	
 	//Initialize the peripheral TIMX based on the parameter specified in TIM_OCINITSTRUCT
-  //����TIM_OCInitStruct��ָ���IJ�����ʼ������TIMx	
+  //����TIM_OCInitStruct��ָ���IJ�����ʼ������TIMx	
 	TIM_OC1Init(TIM12, &TIM_OCInitStructure); 
 		TIM_OC2Init(TIM12, &TIM_OCInitStructure); 
 	//Channel preload enable
-	//ͨ��Ԥװ��ʹ��	 
+	//ͨ��Ԥװ��ʹ��	 
 	TIM_OC1PreloadConfig(TIM12, TIM_OCPreload_Enable);
 		TIM_OC2PreloadConfig(TIM12, TIM_OCPreload_Enable);
-	//-----------�����ʼ��-----------//
+	//-----------�����ʼ��-----------//
 
 
 	TIM_CtrlPWMOutputs(TIM12,ENABLE); 	
-	//Enable timer //ʹ�ܶ�ʱ��
+	//Enable timer //ʹ�ܶ�ʱ��
 	TIM_Cmd(TIM12, ENABLE); 		 
 
   //Initialize both servo channels to the configured steering center.
@@ -471,3 +471,83 @@ void TIM12_SERVO_Init(u16 arr,u16 psc)
 
 }
 
+/**************************************************************************
+Function: Free-running microsecond time base for sensor timestamps
+�������ܣ��������ڴ���������ʱ���΢�뼶����ʱ��׼
+TIM7 counts at 1 MHz (1 tick = 1 us) and wraps every 65.536 ms. An update
+interrupt increments a 32-bit high word, extending the clock to 48 usable
+bits (~8.9 years) that only ever counts up. Timers TIM2-TIM5 are taken by
+the encoders and TIM8 by input capture, so TIM7 is the free APB1 timer.
+TIM7 �� 1MHz ���������� 65.536ms ���ƣ����¸��ж���λ�� 32λ���ָߣ�
+��չ��ֻ��������ĵ���ʱ�䡣TIM2~TIM5 ���������ã�TIM8 ���벶�����ã�
+�� TIM7 ����
+**************************************************************************/
+static volatile uint32_t g_time_high = 0;
+
+void TIM7_Init(void)
+{
+	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+	NVIC_InitTypeDef NVIC_InitStructure;
+
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
+
+	//APB1 timer clock = 84 MHz, prescaler 84 -> 1 MHz (1 us per tick).
+	//APB1 ��ʱ��ʱ�� 84MHz��Ԥ��Ƶ 84 -> 1MHz��ÿ���� 1us��
+	TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+	TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1;
+	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
+	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+	TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStructure);
+
+	TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
+	TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE);
+
+	//Keep the overflow ISR below configMAX_SYSCALL_INTERRUPT_PRIORITY so it
+	//never calls FreeRTOS APIs; it only bumps a counter.
+	//�жϲ����� FreeRTOS API��������һ���������ȼ��ϵͣ��������ȿ��ơ�
+	NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+
+	TIM_Cmd(TIM7, ENABLE);
+}
+
+void TIM7_IRQHandler(void)
+{
+	if(TIM_GetITStatus(TIM7, TIM_IT_Update) != RESET)
+	{
+		TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
+		g_time_high++;
+	}
+}
+
+/**************************************************************************
+Function: Read the 64-bit monotonic microsecond timestamp
+�������ܣ���ȡ 64λ��������΢��ʱ���
+Glitch-free composition: re-read the high word around CNT and, if the
+update flag is pending while CNT is still low, account for the overflow
+that the ISR has not serviced yet.
+��ֹ��϶���ѣ��� CNT ��Χ���ζ��߰��֣����������ط����� CNT ��ϵ�ͣ�
+˵�� ISR ��δ�����ûأ��ֶ���λ��
+**************************************************************************/
+uint64_t mcu_time_us(void)
+{
+	uint32_t high1, high2, cnt, sr;
+
+	do {
+		high1 = g_time_high;
+		cnt = TIM7->CNT;
+		sr = TIM7->SR;
+		high2 = g_time_high;
+	} while(high1 != high2);
+
+	//Overflow happened but the update ISR has not run yet.
+	//�ѻ��Ƶ����¸��ж���δִ�С�
+	if((sr & TIM_SR_UIF) && cnt < 0x8000U)
+		high1++;
+
+	return ((uint64_t)high1 << 16) | cnt;
+}
+
diff --git a/HARDWARE/timer.h b/HARDWARE/timer.h
index 598f2ee..203a56f 100644
--- a/HARDWARE/timer.h
+++ b/HARDWARE/timer.h
@@ -4,6 +4,8 @@
 void TIM8_Cap_Init(u16 arr, u16 psc);
 void TIM12_SERVO_Init(u16 arr,u16 psc);
 void TIM8_SERVO_Init(u16 arr,u16 psc);
+void TIM7_Init(void);
+uint64_t mcu_time_us(void);
 
 extern int L_Remoter_Ch1,L_Remoter_Ch2,L_Remoter_Ch3,L_Remoter_Ch4;
 extern int Remoter_Ch1,Remoter_Ch2,Remoter_Ch3,Remoter_Ch4;
diff --git a/HARDWARE/usartx.c b/HARDWARE/usartx.c
index 9fa47ef..59e960f 100644
--- a/HARDWARE/usartx.c
+++ b/HARDWARE/usartx.c
@@ -3,13 +3,22 @@ SEND_DATA Send_Data;
 RECEIVE_DATA Receive_Data;
 extern int Time_count;
 
+//Sample timestamps written by the acquisition tasks (see usartx.h).
+//Session ID stays constant for one MCU boot; a fixed nonzero marker is
+//enough for the host to detect a restart (counter cleared to 0).
+//�ɲɼ�����д��IJ���ʱ���(�� usartx.h)���ỰID�ڱ���������������䣬
+//�̶���0ֵ���ϴ��������ʶ���ػ���
+volatile unsigned long long g_speed_sample_time_us = 0;
+volatile unsigned long long g_imu_sample_time_us = 0;
+volatile unsigned int g_session_id = 0xA5A5A5A5U;
+
 /**************************************************************************
 Function: Usartx3, Usartx1,Usartx5 and CAN send data task 
 Input   : none
 Output  : none
-�������ܣ�����3������1������5��CAN������������
-��ڲ�������
-����  ֵ����
+�������ܣ�����3������1������5��CAN������������
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void data_task(void *pvParameters)
 {
@@ -17,25 +26,22 @@ void data_task(void *pvParameters)
 	
    while(1)
     {	
-			//The task is run at 20hz
-			//��������20Hz��Ƶ������
-			vTaskDelayUntil(&lastWakeTime, F2T(RATE_20_HZ));
+			//The task is run at 200hz. Only ROS (USART3) is sent, via non-blocking DMA.
+			//��������200Hz��Ƶ��������ֻ��ROS(����3)��������DMA���巢�͡�
+			vTaskDelayUntil(&lastWakeTime, F2T(RATE_200_HZ));
 			//Assign the data to be sent
-			//��Ҫ���з��͵����ݽ��и�ֵ
-			data_transition(); 
-			USART1_SEND();     //Serial port 1 sends data //����1��������
-			USART3_SEND();     //Serial port 3 (ROS) sends data  //����3(ROS)��������
-			USART5_SEND();		 //Serial port 5 sends data //����5��������
-			CAN_SEND();        //CAN send data //CAN��������	
+			//��Ҫ���з��͵����ݽ��и�ֵ
+			data_transition();
+			USART3_SEND();     //Serial port 3 (ROS) sends data  //����3(ROS)��������
 		}
 }
 /**************************************************************************
 Function: The data sent by the serial port is assigned
 Input   : none
 Output  : none
-�������ܣ����ڷ��͵����ݽ��и�ֵ
-��ڲ�������
-����  ֵ����
+�������ܣ����ڷ��͵����ݽ��и�ֵ
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void data_transition(void)
 {
@@ -44,7 +50,7 @@ void data_transition(void)
 	
 	//According to different vehicle types, different kinematics algorithms were selected to carry out the forward kinematics solution, 
 	//and the three-axis velocity was obtained from each wheel velocity
-	//���ݲ�ͬ����ѡ��ͬ�˶�ѧ�㷨�����˶�ѧ���⣬�Ӹ������ٶ���������ٶ�
+	//���ݲ�ͬ����ѡ��ͬ�˶�ѧ�㷨�����˶�ѧ���⣬�Ӹ������ٶ���������ٶ�
 	switch(Car_Mode)
 	{	
 		case Mec_Car:      
@@ -84,32 +90,32 @@ void data_transition(void)
 			break; 
 	}
 	
-	//The acceleration of the triaxial acceleration //���ٶȼ�������ٶ�
-	Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; //The accelerometer Y-axis is converted to the ros coordinate X axis //���ٶȼ�Y��ת����ROS����X��
-	Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; //The accelerometer X-axis is converted to the ros coordinate y axis //���ٶȼ�X��ת����ROS����Y��
-	Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; //The accelerometer Z-axis is converted to the ros coordinate Z axis //���ٶȼ�Z��ת����ROS����Z��
+	//The acceleration of the triaxial acceleration //���ٶȼ�������ٶ�
+	Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; //The accelerometer Y-axis is converted to the ros coordinate X axis //���ٶȼ�Y��ת����ROS����X��
+	Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; //The accelerometer X-axis is converted to the ros coordinate y axis //���ٶȼ�X��ת����ROS����Y��
+	Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; //The accelerometer Z-axis is converted to the ros coordinate Z axis //���ٶȼ�Z��ת����ROS����Z��
 	
-	//The Angle velocity of the triaxial velocity //���ٶȼ�������ٶ�
-	Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; //The Y-axis is converted to the ros coordinate X axis //���ٶȼ�Y��ת����ROS����X��
-	Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; //The X-axis is converted to the ros coordinate y axis //���ٶȼ�X��ת����ROS����Y��
+	//The Angle velocity of the triaxial velocity //���ٶȼ�������ٶ�
+	Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; //The Y-axis is converted to the ros coordinate X axis //���ٶȼ�Y��ת����ROS����X��
+	Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; //The X-axis is converted to the ros coordinate y axis //���ٶȼ�X��ת����ROS����Y��
 	if(Flag_Stop==0) 
 		//If the motor control bit makes energy state, the z-axis velocity is sent normall
-	  //����������λʹ��״̬����ô��������Z����ٶ�
+	  //����������λʹ��״̬����ô��������Z����ٶ�
 		Send_Data.Sensor_Str.Gyroscope.Z_data=gyro[2];  
 	else  
 		//If the robot is static (motor control dislocation), the z-axis is 0
-    //����������Ǿ�ֹ�ģ��������λʧ�ܣ�����ô���͵�Z����ٶ�Ϊ0		
+    //����������Ǿ�ֹ�ģ��������λʧ�ܣ�����ô���͵�Z����ٶ�Ϊ0		
 		Send_Data.Sensor_Str.Gyroscope.Z_data=0;        
 	
 	//Battery voltage (this is a thousand times larger floating point number, which will be reduced by a thousand times as well as receiving the data).
-	//��ص�ѹ(���ｫ�������Ŵ�һǧ�����䣬��Ӧ���ڽ��ն��ڽ��յ����ݺ�Ҳ����Сһǧ��)
+	//��ص�ѹ(���ｫ�������Ŵ�һǧ�����䣬��Ӧ���ڽ��ն��ڽ��յ����ݺ�Ҳ����Сһǧ��)
 	Send_Data.Sensor_Str.Power_Voltage = Voltage*1000; 
 	
 	Send_Data.buffer[0]=Send_Data.Sensor_Str.Frame_Header; //Frame_heade //֡ͷ
-  Send_Data.buffer[1]=Flag_Stop; //Car software loss marker //С������ʧ�ܱ�־λ
+  Send_Data.buffer[1]=Flag_Stop; //Car software loss marker //С������ʧ�ܱ�־λ
 	
 	//The three-axis speed of / / car is split into two eight digit Numbers
-	//С�������ٶ�,���ᶼ���Ϊ����8λ�����ٷ���
+	//С�������ٶ�,���ᶼ���Ϊ����8λ�����ٷ���
 	Send_Data.buffer[2]=Send_Data.Sensor_Str.X_speed >>8; 
 	Send_Data.buffer[3]=Send_Data.Sensor_Str.X_speed ;    
 	Send_Data.buffer[4]=Send_Data.Sensor_Str.Y_speed>>8;  
@@ -118,7 +124,7 @@ void data_transition(void)
 	Send_Data.buffer[7]=Send_Data.Sensor_Str.Z_speed ;    
 	
 	//The acceleration of the triaxial axis of / / imu accelerometer is divided into two eight digit reams
-	//IMU���ٶȼ�������ٶ�,���ᶼ���Ϊ����8λ�����ٷ���
+	//IMU���ٶȼ�������ٶ�,���ᶼ���Ϊ����8λ�����ٷ���
 	Send_Data.buffer[8]=Send_Data.Sensor_Str.Accelerometer.X_data>>8; 
 	Send_Data.buffer[9]=Send_Data.Sensor_Str.Accelerometer.X_data;   
 	Send_Data.buffer[10]=Send_Data.Sensor_Str.Accelerometer.Y_data>>8;
@@ -127,7 +133,7 @@ void data_transition(void)
 	Send_Data.buffer[13]=Send_Data.Sensor_Str.Accelerometer.Z_data;
 	
 	//The axis of the triaxial velocity of the / /imu is divided into two eight digits
-	//IMU���ٶȼ�������ٶ�,���ᶼ���Ϊ����8λ�����ٷ���
+	//IMU���ٶȼ�������ٶ�,���ᶼ���Ϊ����8λ�����ٷ���
 	Send_Data.buffer[14]=Send_Data.Sensor_Str.Gyroscope.X_data>>8;
 	Send_Data.buffer[15]=Send_Data.Sensor_Str.Gyroscope.X_data;
 	Send_Data.buffer[16]=Send_Data.Sensor_Str.Gyroscope.Y_data>>8;
@@ -136,72 +142,112 @@ void data_transition(void)
 	Send_Data.buffer[19]=Send_Data.Sensor_Str.Gyroscope.Z_data;
 	
 	//Battery voltage, split into two 8 digit Numbers
-	//��ص�ѹ,���Ϊ����8λ���ݷ���
-	Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8; 
-	Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage; 
+	//��ص�ѹ,���Ϊ����8λ���ݷ���
+	Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8;
+	Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage;
 
-  //Data check digit calculation, Pattern 1 is a data check
-  //����У��λ���㣬ģʽ1�Ƿ�������У��
-	Send_Data.buffer[22]=Check_Sum(22,1); 
-	
-	Send_Data.buffer[23]=Send_Data.Sensor_Str.Frame_Tail; //Frame_tail //֡β
+	//Snapshot the session ID and the two sample timestamps captured by the
+	//acquisition tasks. All appended fields are big-endian (high byte first)
+	//to match the existing >>8 speed/IMU packing.
+	//�������Բɼ�������¼�ĻỰID��������ʱ�����׷���ֶ��þ���(��λ��ǰ)��
+	//�����е� >>8 ���/IMU�����ʽһ�¡�
+	Send_Data.Sensor_Str.Session_Id     = g_session_id;
+	Send_Data.Sensor_Str.Speed_Time_us  = g_speed_sample_time_us;
+	Send_Data.Sensor_Str.Imu_Time_us    = g_imu_sample_time_us;
+
+	//session_id (uint32, big-endian) //�ỰID
+	Send_Data.buffer[22]=(unsigned char)(Send_Data.Sensor_Str.Session_Id>>24);
+	Send_Data.buffer[23]=(unsigned char)(Send_Data.Sensor_Str.Session_Id>>16);
+	Send_Data.buffer[24]=(unsigned char)(Send_Data.Sensor_Str.Session_Id>>8);
+	Send_Data.buffer[25]=(unsigned char)(Send_Data.Sensor_Str.Session_Id);
+
+	//speed_sample_time_us (uint64, big-endian) //�ٶȲ���ʱ���
+	Send_Data.buffer[26]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>56);
+	Send_Data.buffer[27]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>48);
+	Send_Data.buffer[28]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>40);
+	Send_Data.buffer[29]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>32);
+	Send_Data.buffer[30]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>24);
+	Send_Data.buffer[31]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>16);
+	Send_Data.buffer[32]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>8);
+	Send_Data.buffer[33]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us);
+
+	//imu_sample_time_us (uint64, big-endian) //IMU����ʱ���
+	Send_Data.buffer[34]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>56);
+	Send_Data.buffer[35]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>48);
+	Send_Data.buffer[36]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>40);
+	Send_Data.buffer[37]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>32);
+	Send_Data.buffer[38]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>24);
+	Send_Data.buffer[39]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>16);
+	Send_Data.buffer[40]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>8);
+	Send_Data.buffer[41]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us);
+
+  //Data check digit calculation, Pattern 1 is a data check. Now covers the
+  //42 payload bytes (0..41) preceding the checksum.
+  //����У��λ���㣬ģʽ1�Ƿ�������У�顣����������У��ǰ 42 �ֽ�(0..41)��
+	Send_Data.buffer[42]=Check_Sum(42,1);
+
+	Send_Data.buffer[43]=Send_Data.Sensor_Str.Frame_Tail; //Frame_tail //֡β
 }
 /**************************************************************************
 Function: Serial port 1 sends data
 Input   : none
 Output  : none
-�������ܣ�����1��������
-��ڲ�������
-����  ֵ����
+�������ܣ�����1��������
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void USART1_SEND(void)
 {
-  unsigned char i = 0;	
-	
-	for(i=0; i<24; i++)
+  unsigned char i = 0;
+
+	for(i=0; i<SEND_DATA_SIZE; i++)
 	{
 		usart1_send(Send_Data.buffer[i]);
-	}	 
+	}
 }
 /**************************************************************************
 Function: Serial port 3 sends data
 Input   : none
 Output  : none
-�������ܣ�����3��������
-��ڲ�������
-����  ֵ����
+�������ܣ�����3��������
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void USART3_SEND(void)
 {
-  unsigned char i = 0;	
-	for(i=0; i<24; i++)
-	{
-		usart3_send(Send_Data.buffer[i]);
-	}	 
+	//Non-blocking DMA transmit of the 24-byte frame. If the previous transfer
+	//is still running, skip this cycle rather than busy-wait.
+	//DMA���������24�ֽ�֡����һ֡δ��������������ڣ���æ�ȴ���
+	if(DMA_GetCmdStatus(DMA1_Stream3) != DISABLE) return;
+
+	DMA_ClearFlag(DMA1_Stream3, DMA_FLAG_TCIF3 | DMA_FLAG_HTIF3 |
+	              DMA_FLAG_TEIF3 | DMA_FLAG_DMEIF3 | DMA_FLAG_FEIF3);
+	DMA_SetCurrDataCounter(DMA1_Stream3, SEND_DATA_SIZE);
+	DMA_Cmd(DMA1_Stream3, ENABLE);
 }
 /**************************************************************************
 Function: Serial port 5 sends data
 Input   : none
 Output  : none
-�������ܣ�����5��������
-��ڲ�������
-����  ֵ����
+�������ܣ�����5��������
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void USART5_SEND(void)
 {
-  unsigned char i = 0;	
-	for(i=0; i<24; i++)
+  unsigned char i = 0;
+	for(i=0; i<SEND_DATA_SIZE; i++)
 	{
 		usart5_send(Send_Data.buffer[i]);
-	}	 
+	}
 }
 /**************************************************************************
 Function: CAN sends data
 Input   : none
 Output  : none
-�������ܣ�CAN��������
-��ڲ�������
-�� �� ֵ����
+�������ܣ�CAN��������
+��ڲ�������
+�� �� ֵ����
 **************************************************************************/
 void CAN_SEND(void) 
 {
@@ -229,9 +275,9 @@ void CAN_SEND(void)
 Function: Serial port 1 initialization
 Input   : none
 Output  : none
-�������ܣ�����1��ʼ��
-��ڲ�������
-�� �� ֵ����
+�������ܣ�����1��ʼ��
+��ڲ�������
+�� �� ֵ����
 **************************************************************************/
 void uart1_init(u32 bound)
 {  	 
@@ -239,50 +285,50 @@ void uart1_init(u32 bound)
 	USART_InitTypeDef USART_InitStructure;
 	NVIC_InitTypeDef NVIC_InitStructure;
 	
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);	 //Enable the gpio clock //ʹ��GPIOʱ��
-	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); //Enable the Usart clock //ʹ��USARTʱ��
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);	 //Enable the gpio clock //ʹ��GPIOʱ��
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); //Enable the Usart clock //ʹ��USARTʱ��
 
 	GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);	
 	GPIO_PinAFConfig(GPIOA,GPIO_PinSource10 ,GPIO_AF_USART1);	 
 	
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9|GPIO_Pin_10;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
-	GPIO_Init(GPIOA, &GPIO_InitStructure);  		          //��ʼ��
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
+	GPIO_Init(GPIOA, &GPIO_InitStructure);  		          //��ʼ��
 	
-  //UsartNVIC configuration //UsartNVIC����
+  //UsartNVIC configuration //UsartNVIC����
 	NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
-	//Preempt priority //��ռ���ȼ�
+	//Preempt priority //��ռ���ȼ�
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1 ;
-	//Subpriority //�����ȼ�
+	//Subpriority //�����ȼ�
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
-	//Enable the IRQ channel //IRQͨ��ʹ��
+	//Enable the IRQ channel //IRQͨ��ʹ��
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
   //Initialize the VIC register with the specified parameters 
-	//����ָ���IJ�����ʼ��VIC�Ĵ���	
+	//����ָ���IJ�����ʼ��VIC�Ĵ���	
 	NVIC_Init(&NVIC_InitStructure);	
 	
-  //USART Initialization Settings ��ʼ������
-	USART_InitStructure.USART_BaudRate = bound; //Port rate //���ڲ�����
-	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //�ֳ�Ϊ8λ���ݸ�ʽ
-	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //һ��ֹͣλ
-	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //����żУ��λ
-	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //��Ӳ������������
-	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //�շ�ģʽ
-	USART_Init(USART1, &USART_InitStructure); //Initialize serial port 1 //��ʼ������1
+  //USART Initialization Settings ��ʼ������
+	USART_InitStructure.USART_BaudRate = bound; //Port rate //���ڲ�����
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //�ֳ�Ϊ8λ���ݸ�ʽ
+	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //һ��ֹͣλ
+	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //����żУ��λ
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //��Ӳ������������
+	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //�շ�ģʽ
+	USART_Init(USART1, &USART_InitStructure); //Initialize serial port 1 //��ʼ������1
 	
-	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //�������ڽ����ж�
-	USART_Cmd(USART1, ENABLE);                     //Enable serial port 1 //ʹ�ܴ���1
+	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //�������ڽ����ж�
+	USART_Cmd(USART1, ENABLE);                     //Enable serial port 1 //ʹ�ܴ���1
 }
 /**************************************************************************
 Function: Serial port 2 initialization
 Input   : none
 Output  : none
-�������ܣ�����2��ʼ��
-��ڲ�������
-����  ֵ����
+�������ܣ�����2��ʼ��
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void uart2_init(u32 bound)
 {  	 
@@ -290,50 +336,50 @@ void uart2_init(u32 bound)
 	USART_InitTypeDef USART_InitStructure;
 	NVIC_InitTypeDef NVIC_InitStructure;
 
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //ʹ��GPIOʱ��
-	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); //Enable the Usart clock //ʹ��USARTʱ��
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //ʹ��GPIOʱ��
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); //Enable the Usart clock //ʹ��USARTʱ��
 	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource5,GPIO_AF_USART2);	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource6 ,GPIO_AF_USART2);	 
 	
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5|GPIO_Pin_6;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
-	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //��ʼ��
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
+	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //��ʼ��
 	
-	//UsartNVIC configuration //UsartNVIC����
+	//UsartNVIC configuration //UsartNVIC����
 	NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
-	//Preempt priority //��ռ���ȼ�
+	//Preempt priority //��ռ���ȼ�
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1 ;
-	//Subpriority //�����ȼ�
+	//Subpriority //�����ȼ�
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;	
-  //Enable the IRQ channel //IRQͨ��ʹ��	
+  //Enable the IRQ channel //IRQͨ��ʹ��	
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   //Initialize the VIC register with the specified parameters 
-	//����ָ���IJ�����ʼ��VIC�Ĵ���		
+	//����ָ���IJ�����ʼ��VIC�Ĵ���		
 	NVIC_Init(&NVIC_InitStructure);	
 	
-	//USART Initialization Settings ��ʼ������
-	USART_InitStructure.USART_BaudRate = bound; //Port rate //���ڲ�����
-	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //�ֳ�Ϊ8λ���ݸ�ʽ
-	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //һ��ֹͣ
-	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //����żУ��λ
-	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //��Ӳ������������
-	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //�շ�ģʽ
-	USART_Init(USART2, &USART_InitStructure);      //Initialize serial port 2 //��ʼ������2
+	//USART Initialization Settings ��ʼ������
+	USART_InitStructure.USART_BaudRate = bound; //Port rate //���ڲ�����
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //�ֳ�Ϊ8λ���ݸ�ʽ
+	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //һ��ֹͣ
+	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //����żУ��λ
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //��Ӳ������������
+	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //�շ�ģʽ
+	USART_Init(USART2, &USART_InitStructure);      //Initialize serial port 2 //��ʼ������2
 	
-	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //�������ڽ����ж�
-	USART_Cmd(USART2, ENABLE);                     //Enable serial port 2 //ʹ�ܴ���2 
+	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //�������ڽ����ж�
+	USART_Cmd(USART2, ENABLE);                     //Enable serial port 2 //ʹ�ܴ���2 
 }
 /**************************************************************************
 Function: Serial port 3 initialization
 Input   : none
 Output  : none
-�������ܣ�����3��ʼ��
-��ڲ�������
-����  ֵ����
+�������ܣ�����3��ʼ��
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void uart3_init(u32 bound)
 {  	 
@@ -341,50 +387,86 @@ void uart3_init(u32 bound)
 	USART_InitTypeDef USART_InitStructure;
 	NVIC_InitTypeDef NVIC_InitStructure;
 	
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //ʹ��GPIOʱ��
-  RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); //Enable the Usart clock //ʹ��USARTʱ��
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //ʹ��GPIOʱ��
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); //Enable the Usart clock //ʹ��USARTʱ��
 	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource8,GPIO_AF_USART3);	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource9,GPIO_AF_USART3);	 
 	
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
-	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //��ʼ��
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
+	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //��ʼ��
 	
-  //UsartNVIC configuration //UsartNVIC����
+  //UsartNVIC configuration //UsartNVIC����
   NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
-	//Preempt priority //��ռ���ȼ�
+	//Preempt priority //��ռ���ȼ�
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;
-	//Preempt priority //��ռ���ȼ�
+	//Preempt priority //��ռ���ȼ�
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
-	//Enable the IRQ channel //IRQͨ��ʹ��	
+	//Enable the IRQ channel //IRQͨ��ʹ��	
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
   //Initialize the VIC register with the specified parameters 
-	//����ָ���IJ�����ʼ��VIC�Ĵ���		
+	//����ָ���IJ�����ʼ��VIC�Ĵ���		
 	NVIC_Init(&NVIC_InitStructure);
 	
-  //USART Initialization Settings ��ʼ������
-	USART_InitStructure.USART_BaudRate = bound; //Port rate //���ڲ�����
-	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //�ֳ�Ϊ8λ���ݸ�ʽ
-	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //һ��ֹͣ
-	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //����żУ��λ
-	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //��Ӳ������������
-	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //�շ�ģʽ
-  USART_Init(USART3, &USART_InitStructure);      //Initialize serial port 3 //��ʼ������3
+  //USART Initialization Settings ��ʼ������
+	USART_InitStructure.USART_BaudRate = bound; //Port rate //���ڲ�����
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //�ֳ�Ϊ8λ���ݸ�ʽ
+	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //һ��ֹͣ
+	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //����żУ��λ
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //��Ӳ������������
+	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //�շ�ģʽ
+  USART_Init(USART3, &USART_InitStructure);      //Initialize serial port 3 //��ʼ������3
 	
-  USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //�������ڽ����ж�
-  USART_Cmd(USART3, ENABLE);                     //Enable serial port 3 //ʹ�ܴ���3 
+  USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //�������ڽ����ж�
+  USART_Cmd(USART3, ENABLE);                     //Enable serial port 3 //ʹ�ܴ���3
+
+	uart3_dma_tx_init();                            //Init DMA for USART3 TX //��ʼ������3����DMA
+}
+/**************************************************************************
+Function: USART3 TX DMA initialization (memory->peripheral, one-shot per frame)
+�������ܣ�����3����DMA��ʼ�����ڴ�->���裬ÿ֡����һ�Σ�
+DMA1 Stream3 Channel4 is the TX request line for USART3 on STM32F407.
+STM32F407 �ϴ���3�ķ���DMA����ΪDMA1_Stream3_Channel4��
+**************************************************************************/
+void uart3_dma_tx_init(void)
+{
+	DMA_InitTypeDef DMA_InitStructure;
+
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
+
+	DMA_DeInit(DMA1_Stream3);
+	while(DMA_GetCmdStatus(DMA1_Stream3) != DISABLE);
+
+	DMA_InitStructure.DMA_Channel = DMA_Channel_4;
+	DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)&USART3->DR;
+	DMA_InitStructure.DMA_Memory0BaseAddr = (u32)Send_Data.buffer;
+	DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+	DMA_InitStructure.DMA_BufferSize = SEND_DATA_SIZE;
+	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+	DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+	DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
+	DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+	DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
+	DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+	DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+	DMA_Init(DMA1_Stream3, &DMA_InitStructure);
+
+	USART_DMACmd(USART3, USART_DMAReq_Tx, ENABLE);
 }
 /**************************************************************************
 Function: Serial port 5 initialization
 Input   : none
 Output  : none
-�������ܣ�����5��ʼ��
-��ڲ�������
-����  ֵ����
+�������ܣ�����5��ʼ��
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 void uart5_init(u32 bound)
 {  	 
@@ -393,99 +475,99 @@ void uart5_init(u32 bound)
 	NVIC_InitTypeDef NVIC_InitStructure;
 	
 	//PC12 TX
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);	 //Enable the gpio clock  //ʹ��GPIOʱ��
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);	 //Enable the gpio clock  //ʹ��GPIOʱ��
 		//PD2 RX
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //ʹ��GPIOʱ��
-  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); //Enable the Usart clock //ʹ��USARTʱ��
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //ʹ��GPIOʱ��
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); //Enable the Usart clock //ʹ��USARTʱ��
 
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource12,GPIO_AF_UART5);	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource2 ,GPIO_AF_UART5);	 
 	
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
-	GPIO_Init(GPIOC, &GPIO_InitStructure);  		          //��ʼ��
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
+	GPIO_Init(GPIOC, &GPIO_InitStructure);  		          //��ʼ��
 
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
-	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //��ʼ��
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //���ģʽ
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //�������
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //����50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //����
+	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //��ʼ��
 	
-  //UsartNVIC configuration //UsartNVIC����
+  //UsartNVIC configuration //UsartNVIC����
   NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
-	//Preempt priority //��ռ���ȼ�
+	//Preempt priority //��ռ���ȼ�
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;
-	//Preempt priority //��ռ���ȼ�
+	//Preempt priority //��ռ���ȼ�
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
-	//Enable the IRQ channel //IRQͨ��ʹ��	
+	//Enable the IRQ channel //IRQͨ��ʹ��	
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
   //Initialize the VIC register with the specified parameters 
-	//����ָ���IJ�����ʼ��VIC�Ĵ���		
+	//����ָ���IJ�����ʼ��VIC�Ĵ���		
 	NVIC_Init(&NVIC_InitStructure);
 	
-  //USART Initialization Settings ��ʼ������
-	USART_InitStructure.USART_BaudRate = bound; //Port rate //���ڲ�����
-	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //�ֳ�Ϊ8λ���ݸ�ʽ
-	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //һ��ֹͣ
-	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //����żУ��λ
-	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //��Ӳ������������
-	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //�շ�ģʽ
-  USART_Init(UART5, &USART_InitStructure);      //Initialize serial port 5 //��ʼ������5
-  USART_ITConfig(UART5, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //�������ڽ����ж�
-  USART_Cmd(UART5, ENABLE);                     //Enable serial port 5 //ʹ�ܴ���5
+  //USART Initialization Settings ��ʼ������
+	USART_InitStructure.USART_BaudRate = bound; //Port rate //���ڲ�����
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //�ֳ�Ϊ8λ���ݸ�ʽ
+	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //һ��ֹͣ
+	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //����żУ��λ
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //��Ӳ������������
+	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //�շ�ģʽ
+  USART_Init(UART5, &USART_InitStructure);      //Initialize serial port 5 //��ʼ������5
+  USART_ITConfig(UART5, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //�������ڽ����ж�
+  USART_Cmd(UART5, ENABLE);                     //Enable serial port 5 //ʹ�ܴ���5
 }
 /**************************************************************************
 Function: Serial port 1 receives interrupted
 Input   : none
 Output  : none
-�������ܣ�����1�����ж�
-��ڲ�������
-�� �� ֵ����
+�������ܣ�����1�����ж�
+��ڲ�������
+�� �� ֵ����
 **************************************************************************/
 int USART1_IRQHandler(void)
 {	
-	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //Check if data is received //�ж��Ƿ���յ�����
+	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //Check if data is received //�ж��Ƿ���յ�����
 	{
 		u8 Usart_Receive;
 		static u8 Count;
 		static u8 rxbuf[11];
 		int check=0,error=1,i;
 		
-		Usart_Receive = USART_ReceiveData(USART1); //Read the data //��ȡ����
-		if(Time_count<CONTROL_DELAY)
-			// Data is not processed until 10 seconds after startup
-		  //����10��ǰ����������
+		Usart_Receive = USART_ReceiveData(USART1); //Read the data //��ȡ����
+		if(Time_count<CONTROL_STARTUP_DELAY_COUNT)
+			// Data is not processed until the 2 second startup delay expires.
+		  //����2��ǰ����������
 			return 0;	
 			
 		//Fill the array with serial data
-		//����������������
+		//����������������
     rxbuf[Count]=Usart_Receive;  
 		
 		//Ensure that the first data in the array is FRAME_HEADER
-		//ȷ�������һ������ΪFRAME_HEADER
+		//ȷ�������һ������ΪFRAME_HEADER
     if(Usart_Receive == FRAME_HEADER||Count>0) 
 			Count++; 
 		else 
 			Count=0;
 		
-		if (Count == 11) //Verify the length of the packet //��֤���ݰ��ij���
+		if (Count == 11) //Verify the length of the packet //��֤���ݰ��ij���
 		{   
-				Count=0; //Prepare for the serial port data to be refill into the array //Ϊ����������������������׼��
-				if(rxbuf[10] == FRAME_TAIL) //Verify the frame tail of the packet //��֤���ݰ���֡β
+				Count=0; //Prepare for the serial port data to be refill into the array //Ϊ����������������������׼��
+				if(rxbuf[10] == FRAME_TAIL) //Verify the frame tail of the packet //��֤���ݰ���֡β
 				{			
 					for(i=0; i<9; i++)
 					{
 						//XOR bit check, used to detect data error
-						//���λУ�飬���ڼ�������Ƿ����
+						//���λУ�飬���ڼ�������Ƿ����
 						check=rxbuf[i]^check; 
 					}
 					if(check==rxbuf[9]) 
 						//XOR bit check successful
-					  //���λУ��ɹ�
+					  //���λУ��ɹ�
 					  error=0; 
 					
 					if(error==0)	 
@@ -494,7 +576,7 @@ int USART1_IRQHandler(void)
             if(Usart1_ON_Flag==0)
 						{	
 							//Serial port 1 controls flag position 1, other flag position 0
-							//����1���Ʊ�־λ��1��������־λ��0
+							//����1���Ʊ�־λ��1��������־λ��0
 							//Usart_ON_Flag=1;
 							Usart1_ON_Flag=1;
 							APP_ON_Flag=0;
@@ -502,21 +584,17 @@ int USART1_IRQHandler(void)
 							Remote_ON_Flag=0;
 							CAN_ON_Flag=0;
 						}		
-							command_lost_count=0; //CAN/���ڿ������ʧ��������
+							command_lost_count=0; //CAN/���ڿ������ʧ��������
 		
 						//Calculate the 3-axis target velocity from the serial data, which is divided into 8-bit high and 8-bit low units mm/s
-						//�Ӵ�������������Ŀ���ٶȣ��ָ�8λ�͵�8λ ��λmm/s
+						//�Ӵ�������������Ŀ���ٶȣ��ָ�8λ�͵�8λ ��λmm/s
 						Move_X=XYZ_Target_Speed_transition(rxbuf[3],rxbuf[4]);
 						Move_Y=XYZ_Target_Speed_transition(rxbuf[5],rxbuf[6]);
 						Vz    =XYZ_Target_Speed_transition(rxbuf[7],rxbuf[8]);
-						if(Car_Mode==Akm_Car)
-						{
-							Move_Z=Vz_to_Akm_Angle(Move_X, Vz);
-						}
-						else
-						{
-							Move_Z=XYZ_Target_Speed_transition(rxbuf[7],rxbuf[8]);
-						}		
+						// Ackermann: pass the raw rotation speed wz [rad/s] straight
+						// through. Drive_Motor derives curvature kappa = wz/v itself,
+						// so no front-wheel-angle pre-conversion here.
+						Move_Z=Vz;
 					}
 			  }
 		 }
@@ -527,46 +605,46 @@ int USART1_IRQHandler(void)
 Function: Refresh the OLED screen
 Input   : none
 Output  : none
-�������ܣ�����2�����ж�
-��ڲ�������
-����  ֵ����
+�������ܣ�����2�����ж�
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 int USART2_IRQHandler(void)
 {	
 	int Usart_Receive;
-	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) //Check if data is received //�ж��Ƿ���յ�����
+	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) //Check if data is received //�ж��Ƿ���յ�����
 	{	      
 		static u8 Flag_PID,i,j,Receive[50],Last_Usart_Receive;
 		static float Data;
 				
-		Usart_Receive=USART2->DR; //Read the data //��ȡ����
+		Usart_Receive=USART2->DR; //Read the data //��ȡ����
 		
 		if(Deviation_Count<CONTROL_DELAY)
-			// Data is not processed until 10 seconds after startup
-		  //����10��ǰ����������
+			// Data is not processed until the 2 second startup delay expires.
+		  //����2��ǰ����������
 		  return 0;	
 
 		if(Usart_Receive==0x41&&Last_Usart_Receive==0x41&&APP_ON_Flag==0)
 			//10 seconds after startup, press the forward button of APP to enter APP control mode
 		  //The APP controls the flag position 1 and the other flag position 0
-			//����10��֮�󣬰���APP��ǰ��������APP����ģʽ
-		  //APP���Ʊ�־λ��1��������־λ��0
+			//����10��֮�󣬰���APP��ǰ��������APP����ģʽ
+		  //APP���Ʊ�־λ��1��������־λ��0
 			PS2_ON_Flag=0,Remote_ON_Flag=0,APP_ON_Flag=1,CAN_ON_Flag=0,Usart1_ON_Flag=0,Usart5_ON_Flag=0;
     Last_Usart_Receive=Usart_Receive;			
 	  
 		if(Usart_Receive==0x4B) 
 			//Enter the APP steering control interface
-		  //����APPת����ƽ���
+		  //����APPת����ƽ���
 			Turn_Flag=1;  
 	  else	if(Usart_Receive==0x49||Usart_Receive==0x4A) 
       // Enter the APP direction control interface		
-			//����APP������ƽ���	
+			//����APP������ƽ���	
 			Turn_Flag=0;	
 		
 		if(Turn_Flag==0) 
 		{
 			//App rocker control interface command
-			//APPҡ�˿��ƽ�������
+			//APPҡ�˿��ƽ�������
 			if(Usart_Receive>=0x41&&Usart_Receive<=0x48)  
 			{	
 				Flag_Direction=Usart_Receive-0x40;
@@ -580,27 +658,27 @@ int USART2_IRQHandler(void)
 		else if(Turn_Flag==1)
 		{
 			//APP steering control interface command
-			//APPת����ƽ�������
-			if     (Usart_Receive==0x43) Flag_Left=0,Flag_Right=1; //Right rotation //����ת
-			else if(Usart_Receive==0x47) Flag_Left=1,Flag_Right=0; //Left rotation  //����ת
+			//APPת����ƽ�������
+			if     (Usart_Receive==0x43) Flag_Left=0,Flag_Right=1; //Right rotation //����ת
+			else if(Usart_Receive==0x47) Flag_Left=1,Flag_Right=0; //Left rotation  //����ת
 			else                         Flag_Left=0,Flag_Right=0;
 			if     (Usart_Receive==0x41||Usart_Receive==0x45) Flag_Direction=Usart_Receive-0x40;
 			else  Flag_Direction=0;
 		}
-		if(Usart_Receive==0x58)  RC_Velocity=RC_Velocity+100; //Accelerate the keys, +100mm/s //���ٰ�����+100mm/s
-		if(Usart_Receive==0x59)  RC_Velocity=RC_Velocity-100; //Slow down buttons,   -100mm/s //���ٰ�����-100mm/s
+		if(Usart_Receive==0x58)  RC_Velocity=RC_Velocity+100; //Accelerate the keys, +100mm/s //���ٰ�����+100mm/s
+		if(Usart_Receive==0x59)  RC_Velocity=RC_Velocity-100; //Slow down buttons,   -100mm/s //���ٰ�����-100mm/s
 	  
 	 // The following is the communication with the APP debugging interface
-	 //��������APP���Խ���ͨѶ
-	 if(Usart_Receive==0x7B) Flag_PID=1;   //The start bit of the APP parameter instruction //APP����ָ����ʼλ
-	 if(Usart_Receive==0x7D) Flag_PID=2;   //The APP parameter instruction stops the bit    //APP����ָ��ֹͣλ
+	 //��������APP���Խ���ͨѶ
+	 if(Usart_Receive==0x7B) Flag_PID=1;   //The start bit of the APP parameter instruction //APP����ָ����ʼλ
+	 if(Usart_Receive==0x7D) Flag_PID=2;   //The APP parameter instruction stops the bit    //APP����ָ��ֹͣλ
 
-	 if(Flag_PID==1) //Collect data //�ɼ�����
+	 if(Flag_PID==1) //Collect data //�ɼ�����
 	 {
 		Receive[i]=Usart_Receive;
 		i++;
 	 }
-	 if(Flag_PID==2) //Analyze the data //��������
+	 if(Flag_PID==2) //Analyze the data //��������
 	 {
 			if(Receive[3]==0x50) 	 PID_Send=1;
 			else  if(Receive[1]!=0x23) 
@@ -623,12 +701,12 @@ int USART2_IRQHandler(void)
 				 }
       }		
       //Relevant flag position is cleared			
-      //��ر�־λ����			
+      //��ر�־λ����			
 			Flag_PID=0;
 			i=0;
 			j=0;
 			Data=0;
-			memset(Receive, 0, sizeof(u8)*50); //Clear the array to zero//��������
+			memset(Receive, 0, sizeof(u8)*50); //Clear the array to zero//��������
 	 }
    if(RC_Velocity<0)   RC_Velocity=0; 	 
   }
@@ -638,9 +716,9 @@ int USART2_IRQHandler(void)
 Function: Serial port 3 receives interrupted
 Input   : none
 Output  : none
-�������ܣ�����3�����ж�
-��ڲ�������
-����  ֵ����
+�������ܣ�����3�����ж�
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 float test_movz = 0;
 int USART3_IRQHandler(void)
@@ -648,59 +726,54 @@ int USART3_IRQHandler(void)
 	static u8 Count=0;
 	u8 Usart_Receive;
 
-	if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) //Check if data is received //�ж��Ƿ���յ�����
+	if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) //Check if data is received //�ж��Ƿ���յ�����
 	{
-		Usart_Receive = USART_ReceiveData(USART3);//Read the data //��ȡ����
-		if(Time_count<CONTROL_DELAY)
-			// Data is not processed until 10 seconds after startup
-		  //����10��ǰ����������
+		Usart_Receive = USART_ReceiveData(USART3);//Read the data //��ȡ����
+		if(Time_count<CONTROL_STARTUP_DELAY_COUNT)
+			// Data is not processed until the 2 second startup delay expires.
+		  //����2��ǰ����������
 		  return 0;	
 		
 		//Fill the array with serial data
-		//����������������
+		//����������������
     Receive_Data.buffer[Count]=Usart_Receive;
 		
 		// Ensure that the first data in the array is FRAME_HEADER
-		//ȷ�������һ������ΪFRAME_HEADER
+		//ȷ�������һ������ΪFRAME_HEADER
 		if(Usart_Receive == FRAME_HEADER||Count>0) 
 			Count++; 
 		else 
 			Count=0;
 		
-		if (Count == 11) //Verify the length of the packet //��֤���ݰ��ij���
+		if (Count == 11) //Verify the length of the packet //��֤���ݰ��ij���
 		{   
-				Count=0; //Prepare for the serial port data to be refill into the array //Ϊ����������������������׼��
-				if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //��֤���ݰ���֡β
+				Count=0; //Prepare for the serial port data to be refill into the array //Ϊ����������������������׼��
+				if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //��֤���ݰ���֡β
 				{
 					//Data exclusionary or bit check calculation, mode 0 is sent data check
-					//�������λУ����㣬ģʽ0�Ƿ�������У��
+					//�������λУ����㣬ģʽ0�Ƿ�������У��
 					if(Receive_Data.buffer[9] ==Check_Sum(9,0))	 
 				  {	
 						float Vz;						
 						//All modes flag position 0, USART3 control mode
-            //����ģʽ��־λ��0��ΪUsart3����ģʽ						
+            //����ģʽ��־λ��0��ΪUsart3����ģʽ						
 						PS2_ON_Flag=0;
 						Remote_ON_Flag=0;
 						APP_ON_Flag=0;
 						CAN_ON_Flag=0;
 						Usart1_ON_Flag=0;
 						Usart5_ON_Flag=0;
-						command_lost_count=0; //CAN/���ڿ������ʧ��������
+						command_lost_count=0; //CAN/���ڿ������ʧ��������
 
 						//Calculate the target speed of three axis from serial data, unit m/s
-						//�Ӵ�������������Ŀ���ٶȣ� ��λm/s
+						//�Ӵ�������������Ŀ���ٶȣ� ��λm/s
 						Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
 						Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
 						Vz    =XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
 						test_movz = Vz;
-						if(Car_Mode==Akm_Car)
-						{
-							Move_Z=Vz_to_Akm_Angle(Move_X, Vz);
-						}
-						else
-						{
-							Move_Z=XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
-						}				  }
+						// Ackermann: pass the raw rotation speed wz [rad/s] straight
+						// through; Drive_Motor derives curvature kappa = wz/v itself.
+						Move_Z=Vz;				  }
 
 			}
 		}
@@ -712,66 +785,61 @@ int USART3_IRQHandler(void)
 Function: Serial port 5 receives interrupted
 Input   : none
 Output  : none
-�������ܣ�����5�����ж�
-��ڲ�������
-����  ֵ����
+�������ܣ�����5�����ж�
+��ڲ�������
+����  ֵ����
 **************************************************************************/
 int UART5_IRQHandler(void)
 {	
 	static u8 Count=0;
 	u8 Usart_Receive;
 
-	if(USART_GetITStatus(UART5, USART_IT_RXNE) != RESET) //Check if data is received //�ж��Ƿ���յ�����
+	if(USART_GetITStatus(UART5, USART_IT_RXNE) != RESET) //Check if data is received //�ж��Ƿ���յ�����
 	{
-		Usart_Receive = USART_ReceiveData(UART5);//Read the data //��ȡ����
-		if(Time_count<CONTROL_DELAY)
+		Usart_Receive = USART_ReceiveData(UART5);//Read the data //��ȡ����
+		if(Time_count<CONTROL_STARTUP_DELAY_COUNT)
 			// Data is not processed until 10 seconds after startup
-		  //����10��ǰ����������
+		  //����10��ǰ����������
 		  return 0;	
 		
 		//Fill the array with serial data
-		//����������������
+		//����������������
     Receive_Data.buffer[Count]=Usart_Receive;
 		
 		// Ensure that the first data in the array is FRAME_HEADER
-		//ȷ�������һ������ΪFRAME_HEADER
+		//ȷ�������һ������ΪFRAME_HEADER
 		if(Usart_Receive == FRAME_HEADER||Count>0) 
 			Count++; 
 		else 
 			Count=0;
 		
-		if (Count == 11) //Verify the length of the packet //��֤���ݰ��ij���
+		if (Count == 11) //Verify the length of the packet //��֤���ݰ��ij���
 		{   
-				Count=0; //Prepare for the serial port data to be refill into the array //Ϊ����������������������׼��
-				if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //��֤���ݰ���֡β
+				Count=0; //Prepare for the serial port data to be refill into the array //Ϊ����������������������׼��
+				if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //��֤���ݰ���֡β
 				{
 					//Data exclusionary or bit check calculation, mode 0 is sent data check
-					//�������λУ����㣬ģʽ0�Ƿ�������У��
+					//�������λУ����㣬ģʽ0�Ƿ�������У��
 					if(Receive_Data.buffer[9] ==Check_Sum(9,0))	 
 				  {	
 						float Vz;						
 						//All modes flag position 0, USART3 control mode
-            //����ģʽ��־λ��0��ΪUsart5����ģʽ						
+            //����ģʽ��־λ��0��ΪUsart5����ģʽ						
 						PS2_ON_Flag=0;
 						Remote_ON_Flag=0;
 						APP_ON_Flag=0;
 						CAN_ON_Flag=0;
 						Usart5_ON_Flag=0;
-						command_lost_count=0; //CAN/���ڿ������ʧ��������
+						command_lost_count=0; //CAN/���ڿ������ʧ��������
 
 						//Calculate the target speed of three axis from serial data, unit m/s
-						//�Ӵ�������������Ŀ���ٶȣ� ��λm/s
+						//�Ӵ�������������Ŀ���ٶȣ� ��λm/s
 						Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
 						Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
 						Vz    =XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
-						if(Car_Mode==Akm_Car)
-						{
-							Move_Z=Vz_to_Akm_Angle(Move_X, Vz);
-						}
-						else
-						{
-							Move_Z=XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
-						}				  }
+						// Ackermann: pass the raw rotation speed wz [rad/s] straight
+						// through; Drive_Motor derives curvature kappa = wz/v itself.
+						Move_Z=Vz;				  }
 					
 			}
 		}
@@ -782,77 +850,58 @@ int UART5_IRQHandler(void)
 Function: After the top 8 and low 8 figures are integrated into a short type data, the unit reduction is converted
 Input   : 8 bits high, 8 bits low
 Output  : The target velocity of the robot on the X/Y/Z axis
-�������ܣ�����λ��������Ŀ��ǰ���ٶ�Vx��Ŀ����ٶ�Vz��ת��Ϊ������С������ǰ��ת��
-��ڲ�����Ŀ��ǰ���ٶ�Vx��Ŀ����ٶ�Vz����λ��m/s��rad/s
-����  ֵ��������С������ǰ��ת�ǣ���λ��rad
+�������ܣ�����λ��������Ŀ��ǰ���ٶ�Vx��Ŀ����ٶ�Vz��ת��Ϊ������С������ǰ��ת��
+��ڲ�����Ŀ��ǰ���ٶ�Vx��Ŀ����ٶ�Vz����λ��m/s��rad/s
+����  ֵ��������С������ǰ��ת�ǣ���λ��rad
 **************************************************************************/
 
 float test_z=0;
 float Vz_to_Akm_Angle(float Vx, float Vz)
 {
-	float R, AngleR, Min_Turn_Radius;
-	//float AngleL;
-	
-	//Ackermann car needs to set minimum turning radius
-	//If the target speed requires a turn radius less than the minimum turn radius,
-	//This will greatly improve the friction force of the car, which will seriously affect the control effect
-	//������С����Ҫ������Сת��뾶
-	//���Ŀ���ٶ�Ҫ���ת��뾶С����Сת��뾶��
-	//�ᵼ��С���˶�Ħ���������ߣ�����Ӱ�����Ч��
-	Min_Turn_Radius=MINI_AKM_MIN_TURN_RADIUS;
-	
+	float R, CenterAngle;
+
+	/* Move_Z is the vehicle center steering angle in radians. */
 	if(Vz!=0 && Vx!=0)
 	{
-		//If the target speed requires a turn radius less than the minimum turn radius
-		//���Ŀ���ٶ�Ҫ���ת��뾶С����Сת��뾶
-//		if(float_abs(Vx/Vz)<=Min_Turn_Radius)
-//		{
-//			//Reduce the target angular velocity and increase the turning radius to the minimum turning radius in conjunction with the forward speed
-//			//����Ŀ����ٶȣ����ǰ���ٶȣ����ת��뾶����Сת��뾶
-//			if(Vz>0)
-//				Vz= float_abs(Vx)/(Min_Turn_Radius);
-//			else	
-//				Vz=-float_abs(Vx)/(Min_Turn_Radius);	
-//		}		
-		//R=Vx/Vz;
-		R=Vx/Vz;
-		//AngleL=atan(Axle_spacing/(R+0.5*Wheel_spacing));
-		AngleR=atan(Axle_spacing/(R+0.05*Wheel_spacing));//2025.7.20edit
+		/* R is the turning radius of the vehicle centerline. */
+		/* Vx sets radius magnitude only; Vz owns the steering direction. */
+		R=float_abs(Vx)/float_abs(Vz);
+		CenterAngle=(Vz >= 0.0f ? 1.0f : -1.0f) * atan(Axle_spacing/R);
 	}
 	else
 	{
-		AngleR=0;
+		CenterAngle=0;
 	}
-	test_z = AngleR;//test
-	return AngleR;
+	test_z = CenterAngle;
+	return CenterAngle;
 }
 /**************************************************************************
 Function: After the top 8 and low 8 figures are integrated into a short type data, the unit reduction is converted
 Input   : 8 bits high, 8 bits low
 Output  : The target velocity of the robot on the X/Y/Z axis
-�������ܣ�����λ���������ĸ�8λ�͵�8λ�������ϳ�һ��short�����ݺ�������λ��ԭ����
-��ڲ�������8λ����8λ
-����  ֵ��������X/Y/Z���Ŀ���ٶ�
+�������ܣ�����λ���������ĸ�8λ�͵�8λ�������ϳ�һ��short�����ݺ�������λ��ԭ����
+��ڲ�������8λ����8λ
+����  ֵ��������X/Y/Z���Ŀ���ٶ�
 **************************************************************************/
 float XYZ_Target_Speed_transition(u8 High,u8 Low)
 {
 	//Data conversion intermediate variable
-	//����ת�����м����
+	//����ת�����м����
 	short transition; 
 	
-	//����8λ�͵�8λ���ϳ�һ��16λ��short������
+	//����8λ�͵�8λ���ϳ�һ��16λ��short������
 	//The high 8 and low 8 bits are integrated into a 16-bit short data
 	transition=((High<<8)+Low); 
 	return 
-		transition/1000+(transition%1000)*0.001; //Unit conversion, mm/s->m/s //��λת��, mm/s->m/s						
+		transition/1000+(transition%1000)*0.001; //Unit conversion, mm/s->m/s //��λת��, mm/s->m/s						
 }
 /**************************************************************************
 Function: Serial port 1 sends data
 Input   : The data to send
 Output  : none
-�������ܣ�����1��������
-��ڲ�����Ҫ���͵�����
-����  ֵ����
+�������ܣ�����1��������
+��ڲ�����Ҫ���͵�����
+����  ֵ����
 **************************************************************************/
 void usart1_send(u8 data)
 {
@@ -863,9 +912,9 @@ void usart1_send(u8 data)
 Function: Serial port 2 sends data
 Input   : The data to send
 Output  : none
-�������ܣ�����2��������
-��ڲ�����Ҫ���͵�����
-����  ֵ����
+�������ܣ�����2��������
+��ڲ�����Ҫ���͵�����
+����  ֵ����
 **************************************************************************/
 void usart2_send(u8 data)
 {
@@ -876,9 +925,9 @@ void usart2_send(u8 data)
 Function: Serial port 3 sends data
 Input   : The data to send
 Output  : none
-�������ܣ�����3��������
-��ڲ�����Ҫ���͵�����
-����  ֵ����
+�������ܣ�����3��������
+��ڲ�����Ҫ���͵�����
+����  ֵ����
 **************************************************************************/
 void usart3_send(u8 data)
 {
@@ -890,9 +939,9 @@ void usart3_send(u8 data)
 Function: Serial port 5 sends data
 Input   : The data to send
 Output  : none
-�������ܣ�����5��������
-��ڲ�����Ҫ���͵�����
-����  ֵ����
+�������ܣ�����5��������
+��ڲ�����Ҫ���͵�����
+����  ֵ����
 **************************************************************************/
 void usart5_send(u8 data)
 {
@@ -903,16 +952,16 @@ void usart5_send(u8 data)
 Function: Calculates the check bits of data to be sent/received
 Input   : Count_Number: The first few digits of a check; Mode: 0-Verify the received data, 1-Validate the sent data
 Output  : Check result
-�������ܣ�����Ҫ����/���յ�����У����
-��ڲ�����Count_Number��У���ǰ��λ����Mode��0-�Խ������ݽ���У�飬1-�Է������ݽ���У��
-����  ֵ��У����
+�������ܣ�����Ҫ����/���յ�����У����
+��ڲ�����Count_Number��У���ǰ��λ����Mode��0-�Խ������ݽ���У�飬1-�Է������ݽ���У��
+����  ֵ��У����
 **************************************************************************/
 u8 Check_Sum(unsigned char Count_Number,unsigned char Mode)
 {
 	unsigned char check_sum=0,k;
 	
 	//Validate the data to be sent
-	//��Ҫ���͵����ݽ���У��
+	//��Ҫ���͵����ݽ���У��
 	if(Mode==1)
 	for(k=0;k<Count_Number;k++)
 	{
@@ -920,7 +969,7 @@ u8 Check_Sum(unsigned char Count_Number,unsigned char Mode)
 	}
 	
 	//Verify the data received
-	//�Խ��յ������ݽ���У��
+	//�Խ��յ������ݽ���У��
 	if(Mode==0)
 	for(k=0;k<Count_Number;k++)
 	{
@@ -931,6 +980,3 @@ u8 Check_Sum(unsigned char Count_Number,unsigned char Mode)
 
 
 
-
-
-
diff --git a/HARDWARE/usartx.h b/HARDWARE/usartx.h
index b8853d4..8e9ae8f 100644
--- a/HARDWARE/usartx.h
+++ b/HARDWARE/usartx.h
@@ -10,33 +10,41 @@
 
 #define FRAME_HEADER      0X7B //Frame_header //֡ͷ
 #define FRAME_TAIL        0X7D //Frame_tail   //֡β
-#define SEND_DATA_SIZE    24
+//Frame grew 24 -> 44: appended session_id(4) + two 64-bit sample
+//timestamps(8+8) before the checksum/tail. Bytes 0..21 keep the legacy
+//layout so old parsers stay compatible; new fields live in the tail.
+//֡�� 24 -> 44����У��/֡βǰ׷���� session_id(4) �������� 64λ����ʱ���(8+8)��
+//�ֽ� 0..21 ���ֽ�ϲ��֣��¾ֽ���β����
+#define SEND_DATA_SIZE    44
 #define RECEIVE_DATA_SIZE 11
 
 /*****A structure for storing triaxial data of a gyroscope accelerometer*****/
-/*****���ڴ�������Ǽ��ٶȼ��������ݵĽṹ��*********************************/
+/*****���ڴ�������Ǽ��ٶȼ��������ݵĽṹ��*********************************/
 typedef struct __Mpu6050_Data_ 
 {
-	short X_data; //2 bytes //2���ֽ�
-	short Y_data; //2 bytes //2���ֽ�
-	short Z_data; //2 bytes //2���ֽ�
+	short X_data; //2 bytes //2���ֽ�
+	short Y_data; //2 bytes //2���ֽ�
+	short Z_data; //2 bytes //2���ֽ�
 }Mpu6050_Data;
 
 /*******The structure of the serial port sending data************/
-/*******���ڷ������ݵĽṹ��*************************************/
+/*******���ڷ������ݵĽṹ��*************************************/
 typedef struct _SEND_DATA_  
 {
 	unsigned char buffer[SEND_DATA_SIZE];
 	struct _Sensor_Str_
 	{
-		unsigned char Frame_Header; //1���ֽ�
-		short X_speed;	            //2 bytes //2���ֽ�
-		short Y_speed;              //2 bytes //2���ֽ�
-		short Z_speed;              //2 bytes //2���ֽ�
-		short Power_Voltage;        //2 bytes //2���ֽ�
-		Mpu6050_Data Accelerometer; //6 bytes //6���ֽ�
-		Mpu6050_Data Gyroscope;     //6 bytes //6���ֽ�	
-		unsigned char Frame_Tail;   //1 bytes //1���ֽ�
+		unsigned char Frame_Header; //1���ֽ�
+		short X_speed;	            //2 bytes //2���ֽ�
+		short Y_speed;              //2 bytes //2���ֽ�
+		short Z_speed;              //2 bytes //2���ֽ�
+		short Power_Voltage;        //2 bytes //2���ֽ�
+		Mpu6050_Data Accelerometer; //6 bytes //6���ֽ�
+		Mpu6050_Data Gyroscope;     //6 bytes //6���ֽ�
+		unsigned int  Session_Id;         //4 bytes: MCU boot session ID //��MCU�����ỰID
+		unsigned long long Speed_Time_us; //8 bytes: encoder sample time //�������������ʱ��
+		unsigned long long Imu_Time_us;   //8 bytes: IMU sample time //IMU����ʱ��
+		unsigned char Frame_Tail;   //1 bytes //1���ֽ�
 	}Sensor_Str;
 }SEND_DATA;
 
@@ -45,14 +53,21 @@ typedef struct _RECEIVE_DATA_
 	unsigned char buffer[RECEIVE_DATA_SIZE];
 	struct _Control_Str_
 	{
-		unsigned char Frame_Header; //1 bytes //1���ֽ�
-		float X_speed;	            //4 bytes //4���ֽ�
-		float Y_speed;              //4 bytes //4���ֽ�
-		float Z_speed;              //4 bytes //4���ֽ�
-		unsigned char Frame_Tail;   //1 bytes //1���ֽ�
+		unsigned char Frame_Header; //1 bytes //1���ֽ�
+		float X_speed;	            //4 bytes //4���ֽ�
+		float Y_speed;              //4 bytes //4���ֽ�
+		float Z_speed;              //4 bytes //4���ֽ�
+		unsigned char Frame_Tail;   //1 bytes //1���ֽ�
 	}Control_Str;
 }RECEIVE_DATA;
 
+//Sample timestamps captured at acquisition time by their owning tasks, and
+//the per-boot session ID. Read by data_transition() when packing the frame.
+//�ɸ��Բɼ��������ڲ���˲�����¼������ʱ���������ϵͳ���лỰID��
+//�� data_transition() ���֡ʱ��ȡ��
+extern volatile unsigned long long g_speed_sample_time_us;
+extern volatile unsigned long long g_imu_sample_time_us;
+extern volatile unsigned int g_session_id;
 
 void data_task(void *pvParameters);
 void data_transition(void);
@@ -64,6 +79,7 @@ void CAN_SEND(void);
 void uart1_init(u32 bound);
 void uart2_init(u32 bound);
 void uart3_init(u32 bound);
+void uart3_dma_tx_init(void);
 void uart5_init(u32 bound);
 
 int USART1_IRQHandler(void);
diff --git a/Makefile b/Makefile
index 86dbdaf..9b21202 100644
--- a/Makefile
+++ b/Makefile
@@ -31,7 +31,8 @@ FWLIB_SRC := \
   FWLIB/src/misc.c FWLIB/src/stm32f4xx_gpio.c FWLIB/src/stm32f4xx_rcc.c \
   FWLIB/src/stm32f4xx_syscfg.c FWLIB/src/stm32f4xx_usart.c \
   FWLIB/src/stm32f4xx_adc.c FWLIB/src/stm32f4xx_can.c \
-  FWLIB/src/stm32f4xx_tim.c FWLIB/src/stm32f4xx_pwr.c
+  FWLIB/src/stm32f4xx_tim.c FWLIB/src/stm32f4xx_pwr.c \
+  FWLIB/src/stm32f4xx_dma.c
 
 FREERTOS_SRC := \
   FreeRTOS/croutine.c FreeRTOS/event_groups.c FreeRTOS/list.c \
diff --git a/doc/ackermann_steering_calibration.md b/doc/ackermann_steering_calibration.md
new file mode 100644
index 0000000..f85f4a8
--- /dev/null
+++ b/doc/ackermann_steering_calibration.md
@@ -0,0 +1,374 @@
+# 阿克曼转向标定与拟合说明
+
+本文记录阿克曼小车「(Vx, Vz) → 舵机 PWM + 左右后轮速度」控制律的标定数据、
+拟合方法、单位约定，三种控制模式（标定运动学 / 直接映射 / 横摆闭环），以及
+串口通信（200 Hz DMA 发送、921600 波特率）。对应代码：
+- 控制律：`BALANCE/balance.c` 的 `Akm_Car` 分支，
+  `Akm_Curvature_To_Servo()`（拟合）与 `Akm_Norm_To_Servo()`（直接满行程映射）。
+- 开关宏：`BALANCE/robot_select_init.h`；舵机直行点 `SERVO_INIT` 在 `HARDWARE/motor.h`。
+- 串口：`HARDWARE/usartx.c`（`data_task` / `USART3_SEND` / `uart3_dma_tx_init`），
+  波特率在 `BALANCE/system.c` 的 `systemInit`。
+
+## 1. 硬件与坐标约定
+
+- 后驱阿克曼：`MOTOR_A` = 左后轮，`MOTOR_B` = 右后轮，前轮由舵机（TIM12 CCR2）转向。
+- 输入语义（上位机下发）：
+  - `Vx` = 后轴中心线速度，单位 m/s。
+  - `Vz` = 绕转弯中心的旋转角速度 wz，单位 rad/s。
+- 符号约定：
+  - 上位机 `Vz` 遵循 ROS 约定：**Vz > 0 = 逆时针 = 左转**。
+  - 标定表 / 舵机拟合使用相反符号：**曲率 κ > 0 = 右转**。
+  - 代码中 `kappa_fit = -Vz/Vx`，使 `Vz > 0` 得到 `kappa_fit < 0`（左转），
+    与拟合域一致。
+
+## 2. 标定方法（手推法）
+
+电机失能、舵机使能，手动推动小车走出稳定圆弧，对每个舵机 PWM 记录：
+
+- `y 向半截距`（弧对应的纵向半截距，cm）
+- `x 向半弓高`（弦的矢高 / 半弓高，cm）
+- 由几何反推后轴中心转弯半径 `R`（cm）与 `1/R`
+
+x 向半弓高的符号用于判定左右：PWM > ~1670 为右转（κ 取正），
+低于该点为左转（κ 取负）。
+
+### 三个"中位"值（易混，务必区分）
+
+| 名称 | 值 | 含义 | 代码位置 |
+|------|---:|------|----------|
+| 遥控 CH1 中位 | 1500 | 遥控摇杆物理中点 | `AKM_REMOTER_CH1_MID`（balance.c） |
+| 舵机机械中位 | 1600 | 舵机行程几何中点 (MIN+MAX)/2 | —（不再直接使用） |
+| **直行点 SERVO_INIT** | **1670** | **实测 1/R≈0、车真正走直线的点** | `SERVO_INIT`（motor.h:52） |
+
+关键：**所有控制律的舵机中位都对齐到 `SERVO_INIT = 1670`**（真正的直行点），
+而非机械中位 1600。开机上电、Mode 1 / Mode 2 的零位、CH1 覆盖都以它为基准：
+- CH1 覆盖时舵机 = `Remoter_Ch1 + (SERVO_INIT - AKM_REMOTER_CH1_MID)`，即整体
+  平移 `1670 - 1500 = +170`，使摇杆居中 = 车轮回正。
+- 改 `SERVO_INIT` 一处，下游所有路径自动对齐。
+
+### 原始标定数据
+
+| servo(pwm) | y半截距/cm | x半弓高/cm | R/cm | 1/R (1/cm) |
+|-----------:|----------:|----------:|-----:|-----------:|
+| 2000 | 34    | 34   | 34.00   | 0.029412 |
+| 1900 | 52.5  | 49.5 | 52.59   | 0.019015 |
+| 1800 | 90    | 60   | 97.50   | 0.010256 |
+| 1700 | 167.2 | 60   | 262.97  | 0.003803 |
+| 1670 | 120   | -1   | 7200.5  | 0.000139 |
+| 1600 | 120   | -23  | 324.54  | 0.003081 |
+| 1500 | 89    | -60  | 96.01   | 0.010416 |
+| 1400 | 61    | -60  | 61.01   | 0.016391 |
+| 1300 | 43    | -44  | 43.01   | 0.023250 |
+| 1200 | 35.5  | -34.3| 35.52   | 0.028152 |
+| 1100 | 30    | -28.9| 30.02   | 0.033310 |
+
+> 直行点约在 PWM 1670（1/R ≈ 0，x 半弓高 ≈ 0），并非舵机机械中位 1600。
+
+## 3. 单位换算（关键，曾导致 100 倍错误）
+
+标定表 `1/R` 列以 **1/cm** 为单位（R 用厘米）。而代码里的曲率来自
+`kappa = wz / Vx`（均为 SI），单位是 **1/m**。两者相差 100 倍。
+
+拟合前必须把 R 从 cm 换算为 m，再取 `kappa = 1/R_m`（并带符号）：
+
+| servo | kappa (1/m, +=右) |
+|------:|------------------:|
+| 2000  | +2.9412 |
+| 1900  | +1.9015 |
+| 1800  | +1.0256 |
+| 1700  | +0.3803 |
+| 1670  | +0.0139 |
+| 1600  | -0.3081 |
+| 1500  | -1.0416 |
+| 1400  | -1.6391 |
+| 1300  | -2.3250 |
+| 1200  | -2.8152 |
+| 1100  | -3.3310 |
+
+- 曲率量程：κ ∈ [−3.331, +2.941] 1/m。
+- 最小转弯半径 R_min ≈ 0.30 m（左）/ 0.34 m（右）。
+- 代码中 `AKM_KAPPA_MAX = 3.331f`。
+
+## 4. 拟合方法与结果
+
+以 `servo = f(kappa)`（kappa 为自变量，1/m）做多项式最小二乘。
+比较一/二/三次，并用留一交叉验证（LOO）评估对未见点的泛化：
+
+| 拟合 | 全量 max残差 | 全量 rms | LOO max误差 | LOO rms |
+|------|-----------:|--------:|----------:|-------:|
+| 一次 | 56.8 | 28.3 | — | — |
+| 二次 | 12.3 | 7.6 | 15.1 | 10.2 |
+| 三次 | 12.6 | 7.5 | — | — |
+
+结论：**二次拟合最优**。三次不再改善（过拟合），一次残差过大。
+
+### 最终系数（1/m 单位）
+
+```
+servo = 1656.373 + 140.548 * kappa - 7.654 * kappa^2
+```
+
+代码宏（`BALANCE/balance.c`）：
+
+```c
+#define AKM_SERVO_C0   1656.373f
+#define AKM_SERVO_C1    140.548f
+#define AKM_SERVO_C2     (-7.654f)
+#define AKM_SERVO_MIN   1100
+#define AKM_SERVO_MAX   2000
+#define AKM_KAPPA_MAX   3.331f
+```
+
+### 为什么用拟合而非查表 + 线性插值
+
+- 数据是手推测得，含测量噪声。查表被迫穿过每个噪声点，两点间直线段会
+  放大噪声；LOO 显示查表 max 误差 23 / rms 14，明显差于二次拟合（15 / 10）。
+- 拟合在端点外推时形状正确（曲线两端明显弯），线性插值只能按最后一段
+  斜率外推，越推越偏。
+- 拟合在节点处平滑，无斜率突变；运行成本仅两次乘加，比查找区间还省。
+
+### 轴距 L 去哪了
+
+标定表端到端直接测了 `servo → R`，已把「舵机 PWM → 前轮转角 →
+`R = L/tan(δ)`」整条链路及传动比、轮胎侧偏等真实效应吸收进拟合系数。
+因此无需再显式写 `R = Axle_spacing/tan(δ)`（那反而依赖不准的假设传动比）。
+后轮差速只用轮距 track 与 κ，本就不含 L。阿克曼几何没有丢，而是以更贴合
+实车的实测形式嵌入拟合。
+
+## 5. 控制律（Mode 0：标定运动学，默认）
+
+`AKM_DIRECT_MAP = 0` 时：
+
+```
+kappa_geom = Vz / Vx            (Vx≈0 时置 0，阿克曼无前进不能转向)
+kappa_fit  = clamp(-kappa_geom, ±AKM_KAPPA_MAX)
+Servo      = f(kappa_fit)                          # 二次拟合
+MOTOR_A(左) = Vx * (1 + 0.5*track*kappa_fit)       # 后轮差速
+MOTOR_B(右) = Vx * (1 - 0.5*track*kappa_fit)
+```
+
+物理特性：舵机角只决定转弯半径 R = 1/κ。
+- 固定舵机角、改 Vx → wz 随之变（走同一圆，快慢不同）。
+- 固定 wz、改 Vx → κ = wz/Vx 变 → 舵机角变（正确行为）。
+
+### Vx–wz 可行域（κ_max ≈ 3.33 /m）
+
+`|wz| <= Vx * κ_max`：
+
+| Vx (m/s) | 可用 wz 范围 (rad/s) |
+|---------:|:--------------------|
+| 0.2 | [−0.67, 0.67] |
+| 0.3 | [−1.00, 1.00] |
+| 0.5 | [−1.67, 1.67] |
+| 1.0 | [−3.33, 3.33] |
+
+> 例：固定 wz=0.12，需 Vx ≥ 0.036 m/s 才不被夹紧。之前 `AKM_KAPPA_MAX`
+> 误设为 0.0334（对应 R≈30 m）导致任何速度下都被夹死、改速度舵机不动，
+> 已随单位修正解决。
+
+## 6. 控制律（Mode 1：直接映射，调试用）
+
+`AKM_DIRECT_MAP = 1` 时（脱离阿克曼物理，用于隔离调试舵机/电机）：
+
+```
+vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1)
+Servo   = Akm_Norm_To_Servo(vz_norm)     # 以 SERVO_INIT 为零位的分段满行程映射
+MOTOR_A(左) = Vx                          # 无差速
+MOTOR_B(右) = Vx
+```
+
+- `Vz` 线性铺满整个舵机行程 [AKM_SERVO_MIN, AKM_SERVO_MAX]，与速度无关。
+- **零位对齐直行点**：`Akm_Norm_To_Servo` 以 `SERVO_INIT=1670`（而非机械中位
+  1600）为中心，左右两侧各自缩放到自己的端点，即使中位偏置也能用满全行程：
+  ```
+  span = (norm>=0) ? (SERVO_INIT - AKM_SERVO_MIN)   # 左侧行程 1670-1100=570
+                   : (AKM_SERVO_MAX - SERVO_INIT)   # 右侧行程 2000-1670=330
+  Servo = SERVO_INIT - norm * span
+  ```
+- `Vz > 0`（左）→ `norm>0` → 靠近 AKM_SERVO_MIN（左端），符号与 ROS 一致。
+- `Vx` 原样给左右电机，不做曲率/差速运算。
+- 满量程输入由 `AKM_DIRECT_VZ_FULL`（默认 **1.0 rad/s**）设定。
+
+> **`AKM_DIRECT_VZ_FULL` 的物理含义** = 上位机会发的最大 angular.z，Vz 到该值时
+> 舵机打满。它只决定"Vz→行程"的比例，不改变峰值转角（峰值由舵机端点 1100/2000
+> 决定，恒为 ~28°左 / 25°右）。太小→小 Vz 就饱和、失去比例控制；太大→常用区间
+> 只用到一小段行程、转角偏小。当前定为 1.0（Vz=±1.0 打满），把行程摊到 ±1 rad/s
+> 全区间，规划分辨率比 0.5 时翻倍。Vz 在 RX 中断里以 rad/s 到达
+> （usartx.c `XYZ_Target_Speed_transition`: raw/1000）。
+
+## 7. 控制律（Mode 2：横摆角速度闭环 / 简化扭矩矢量）
+
+`AKM_DIRECT_MAP = 0` 且 `AKM_YAW_ASSIST = 1` 时启用。
+
+**动机（为什么要有 Mode 2）**：Mode 0 里 `κ = Vz/Vx`，同一个转向指令 `Vz` 在
+高速时曲率被 `Vx` 除小，舵机自动回正 —— 这就是"高速转弯打不动"的根源。Mode 2
+**主动解耦 w 与 v**：舵机由 `Vz` 直接决定（与 Mode 1 一样，不再除以 Vx），
+所以大 `Vz` 在任何速度都给出大前轮角；阿克曼只作为后轮差速的**前馈参考**，
+再叠加 IMU 横摆角速度 PI 闭环（简化扭矩矢量）。控制链路：
+
+```
+Vz ─(直接满行程映射, 与Vx无关)→ 舵机主转向 Akm_Norm_To_Servo   ← 方向主控，解耦
+Vx ───────────────────────────→ 左右轮基速                      ← 驱动主控，解耦
+(Vx, κ_cmd) ─(阿克曼几何)→ r_ref ─→ 后轮差速前馈 + IMU PI          ← akm 仅做前馈
+```
+
+计算（fit 域 κ>0=右；ROS Vz>0=左=CCW；r>0=左转）：
+
+```
+vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1)   # 转向指令，不含 Vx —— 关键解耦点
+Servo   = Akm_Norm_To_Servo(vz_norm)          # 与 Mode 1 完全相同的满行程直接映射
+                                              # 以 SERVO_INIT 为零位，不走拟合曲线
+κ_cmd   = -vz_norm * AKM_KAPPA_MAX             # 仅用于构造下方前馈参考，不驱动舵机
+
+r_ref   = -κ_cmd * Vx                         # 阿克曼几何前馈的期望横摆角速度
+r_meas  = AKM_GYRO_Z_SIGN * gyro[2] / AKM_GYRO_Z_TO_RADPS
+r_filt += AKM_YAW_IMU_LPF * (r_meas - r_filt)  # 轻度一阶低通
+
+e_r     = r_ref - r_filt
+dv_ff   = AKM_YAW_FF_ALPHA * 0.5 * track * r_ref  # 几何前馈
+dv_fb   = AKM_YAW_KP * e_r + AKM_YAW_KI * ∫e_r    # PI 反馈（矩形积分, dt=1/200s）
+dv      = clamp(dv_ff + dv_fb, ±AKM_YAW_MAX_DIFF_RATIO*|Vx|)  # 带抗积分饱和
+
+MOTOR_A(左) = Vx - dv
+MOTOR_B(右) = Vx + dv
+```
+
+要点：
+- **解耦是核心**：`vz_norm` 只由 `Vz` 决定，不再 `Vz/Vx`，所以高速大转向不再被
+  几何"稀释"。`Vx` 独立设定驱动基速。
+- **舵机走直接满行程映射，不走拟合**：Mode 2 的舵机与 Mode 1 / CH1 调试一致，
+  用 `Akm_Norm_To_Servo(vz_norm)` 把转向指令线性铺满行程（以 `SERVO_INIT` 为
+  零位），**不再调用标定拟合 `f(κ)`**。这样"高速转弯打不动"从根上消失——舵机角
+  只看指令、与速度无关。（早期版本这里错用了 `f(κ_cmd)`，仍隐含耦合，已改正。）
+- **阿克曼降级为纯前馈**：`κ_cmd = -vz_norm*AKM_KAPPA_MAX` 只用来构造 `r_ref`
+  ——"若车按该曲率走出的理想横摆角速度"，仅喂给后轮差速前馈；实际横摆由 IMU PI
+  收敛，可克服前轮几何/打滑带来的偏差。`κ_cmd` 不参与舵机计算。
+- **退化关系**：`AKM_YAW_FF_ALPHA = 1` 且 `AKM_YAW_KP = AKM_YAW_KI = 0` 时，
+  `dv = 0.5*track*r_ref = -0.5*track*κ_cmd*Vx`，即纯几何差速。α 是平滑旋钮：
+  0 = 纯 IMU 反馈，1 = 纯几何前馈。
+- **低速冻结**：`|Vx| < AKM_YAW_MIN_SPEED` 时清零积分并停用差速（低速横摆角
+  速度信噪比太差）。
+- 用原始角速度（`gyro[2]` 去零偏后的 LSB），不做航向角积分。
+
+### 上位机规划用：发布 Vz → 大致前轮转角 δ
+
+Mode 2 里舵机只由 `Vz` 决定（与 `Vx` 无关），所以上位机可直接按下表估转角。
+换算链路（非线性，因为 PWM→R 那段是标定曲线）：
+
+```
+vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1)      # 默认 VZ_FULL=1.0 → Vz=±1.0 打满
+Servo   = SERVO_INIT - vz_norm * span             # 左 span=570, 右 span=330
+R       = 1 / |1/R(Servo)|                         # 1/R 由 §2 标定表(1/cm)插值再×100
+δ       = atan(L / R)                              # L = Akm_axlespacing = 0.160 m
+```
+
+用当前宏值（`SERVO_INIT=1670`, `MIN=1100`, `MAX=2000`, `VZ_FULL=1.0`）、按 §2 标定表
+`1/R` 列插值算出的对照表（δ 为后轴等效前轮转角，正数只表大小，方向见末列）：
+
+| Vz (rad/s) | vz_norm | Servo(PWM) | R(m) | δ(°) | 方向 |
+|-----------:|--------:|-----------:|-----:|-----:|:----:|
+| +1.0 | +1.0 | 1100 | 0.30 | 28.1 | 左 |
+| +0.8 | +0.8 | 1214 | 0.36 | 23.7 | 左 |
+| +0.6 | +0.6 | 1328 | 0.47 | 18.8 | 左 |
+| +0.5 | +0.5 | 1385 | 0.57 | 15.6 | 左 |
+| +0.4 | +0.4 | 1442 | 0.72 | 12.5 | 左 |
+| +0.3 | +0.3 | 1499 | 0.95 |  9.5 | 左 |
+| +0.2 | +0.2 | 1556 | 1.59 |  5.8 | 左 |
+| +0.1 | +0.1 | 1613 | 3.95 |  2.3 | 左 |
+|  0.0 |  0.0 | 1670 | ∞    |  0.0 | 直 |
+| −0.1 | −0.1 | 1703 | 2.50 |  3.7 | 右 |
+| −0.2 | −0.2 | 1736 | 1.63 |  5.6 | 右 |
+| −0.3 | −0.3 | 1769 | 1.21 |  7.5 | 右 |
+| −0.4 | −0.4 | 1802 | 0.96 |  9.5 | 右 |
+| −0.5 | −0.5 | 1835 | 0.75 | 12.0 | 右 |
+| −0.6 | −0.6 | 1868 | 0.62 | 14.5 | 右 |
+| −0.8 | −0.8 | 1934 | 0.44 | 19.8 | 右 |
+| −1.0 | −1.0 | 2000 | 0.34 | 25.2 | 右 |
+
+要点（上位机规划务必注意）：
+- **左右不对称**：直行点 1670 偏向右端，左侧行程 570、右侧仅 330，所以同样
+  `|Vz|` 左转角比右转角略大（满量程 28° 左 vs 25° 右）。这是机械中位偏置导致的，
+  已被 `Akm_Norm_To_Servo` 的分段缩放吸收。
+- **`|Vz| ≥ 1.0` 全部饱和**到端点（28°左 / 25°右）；再大的 Vz 不会有更大转角。
+  `VZ_FULL` 从 0.5 提到 1.0 后，满量程指令 = ±1.0 rad/s，同样的舵机行程摊到更宽的
+  Vz 区间，规划分辨率翻倍（峰值转角不变）。
+- **非线性**：Vz→δ 不是直线（低 Vz 段每 0.1 约 +3°，高 Vz 段趋缓），规划时按表
+  插值而非线性外推。
+- R 是**后轴中心转弯半径**，δ = atan(L/R) 是等效前轮转角；实车受轮胎侧偏/打滑
+  影响，表值为标定静推的近似，动态下 IMU 横摆环会再做修正。
+
+调参顺序（用户建议）：
+1. 先标定陀螺零偏（开机静止采样，代码已做）。
+2. **实测确认 `gyro[2]` 符号**：命令左转，确认 `gyro[2] > 0`；若相反把
+   `AKM_GYRO_Z_SIGN` 改为 `-1.0f`。
+3. 小 `Kp`（0.05~0.10）起步，逐步加大到临界前。
+4. 有稳态误差再加一点 `Ki`。
+5. 最后引入前馈 `α = 0.2~0.5` 减轻 PI 负担。
+
+## 8. 开关一览（`BALANCE/robot_select_init.h`）
+
+| 宏 | 默认 | 含义 |
+|----|-----:|------|
+| `AKM_SERVO_DEBUG_REMOTE_CH1` | 1 | 1 = 舵机直接由遥控 CH1 驱动，覆盖所有控制律（板级调试） |
+| `AKM_DIRECT_MAP` | 1 | 1 = 直接映射调试模式；0 = 交给 Mode 0 / Mode 2 |
+| `AKM_DIRECT_VZ_FULL` | 1.0f | Mode 1 & Mode 2 共用：Vz 满行程量程（rad/s）= 上位机会发的最大 angular.z |
+| `AKM_YAW_ASSIST` | 0 | 1 = 横摆闭环（Mode 2）；仅在 `AKM_DIRECT_MAP=0` 时生效 |
+| `AKM_YAW_KP` / `AKM_YAW_KI` | 0.10 / 0.00 | 横摆角速度误差 PI 增益（m/s per rad/s） |
+| `AKM_YAW_FF_ALPHA` | 0.00f | 几何前馈混合系数（0=纯反馈，1=纯几何差速=Mode 0） |
+| `AKM_YAW_MIN_SPEED` | 0.10f | 低于此速度冻结横摆环（m/s） |
+| `AKM_YAW_MAX_DIFF_RATIO` | 0.35f | 差速幅度上限（占 |Vx| 的比例） |
+| `AKM_GYRO_Z_TO_RADPS` | 3754.9f | gyro[2] LSB→rad/s（FS ±500dps） |
+| `AKM_GYRO_Z_SIGN` | +1.0f | IMU +z 与 ROS（+=左）符号对齐，实测确认 |
+| `AKM_YAW_IMU_LPF` | 0.30f | 横摆角速度一阶低通系数（0=无滤波） |
+
+模式互斥与优先级：
+- `AKM_SERVO_DEBUG_REMOTE_CH1 = 1` 时，**舵机总是由 CH1 覆盖**，优先于任何控制
+  律计算出的舵机值（电机差速仍按所选模式运行）。
+- `AKM_DIRECT_MAP` 与 `AKM_YAW_ASSIST` 都作用于整条控制律：`AKM_DIRECT_MAP = 1`
+  优先（直接映射）；`= 0` 时再看 `AKM_YAW_ASSIST`（1 = Mode 2 横摆闭环，
+  0 = Mode 0 标定运动学）。
+
+## 9. 串口通信（ROS ↔ STM32）
+
+### 频率与方向
+
+| 方向 | 频率 | 机制 | 代码位置 |
+|------|-----:|------|----------|
+| 控制环 | 200 Hz | `Balance_task`，`RATE_200_HZ` | balance.c:350 |
+| 发送（TX，STM32→ROS） | 200 Hz | **DMA 非阻塞** | usartx.c `data_task` |
+| 接收（RX，ROS→STM32） | 中断驱动 | `USART3_IRQHandler`（RXNE） | usartx.c |
+
+- 只保留 **USART3（ROS）**。原先 `data_task` 20 Hz 阻塞式群发 USART1/3/5+CAN，
+  现已删掉 USART1/USART5/CAN 发送，只留 USART3。
+- 帧长 24 字节（`SEND_DATA_SIZE`），`Vz` 在 RX 中断里由
+  `XYZ_Target_Speed_transition` 解析：`raw/1000 + (raw%1000)*0.001`（rad/s）。
+
+### 为什么改 DMA 非阻塞发送
+
+旧的 `usart3_send` 是忙等：`USART3->DR = data; while((USART3->SR&0x40)==0);`。
+`data_task` 与 `Balance_task` **同为优先级 4**（FreeRTOS 抢占 + 时间片，1ms tick）。
+同优先级下忙等无法被控制环抢占，20→200 Hz 会放大抖动、白耗 CPU。改成 DMA 后：
+
+- `USART3_SEND` 只触发一次 DMA 传输（DMA1 Stream3 / Channel4 = USART3_TX），
+  CPU 立即返回，shift-out 期间不占用控制环。
+- 上一帧未发完则跳过本周期（`DMA_GetCmdStatus != DISABLE` 判定），不忙等。
+- 需要把 `FWLIB/src/stm32f4xx_dma.c` 加入 Makefile（原先未编译该驱动）。
+
+### 波特率 921600
+
+USART3 波特率 **115200 → 921600**（system.c:129，`uart3_init(921600)`）。
+
+- 一帧 24 字节 = 240 bit（8N1，10 bit/byte）的 shift-out 时间：
+  - 115200：240/115200 ≈ **2.08 ms**
+  - 921600：240/921600 ≈ **0.26 ms**（快 8 倍，远低于 5 ms 周期）
+- 带宽占用：200 Hz × 24 B = 4800 B/s，两档都绰绰有余；提速主要是留余量。
+- APB1=42 MHz，16 倍过采样分频后实际约 913 k，偏差 ~0.9%（UART 容忍 <2.5%）。
+- **ROS 端必须同步改 921600**，否则乱码——这是最易漏的一步。
+
+## 10. 复现拟合
+
+标定原始脚本与中间产物在 `calibration/` 目录。核心步骤：
+1. R(cm) → R(m)，`kappa = sign / R_m`（sign 由 x 半弓高定，+ 为右）。
+2. 对 `(kappa, servo)` 做二次最小二乘，得 C0/C1/C2。
+3. 用 LOO 交叉验证确认二次优于查表与三次。
+4. `AKM_KAPPA_MAX = max(|kappa|)`。
diff --git a/doc/uart_protocol_v2_timestamp.md b/doc/uart_protocol_v2_timestamp.md
new file mode 100644
index 0000000..c1197eb
--- /dev/null
+++ b/doc/uart_protocol_v2_timestamp.md
@@ -0,0 +1,111 @@
+# 下位机 UART 发送协议 v2（带采样时间戳）
+
+本文件描述 STM32 下位机通过 **USART3（ROS 口，波特率 921600）** 周期发送给上位机的
+传感器数据帧格式。v2 在原 24 字节帧的基础上，于**帧尾追加**了会话 ID 与两个采样
+时间戳字段，用于上位机做 IMU / 轮速 / 雷达的时间对齐。
+
+- 对应源码：[HARDWARE/usartx.c](../HARDWARE/usartx.c)、[HARDWARE/usartx.h](../HARDWARE/usartx.h)
+- 时间源实现：[HARDWARE/timer.c](../HARDWARE/timer.c) 的 `TIM7_Init()` / `mcu_time_us()`
+- 设计背景与总体原则见 [时间戳.md](时间戳.md)
+
+## 1. 帧总览
+
+| 项目 | 值 |
+| ---- | ---- |
+| 帧头 `FRAME_HEADER` | `0x7B` |
+| 帧尾 `FRAME_TAIL` | `0x7D` |
+| 帧长 `SEND_DATA_SIZE` | **44 字节**（v1 为 24 字节） |
+| 发送周期 | 200 Hz（`data_task`，USART3 非阻塞 DMA 发送） |
+| 字节序 | 多字节字段一律 **大端**（高字节在前） |
+| 校验 | 单字节 XOR，覆盖 byte 0..41 |
+
+设计约束：byte 0..21 与 v1 完全一致，新增字段全部放在校验位之前的尾部，
+使只读取旧字段的解析器仍然兼容。
+
+## 2. 字节布局
+
+| 偏移 | 长度 | 字段 | 类型 | 说明 |
+| ---- | ---- | ---- | ---- | ---- |
+| 0 | 1 | Frame_Header | u8 | 帧头 `0x7B` |
+| 1 | 1 | Flag_Stop | u8 | 软件失能标志（1=停车） |
+| 2 | 2 | X_speed | int16 | X 轴线速度，单位 mm/s，大端 |
+| 4 | 2 | Y_speed | int16 | Y 轴线速度，单位 mm/s，大端 |
+| 6 | 2 | Z_speed | int16 | Z 轴角速度，单位 mm/s（等效），大端 |
+| 8 | 2 | Accel_X | int16 | 加速度计 X（ROS 坐标），大端 |
+| 10 | 2 | Accel_Y | int16 | 加速度计 Y（ROS 坐标），大端 |
+| 12 | 2 | Accel_Z | int16 | 加速度计 Z（ROS 坐标），大端 |
+| 14 | 2 | Gyro_X | int16 | 陀螺仪 X（ROS 坐标），大端 |
+| 16 | 2 | Gyro_Y | int16 | 陀螺仪 Y（ROS 坐标），大端 |
+| 18 | 2 | Gyro_Z | int16 | 陀螺仪 Z（ROS 坐标），大端 |
+| 20 | 2 | Power_Voltage | int16 | 电池电压×1000，大端 |
+| **22** | **4** | **session_id** | **uint32** | 本次 MCU 启动会话 ID，大端 |
+| **26** | **8** | **speed_sample_time_us** | **uint64** | 编码器采样时刻（μs），大端 |
+| **34** | **8** | **imu_sample_time_us** | **uint64** | IMU 采样时刻（μs），大端 |
+| 42 | 1 | Checksum | u8 | byte 0..41 的 XOR |
+| 43 | 1 | Frame_Tail | u8 | 帧尾 `0x7D` |
+
+加粗行为 v2 相对 v1 的新增字段。
+
+## 3. 时间源
+
+时间戳由自由运行的 **TIM7** 提供，见 [HARDWARE/timer.c](../HARDWARE/timer.c)：
+
+- TIM7 挂在 APB1，定时器时钟 84 MHz，预分频 84 → **1 MHz 计数（1 tick = 1 μs）**。
+- 16 位计数器每 65.536 ms 回绕一次，更新中断把高 32 位软件计数 `g_time_high` 加一。
+- `mcu_time_us()` 返回 `(g_time_high << 16) | CNT`，即**自上电以来的单调递增微秒数**，
+  可用位宽 48 位（约 8.9 年不回绕）。
+- TIM2~TIM5 被四路编码器占用、TIM8 被航模遥控输入捕获占用，故选空闲的 TIM7。
+- TIM7 更新中断优先级设为 5（数值大于 `configMAX_SYSCALL_INTERRUPT_PRIORITY`
+  对应的抢占门限），ISR 内只做自增，不调用任何 FreeRTOS API。
+
+> 注意：`mcu_time_us()` 只增不减，绝不因校时而跳变。上位机负责维护
+> `T_host = a·t_mcu + b` 的时钟映射（见 [时间戳.md](时间戳.md) 第 7、8 节）。
+
+## 4. 两个时间戳分别打在哪里
+
+v1 只有"发送时刻"隐含语义，无法表达 IMU（100 Hz）与轮速（200 Hz）是**异步采集**的。
+v2 在各自真实采样点打点，写入全局变量，`data_transition()` 打包时快照：
+
+| 时间戳 | 打点位置 | 采样率 |
+| ---- | ---- | ---- |
+| `speed_sample_time_us` | [balance.c](../BALANCE/balance.c) `Get_Velocity_Form_Encoder()` 读编码器锁存计数器的瞬间 | 200 Hz |
+| `imu_sample_time_us` | [MPU6050.c](../HARDWARE/MPU6050/MPU6050.c) `MPU6050_task` I2C 读取前 | 100 Hz |
+
+因此一帧内同时带两个时间戳：上位机应把加速度/角速度对齐到 `imu_sample_time_us`，
+把 X/Y/Z 速度对齐到 `speed_sample_time_us`，而不是共用一个发送时刻。
+
+> IMU 打点在 I2C 突发读取之前，是在没有 DRDY（数据就绪）中断条件下能取到的、
+> 最接近物理采样的时刻。若日后接入 MPU 的 DRDY 引脚中断，可把打点移到中断里进一步减小抖动。
+
+## 5. session_id
+
+- 定义在 [usartx.c](../HARDWARE/usartx.c)：`g_session_id`。
+- 当前实现为**固定值 `0xA5A5A5A5`**。
+- 用途：上位机据此判断下位机是否重启过（重启后 `mcu_time_us()` 从 0 重新计时，
+  时钟映射 `a, b` 必须重新拟合）。
+- 待办：如需真正区分每次上电，应在启动时用熵源（如未初始化 RAM、启动 tick 等）
+  生成一次性随机值。当前固定值只能表明"协议版本一致"，不能区分不同启动会话。
+
+## 6. 校验与解析建议
+
+- 校验位 `buffer[42] = XOR(buffer[0..41])`，与 v1 的计算方式相同，只是范围从 22 扩到 42。
+- 上位机解析步骤：
+  1. 同步帧头 `0x7B`；
+  2. 读满 44 字节；
+  3. 校验 `buffer[43] == 0x7D` 且 `XOR(buffer[0..41]) == buffer[42]`；
+  4. 按上表大端解出各字段。
+- 多字节字段务必按大端手动组装，不要直接 `memcpy` 结构体（编译器填充/对齐/端序差异）。
+
+## 7. 其他发送通道
+
+| 通道 | 现状 |
+| ---- | ---- |
+| USART3（ROS 主口，DMA） | 已用 `SEND_DATA_SIZE`，自动发送完整 44 字节 |
+| USART1 / USART5 | 已改用 `SEND_DATA_SIZE`，发送完整 44 字节 |
+| CAN | 当前未被调用；`CAN_SEND()` 仍按 8×3=24 字节分包，**不含时间戳字段**。如需经 CAN 传时间戳需另行扩展分包 |
+
+## 8. 上位机适配提醒
+
+USART3 收到的帧已从 24 字节变为 **44 字节**。上位机（ROS 端）解析代码必须同步更新：
+帧长、校验范围、以及尾部三个新字段的读取。否则会因帧长不符导致解析错位。
+
diff --git a/doc/时间戳.md b/doc/时间戳.md
new file mode 100644
index 0000000..0321bc3
--- /dev/null
+++ b/doc/时间戳.md
@@ -0,0 +1,349 @@
+针对现在的“FreeRTOS 下位机 + Linux/ROS 2 上位机 + 后续轮速/IMU/2D雷达融合”，建议直接制定一套统一的时间戳与通信协议。
+
+最核心的原则是：
+
+[
+\boxed{
+\text{MCU原始单调时间戳永不修改}
+\quad+\quad
+\text{上位机维护时钟映射}
+}
+]
+
+即：
+
+[
+T_{\rm host}=a,t_{\rm mcu}+b
+]
+
+## 1. 下位机统一时间基准
+
+不要把 `xTaskGetTickCount()` 作为传感器主时间戳。
+
+它适合：
+
+* `vTaskDelayUntil()` 周期调度；
+* 超时判断；
+* 任务延时；
+* 毫秒级状态机。
+
+它不适合：
+
+* IMU与编码器精确对齐；
+* 200 Hz以上传感器采样；
+* 分析通信延迟；
+* 后续雷达、IMU、轮式里程计融合。
+
+FreeRTOS Tick 的分辨率由 `configTICK_RATE_HZ` 决定；ISR 中若确实要读 Tick，应使用 `xTaskGetTickCountFromISR()`。[FreeRTOS Task Utilities](https://www.freertos.org/Documentation/02-Kernel/04-API-references/03-Task-utilities/00-Task-utilities)
+
+### 推荐时间源
+
+使用一个32位硬件定时器：
+
+```text
+计数频率：1 MHz
+计数单位：1 us
+自动重装值：0xFFFFFFFF
+软件扩展：64 bit
+时间含义：MCU自本次上电以来经过的微秒数
+```
+
+例如 STM32 可以使用 TIM2 或 TIM5：
+
+[
+t_{\rm mcu}\in uint64_t,\qquad 单位=\mu s
+]
+
+32位、1 MHz计数器约每71.58分钟回绕一次，所以用更新中断增加高32位。
+
+```c
+static volatile uint32_t g_time_high = 0;
+
+void TIM2_IRQHandler(void)
+{
+    if (__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_UPDATE) != RESET) {
+        __HAL_TIM_CLEAR_IT(&htim2, TIM_IT_UPDATE);
+        g_time_high++;
+    }
+}
+
+uint64_t mcu_time_us(void)
+{
+    uint32_t high1;
+    uint32_t high2;
+    uint32_t low;
+    uint32_t status;
+
+    do {
+        high1 = g_time_high;
+        __DMB();
+
+        low = TIM2->CNT;
+        status = TIM2->SR;
+
+        __DMB();
+        high2 = g_time_high;
+    } while (high1 != high2);
+
+    /*
+     * 计数器已经回绕，但更新中断可能还没有得到执行。
+     * low 小于半量程，说明读取发生在回绕之后。
+     */
+    if ((status & TIM_SR_UIF) && low < 0x80000000U) {
+        high1++;
+    }
+
+    return ((uint64_t)high1 << 32) | low;
+}
+```
+
+注意：
+
+* 定时器时钟必须确认是否受 APB 分频后的“定时器倍频”影响；
+* 不要直接读取一个由中断写入的普通 `uint64_t`，32位MCU上未必原子；
+* 如果启用了 STOP 模式或 Tickless Idle，要确认这个定时器休眠时是否继续工作；
+* 时间戳只能递增，不能因为校时而向前或向后跳变。
+
+## 2. 时间戳应该在哪里打
+
+时间戳必须尽可能靠近数据真正产生的位置。
+
+| 数据       | 推荐时间戳位置       |
+| -------- | ------------- |
+| IMU      | DRDY中断发生时     |
+| SPI读取IMU | 不要等SPI读取完成才打点 |
+| 编码器      | 控制定时器锁存计数器时   |
+| 电机控制量    | PWM寄存器更新时     |
+| 控制周期     | 控制周期入口        |
+| 上位机接收    | 完整帧接收完成时，单独记录 |
+
+例如：
+
+```c
+typedef struct {
+    uint64_t sample_time_us;
+    int16_t gyro_raw[3];
+    int16_t accel_raw[3];
+} ImuSample;
+```
+
+IMU中断中只做：
+
+1. 读取 `mcu_time_us()`；
+2. 保存时间戳；
+3. 通知采集任务；
+4. 立即退出中断。
+
+不要在中断中完成协议编码、CRC计算和串口发送。
+
+如果传感器内部启用了低通滤波，DRDY时间仍然可能比真实物理测量晚一个固定群延迟。后续可以增加：
+
+```c
+corrected_time_us = drdy_time_us - imu_filter_delay_us;
+```
+
+但必须在确定传感器滤波器延迟后再补偿。
+
+## 3. FreeRTOS任务结构
+
+建议使用：
+
+```mermaid
+flowchart TD
+    A["IMU DRDY / 控制定时器 ISR"] --> B["采样环形缓冲区"]
+    B --> C["传感器处理任务"]
+    C --> D["协议发送队列"]
+    D --> E["唯一 UART TX 任务"]
+    F["UART DMA / IDLE ISR"] --> G["接收环形缓冲区"]
+    G --> H["协议解析任务"]
+```
+
+关键规则：
+
+* UART发送只能有一个所有者任务，避免多个任务发送的数据交叉；
+* UART接收使用DMA、IDLE中断和环形缓冲区；
+* ISR只负责搬运数据和通知任务；
+* 二进制协议和 `printf()` 调试日志不要共用同一个UART；
+* 如果ISR调用 FreeRTOS 的 `...FromISR()` 接口，中断优先级必须符合 `configMAX_SYSCALL_INTERRUPT_PRIORITY` 约束；
+* 周期任务使用 `vTaskDelayUntil()`，它能避免普通相对延时产生的累计漂移，但实际执行时刻仍应调用硬件微秒时钟记录。[FreeRTOS `vTaskDelayUntil()`](https://freertos.org/xtaskdelayuntiltask-control.html)
+
+例如200 Hz低精度周期任务：
+
+```c
+void SensorTask(void *argument)
+{
+    TickType_t last_wake = xTaskGetTickCount();
+    const TickType_t period = pdMS_TO_TICKS(5);
+
+    for (;;) {
+        vTaskDelayUntil(&last_wake, period);
+
+        uint64_t actual_time_us = mcu_time_us();
+
+        /* 采集和处理 */
+    }
+}
+```
+
+如果要求更稳定的200 Hz采样，应由硬件定时器产生中断，再通知任务，而不是完全依赖 FreeRTOS Tick。
+
+## 4. 推荐二进制帧规范 v1
+
+假设使用 UART 或 USB CDC，建议采用：
+
+```text
+COBS编码（协议头 + 负载 + CRC32） + 0x00帧分隔符
+```
+
+COBS的好处是任意解析错误后，都可以在下一个 `0x00` 重新找到帧边界。
+
+### 统一协议头：24字节
+
+之前已有的基础上额外增加
+| session_id     | uint32 |  4 | 本次MCU启动会话ID  |
+| sample_time_us | uint64 |  8 | MCU采样时间      |
+
+
+但不要直接执行：
+
+```c
+uart_send((uint8_t *)&header, sizeof(header)); // 不推荐
+```
+
+因为不同编译器可能存在：
+
+* 结构体填充；
+* 对齐差异；
+* 大小端差异；
+* 浮点格式差异。
+
+             |
+
+
+## 7. 四时间戳同步协议
+
+每次同步记录：
+
+* (T_1)：上位机发送请求；
+* (t_2)：MCU收到并解析请求；
+* (t_3)：MCU准备发送响应；
+* (T_4)：上位机收到完整响应。
+
+定义MCU时间减上位机时间为：
+
+[
+\theta=
+\frac{(t_2-T_1)+(t_3-T_4)}{2}
+]
+
+往返延迟：
+
+[
+d=(T_4-T_1)-(t_3-t_2)
+]
+
+初始转换：
+
+[
+T_{\rm host}\approx t_{\rm mcu}-\theta
+]
+
+运行中收集多组：
+
+[
+x_i=\frac{t_{2,i}+t_{3,i}}{2}
+]
+
+[
+y_i=\frac{T_{1,i}+T_{4,i}}{2}
+]
+
+拟合：
+
+[
+\boxed{T_{\rm host}=a,t_{\rm mcu}+b}
+]
+
+推荐参数：
+
+* 启动时连续同步20～50次；
+* 初始阶段先令 (a=1)，从最小RTT样本估计 (b)；
+* 运行中每1秒同步一次；
+* 保存最近60～120组；
+* 丢弃RTT明显偏大的样本；
+* 使用RTT最小的20%～30%拟合 (a,b)；
+* MCU时间戳本身永远不被“校准”或重写。
+
+上位机使用 `CLOCK_MONOTONIC_RAW` 或 `std::chrono::steady_clock`，不要使用会被NTP校时改变的墙上时间参与控制。
+
+## 8. ROS 2时间戳转换
+
+MCU的“上电微秒数”不能直接填进 ROS 消息的 `header.stamp`，否则无法和激光雷达时间对齐。
+
+上位机先得到：
+
+[
+T_{\rm steady}=a,t_{\rm mcu}+b
+]
+
+再在ROS桥接节点中保存一对锚点：
+
+[
+(T_{\rm steady,0},T_{\rm ros,0})
+]
+
+转换为：
+
+[
+T_{\rm ros,sample}
+==================
+
+T_{\rm ros,0}
++
+(T_{\rm steady,sample}-T_{\rm steady,0})
+]
+
+然后将这个时间填入：
+
+* `sensor_msgs/Imu.header.stamp`；
+* `nav_msgs/Odometry.header.stamp`；
+* 编码器或轮速自定义消息时间戳。
+
+这样轮速、IMU和激光雷达才能在同一ROS时间轴上融合。
+
+## 9. 上位机命令不要依赖绝对时间
+
+普通运动命令建议使用“接收后立即执行＋超时”：
+
+```c
+typedef struct {
+    uint32_t command_sequence;
+    uint32_t valid_for_us;
+    float velocity_ref;
+    float yaw_rate_ref;
+} MotionCommand;
+```
+
+MCU收到时记录：
+
+```c
+last_command_rx_us = mcu_time_us();
+```
+
+安全判断：
+
+```c
+if (mcu_time_us() - last_command_rx_us > command.valid_for_us) {
+    enter_safe_deceleration();
+}
+```
+
+只有真正需要未来定时执行时，才让上位机通过逆映射计算：
+
+[
+t_{\rm mcu,execute}
+===================
+
+\frac{T_{\rm host,execute}-b}{a}
+]
+