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

add uart rx; 4 bytes cmd parse

Browse Source
This commit is contained in:
4x-tech
2025-11-17 10:13:18 +08:00
parent 5b49a0ee24
commit 6de9d53e1c
7 changed files with 237 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
3rd/mt6701.c
View File

@@ -10,6 +10,8 @@ volatile float angle_f_rad;
volatile bool gIsI2cError = false;
#define DEBUG_I2C false
/* Data sent to the Target */
uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
{
@@ -52,7 +54,7 @@ void MT6701_iic_read_angel(void)
*/
gDelayCycles = (5 * (gI2CclockConfig.divideRatio + 1)) *
(CPUCLK_FREQ / gClockSelFreq);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c before writing -------\n");
@@ -75,7 +77,7 @@ void MT6701_iic_read_angel(void)
DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_TX, I2C_TX_PACKET_SIZE);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c writing done -------\n");
}
@@ -94,7 +96,7 @@ void MT6701_iic_read_angel(void)
{
gIsI2cError = true;
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c error ------------------------------------------\n");
}
@@ -111,7 +113,7 @@ void MT6701_iic_read_angel(void)
/* Add delay between transfers */
delay_cycles(1000);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c before reading -------\n");
}
@@ -131,7 +133,7 @@ void MT6701_iic_read_angel(void)
gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
}
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c reading done -------\n");
}
@@ -140,29 +142,36 @@ void MT6701_iic_read_angel(void)
}
volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;
// 单圈值
float GetAngle_NoTrack(void)
{
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384;
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
{
printf("angle_rad read back is %f \n", angle_f_rad);
}
return angle_f_rad;
}
volatile float full_rotations = 0.0;
volatile float Last_Angle = 0.0;
volatile float Last_Angle_rad = 0.0;
//多圈值
float GetAngle(void)
{
volatile float D_Angle = 0.0;
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
volatile float D_Angle_rad = 0.0;
volatile float Angle_rad = GetAngle_NoTrack();
D_Angle_rad = Angle_rad - Last_Angle_rad;
if(fabs(D_Angle) > (0.8f * 2 * PI))
if(fabs(D_Angle_rad) > (0.8f * 2 * PI))
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 : 1);
full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1);
}
Last_Angle = Angle;
Last_Angle_rad = Angle_rad;
return (full_rotations * 2 * PI + Last_Angle);
return (full_rotations * 2 * PI + Last_Angle_rad);
}
volatile float Last_Vel_ts = 0.0;
@@ -173,11 +182,11 @@ float GetVelocity(void)
volatile float Vel_ts = SysTick -> VAL;
if(Vel_ts < Last_Vel_ts)
{
dt = (Last_Vel_ts - Vel_ts) / 9 * 1e-6f;
dt = (Last_Vel_ts - Vel_ts) / 80 * 1e-6f;
}
else
{
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 9 * 1e-6f;
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f;
}
if(dt < 0.0001)