cyy/origincar_controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

完善阿克曼控制与高速串口遥测

Browse Source 
- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
This commit is contained in:
cyy_mac
2026-08-12 18:51:48 +08:00
parent 576784bb4d
commit 93ca37e36c
17 changed files with 2151 additions and 932 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -1,2 +1,3 @@
build/ build/
.firecrawl/
.DS_Store .DS_Store

646
BALANCE/balance.c
View File

File diff suppressed because it is too large Load Diff

9
BALANCE/balance.h
View File

@@ -22,10 +22,10 @@ float target_limit_float(float insert,float low,float high);
int target_limit_int(int insert,int low,int high); int target_limit_int(int insert,int low,int high);
u8 Turn_Off( int voltage); u8 Turn_Off( int voltage);
u32 myabs(long int a); u32 myabs(long int a);
int Incremental_PI_A (float Encoder,float Target); int Incremental_PI_A (float Encoder,float Target,float dt);
int Incremental_PI_B (float Encoder,float Target); int Incremental_PI_B (float Encoder,float Target,float dt);
int Incremental_PI_C (float Encoder,float Target); int Incremental_PI_C (float Encoder,float Target,float dt);
int Incremental_PI_D (float Encoder,float Target); int Incremental_PI_D (float Encoder,float Target,float dt);
void Get_RC(void); void Get_RC(void);
void Remote_Control(void); void Remote_Control(void);
void Drive_Motor(float Vx,float Vy,float Vz); void Drive_Motor(float Vx,float Vy,float Vz);
@@ -36,4 +36,3 @@ void PS2_control(void);
float float_abs(float insert); float float_abs(float insert);
void robot_mode_check(void); void robot_mode_check(void);
#endif #endif

96
BALANCE/robot_select_init.h
View File

@@ -4,19 +4,19 @@
#include "system.h" #include "system.h"
//Parameter structure of robot //Parameter structure of robot
//机器人参数结构体 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
typedef struct typedef struct
{ {
float WheelSpacing; //Wheelspacing, Mec_Car is half wheelspacing //轮距 麦轮车为半轮距 float WheelSpacing; //Wheelspacing, Mec_Car is half wheelspacing //<EFBFBD>־<EFBFBD> <20><><EFBFBD>ֳ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>־<EFBFBD>
float AxleSpacing; //Axlespacing, Mec_Car is half axlespacing //轴距 麦轮车为半轴距 float AxleSpacing; //Axlespacing, Mec_Car is half axlespacing //<EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>ֳ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>
int GearRatio; //Motor_gear_ratio //电机减速比 int GearRatio; //Motor_gear_ratio //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٱ<EFBFBD>
int EncoderAccuracy; //Number_of_encoder_lines //编码器精度(编码器线数) int EncoderAccuracy; //Number_of_encoder_lines //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
float WheelDiameter; //Diameter of driving wheel //主动轮直径 float WheelDiameter; //Diameter of driving wheel //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD>
float OmniTurnRadiaus; //Rotation radius of omnidirectional trolley //全向轮小车旋转半径 float OmniTurnRadiaus; //Rotation radius of omnidirectional trolley //ȫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>뾶
}Robot_Parament_InitTypeDef; }Robot_Parament_InitTypeDef;
// Encoder structure // Encoder structure
//编码器结构体 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
typedef struct typedef struct
{ {
int A; int A;
@@ -27,28 +27,74 @@ typedef struct
//The minimum turning radius of Ackermann models is determined by the mechanical structure: //The minimum turning radius of Ackermann models is determined by the mechanical structure:
//the maximum Angle of the wheelbase, wheelbase and front wheels //the maximum Angle of the wheelbase, wheelbase and front wheels
//阿克曼车型的最小转弯半径，由机械结构决定：轮距、轴距、前轮最大转角 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD><EFBFBD><EFBFBD>Сת<EFBFBD><EFBFBD>뾶<EFBFBD><EFBFBD><EFBFBD>ɻ<EFBFBD>е<EFBFBD>ṹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־ࡢ<EFBFBD><EFBFBD>ࡢǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
#define MINI_AKM_MIN_TURN_RADIUS 0.350f #define MINI_AKM_MIN_TURN_RADIUS 0.350f
//Wheelspacing, Mec_Car is half wheelspacing //Wheelspacing, Mec_Car is half wheelspacing
//轮距 麦轮是一半 //<EFBFBD>־<EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
//#define MEC_wheelspacing 0.109 //#define MEC_wheelspacing 0.109
#define MEC_wheelspacing 0.0930 //修正2021.03.30 #define MEC_wheelspacing 0.0930 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>2021.03.30
#define Akm_wheelspacing 0.162f #define Akm_wheelspacing 0.160f
#define Diff_wheelSpacing 0.177f #define Diff_wheelSpacing 0.177f
#define Four_Mortor_wheelSpacing 0.26f #define Four_Mortor_wheelSpacing 0.26f
#define Tank_wheelSpacing 0.235f #define Tank_wheelSpacing 0.235f
//Axlespacing, Mec_Car is half axlespacing //Axlespacing, Mec_Car is half axlespacing
//轴距 麦轮是一半 //<EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
#define MEC_axlespacing 0.085 #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 Diff_axlespacing 0.155f
#define Four_Mortor__axlespacing 0.28f #define Four_Mortor__axlespacing 0.28f
#define Tank_axlespacing 0.222f #define Tank_axlespacing 0.222f
//Motor_gear_ratio //Motor_gear_ratio
//电机减速比 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٱ<EFBFBD>
#define HALL_30F 30 #define HALL_30F 30
#define HALL_60F 60 #define HALL_60F 60
#define MD36N_5_18 5.18 #define MD36N_5_18 5.18
@@ -59,12 +105,12 @@ typedef struct
#define MD60N_47 47 #define MD60N_47 47
//Number_of_encoder_lines //Number_of_encoder_lines
//编码器精度 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define Photoelectric_500 500 #define Photoelectric_500 500
#define Hall_13 13 #define Hall_13 13
//Mecanum wheel tire diameter series //Mecanum wheel tire diameter series
//麦轮轮胎直径 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ̥<EFBFBD><EFBFBD>
#define Mecanum_60 0.060f #define Mecanum_60 0.060f
#define Mecanum_75 0.075f #define Mecanum_75 0.075f
#define Mecanum_100 0.100f #define Mecanum_100 0.100f
@@ -72,7 +118,7 @@ typedef struct
#define Mecanum_152 0.152f #define Mecanum_152 0.152f
//Omni wheel tire diameter series //Omni wheel tire diameter series
//轮径全向轮直径系列 //<EFBFBD>־<EFBFBD>ȫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>
#define FullDirecion_60 0.060 #define FullDirecion_60 0.060
#define FullDirecion_75 0.075 #define FullDirecion_75 0.075
#define FullDirecion_127 0.127 #define FullDirecion_127 0.127
@@ -81,26 +127,26 @@ typedef struct
#define FullDirecion_217 0.217 #define FullDirecion_217 0.217
//Black tire, tank_car wheel diameter //Black tire, tank_car wheel diameter
//黑色轮胎、履带车轮直径 //<EFBFBD><EFBFBD>ɫ<EFBFBD><EFBFBD>̥<EFBFBD><EFBFBD><EFBFBD>Ĵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD>
#define Black_WheelDiameter 0.065 #define Black_WheelDiameter 0.065
//#define Tank_WheelDiameter 0.047 //#define Tank_WheelDiameter 0.047
#define Tank_WheelDiameter 0.043 #define Tank_WheelDiameter 0.043
//Rotation radius of omnidirectional trolley //Rotation radius of omnidirectional trolley
//全向轮小车旋转半径 //ȫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>뾶
#define Omni_Turn_Radiaus_109 0.109 #define Omni_Turn_Radiaus_109 0.109
#define Omni_Turn_Radiaus_164 0.164 #define Omni_Turn_Radiaus_164 0.164
#define Omni_Turn_Radiaus_180 0.180 #define Omni_Turn_Radiaus_180 0.180
#define Omni_Turn_Radiaus_290 0.290 #define Omni_Turn_Radiaus_290 0.290
//The encoder octave depends on the encoder initialization Settings //The encoder octave depends on the encoder initialization Settings
//编码器倍频数，取决于编码器初始化设置 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>ڱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define EncoderMultiples 4 #define EncoderMultiples 4
//Encoder data reading frequency //Wheel-speed control and encoder reading frequency
//编码器数据读取频率 //<EFBFBD><EFBFBD><EFBFBD>ٿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡƵ<EFBFBD><EFBFBD>
#define CONTROL_FREQUENCY 100 #define CONTROL_FREQUENCY 200
//#define PI 3.1415f //PI //圆周率 //#define PI 3.1415f //PI //Բ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void Robot_Select(void); void Robot_Select(void);
void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter); void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter);

6
BALANCE/show.c
View File

@@ -26,8 +26,8 @@ void show_task(void *pvParameters)
//开机时蜂鸣器短暂蜂鸣，开机提醒 //开机时蜂鸣器短暂蜂鸣，开机提醒
//The buzzer will beep briefly when the machine is switched on //The buzzer will beep briefly when the machine is switched on
if(Time_count<50)Buzzer=1; if(Time_count<(CONTROL_FREQUENCY/2))Buzzer=1;
else if(Time_count>=51 && Time_count<100)Buzzer=0; else if(Time_count>=(CONTROL_FREQUENCY/2) && Time_count<CONTROL_FREQUENCY)Buzzer=0;
if(LowVoltage_1==1 || LowVoltage_2==1)Buzzer_count=0; if(LowVoltage_1==1 || LowVoltage_2==1)Buzzer_count=0;
if(Buzzer_count<5)Buzzer_count++; 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]); printf("{B%d:%d:%d}$",(int)gyro[0],(int)gyro[1],(int)gyro[2]);
} }
} }

124
BALANCE/system.c
View File

