4x-tech/foc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add systick ； issue -- i2c transfer unstable

Browse Source
This commit is contained in:
4x-tech
2025-11-12 21:26:40 +08:00
parent 4dc04bdd7e
commit 79d7fca725
11 changed files with 241 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
3rd/mt6701.c
View File

@@ -78,9 +78,10 @@ void MT6701_iic_read_angel(void)
if(DL_I2C_getControllerStatus(I2C_1_INST) &
DL_I2C_CONTROLLER_STATUS_ERROR)
{
//printf("i2c error \n");
printf("i2c error --------------------------------------------------------------------------------\n");
/* LED will remain high if there is an error */
__BKPT(0);
return;
}
/* Wait for I2C to be Idle */
@@ -110,51 +111,60 @@ volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;
float GetAngle_NoTrack(void)
{
MT6701_iic_read_angel();
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384;
return angle_f_rad;
angle_f_rad = (float)angle * _2PI / 16384;
return angle_f_rad;
}
volatile float full_rotations = 0.0;
volatile float Last_Angle = 0.0;
float GetAngle(void)
{
volatile float D_Angle = 0.0;
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
if( fabs(D_Angle) > (0.8f*2*PI) )
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 :1);
}
Last_Angle = Angle;
return (full_rotations * 2 * PI + Last_Angle);
volatile float D_Angle = 0.0;
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
if(fabs(D_Angle) > (0.8f * 2 * PI))
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 : 1);
}
Last_Angle = Angle;
return (full_rotations * 2 * PI + Last_Angle);
}
volatile float Last_Vel_ts = 0.0;
volatile float Vel_Last_Angle = 0.0;
float GetVelocity(void)
{
volatile float dt = 0.0;
volatile float Vel_ts = SysTick -> VAL;
if(Vel_ts < Last_Vel_ts) dt = (Last_Vel_ts - Vel_ts)/9*1e-6f;
else dt = (0xFFFFFF - Vel_ts + Last_Vel_ts)/9*1e-6f;
if(dt < 0.0001) dt = 10000;
float Vel_Angle = GetAngle();
float dv = Vel_Angle - Vel_Last_Angle;
volatile float dt = 0.0;
volatile float Vel_ts = SysTick -> VAL;
if(Vel_ts < Last_Vel_ts)
{
dt = (Last_Vel_ts - Vel_ts) / 9 * 1e-6f;
}
else
{
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 9 * 1e-6f;
}
float velocity = (Vel_Angle - Vel_Last_Angle)/dt;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;
return velocity;
if(dt < 0.0001)
{
dt = 10000;
}
float Vel_Angle = GetAngle();
float dv = Vel_Angle - Vel_Last_Angle;
float velocity = (Vel_Angle - Vel_Last_Angle) / dt;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;
return velocity;
}
void MT6701_get_angle_degree(void)