@@ -1,197 +1,203 @@
#include "system.h" #include "system.h"
//Robot software fails to flag bits //Robot software fails to flag bits
//机器人软件失能标志位 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܱ<EFBFBD>־λ
u8 Flag_Stop=1; u8 Flag_Stop=1;
//The ADC value is variable in segments, depending on the number of car models. Currently there are 6 car models //The ADC value is variable in segments, depending on the number of car models. Currently there are 6 car models
//ADC值分段变量，取决于小车型号数量，目前有6种小车型号 //ADCֵ<EFBFBD>ֶα<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀǰ<EFBFBD><EFBFBD>6<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD>
int Divisor_Mode; int Divisor_Mode;
// Robot type variable // Robot type variable
//机器人型号变量 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺű<EFBFBD><EFBFBD><EFBFBD>
//0=Mec_Car，1=Omni_Car，2=Akm_Car，3=Diff_Car，4=FourWheel_Car，5=Tank_Car //0=Mec_Car<EFBFBD><EFBFBD>1=Omni_Car<EFBFBD><EFBFBD>2=Akm_Car<EFBFBD><EFBFBD>3=Diff_Car<EFBFBD><EFBFBD>4=FourWheel_Car<EFBFBD><EFBFBD>5=Tank_Car
u8 Car_Mode=4; u8 Car_Mode=4;
//Servo control PWM value, Ackerman car special //Servo control PWM value, Ackerman car special
//舵机控制PWM值，阿克曼小车专用 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWMֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD>ר<EFBFBD><EFBFBD>
int Servo = SERVO_INIT; int Servo = SERVO_INIT;
//Default speed of remote control car, unit: mm/s //Default speed of remote control car, unit: mm/s
//遥控小车的默认速度，单位：mm/s //ң<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>mm/s
float RC_Velocity=500; float RC_Velocity=500;
//Vehicle three-axis target moving speed, unit: m/s //Vehicle three-axis target moving speed, unit: m/s
//小车三轴目标运动速度，单位：m/s //С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m/s
float Move_X, Move_Y, Move_Z; float Move_X, Move_Y, Move_Z;
//PID parameters of Speed control //Speed PI parameters: Ki is in PWM/(m/s*s) and multiplies measured dt.
//速度控制PID参数 //<EFBFBD>ٶ<EFBFBD>PI<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ki<EFBFBD><EFBFBD>λΪPWM/(m/s*s)<29><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>dt<64><74><EFBFBD>
float Velocity_KP=300,Velocity_KI=800; float Velocity_KP=300,Velocity_KI=80000;
//Smooth control of intermediate variables, dedicated to omni-directional moving cars //Smooth control of intermediate variables, dedicated to omni-directional moving cars
//平滑控制中间变量，全向移动小车专用 //ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȫ<EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD>С<EFBFBD><EFBFBD>ר<EFBFBD><EFBFBD>
Smooth_Control smooth_control; Smooth_Control smooth_control;
//The parameter structure of the motor //The parameter structure of the motor
//电机的参数结构体 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
Motor_parameter MOTOR_A,MOTOR_B,MOTOR_C,MOTOR_D; Motor_parameter MOTOR_A,MOTOR_B,MOTOR_C,MOTOR_D;
/************ 小车型号相关变量 **************************/ /************ С<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><EFBFBD><EFBFBD> **************************/
/************ Variables related to car model ************/ /************ Variables related to car model ************/
//Encoder accuracy //Encoder accuracy
//编码器精度 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Encoder_precision; float Encoder_precision;
//Wheel circumference, unit: m //Wheel circumference, unit: m
//轮子周长，单位：m //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m
float Wheel_perimeter; float Wheel_perimeter;
//Drive wheel base, unit: m //Drive wheel base, unit: m
//主动轮轮距，单位：m //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־࣬<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m
float Wheel_spacing; float Wheel_spacing;
//The wheelbase of the front and rear axles of the trolley, unit: m //The wheelbase of the front and rear axles of the trolley, unit: m
//小车前后轴的轴距，单位：m //С<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m
float Axle_spacing; float Axle_spacing;
//All-directional wheel turning radius, unit: m //All-directional wheel turning radius, unit: m
//全向轮转弯半径，单位：m //ȫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>뾶<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m
float Omni_turn_radiaus; float Omni_turn_radiaus;
/************ 小车型号相关变量 **************************/ /************ С<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><EFBFBD><EFBFBD> **************************/
/************ Variables related to car model ************/ /************ Variables related to car model ************/
//PS2 controller, Bluetooth APP, aircraft model controller, CAN communication, serial port 1, serial port 5 communication control flag bit. //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 //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<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD>CANͨ<EFBFBD>š<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>5ͨ<EFBFBD>ſ<EFBFBD><EFBFBD>Ʊ<EFBFBD>־λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>6<EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λĬ<EFBFBD>϶<EFBFBD>Ϊ0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
u8 PS2_ON_Flag=0, APP_ON_Flag=0, Remote_ON_Flag=0, CAN_ON_Flag=0, Usart1_ON_Flag, Usart5_ON_Flag; 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 //Bluetooth remote control associated flag bits
//蓝牙遥控相关的标志位 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ң<EFBFBD><EFBFBD><EFBFBD><EFBFBD>صı<EFBFBD>־λ
u8 Flag_Left, Flag_Right, Flag_Direction=0, Turn_Flag; u8 Flag_Left, Flag_Right, Flag_Direction=0, Turn_Flag;
//Sends the parameter's flag bit to the Bluetooth APP //Sends the parameter's flag bit to the Bluetooth APP
//向蓝牙APP发送参数的标志位 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<EFBFBD><EFBFBD><EFBFBD>Ͳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD>־λ
u8 PID_Send; u8 PID_Send;
//The PS2 gamepad controls related variables //The PS2 gamepad controls related variables
//PS2手柄控制相关变量 //PS2<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><EFBFBD><EFBFBD>
float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY; float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY;
//Self-check the relevant flag variables //Self-check the relevant flag variables
//自检相关标志变量 //<EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int Check=0, Checking=0, Checked=0, CheckCount=0, CheckPhrase1=0, CheckPhrase2=0; int Check=0, Checking=0, Checked=0, CheckCount=0, CheckPhrase1=0, CheckPhrase2=0;
//Check the result code //Check the result code
//自检结果代码 //<EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
long int ErrorCode=0; long int ErrorCode=0;
void systemInit(void) void systemInit(void)
{ {
// //Interrupt priority group setti ng // //Interrupt priority group setti ng
// //中断优先级分组设置 // //<EFBFBD>ж<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
// //
// //Delay function initialization // //Delay function initialization
// //延时函数初始化 // //<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
delay_init(168); delay_init(168);
//Initialize the hardware interface connected to the LED lamp //Initialize the hardware interface connected to the LED lamp
//初始化与LED灯连接的硬件接口 //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>LED<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
LED_Init(); LED_Init();
//Initialize the hardware interface connected to the buzzer //Initialize the hardware interface connected to the buzzer
//初始化与蜂鸣器连接的硬件接口 //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
Buzzer_Init(); Buzzer_Init();
//Initialize the hardware interface connected to the enable switch //Initialize the hardware interface connected to the enable switch
//初始化与使能开关连接的硬件接口 //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ܿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
Enable_Pin(); Enable_Pin();
//Initialize the hardware interface connected to the OLED display //Initialize the hardware interface connected to the OLED display
//初始化与OLED显示屏连接的硬件接口 //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>OLED<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
OLED_Init(); OLED_Init();
//Initialize the hardware interface connected to the user's key //Initialize the hardware interface connected to the user's key
//初始化与用户按键连接的硬件接口 //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
KEY_Init(); KEY_Init();
//Serial port 1 initialization, communication baud rate 115200, //Serial port 1 initialization, communication baud rate 115200,
//can be used to communicate with ROS terminal //can be used to communicate with ROS terminal
//串口1初始化，通信波特率115200，可用于与ROS端通信 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>115200<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROS<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>
uart1_init(115200); uart1_init(115200);
//Serial port 2 initialization, communication baud rate 9600, //Serial port 2 initialization, communication baud rate 9600,
//used to communicate with Bluetooth APP terminal //used to communicate with Bluetooth APP terminal
//串口2初始化，通信波特率9600，用于与蓝牙APP端通信 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>9600<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>
uart2_init(9600); uart2_init(9600);
//Serial port 3 is initialized and the baud rate is 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 //Serial port 3 is the default port used to communicate with ROS terminal.
//串口3初始化，通信波特率115200，串口3为默认用于与ROS端通信的串口 //NOTE: the ROS side must open this port at 921600 to match.
uart3_init(115200); //<2F><><EFBFBD><EFBFBD>3<EFBFBD><33>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><C5B2><EFBFBD><EFBFBD><EFBFBD>921600<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3ΪĬ<CEAA><C4AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROS<4F><53>ͨ<EFBFBD>ŵĴ<C5B5><C4B4>ڡ<EFBFBD>
//ע<>⣺ROS <20>˴򿪴<CBB4><F2BFAAB4><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD> 921600<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0>벻ƥ<EBB2BB>䡣
uart3_init(921600);
//Serial port 5 initialization, communication baud rate 115200, //Serial port 5 initialization, communication baud rate 115200,
//can be used to communicate with ROS terminal //can be used to communicate with ROS terminal
//串口5初始化，通信波特率115200，可用于与ROS端通信 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>5<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>115200<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROS<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>
uart5_init(115200); uart5_init(115200);
//ADC pin initialization, used to read the battery voltage and potentiometer gear, //ADC pin initialization, used to read the battery voltage and potentiometer gear,
//potentiometer gear determines the car after the boot of the car model //potentiometer gear determines the car after the boot of the car model
//ADC引脚初始化，用于读取电池电压与电位器档位，电位器档位决定小车开机后的小车适配型号 //ADC<EFBFBD><EFBFBD><EFBFBD>ų<EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD>ص<EFBFBD>ѹ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD>
Adc_Init(); Adc_Init();
Adc_POWER_Init(); Adc_POWER_Init();
//Initialize the CAN communication interface //Initialize the CAN communication interface
//CAN通信接口初始化 //CANͨ<EFBFBD>Žӿڳ<EFBFBD>ʼ<EFBFBD><EFBFBD>
CAN1_Mode_Init(1,7,6,3,0); 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, //According to the tap position of the potentiometer, determine which type of car needs to be matched,
//and then initialize the corresponding parameters //and then initialize the corresponding parameters
//根据电位器的档位判断需要适配的是哪一种型号的小车，然后进行对应的参数初始化 //<EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>λ<EFBFBD>ж<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>ͺŵ<EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>Ӧ<EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
Robot_Select(); Robot_Select();
//Encoder A is initialized to read the real time speed of motor C //Encoder A is initialized to read the real time speed of motor C
//编码器A初始化，用于读取电机C的实时速度 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>C<EFBFBD><EFBFBD>ʵʱ<EFBFBD>ٶ<EFBFBD>
Encoder_Init_TIM2(); Encoder_Init_TIM2();
//Encoder B is initialized to read the real time speed of motor D //Encoder B is initialized to read the real time speed of motor D
//编码器B初始化，用于读取电机D的实时速度 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>B<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>D<EFBFBD><EFBFBD>ʵʱ<EFBFBD>ٶ<EFBFBD>
Encoder_Init_TIM3(); Encoder_Init_TIM3();
//Encoder C is initialized to read the real time speed of motor B //Encoder C is initialized to read the real time speed of motor B
//编码器C初始化，用于读取电机B的实时速度 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>C<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>B<EFBFBD><EFBFBD>ʵʱ<EFBFBD>ٶ<EFBFBD>
Encoder_Init_TIM4(); Encoder_Init_TIM4();
//Encoder D is initialized to read the real time speed of motor A //Encoder D is initialized to read the real time speed of motor A
//编码器D初始化，用于读取电机A的实时速度 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>D<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>A<EFBFBD><EFBFBD>ʵʱ<EFBFBD>ٶ<EFBFBD>
Encoder_Init_TIM5(); Encoder_Init_TIM5();
//定时器12用作舵机的PWM接口 //<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>12<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWM<EFBFBD>ӿ<EFBFBD>
TIM12_SERVO_Init(9999,84-1); //APB1的时钟频率为84M , 频率=84M/((9999+1)*(83+1))=100Hz TIM12_SERVO_Init(9999,84-1); //APB1<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>Ϊ84M , Ƶ<EFBFBD><EFBFBD>=84M/((9999+1)*(83+1))=100Hz
//普通小车默认定时器8用作航模接口 //<EFBFBD><EFBFBD>ͨС<EFBFBD><EFBFBD>Ĭ<EFBFBD>϶<EFBFBD>ʱ<EFBFBD><EFBFBD>8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>ӿ<EFBFBD>
// TIM8_SERVO_Init(9999,168-1);//APB2的时钟频率为168M , 频率=168M/((9999+1)*(167+1))=100Hz // TIM8_SERVO_Init(9999,168-1);//APB2<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>Ϊ168M , Ƶ<EFBFBD><EFBFBD>=168M/((9999+1)*(167+1))=100Hz
//Initialize the model remote control interface //Initialize the model remote control interface
//初始化航模遥控接口 //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģң<EFBFBD>ؽӿ<EFBFBD>
TIM8_Cap_Init(9999,168-1); //高级定时器TIM8的时钟频率为168M TIM8_Cap_Init(9999,168-1); //<EFBFBD>߼<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>TIM8<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>Ϊ168M
//Free-running 1 MHz microsecond time base for sensor sample timestamps.
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD> 1MHz ΢<>뼶<EFBFBD><EBBCB6><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>׼<EFBFBD><D7BC>
TIM7_Init();
//Initialize motor speed control and, for controlling motor speed, PWM frequency 10kHz //Initialize motor speed control and, for controlling motor speed, PWM frequency 10kHz
//初始化电机速度控制以及，用于控制电机速度，PWM频率10KHZ //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڿ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD>PWMƵ<EFBFBD><EFBFBD>10KHZ
//APB2时钟频率为168M，满PWM为16799，频率=168M/((16799+1)*(0+1))=10k //APB2ʱ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>Ϊ168M<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWMΪ16799<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>=168M/((16799+1)*(0+1))=10k
TIM1_PWM_Init(16799,0); TIM1_PWM_Init(16799,0);
TIM9_PWM_Init(16799,0); TIM9_PWM_Init(16799,0);
TIM10_PWM_Init(16799,0); TIM10_PWM_Init(16799,0);
TIM11_PWM_Init(16799,0); TIM11_PWM_Init(16799,0);
//IIC initialization for MPU6050 //IIC initialization for MPU6050
//IIC初始化，用于MPU6050 //IIC<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPU6050
I2C_GPIOInit(); I2C_GPIOInit();
//MPU6050 is initialized to read the vehicle's three-axis attitude, //MPU6050 is initialized to read the vehicle's three-axis attitude,
//three-axis angular velocity and three-axis acceleration information //three-axis angular velocity and three-axis acceleration information
//MPU6050 初始化，用于读取小车三轴姿态、三轴角速度、三轴加速度信息 //MPU6050 <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡС<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD>Ϣ
MPU6050_initialize(); MPU6050_initialize();
//Initialize the hardware interface to the PS2 controller //Initialize the hardware interface to the PS2 controller
//初始化与PS2手柄连接的硬件接口 //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PS2<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
PS2_Init(); PS2_Init();
//PS2 gamepad configuration is initialized and configured in analog mode //PS2 gamepad configuration is initialized and configured in analog mode
//PS2手柄配置初始化,配置为模拟量模式 //PS2<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD>ʼ<EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>Ϊģ<CEAA><C4A3><EFBFBD><EFBFBD>ģʽ
PS2_SetInit(); PS2_SetInit();
} }

8
BALANCE/system.h
View File

@@ -94,9 +94,11 @@ extern long int ErrorCode;
void systemInit(void); void systemInit(void);
/***Macros define***/ /***宏定义***/ /***Macros define***/ /***宏定义***/
//After starting the car (1000/100Hz =10) for seconds, it is allowed to control the car to move //Collect IMU zero-bias samples for 2 seconds at 100 Hz before enabling control.
//开机(1000/100hz=10)秒后才允许控制小车进行运动 //开机后以100Hz采集2秒IMU零偏，再允许控制小车运动
#define CONTROL_DELAY 1000 #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 //The number of robot types to determine the value of Divisor_Mode. There are currently 6 car types
//机器人型号数量，决定Divisor_Mode的值，目前有6种小车类型 //机器人型号数量，决定Divisor_Mode的值，目前有6种小车类型
#define CAR_NUMBER 6 #define CAR_NUMBER 6

222
HARDWARE/MPU6050/MPU6050.c
View File

@@ -13,14 +13,14 @@
#define FLASH_MEM_START ((void*)0x1800) #define FLASH_MEM_START ((void*)0x1800)
#define q30 1073741824.0f #define q30 1073741824.0f
short gyro[3], accel[3], sensors; short gyro[3], accel[3], sensors;
//零点漂移计数 //<EFBFBD><EFBFBD><EFBFBD>Ư<EFBFBD>Ƽ<EFBFBD><EFBFBD><EFBFBD>
int Deviation_Count; int Deviation_Count;
short sum_gyro[3]; short sum_gyro[3];
short sum_accel[3]; short sum_accel[3];
// Gyro static error, raw data // Gyro static error, raw data
//陀螺仪静差，原始数据 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD><EFBFBD>ԭʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
short Deviation_gyro[3],Original_gyro[3]; short Deviation_gyro[3],Original_gyro[3];
short Deviation_accel[3],Original_accel[3]; short Deviation_accel[3],Original_accel[3];
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
@@ -55,11 +55,11 @@ void MPU6050_task(void *pvParameters)
while(1) while(1)
{ {
//This task runs at 100Hz //This task runs at 100Hz
//此任务以100Hz的频率运行 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>100Hz<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ)); vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));
//Read the gyroscope zero before starting //Read the gyroscope zero before starting
//开机前，读取陀螺仪零点 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(Deviation_Count<CONTROL_DELAY) if(Deviation_Count<CONTROL_DELAY)
{ {
Deviation_Count++; Deviation_Count++;
@@ -78,10 +78,14 @@ void MPU6050_task(void *pvParameters)
memcpy(Deviation_accel,accel,sizeof(accel)); memcpy(Deviation_accel,accel,sizeof(accel));
} }
MPU_Get_Gyroscope(); //得到陀螺仪数据 //Stamp just before the I2C burst read, as close to the physical IMU
MPU_Get_Accelscope(); //获得加速度计值(原始值) //sample as we can get without a data-ready interrupt.
//<2F><> I2C <20><>ȡǰ<C8A1><C7B0>㣬<EFBFBD>ھ<EFBFBD><DABE><EFBFBD> DRDY <20>жϵ<D0B6><CFB5><EFBFBD><EFBFBD><EFBFBD>¾<EFBFBD><C2BE><EFBFBD><EFBFBD>ӽ<EFBFBD><D3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>̡<EFBFBD>
g_imu_sample_time_us = mcu_time_us();
MPU_Get_Gyroscope(); //<2F>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
MPU_Get_Accelscope(); //<2F><>ü<EFBFBD><C3BC>ٶȼ<D9B6>ֵ(ԭʼֵ)
} }
} }
//static unsigned short inv_orientation_matrix_to_scalar( //static unsigned short inv_orientation_matrix_to_scalar(
// const signed char *mtx) // 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 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<EFBFBD><EFBFBD>ay<EFBFBD><EFBFBD>az<EFBFBD><EFBFBD>x<EFBFBD><EFBFBD>y, z-axis acceleration data<EFBFBD><EFBFBD>gx<EFBFBD><EFBFBD>gy<EFBFBD><EFBFBD>gz<EFBFBD><EFBFBD>x. Y, z-axis angular acceleration data
Output : none Output : none
函数功能：将新的ADC数据更新到 FIFO数组，进行滤波处理 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>ADC<EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><EFBFBD>µ<EFBFBD> FIFO<46><4F><EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>
入口参数：ax，ay，az：x，y，z轴加速度数据；gx，gy，gz：x，y，z轴角加速度数据 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ax<EFBFBD><EFBFBD>ay<EFBFBD><EFBFBD>az<EFBFBD><EFBFBD>x<EFBFBD><EFBFBD>y<EFBFBD><EFBFBD>z<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>gx<EFBFBD><EFBFBD>gy<EFBFBD><EFBFBD>gz<EFBFBD><EFBFBD>x<EFBFBD><EFBFBD>y<EFBFBD><EFBFBD>z<EFBFBD><EFBFBD>Ǽ<EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz) void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
{ {
unsigned char i ; unsigned char i ;
int32_t sum=0; int32_t sum=0;
for(i=1;i<10;i++){ //FIFO 操作 for(i=1;i<10;i++){ //FIFO <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i]; MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i]; MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][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[4][i-1]=MPU6050_FIFO[4][i];
MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i]; MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
} }
MPU6050_FIFO[0][9]=ax;//将新的数据放置到 数据的最后面 MPU6050_FIFO[0][9]=ax;//<EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD><EFBFBD>õ<EFBFBD> <20><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
MPU6050_FIFO[1][9]=ay; MPU6050_FIFO[1][9]=ay;
MPU6050_FIFO[2][9]=az; MPU6050_FIFO[2][9]=az;
MPU6050_FIFO[3][9]=gx; MPU6050_FIFO[3][9]=gx;
@@ -158,7 +162,7 @@ MPU6050_FIFO[4][9]=gy;
MPU6050_FIFO[5][9]=gz; MPU6050_FIFO[5][9]=gz;
sum=0; sum=0;
for(i=0;i<10;i++){ //求当前数组的合，再取平均值 for(i=0;i<10;i++){ //<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĺϣ<EFBFBD><EFBFBD><EFBFBD>ȡƽ<EFBFBD><EFBFBD>ֵ
sum+=MPU6050_FIFO[0][i]; sum+=MPU6050_FIFO[0][i];
} }
MPU6050_FIFO[0][10]=sum/10; MPU6050_FIFO[0][10]=sum/10;
@@ -196,11 +200,11 @@ MPU6050_FIFO[5][10]=sum/10;
/************************************************************************** /**************************************************************************
Function: Setting the clock source of mpu6050 Function: Setting the clock source of mpu6050
Input : source：Clock source number Input : source<EFBFBD><EFBFBD>Clock source number
Output : none Output : none
函数功能：设置 MPU6050 的时钟源 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Դ
入口参数：source：时钟源编号 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>source<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
* CLK_SEL | Clock Source * CLK_SEL | Clock Source
* --------+-------------------------------------- * --------+--------------------------------------
* 0 | Internal oscillator * 0 | Internal oscillator
@@ -231,27 +235,27 @@ void MPU6050_setFullScaleGyroRange(uint8_t range) {
/************************************************************************** /**************************************************************************
Function: Setting the maximum range of mpu6050 accelerometer Function: Setting the maximum range of mpu6050 accelerometer
Input : range：Acceleration maximum range number Input : range<EFBFBD><EFBFBD>Acceleration maximum range number
Output : none Output : none
函数功能：设置 MPU6050 加速度计的最大量程 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20><><EFBFBD>ٶȼƵ<C8BC><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
入口参数：range：加速度最大量程编号 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>range<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̱<EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
//#define MPU6050_ACCEL_FS_2 0x00 //===最大量程+-2G //#define MPU6050_ACCEL_FS_2 0x00 //===<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-2G
//#define MPU6050_ACCEL_FS_4 0x01 //===最大量程+-4G //#define MPU6050_ACCEL_FS_4 0x01 //===<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-4G
//#define MPU6050_ACCEL_FS_8 0x02 //===最大量程+-8G //#define MPU6050_ACCEL_FS_8 0x02 //===<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-8G
//#define MPU6050_ACCEL_FS_16 0x03 //===最大量程+-16G //#define MPU6050_ACCEL_FS_16 0x03 //===<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-16G
void MPU6050_setFullScaleAccelRange(uint8_t range) { void MPU6050_setFullScaleAccelRange(uint8_t range) {
I2C_WriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, 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 Function: Set mpu6050 to sleep mode or not
Input : enable：1，sleep；0，work； Input : enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD>sleep<EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>work<EFBFBD><EFBFBD>
Output : none Output : none
函数功能：设置 MPU6050 是否进入睡眠模式 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <20>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>˯<EFBFBD><CBAF>ģʽ
入口参数：enable：1，睡觉；0，工作； <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD>˯<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) { void MPU6050_setSleepEnabled(uint8_t enabled) {
I2C_WriteOneBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 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 Function: Read identity
Input : none Input : none
Output : 0x68 Output : 0x68
函数功能：读取 MPU6050 WHO_AM_I 标识 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ȡ MPU6050 WHO_AM_I <EFBFBD><EFBFBD>ʶ
入口参数：无 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：0x68 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5>0x68
**************************************************************************/ **************************************************************************/
uint8_t MPU6050_getDeviceID(void) { uint8_t MPU6050_getDeviceID(void) {
@@ -277,10 +281,10 @@ uint8_t MPU6050_getDeviceID(void) {
/************************************************************************** /**************************************************************************
Function: Check whether mpu6050 is connected Function: Check whether mpu6050 is connected
Input : none Input : none
Output : 1：Connected；0：Not connected Output : 1<EFBFBD><EFBFBD>Connected<EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>Not connected
函数功能：检测MPU6050 是否已经连接 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPU6050 <20>Ƿ<EFBFBD><C7B7>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
入口参数：无 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：1：已连接；0：未连接 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӣ<EFBFBD>0<EFBFBD><30>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
uint8_t MPU6050_testConnection(void) { uint8_t MPU6050_testConnection(void) {
if(MPU6050_getDeviceID() == 0x68) //0b01101000; 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 Function: Setting whether mpu6050 is the host of aux I2C cable
Input : enable：1，yes；0;not Input : enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD>yes<EFBFBD><EFBFBD>0;not
Output : none Output : none
函数功能：设置 MPU6050 是否为AUX I2C线的主机 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <EFBFBD>Ƿ<EFBFBD>ΪAUX I2C<EFBFBD>ߵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
入口参数：enable：1，是；0：否 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD>0<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) { void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
I2C_WriteOneBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 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 Function: Setting whether mpu6050 is the host of aux I2C cable
Input : enable：1，yes；0;not Input : enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD>yes<EFBFBD><EFBFBD>0;not
Output : none Output : none
函数功能：设置 MPU6050 是否为AUX I2C线的主机 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MPU6050 <EFBFBD>Ƿ<EFBFBD>ΪAUX I2C<EFBFBD>ߵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
入口参数：enable：1，是；0：否 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>enable<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD>0<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) { void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
I2C_WriteOneBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, 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 Function: initialization Mpu6050 to enter the available state
Input : none Input : none
Output : none Output : none
函数功能：初始化 MPU6050 以进入可用状态 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD> MPU6050 <20>Խ<EFBFBD><D4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
入口参数：无 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
u8 MPU6050_initialize(void) u8 MPU6050_initialize(void)
{ {
u8 res; u8 res;
//IIC_Init(); //Initialize the IIC bus //初始化IIC总线 //IIC_Init(); //Initialize the IIC bus //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>IIC<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X80); //Reset MPUrobot_select_init.h //复位MPUrobot_select_init.h I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X80); //Reset MPUrobot_select_init.h //<EFBFBD><EFBFBD>λMPUrobot_select_init.h
delay_ms(200); //Delay 200 ms //延时200ms delay_ms(200); //Delay 200 ms //<EFBFBD><EFBFBD>ʱ200ms
I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X00); //Wake mpurobot_select_init.h //唤醒MPUrobot_select_init.h I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X00); //Wake mpurobot_select_init.h //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>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 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><>500dps=<EFBFBD><EFBFBD>500<EFBFBD><EFBFBD>/s <EFBFBD><EFBFBD>32768 (gyro/32768*500)*PI/180(rad/s)=gyro/3754.9(rad/s)
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_500); 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 //<EFBFBD><EFBFBD><EFBFBD>ٶȴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><>2g=<EFBFBD><EFBFBD>2*9.8m/s^2 <EFBFBD><EFBFBD>32768 accel/32768*19.6=accel/1671.84
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); 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 //<EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>50Hz
I2C_WriteOneByte(devAddr,MPU6050_RA_INT_ENABLE,0X00); //Turn off all interrupts //关闭所有中断 I2C_WriteOneByte(devAddr,MPU6050_RA_INT_ENABLE,0X00); //Turn off all interrupts //<EFBFBD>ر<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_USER_CTRL,0X00); //The I2C main mode is off //I2C主模式关闭 I2C_WriteOneByte(devAddr,MPU6050_RA_USER_CTRL,0X00); //The I2C main mode is off //I2C<EFBFBD><EFBFBD>ģʽ<EFBFBD>ر<EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_FIFO_EN,0X00); //Close the FIFO //关闭FIFO I2C_WriteOneByte(devAddr,MPU6050_RA_FIFO_EN,0X00); //Close the FIFO //<EFBFBD>ر<EFBFBD>FIFO
//The INT pin is low, enabling bypass mode to read the magnetometer directly //The INT pin is low, enabling bypass mode to read the magnetometer directly
//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计 //INT<EFBFBD><EFBFBD><EFBFBD>ŵ͵<EFBFBD>ƽ<EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>bypassģʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>Ӷ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_INT_PIN_CFG,0X80); I2C_WriteOneByte(devAddr,MPU6050_RA_INT_PIN_CFG,0X80);
//Read the ID of MPU6050 //Read the ID of MPU6050
//读取MPU6050的ID //<EFBFBD><EFBFBD>ȡMPU6050<EFBFBD><EFBFBD>ID
res=I2C_ReadOneByte(devAddr,MPU6050_RA_WHO_AM_I); 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 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ID<EFBFBD><EFBFBD>ȷ, <20><><EFBFBD><EFBFBD>ID<49><44><EFBFBD><EFBFBD>ȷȡ<C8B7><C8A1><EFBFBD><EFBFBD>AD<41><44><EFBFBD><EFBFBD>
{ {
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_1,0X01); //Set CLKSEL,PLL X axis as reference //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>CLKSEL,PLL X<EFBFBD><EFBFBD>Ϊ<EFBFBD>ο<EFBFBD>
I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_2,0X00); //Acceleration and gyroscope both work //加速度与陀螺仪都工作 I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_2,0X00); //Acceleration and gyroscope both work //<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
MPU6050_Set_Rate(50); //Set the sampling rate to 50Hz //设置采样率为50Hz MPU6050_Set_Rate(50); //Set the sampling rate to 50Hz //<EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ50Hz
}else return 1; }else return 1;
return 0; return 0;
@@ -357,9 +361,9 @@ u8 MPU6050_initialize(void)
Function: Initialization of DMP in mpu6050 Function: Initialization of DMP in mpu6050
Input : none Input : none
Output : none Output : none
函数功能：MPU6050内置DMP的初始化 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>MPU6050<EFBFBD><EFBFBD><EFBFBD><EFBFBD>DMP<EFBFBD>ij<EFBFBD>ʼ<EFBFBD><EFBFBD>
入口参数：无 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
//void DMP_Init(void) //void DMP_Init(void)
//{ //{
@@ -395,9 +399,9 @@ Output : none
Function: Read the attitude information of DMP in mpu6050 Function: Read the attitude information of DMP in mpu6050
Input : none Input : none
Output : none Output : none
函数功能：读取MPU6050内置DMP的姿态信息 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ȡMPU6050<EFBFBD><EFBFBD><EFBFBD><EFBFBD>DMP<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>Ϣ
入口参数：无 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
//void Read_DMP(void) //void Read_DMP(void)
//{ //{
@@ -405,16 +409,16 @@ Output : none
// unsigned char more; // unsigned char more;
// long quat[4]; // long quat[4];
// dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); //读取DMP数据 // dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); //<EFBFBD><EFBFBD>ȡDMP<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// if (sensors & INV_WXYZ_QUAT ) // if (sensors & INV_WXYZ_QUAT )
// { // {
// q0=quat[0] / q30; // q0=quat[0] / q30;
// q1=quat[1] / q30; // q1=quat[1] / q30;
// q2=quat[2] / q30; // q2=quat[2] / q30;
// q3=quat[3] / q30; //四元数 // q3=quat[3] / q30; //<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
// Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //计算出横滚角 // Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // 计算出俯仰角 // Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //计算出偏航角 // Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// } // }
//} //}
@@ -422,16 +426,16 @@ Output : none
Function: Read mpu6050 built-in temperature sensor data Function: Read mpu6050 built-in temperature sensor data
Input : none Input : none
Output : Centigrade temperature Output : Centigrade temperature
函数功能：读取MPU6050内置温度传感器数据 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ȡMPU6050<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶ȴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
入口参数：无 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：摄氏温度 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
int Read_Temperature(void) int Read_Temperature(void)
{ {
float Temp; float Temp;
Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L); Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L);
if(Temp>32768) Temp-=65536; //数据类型转换 if(Temp>32768) Temp-=65536; //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
Temp=(36.53f+Temp/340)*10; //温度放大十倍存放 Temp=(36.53f+Temp/340)*10; //<EFBFBD>¶ȷŴ<EFBFBD>ʮ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
return (int)Temp; return (int)Temp;
} }
@@ -439,9 +443,9 @@ int Read_Temperature(void)
Function: Initialize TIM2 as the encoder interface mode Function: Initialize TIM2 as the encoder interface mode
Input : LPF: Digital low-pass filtering frequency (Hz) Input : LPF: Digital low-pass filtering frequency (Hz)
Output : 0: Settings successful, others: Settings failed Output : 0: Settings successful, others: Settings failed
函数功能：设置MPUrobot_select_init.h的数字低通滤波器 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPUrobot_select_init.h<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>
入口参数：lpf:数字低通滤波频率(Hz) <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>lpf:<3A><><EFBFBD>ֵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD>Ƶ<EFBFBD><C6B5>(Hz)
返回 值：0:设置成功, 其他:设置失败 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5>0:<3A><><EFBFBD>óɹ<C3B3>, <20><><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
**************************************************************************/ **************************************************************************/
unsigned char MPU6050_Set_LPF(u16 lpf) 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>=20)data=4;
else if(lpf>=10)data=5; else if(lpf>=10)data=5;
else data=6; 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//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>
} }
/************************************************************************** /**************************************************************************
Function: Initialize TIM2 as the encoder interface mode Function: Initialize TIM2 as the encoder interface mode
Input : rate:4~1000(Hz) Input : rate:4~1000(Hz)
Output : 0: Settings successful, others: Settings failed Output : 0: Settings successful, others: Settings failed
函数功能：设置MPUrobot_select_init.h的采样率(假定Fs=1KHz) <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPUrobot_select_init.h<EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<28>ٶ<EFBFBD>Fs=1KHz)
入口参数：rate:4~1000(Hz) <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>rate:4~1000(Hz)
返回 值：0:设置成功, 其他:设置失败 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5>0:<3A><><EFBFBD>óɹ<C3B3>, <20><><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
**************************************************************************/ **************************************************************************/
unsigned char MPU6050_Set_Rate(u16 rate) 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>1000)rate=1000;
if(rate<4)rate=4; if(rate<4)rate=4;
data=1000/rate-1; data=1000/rate-1;
data=I2C_WriteOneByte(devAddr,MPU6050_RA_SMPLRT_DIV,data); //Set the digital lowpass filter//设置数字低通滤波器 data=I2C_WriteOneByte(devAddr,MPU6050_RA_SMPLRT_DIV,data); //Set the digital lowpass filter//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>
return MPU6050_Set_LPF(rate/2); //Automatically sets LPF to half of the sampling rate //自动设置LPF为采样率的一半 return MPU6050_Set_LPF(rate/2); //Automatically sets LPF to half of the sampling rate //<EFBFBD>Զ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>LPFΪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>һ<EFBFBD><EFBFBD>
} }
/************************************************************************** /**************************************************************************
Function: Initialize TIM2 as the encoder interface mode 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 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 Output : 0: success, others: error code
函数功能：获得陀螺仪值(原始值) <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ(ԭʼֵ)
**************************************************************************/ **************************************************************************/
void MPU_Get_Gyroscope(void) 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[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L); //<EFBFBD><EFBFBD>ȡX<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
gyro[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //读取Y轴陀螺仪 gyro[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //<EFBFBD><EFBFBD>ȡY<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
gyro[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪 gyro[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //<EFBFBD><EFBFBD>ȡZ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //开机前10秒 if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ10<EFBFBD><EFBFBD>
{ {
Led_Count=1; //LED high frequency flashing //LED高频闪烁 Led_Count=1; //LED high frequency flashing //LED<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>˸
Flag_Stop=1; //The software fails to flag location 1 //软件失能标志位置1 Flag_Stop=1; //The software fails to flag location 1 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܱ<EFBFBD>־λ<EFBFBD><EFBFBD>1
} }
else //10 seconds after starting //开机10秒后 else //10 seconds after starting //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<EFBFBD><EFBFBD><EFBFBD>
{ {
if(Deviation_Count==CONTROL_DELAY) if(Deviation_Count==CONTROL_DELAY)
Flag_Stop=0; //The software fails to flag location 0 //软件失能标志位置0 Flag_Stop=0; //The software fails to flag location 0 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܱ<EFBFBD>־λ<EFBFBD><EFBFBD>0
Led_Count=300; //The LED returns to normal flicker frequency //LED恢复正常闪烁频率 Led_Count=300; //The LED returns to normal flicker frequency //LED<EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˸Ƶ<EFBFBD><EFBFBD>
//Save the raw data to update zero by clicking the user button //Save the raw data to update zero by clicking the user button
//保存原始数据用于单击用户按键更新零点 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Original_gyro[0] =gyro[0]; Original_gyro[0] =gyro[0];
Original_gyro[1] =gyro[1]; Original_gyro[1] =gyro[1];
Original_gyro[2]= gyro[2]; Original_gyro[2]= gyro[2];
//Removes zero drift data //Removes zero drift data
//去除零点漂移的数据 //ȥ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ư<EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
gyro[0] =Original_gyro[0]-Deviation_gyro[0]; gyro[0] =Original_gyro[0]-Deviation_gyro[0];
gyro[1] =Original_gyro[1]-Deviation_gyro[1]; gyro[1] =Original_gyro[1]-Deviation_gyro[1];
gyro[2]= Original_gyro[2]-Deviation_gyro[2]; 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 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 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 Output : 0: success, others: error code
函数功能：获得加速度计值(原始值) <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ü<EFBFBD><EFBFBD>ٶȼ<EFBFBD>ֵ(ԭʼֵ)
**************************************************************************/ **************************************************************************/
void MPU_Get_Accelscope(void) 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[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //<EFBFBD><EFBFBD>ȡX<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼ<EFBFBD>
accel[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //读取X轴加速度计 accel[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //<EFBFBD><EFBFBD>ȡX<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼ<EFBFBD>
accel[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度计 accel[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //<EFBFBD><EFBFBD>ȡZ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼ<EFBFBD>
if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //开机前10秒 if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ10<EFBFBD><EFBFBD>
{ {
} }
else //10 seconds after starting //开机10秒后 else //10 seconds after starting //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<EFBFBD><EFBFBD><EFBFBD>
{ {
//Save the raw data to update zero by clicking the user button //Save the raw data to update zero by clicking the user button
//保存原始数据用于单击用户按键更新零点 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Original_accel[0] =accel[0]; Original_accel[0] =accel[0];
Original_accel[1] =accel[1]; Original_accel[1] =accel[1];
Original_accel[2]= accel[2]; Original_accel[2]= accel[2];
//Removes zero drift data //Removes zero drift data
//去除零点漂移的数据 //ȥ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ư<EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
accel[0] =Original_accel[0]-Deviation_accel[0]; accel[0] =Original_accel[0]-Deviation_accel[0];
accel[1] =Original_accel[1]-Deviation_accel[1]; accel[1] =Original_accel[1]-Deviation_accel[1];
accel[2]= Original_accel[2]-Deviation_accel[2]+16384; accel[2]= Original_accel[2]-Deviation_accel[2]+16384;

2
HARDWARE/motor.h
View File

@@ -49,7 +49,7 @@
#define EN PDin(3) #define EN PDin(3)
#define Servo_PWM TIM12->CCR2 #define Servo_PWM TIM12->CCR2
#define SERVO_INIT 1590 //Servo zero point //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> #define SERVO_INIT 1670 //Servo straight-ahead point (calibrated 1/R~=0) //<2F><>ֱ<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD>
void Enable_Pin(void); void Enable_Pin(void);
void Servo_PWM_Init(u16 arr,u16 psc); void Servo_PWM_Init(u16 arr,u16 psc);

380
HARDWARE/timer.c
View File

@@ -2,28 +2,28 @@
//Input the capture flag for channel 1, //Input the capture flag for channel 1,
//the capture flag for the higher bits, and the overflow flag for the lower 6 bits //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
//通道1输入捕获标志，高两位做捕获标志，低6位做溢出标志 //ͨ<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD>6λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
u8 TIM8CH1_CAPTURE_STA = 0; u8 TIM8CH1_CAPTURE_STA = 0;
u16 TIM8CH1_CAPTURE_UPVAL; u16 TIM8CH1_CAPTURE_UPVAL;
u16 TIM8CH1_CAPTURE_DOWNVAL; u16 TIM8CH1_CAPTURE_DOWNVAL;
//Input the capture flag for channel 2, //Input the capture flag for channel 2,
//the capture flag for the higher bits, and the overflow flag for the lower 6 bits //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
//通道2输入捕获标志，高两位做捕获标志，低6位做溢出标志 //ͨ<EFBFBD><EFBFBD>2<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD>6λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
u8 TIM8CH2_CAPTURE_STA = 0; u8 TIM8CH2_CAPTURE_STA = 0;
u16 TIM8CH2_CAPTURE_UPVAL; u16 TIM8CH2_CAPTURE_UPVAL;
u16 TIM8CH2_CAPTURE_DOWNVAL; u16 TIM8CH2_CAPTURE_DOWNVAL;
//Input the capture flag for channel 3, //Input the capture flag for channel 3,
//the capture flag for the higher bits, and the overflow flag for the lower 6 bits //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
//通道3输入捕获标志，高两位做捕获标志，低6位做溢出标志 //ͨ<EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD>6λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
u8 TIM8CH3_CAPTURE_STA = 0; u8 TIM8CH3_CAPTURE_STA = 0;
u16 TIM8CH3_CAPTURE_UPVAL; u16 TIM8CH3_CAPTURE_UPVAL;
u16 TIM8CH3_CAPTURE_DOWNVAL; u16 TIM8CH3_CAPTURE_DOWNVAL;
//Input the capture flag for channel 4, //Input the capture flag for channel 4,
//the capture flag for the higher bits, and the overflow flag for the lower 6 bits //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
//通道4输入捕获标志，高两位做捕获标志，低6位做溢出标志 //ͨ<EFBFBD><EFBFBD>4<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD>6λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
u8 TIM8CH4_CAPTURE_STA = 0; u8 TIM8CH4_CAPTURE_STA = 0;
u16 TIM8CH4_CAPTURE_UPVAL; u16 TIM8CH4_CAPTURE_UPVAL;
u16 TIM8CH4_CAPTURE_DOWNVAL; u16 TIM8CH4_CAPTURE_DOWNVAL;
@@ -34,19 +34,19 @@ u32 TIM8_T3;
u32 TIM8_T4; u32 TIM8_T4;
//Variables related to remote control acquisition of model aircraft //Variables related to remote control acquisition of model aircraft
//航模遥控采集相关变量 //<EFBFBD><EFBFBD>ģң<EFBFBD>زɼ<EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><EFBFBD><EFBFBD>
int Remoter_Ch1=1500,Remoter_Ch2=1500,Remoter_Ch3=1500,Remoter_Ch4=1500; int Remoter_Ch1=1500,Remoter_Ch2=1500,Remoter_Ch3=1500,Remoter_Ch4=1500;
//Model aircraft remote control receiver variable //Model aircraft remote control receiver variable
//航模遥控接收变量 //<EFBFBD><EFBFBD>ģң<EFBFBD>ؽ<EFBFBD><EFBFBD>ձ<EFBFBD><EFBFBD><EFBFBD>
int L_Remoter_Ch1=1500,L_Remoter_Ch2=1500,L_Remoter_Ch3=1500,L_Remoter_Ch4=1500; 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 Function: Model aircraft remote control initialization function, timer 1 input capture initialization
Input : arr: Automatic reload value, psc: clock preset frequency Input : arr: Automatic reload value, psc: clock preset frequency
Output : none Output : none
函数功能：航模遥控初始化函数，定时器1输入捕获初始化 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ģң<EFBFBD>س<EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
入口参数：arr：自动重装值，psc：时钟预分频数 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>arr<EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><EFBFBD><EFBFBD>װֵ<EFBFBD><EFBFBD>psc<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>
返 回 值：无 <EFBFBD><EFBFBD> <20><> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
void TIM8_Cap_Init(u16 arr, u16 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; NVIC_InitTypeDef NVIC_InitStructure;
TIM_ICInitTypeDef TIM_ICInitStructure; TIM_ICInitTypeDef TIM_ICInitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE); //TIM1时钟使能 RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE); //TIM1ʱ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); //使能PORTE时钟 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); //ʹ<EFBFBD><EFBFBD>PORTEʱ<EFBFBD><EFBFBD>
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; //GPIOC 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_Mode = GPIO_Mode_AF;//<EFBFBD><EFBFBD><EFBFBD>ù<EFBFBD><EFBFBD><EFBFBD>
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //速度100MHz GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //<EFBFBD>ٶ<EFBFBD>100MHz
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出 GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //<EFBFBD><EFBFBD><EFBFBD>츴<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; //下拉 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GPIO_Init(GPIOC,&GPIO_InitStructure); GPIO_Init(GPIOC,&GPIO_InitStructure);
GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); 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_PinSource8,GPIO_AF_TIM8);
GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_TIM8); GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_TIM8);
/*** Initialize timer 1 || 初始化定时器1 ***/ /*** Initialize timer 1 || <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>1 ***/
//Set the counter to automatically reload //设定计数器自动重装值 //Set the counter to automatically reload //<EFBFBD>趨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><EFBFBD><EFBFBD>װֵ
TIM_TimeBaseStructure.TIM_Period = arr; TIM_TimeBaseStructure.TIM_Period = arr;
//Pre-divider //预分频器 //Pre-divider //Ԥ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>
TIM_TimeBaseStructure.TIM_Prescaler = psc; TIM_TimeBaseStructure.TIM_Prescaler = psc;
//Set the clock split: TDTS = Tck_tim //设置时钟分割:TDTS = Tck_tim //Set the clock split: TDTS = Tck_tim //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ӷָ<EFBFBD>:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
//TIM up count mode //TIM向上计数模式 //TIM up count mode //TIM<EFBFBD><EFBFBD><EFBFBD>ϼ<EFBFBD><EFBFBD><EFBFBD>ģʽ
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct //Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
//根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>TIM_TimeBaseInitStruct<EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>TIMx<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
/*** 初始化TIM1输入捕获参数，通道1 || Initialize TIM1 for the capture parameter, channel 1 ***/ /*** <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>TIM1<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>1 || Initialize TIM1 for the capture parameter, channel 1 ***/
//Select input //选择输入端 //Select input //ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
//Rising edge capture //上升沿捕获 //Rising edge capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
//Configure input frequency division, regardless of frequency //配置输入分频,不分频 //Configure input frequency division, regardless of frequency //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ,<2C><><EFBFBD><EFBFBD>Ƶ
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
//IC1F=0000 Configure input filter //配置输入滤波器 //IC1F=0000 Configure input filter //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_ICFilter = 0x0F; TIM_ICInitStructure.TIM_ICFilter = 0x0F;
TIM_ICInit(TIM8, &TIM_ICInitStructure); TIM_ICInit(TIM8, &TIM_ICInitStructure);
/*** 初始化TIM1输入捕获参数，通道2 || Initialize TIM1 for the capture parameter, channel 2 ***/ /*** <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>TIM1<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>2 || Initialize TIM1 for the capture parameter, channel 2 ***/
//CC1S=01 Select input //选择输入端 //CC1S=01 Select input //ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
//Rising edge capture //上升沿捕获 //Rising edge capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
//Configure input frequency division, regardless of frequency //配置输入分频,不分频 //Configure input frequency division, regardless of frequency //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ,<2C><><EFBFBD><EFBFBD>Ƶ
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x00; //IC1F=0000 配置输入滤波器 TIM_ICInitStructure.TIM_ICFilter = 0x00; //IC1F=0000 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>
TIM_ICInit(TIM8, &TIM_ICInitStructure); TIM_ICInit(TIM8, &TIM_ICInitStructure);
/*** 初始化TIM1输入捕获参数，通道3 || Initialize TIM1 for the capture parameter, channel 3 ***/ /*** <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>TIM1<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>3 || Initialize TIM1 for the capture parameter, channel 3 ***/
//Select input //选择输入端 //Select input //ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_Channel = TIM_Channel_3; TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
//Rising edge capture //上升沿捕获 //Rising edge capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
//Configure input frequency division, regardless of frequency //配置输入分频,不分频 //Configure input frequency division, regardless of frequency //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ,<2C><><EFBFBD><EFBFBD>Ƶ
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
//IC1F=0000 Configure input filter //配置输入滤波器，不滤波 //IC1F=0000 Configure input filter //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>
TIM_ICInitStructure.TIM_ICFilter = 0x00; TIM_ICInitStructure.TIM_ICFilter = 0x00;
TIM_ICInit(TIM8, &TIM_ICInitStructure); TIM_ICInit(TIM8, &TIM_ICInitStructure);
/*** 初始化TIM1输入捕获参数，通道4 || Initialize TIM1 for the capture parameter, channel 4 ***/ /*** <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>TIM1<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>4 || Initialize TIM1 for the capture parameter, channel 4 ***/
//Select input //选择输入端 //Select input //ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_Channel = TIM_Channel_4; TIM_ICInitStructure.TIM_Channel = TIM_Channel_4;
//Rising edge capture //上升沿捕获 //Rising edge capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
//Configure input frequency division, regardless of frequency //配置输入分频,不分频 //Configure input frequency division, regardless of frequency //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ,<2C><><EFBFBD><EFBFBD>Ƶ
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
//IC1F=0000 Configure input filter //配置输入滤波器，不滤波 //IC1F=0000 Configure input filter //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>
TIM_ICInitStructure.TIM_ICFilter = 0x00; TIM_ICInitStructure.TIM_ICFilter = 0x00;
TIM_ICInit(TIM8, &TIM_ICInitStructure); TIM_ICInit(TIM8, &TIM_ICInitStructure);
/*** interrupt packet initialization || 中断分组初始化 ***/ /*** interrupt packet initialization || <EFBFBD>жϷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD> ***/
//TIM1 interrupts //TIM1中断 //TIM1 interrupts //TIM1<EFBFBD>ж<EFBFBD>
NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn; NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn;
//Preempt priority 0 //先占优先级0级 //Preempt priority 0 //<EFBFBD><EFBFBD>ռ<EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>0<EFBFBD><EFBFBD>
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
//Level 0 from priority //从优先级0级 //Level 0 from priority //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>0<EFBFBD><EFBFBD>
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
//IRQ channels are enabled //IRQ通道被使能 //IRQ channels are enabled //IRQͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
//Initializes the peripheral NVIC register according to the parameters specified in NVIC_InitStruct //Initializes the peripheral NVIC register according to the parameters specified in NVIC_InitStruct
//根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>NVIC_InitStruct<EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>NVIC<EFBFBD>Ĵ<EFBFBD><EFBFBD><EFBFBD>
NVIC_Init(&NVIC_InitStructure); NVIC_Init(&NVIC_InitStructure);
//Allow CC1IE,CC2IE,CC3IE,CC4IE to catch interrupts, not allowed update_interrupts //Allow CC1IE,CC2IE,CC3IE,CC4IE to catch interrupts, not allowed update_interrupts
//不允许更新中断，允许CC1IE,CC2IE,CC3IE,CC4IE捕获中断 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>CC1IE,CC2IE,CC3IE,CC4IE<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
TIM_ITConfig(TIM8, TIM_IT_CC1|TIM_IT_CC2|TIM_IT_CC3|TIM_IT_CC4, ENABLE); 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 //<EFBFBD>߼<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_CtrlPWMOutputs(TIM8,ENABLE); TIM_CtrlPWMOutputs(TIM8,ENABLE);
//Enable timer //使能定时器 //Enable timer //ʹ<EFBFBD>ܶ<EFBFBD>ʱ<EFBFBD><EFBFBD>
TIM_Cmd(TIM8, ENABLE); TIM_Cmd(TIM8, ENABLE);
} }
/************************************************************************** /**************************************************************************
Function: Model aircraft remote control receiving interrupt, namely timer 8 input capture interrupt Function: Model aircraft remote control receiving interrupt, namely timer 8 input capture interrupt
Input : none Input : none
Output : none Output : none
函数功能：航模遥控接收中断，即定时器8输入捕获中断 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ģң<EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>8<EFBFBD><EFBFBD><EFBFBD>벶<EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
入口参数：无 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返 回 值：无 <EFBFBD><EFBFBD> <20><> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
void TIM8_CC_IRQHandler(void) void TIM8_CC_IRQHandler(void)
{ {
//连接航模遥遥控器后，需要推下前进杆，才可以正式航模控制小车 //<EFBFBD><EFBFBD><EFBFBD>Ӻ<EFBFBD>ģңң<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD><EFBFBD>ſ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD>
//After connecting the remote controller of the model aircraft, //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 //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) if(Remoter_Ch2>1600&&Remote_ON_Flag==0&&Deviation_Count>=CONTROL_DELAY)
{ {
//Model aircraft remote control mark position 1, other marks position 0 //Model aircraft remote control mark position 1, other marks position 0
//航模遥控标志位置1，其它标志位置0 //<EFBFBD><EFBFBD>ģң<EFBFBD>ر<EFBFBD>־λ<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ<EFBFBD><EFBFBD>0
Remote_ON_Flag=1; Remote_ON_Flag=1;
APP_ON_Flag=0; APP_ON_Flag=0;
PS2_ON_Flag=0; PS2_ON_Flag=0;
@@ -179,131 +179,131 @@ void TIM8_CC_IRQHandler(void)
// //Channel 1 //通道一 // //Channel 1 //ͨ<EFBFBD><EFBFBD>һ
if ((TIM8CH1_CAPTURE_STA & 0X80) == 0) 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 //ͨ<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD>
{ {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC1); //Clear the interrupt flag bit //清除中断标志位 TIM_ClearITPendingBit(TIM8, TIM_IT_CC1); //Clear the interrupt flag bit //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϱ<EFBFBD>־λ
if (TIM8CH1_CAPTURE_STA & 0X40) //A falling edge is caught //捕获到一个下降沿 if (TIM8CH1_CAPTURE_STA & 0X40) //A falling edge is caught //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD><EFBFBD>
{ {
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 //<EFBFBD><EFBFBD>¼<EFBFBD>´<EFBFBD>ʱ<EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
if (TIM8CH1_CAPTURE_DOWNVAL < TIM8CH1_CAPTURE_UPVAL) if (TIM8CH1_CAPTURE_DOWNVAL < TIM8CH1_CAPTURE_UPVAL)
{ {
TIM8_T1 = 9999; TIM8_T1 = 9999;
} }
else else
TIM8_T1 = 0; TIM8_T1 = 0;
Remoter_Ch1 = TIM8CH1_CAPTURE_DOWNVAL - TIM8CH1_CAPTURE_UPVAL + TIM8_T1; //Time to get the total high level //得到总的高电平的时间 Remoter_Ch1 = TIM8CH1_CAPTURE_DOWNVAL - TIM8CH1_CAPTURE_UPVAL + TIM8_T1; //Time to get the total high level //<EFBFBD>õ<EFBFBD><EFBFBD>ܵĸߵ<EFBFBD>ƽ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
if(abs(Remoter_Ch1-L_Remoter_Ch1)>500) Remoter_Ch1=L_Remoter_Ch1; //Filter //滤波 if(abs(Remoter_Ch1-L_Remoter_Ch1)>500) Remoter_Ch1=L_Remoter_Ch1; //Filter //<EFBFBD>˲<EFBFBD>
L_Remoter_Ch1=Remoter_Ch1; L_Remoter_Ch1=Remoter_Ch1;
TIM8CH1_CAPTURE_STA = 0; //Capture flag bit to zero //捕获标志位清零 TIM8CH1_CAPTURE_STA = 0; //Capture flag bit to zero //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //设置为上升沿捕获 TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
} }
else 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 //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
//发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䵫<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD>ʱ<EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
TIM8CH1_CAPTURE_UPVAL = TIM_GetCapture1(TIM8); //Obtain rising edge data //获取上升沿数据 TIM8CH1_CAPTURE_UPVAL = TIM_GetCapture1(TIM8); //Obtain rising edge data //<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM8CH1_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //标记已捕获到上升沿 TIM8CH1_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //设置为下降沿捕获 TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>½<EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
} }
} }
} }
//Channel 2 //通道二 //Channel 2 //ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if ((TIM8CH2_CAPTURE_STA & 0X80) == 0) 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 //ͨ<EFBFBD><EFBFBD>2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD>
{ {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC2); //Clear the interrupt flag bit //清除中断标志位 TIM_ClearITPendingBit(TIM8, TIM_IT_CC2); //Clear the interrupt flag bit //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϱ<EFBFBD>־λ
if (TIM8CH2_CAPTURE_STA & 0X40) //A falling edge is caught //捕获到一个下降沿 if (TIM8CH2_CAPTURE_STA & 0X40) //A falling edge is caught //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD><EFBFBD>
{ {
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 //<EFBFBD><EFBFBD>¼<EFBFBD>´<EFBFBD>ʱ<EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
if (TIM8CH2_CAPTURE_DOWNVAL < TIM8CH2_CAPTURE_UPVAL) if (TIM8CH2_CAPTURE_DOWNVAL < TIM8CH2_CAPTURE_UPVAL)
{ {
TIM8_T2 = 9999; TIM8_T2 = 9999;
} }
else else
TIM8_T2 = 0; TIM8_T2 = 0;
Remoter_Ch2 = TIM8CH2_CAPTURE_DOWNVAL - TIM8CH2_CAPTURE_UPVAL + TIM8_T2; //Time to get the total high level //得到总的高电平的时间 Remoter_Ch2 = TIM8CH2_CAPTURE_DOWNVAL - TIM8CH2_CAPTURE_UPVAL + TIM8_T2; //Time to get the total high level //<EFBFBD>õ<EFBFBD><EFBFBD>ܵĸߵ<EFBFBD>ƽ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
if(abs(Remoter_Ch2-L_Remoter_Ch2)>500)Remoter_Ch2=L_Remoter_Ch2; //Filter //滤波 if(abs(Remoter_Ch2-L_Remoter_Ch2)>500)Remoter_Ch2=L_Remoter_Ch2; //Filter //<EFBFBD>˲<EFBFBD>
L_Remoter_Ch2=Remoter_Ch2; L_Remoter_Ch2=Remoter_Ch2;
TIM8CH2_CAPTURE_STA = 0; //Capture flag bit to zero //捕获标志位清零 TIM8CH2_CAPTURE_STA = 0; //Capture flag bit to zero //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //设置为上升沿捕获 TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
} }
else 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 //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
//发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䵫<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD>ʱ<EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
TIM8CH2_CAPTURE_UPVAL = TIM_GetCapture2(TIM8); //Obtain rising edge data //获取上升沿数据 TIM8CH2_CAPTURE_UPVAL = TIM_GetCapture2(TIM8); //Obtain rising edge data //<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM8CH2_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //标记已捕获到上升沿 TIM8CH2_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //设置为下降沿捕获 TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>½<EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
} }
} }
} }
//Channel 3 //通道三 //Channel 3 //ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if ((TIM8CH3_CAPTURE_STA & 0X80) == 0) 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 //ͨ<EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD>
{ {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC3); //Clear the interrupt flag bit //清除中断标志位 TIM_ClearITPendingBit(TIM8, TIM_IT_CC3); //Clear the interrupt flag bit //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϱ<EFBFBD>־λ
if (TIM8CH3_CAPTURE_STA & 0X40) //A falling edge is caught //捕获到一个下降沿 if (TIM8CH3_CAPTURE_STA & 0X40) //A falling edge is caught //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD><EFBFBD>
{ {
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 //<EFBFBD><EFBFBD>¼<EFBFBD>´<EFBFBD>ʱ<EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
if (TIM8CH3_CAPTURE_DOWNVAL < TIM8CH3_CAPTURE_UPVAL) if (TIM8CH3_CAPTURE_DOWNVAL < TIM8CH3_CAPTURE_UPVAL)
{ {
TIM8_T3 = 9999; TIM8_T3 = 9999;
} }
else else
TIM8_T3 = 0; TIM8_T3 = 0;
Remoter_Ch3 = TIM8CH3_CAPTURE_DOWNVAL - TIM8CH3_CAPTURE_UPVAL + TIM8_T3; //Time to get the total high level //得到总的高电平的时间 Remoter_Ch3 = TIM8CH3_CAPTURE_DOWNVAL - TIM8CH3_CAPTURE_UPVAL + TIM8_T3; //Time to get the total high level //<EFBFBD>õ<EFBFBD><EFBFBD>ܵĸߵ<EFBFBD>ƽ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
if(abs(Remoter_Ch3-L_Remoter_Ch3)>500)Remoter_Ch3=L_Remoter_Ch3; //Filter //滤波 if(abs(Remoter_Ch3-L_Remoter_Ch3)>500)Remoter_Ch3=L_Remoter_Ch3; //Filter //<EFBFBD>˲<EFBFBD>
L_Remoter_Ch3=Remoter_Ch3; L_Remoter_Ch3=Remoter_Ch3;
TIM8CH3_CAPTURE_STA = 0; //Capture flag bit to zero //捕获标志位清零 TIM8CH3_CAPTURE_STA = 0; //Capture flag bit to zero //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //设置为上升沿捕获 TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
} }
else 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 //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
//发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䵫<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD>ʱ<EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
TIM8CH3_CAPTURE_UPVAL = TIM_GetCapture3(TIM8); //Obtain rising edge data //获取上升沿数据 TIM8CH3_CAPTURE_UPVAL = TIM_GetCapture3(TIM8); //Obtain rising edge data //<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM8CH3_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //标记已捕获到上升沿 TIM8CH3_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //设置为下降沿捕获 TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>½<EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
} }
} }
} }
// //
//Channel 4 //通道四 //Channel 4 //ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if ((TIM8CH4_CAPTURE_STA & 0X80) == 0) 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 //ͨ<EFBFBD><EFBFBD>4<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD>
{ {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC4); //Clear the interrupt flag bit //清除中断标志位 TIM_ClearITPendingBit(TIM8, TIM_IT_CC4); //Clear the interrupt flag bit //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϱ<EFBFBD>־λ
if (TIM8CH4_CAPTURE_STA & 0X40) //A falling edge is caught //捕获到一个下降沿 if (TIM8CH4_CAPTURE_STA & 0X40) //A falling edge is caught //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD><EFBFBD>
{ {
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 //<EFBFBD><EFBFBD>¼<EFBFBD>´<EFBFBD>ʱ<EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
if (TIM8CH4_CAPTURE_DOWNVAL < TIM8CH4_CAPTURE_UPVAL) if (TIM8CH4_CAPTURE_DOWNVAL < TIM8CH4_CAPTURE_UPVAL)
{ {
TIM8_T4 = 9999; TIM8_T4 = 9999;
} }
else else
TIM8_T4 = 0; TIM8_T4 = 0;
Remoter_Ch4 = TIM8CH4_CAPTURE_DOWNVAL - TIM8CH4_CAPTURE_UPVAL + TIM8_T4; //Time to get the total high level //得到总的高电平的时间 Remoter_Ch4 = TIM8CH4_CAPTURE_DOWNVAL - TIM8CH4_CAPTURE_UPVAL + TIM8_T4; //Time to get the total high level //<EFBFBD>õ<EFBFBD><EFBFBD>ܵĸߵ<EFBFBD>ƽ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
if(abs(Remoter_Ch4-L_Remoter_Ch4)>500)Remoter_Ch4=L_Remoter_Ch4; //Filter //滤波 if(abs(Remoter_Ch4-L_Remoter_Ch4)>500)Remoter_Ch4=L_Remoter_Ch4; //Filter //<EFBFBD>˲<EFBFBD>
L_Remoter_Ch4=Remoter_Ch4; L_Remoter_Ch4=Remoter_Ch4;
TIM8CH4_CAPTURE_STA = 0; //Capture flag bit to zero //捕获标志位清零 TIM8CH4_CAPTURE_STA = 0; //Capture flag bit to zero //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //设置为上升沿捕获 TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
} }
else 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 //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
//发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䵫<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD>ʱ<EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
TIM8CH4_CAPTURE_UPVAL = TIM_GetCapture4(TIM8); //Obtain rising edge data //获取上升沿数据 TIM8CH4_CAPTURE_UPVAL = TIM_GetCapture4(TIM8); //Obtain rising edge data //<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM8CH4_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //标记已捕获到上升沿 TIM8CH4_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //设置为下降沿捕获 TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>½<EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
} }
} }
} }
@@ -312,25 +312,25 @@ void TIM8_CC_IRQHandler(void)
Function: TIM1 Update Interrupt Function: TIM1 Update Interrupt
Input : none Input : none
Output : none Output : none
函数功能：定时器8更新中断 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
入口参数：无 <EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
返回 值：无 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/ **************************************************************************/
void TIM8_UP_TIM13_IRQHandler(void) void TIM8_UP_TIM13_IRQHandler(void)
{ {
//Clear the interrupt flag bit //Clear the interrupt flag bit
//清除中断标志位 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϱ<EFBFBD>־λ
TIM8->SR&=~(1<<0); TIM8->SR&=~(1<<0);
} }
void TIM8_SERVO_Init(u16 arr,u16 psc) void TIM8_SERVO_Init(u16 arr,u16 psc)
{ {
GPIO_InitTypeDef GPIO_InitStructure; //IO GPIO_InitTypeDef GPIO_InitStructure; //IO
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //定时器 TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
TIM_OCInitTypeDef TIM_OCInitStructure; //PWM输出 TIM_OCInitTypeDef TIM_OCInitStructure; //PWM<EFBFBD><EFBFBD><EFBFBD>
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE); //TIM1时钟使能 RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE); //TIM1ʱ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); //使能PORTE时钟 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); //ʹ<EFBFBD><EFBFBD>PORTEʱ<EFBFBD><EFBFBD>
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF; GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9; 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); GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_TIM8);
/*** Initialize timer 1 || 初始化定时器1 ***/ /*** Initialize timer 1 || <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>1 ***/
//Set the counter to automatically reload //设定计数器自动重装值 //Set the counter to automatically reload //<EFBFBD>趨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><EFBFBD><EFBFBD>װֵ
TIM_TimeBaseStructure.TIM_Period = arr; TIM_TimeBaseStructure.TIM_Period = arr;
//Pre-divider //预分频器 //Pre-divider //Ԥ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>
TIM_TimeBaseStructure.TIM_Prescaler = psc; TIM_TimeBaseStructure.TIM_Prescaler = psc;
//Set the clock split: TDTS = Tck_tim //设置时钟分割:TDTS = Tck_tim //Set the clock split: TDTS = Tck_tim //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ӷָ<EFBFBD>:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
//TIM up count mode //TIM向上计数模式 //TIM up count mode //TIM<EFBFBD><EFBFBD><EFBFBD>ϼ<EFBFBD><EFBFBD><EFBFBD>ģʽ
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct //Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
//根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>TIM_TimeBaseInitStruct<EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>TIMx<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
//-----------舵机初始化-----------// //-----------<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>-----------//
//Select Timer mode :TIM Pulse Width Modulation mode 1 //Select Timer mode :TIM Pulse Width Modulation mode 1
//选择定时器模式:TIM脉冲宽度调制模式1 //ѡ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>ģʽ:TIM<49><4D><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD>ģʽ1
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
//Compare output enablement //Compare output enablement
//比较输出使能 //<EFBFBD>Ƚ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
//Set the pulse value of the capture comparison register to be loaded //Set the pulse value of the capture comparison register to be loaded
//设置待装入捕获比较寄存器的脉冲值 //<EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD>װ<EFBFBD>벶<EFBFBD><EFBFBD>ȽϼĴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_Pulse = 0;
//Output polarity :TIM output polarity is higher //Output polarity :TIM output polarity is higher
//输出极性:TIM输出比较极性高 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:TIM<49><4D><EFBFBD><EFBFBD>Ƚϼ<C8BD><CFBC>Ը<EFBFBD>
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
//Initialize the peripheral TIMX based on the parameter specified in TIM_OCINITSTRUCT //Initialize the peripheral TIMX based on the parameter specified in TIM_OCINITSTRUCT
//根据TIM_OCInitStruct中指定的参数初始化外设TIMx //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>TIM_OCInitStruct<EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>TIMx
TIM_OC1Init(TIM8, &TIM_OCInitStructure); TIM_OC1Init(TIM8, &TIM_OCInitStructure);
TIM_OC2Init(TIM8, &TIM_OCInitStructure); TIM_OC2Init(TIM8, &TIM_OCInitStructure);
TIM_OC3Init(TIM8, &TIM_OCInitStructure); TIM_OC3Init(TIM8, &TIM_OCInitStructure);
TIM_OC4Init(TIM8, &TIM_OCInitStructure); TIM_OC4Init(TIM8, &TIM_OCInitStructure);
//Channel preload enable //Channel preload enable
//通道预装载使能 //ͨ<EFBFBD><EFBFBD>Ԥװ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable); TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable); TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable); TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM8, TIM_OCPreload_Enable); TIM_OC4PreloadConfig(TIM8, TIM_OCPreload_Enable);
//-----------舵机初始化-----------// //-----------<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>-----------//
TIM_CtrlPWMOutputs(TIM8,ENABLE); TIM_CtrlPWMOutputs(TIM8,ENABLE);
//Enable timer //使能定时器 //Enable timer //ʹ<EFBFBD>ܶ<EFBFBD>ʱ<EFBFBD><EFBFBD>
TIM_Cmd(TIM8, ENABLE); TIM_Cmd(TIM8, ENABLE);
//The channel value is initialized to 1500, corresponding to the steering gear zero //The channel value is initialized to 1500, corresponding to the steering gear zero
//通道值初始化为1500，舵机零点对应值 //ͨ<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>Ϊ1500<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧֵ
// TIM8->CCR1=1500; // TIM8->CCR1=1500;
// TIM8->CCR2=1500; // TIM8->CCR2=1500;
// TIM8->CCR3=1500; // TIM8->CCR3=1500;
@@ -404,11 +404,11 @@ void TIM8_SERVO_Init(u16 arr,u16 psc)
void TIM12_SERVO_Init(u16 arr,u16 psc) void TIM12_SERVO_Init(u16 arr,u16 psc)
{ {
GPIO_InitTypeDef GPIO_InitStructure; //IO GPIO_InitTypeDef GPIO_InitStructure; //IO
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //定时器 TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
TIM_OCInitTypeDef TIM_OCInitStructure; //PWM输出 TIM_OCInitTypeDef TIM_OCInitStructure; //PWM<EFBFBD><EFBFBD><EFBFBD>
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE); //TIM1时钟使能 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE); //TIM1ʱ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); //使能PORTE时钟 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); //ʹ<EFBFBD><EFBFBD>PORTEʱ<EFBFBD><EFBFBD>
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF; GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14|GPIO_Pin_15; 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); GPIO_PinAFConfig(GPIOB,GPIO_PinSource15,GPIO_AF_TIM12);
/*** Initialize timer 1 || 初始化定时器1 ***/ /*** Initialize timer 1 || <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>1 ***/
//Set the counter to automatically reload //设定计数器自动重装值 //Set the counter to automatically reload //<EFBFBD>趨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><EFBFBD><EFBFBD>װֵ
TIM_TimeBaseStructure.TIM_Period = arr; TIM_TimeBaseStructure.TIM_Period = arr;
//Pre-divider //预分频器 //Pre-divider //Ԥ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>
TIM_TimeBaseStructure.TIM_Prescaler = psc; TIM_TimeBaseStructure.TIM_Prescaler = psc;
//Set the clock split: TDTS = Tck_tim //设置时钟分割:TDTS = Tck_tim //Set the clock split: TDTS = Tck_tim //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ӷָ<EFBFBD>:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
//TIM up count mode //TIM向上计数模式 //TIM up count mode //TIM<EFBFBD><EFBFBD><EFBFBD>ϼ<EFBFBD><EFBFBD><EFBFBD>ģʽ
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct //Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
//根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>TIM_TimeBaseInitStruct<EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>TIMx<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
TIM_TimeBaseInit(TIM12, &TIM_TimeBaseStructure); TIM_TimeBaseInit(TIM12, &TIM_TimeBaseStructure);
//-----------舵机初始化-----------// //-----------<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>-----------//
//Select Timer mode :TIM Pulse Width Modulation mode 1 //Select Timer mode :TIM Pulse Width Modulation mode 1
//选择定时器模式:TIM脉冲宽度调制模式1 //ѡ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>ģʽ:TIM<49><4D><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD>ģʽ1
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
//Compare output enablement //Compare output enablement
//比较输出使能 //<EFBFBD>Ƚ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
//Set the pulse value of the capture comparison register to be loaded //Set the pulse value of the capture comparison register to be loaded
//设置待装入捕获比较寄存器的脉冲值 //<EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD>װ<EFBFBD>벶<EFBFBD><EFBFBD>ȽϼĴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_Pulse = 0;
//Output polarity :TIM output polarity is higher //Output polarity :TIM output polarity is higher
//输出极性:TIM输出比较极性高 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:TIM<49><4D><EFBFBD><EFBFBD>Ƚϼ<C8BD><CFBC>Ը<EFBFBD>
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
//Initialize the peripheral TIMX based on the parameter specified in TIM_OCINITSTRUCT //Initialize the peripheral TIMX based on the parameter specified in TIM_OCINITSTRUCT
//根据TIM_OCInitStruct中指定的参数初始化外设TIMx //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>TIM_OCInitStruct<EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>TIMx
TIM_OC1Init(TIM12, &TIM_OCInitStructure); TIM_OC1Init(TIM12, &TIM_OCInitStructure);
TIM_OC2Init(TIM12, &TIM_OCInitStructure); TIM_OC2Init(TIM12, &TIM_OCInitStructure);
//Channel preload enable //Channel preload enable
//通道预装载使能 //ͨ<EFBFBD><EFBFBD>Ԥװ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
TIM_OC1PreloadConfig(TIM12, TIM_OCPreload_Enable); TIM_OC1PreloadConfig(TIM12, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM12, TIM_OCPreload_Enable); TIM_OC2PreloadConfig(TIM12, TIM_OCPreload_Enable);
//-----------舵机初始化-----------// //-----------<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>-----------//
TIM_CtrlPWMOutputs(TIM12,ENABLE); TIM_CtrlPWMOutputs(TIM12,ENABLE);
//Enable timer //使能定时器 //Enable timer //ʹ<EFBFBD>ܶ<EFBFBD>ʱ<EFBFBD><EFBFBD>
TIM_Cmd(TIM12, ENABLE); TIM_Cmd(TIM12, ENABLE);
//Initialize both servo channels to the configured steering center. //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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>΢<EFBFBD>뼶<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>׼
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 <20><> 1MHz <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 65.536ms <20><><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>¸<EFBFBD><C2B8>ж<EFBFBD><D0B6><EFBFBD>λ<EFBFBD><CEBB> 32λ<32><CEBB><EFBFBD>ָߣ<D6B8>
<EFBFBD><EFBFBD>չ<EFBFBD><EFBFBD>ֻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䡣TIM2~TIM5 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD>TIM8 <20><><EFBFBD>벶<EFBFBD><EBB2B6><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD>
<EFBFBD><EFBFBD> TIM7 <20><><EFBFBD>С<EFBFBD>
**************************************************************************/
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 <20><>ʱ<EFBFBD><CAB1>ʱ<EFBFBD><CAB1> 84MHz<48><7A>Ԥ<EFBFBD><D4A4>Ƶ 84 -> 1MHz<48><7A>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD> 1us<75><73>
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.
//<2F>жϲ<D0B6><CFB2><EFBFBD><EFBFBD><EFBFBD> FreeRTOS API<50><49><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC>ϵͣ<CFB5><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȿ<EFBFBD><C8BF>ơ<EFBFBD>
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
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ȡ 64λ<34><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>΢<EFBFBD><CEA2>ʱ<EFBFBD><CAB1><EFBFBD>
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.
<EFBFBD><EFBFBD>ֹ<EFBFBD><EFBFBD>϶<EFBFBD><EFBFBD><EFBFBD>ѣ<EFBFBD><EFBFBD><EFBFBD> CNT <20><>Χ<EFBFBD><CEA7><EFBFBD>ζ<EFBFBD><CEB6>߰<EFBFBD><DFB0>֣<EFBFBD><D6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ط<EFBFBD><D8B7><EFBFBD><EFBFBD><EFBFBD> CNT <20><>ϵ<EFBFBD>ͣ<EFBFBD>
˵<EFBFBD><EFBFBD> ISR <20><>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD>ûأ<C3BB><D8A3>ֶ<EFBFBD><D6B6><EFBFBD>λ<EFBFBD><CEBB>
**************************************************************************/
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.
//<2F>ѻ<EFBFBD><D1BB>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD>¸<EFBFBD><C2B8>ж<EFBFBD><D0B6><EFBFBD>δִ<CEB4>С<EFBFBD>
if((sr & TIM_SR_UIF) && cnt < 0x8000U)
high1++;
return ((uint64_t)high1 << 16) | cnt;
}

2
HARDWARE/timer.h
View File

@@ -4,6 +4,8 @@
void TIM8_Cap_Init(u16 arr, u16 psc); void TIM8_Cap_Init(u16 arr, u16 psc);
void TIM12_SERVO_Init(u16 arr,u16 psc); void TIM12_SERVO_Init(u16 arr,u16 psc);
void TIM8_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 L_Remoter_Ch1,L_Remoter_Ch2,L_Remoter_Ch3,L_Remoter_Ch4;
extern int Remoter_Ch1,Remoter_Ch2,Remoter_Ch3,Remoter_Ch4; extern int Remoter_Ch1,Remoter_Ch2,Remoter_Ch3,Remoter_Ch4;

696
HARDWARE/usartx.c
View File

File diff suppressed because it is too large Load Diff

54
HARDWARE/usartx.h
View File

@@ -10,33 +10,41 @@
#define FRAME_HEADER 0X7B //Frame_header //֡ͷ #define FRAME_HEADER 0X7B //Frame_header //֡ͷ
#define FRAME_TAIL 0X7D //Frame_tail //֡β #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.
//֡<><D6A1> 24 -> 44<34><34><EFBFBD><EFBFBD>У<EFBFBD><D0A3>/֡βǰ׷<C7B0><D7B7><EFBFBD><EFBFBD> session_id(4) <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 64λ<34><CEBB><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>(8+8)<29><>
//<2F>ֽ<EFBFBD> 0..21 <20><><EFBFBD>ֽ<EFBFBD>ϲ<EFBFBD><CFB2>֣<EFBFBD><D6A3>¾ֽ<C2BE><D6BD><EFBFBD>β<EFBFBD><CEB2><EFBFBD><EFBFBD>
#define SEND_DATA_SIZE 44
#define RECEIVE_DATA_SIZE 11 #define RECEIVE_DATA_SIZE 11
/*****A structure for storing triaxial data of a gyroscope accelerometer*****/ /*****A structure for storing triaxial data of a gyroscope accelerometer*****/
/*****用于存放陀螺仪加速度计三轴数据的结构体*********************************/ /*****<EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><EFBFBD>ٶȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵĽṹ<EFBFBD><EFBFBD>*********************************/
typedef struct __Mpu6050_Data_ typedef struct __Mpu6050_Data_
{ {
short X_data; //2 bytes //2个字节 short X_data; //2 bytes //2<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
short Y_data; //2 bytes //2个字节 short Y_data; //2 bytes //2<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
short Z_data; //2 bytes //2个字节 short Z_data; //2 bytes //2<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
}Mpu6050_Data; }Mpu6050_Data;
/*******The structure of the serial port sending data************/ /*******The structure of the serial port sending data************/
/*******串口发送数据的结构体*************************************/ /*******<EFBFBD><EFBFBD><EFBFBD>ڷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵĽṹ<EFBFBD><EFBFBD>*************************************/
typedef struct _SEND_DATA_ typedef struct _SEND_DATA_
{ {
unsigned char buffer[SEND_DATA_SIZE]; unsigned char buffer[SEND_DATA_SIZE];
struct _Sensor_Str_ struct _Sensor_Str_
{ {
unsigned char Frame_Header; //1个字节 unsigned char Frame_Header; //1<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
short X_speed; //2 bytes //2个字节 short X_speed; //2 bytes //2<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
short Y_speed; //2 bytes //2个字节 short Y_speed; //2 bytes //2<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
short Z_speed; //2 bytes //2个字节 short Z_speed; //2 bytes //2<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
short Power_Voltage; //2 bytes //2个字节 short Power_Voltage; //2 bytes //2<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
Mpu6050_Data Accelerometer; //6 bytes //6个字节 Mpu6050_Data Accelerometer; //6 bytes //6<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
Mpu6050_Data Gyroscope; //6 bytes //6个字节 Mpu6050_Data Gyroscope; //6 bytes //6<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
unsigned char Frame_Tail; //1 bytes //1个字节 unsigned int Session_Id; //4 bytes: MCU boot session ID //<2F><>MCU<43><55><EFBFBD><EFBFBD><EFBFBD>ỰID
unsigned long long Speed_Time_us; //8 bytes: encoder sample time //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
unsigned long long Imu_Time_us; //8 bytes: IMU sample time //IMU<4D><55><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
unsigned char Frame_Tail; //1 bytes //1<><31><EFBFBD>ֽ<EFBFBD>
}Sensor_Str; }Sensor_Str;
}SEND_DATA; }SEND_DATA;
@@ -45,14 +53,21 @@ typedef struct _RECEIVE_DATA_
unsigned char buffer[RECEIVE_DATA_SIZE]; unsigned char buffer[RECEIVE_DATA_SIZE];
struct _Control_Str_ struct _Control_Str_
{ {
unsigned char Frame_Header; //1 bytes //1个字节 unsigned char Frame_Header; //1 bytes //1<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
float X_speed; //4 bytes //4个字节 float X_speed; //4 bytes //4<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
float Y_speed; //4 bytes //4个字节 float Y_speed; //4 bytes //4<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
float Z_speed; //4 bytes //4个字节 float Z_speed; //4 bytes //4<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
unsigned char Frame_Tail; //1 bytes //1个字节 unsigned char Frame_Tail; //1 bytes //1<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>
}Control_Str; }Control_Str;
}RECEIVE_DATA; }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.
//<2F>ɸ<EFBFBD><C9B8>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵͳ<CFB5><CDB3><EFBFBD>лỰID<49><44>
//<2F><> data_transition() <20><><EFBFBD>֡ʱ<D6A1><CAB1>ȡ<EFBFBD><C8A1>
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_task(void *pvParameters);
void data_transition(void); void data_transition(void);
@@ -64,6 +79,7 @@ void CAN_SEND(void);
void uart1_init(u32 bound); void uart1_init(u32 bound);
void uart2_init(u32 bound); void uart2_init(u32 bound);
void uart3_init(u32 bound); void uart3_init(u32 bound);
void uart3_dma_tx_init(void);
void uart5_init(u32 bound); void uart5_init(u32 bound);
int USART1_IRQHandler(void); int USART1_IRQHandler(void);

3
Makefile
View File

@@ -31,7 +31,8 @@ FWLIB_SRC := \
FWLIB/src/misc.c FWLIB/src/stm32f4xx_gpio.c FWLIB/src/stm32f4xx_rcc.c \ 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_syscfg.c FWLIB/src/stm32f4xx_usart.c \
FWLIB/src/stm32f4xx_adc.c FWLIB/src/stm32f4xx_can.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_SRC := \
FreeRTOS/croutine.c FreeRTOS/event_groups.c FreeRTOS/list.c \ FreeRTOS/croutine.c FreeRTOS/event_groups.c FreeRTOS/list.c \

374
doc/ackermann_steering_calibration.md Normal file
View File

@@ -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|)`。

111
doc/uart_protocol_v2_timestamp.md Normal file
View File

@@ -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 端）解析代码必须同步更新：
帧长、校验范围、以及尾部三个新字段的读取。否则会因帧长不符导致解析错位。

349
doc/时间戳.md Normal file
View File

@@ -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}
]