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

add softi2c , test failed

Browse Source
This commit is contained in:
4x-tech
2025-11-17 21:31:48 +08:00
parent 8acb95d566
commit 73733da6dd
6 changed files with 366 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
3rd/config.h
View File

@@ -2,6 +2,6 @@
#define CONFIG_H
#define DEBUG_ENABLED true
#define DEBUG_MT_ENABLED false
#define DEBUG_MT_ENABLED true
#define DEBUG_DFOC_ENABLED false
#endif

314
3rd/mt6701.c
View File

@@ -1,8 +1,9 @@
#include "mt6701.h"
#include "config.h"
#include "soft_i2c.h"
#include "stdio.h"
#include "ti_msp_dl_config.h"
#include "uart_redircet.h"
#include "stdio.h"
#include "config.h"
volatile int16_t angle;
volatile float angle_f;
@@ -10,13 +11,10 @@ volatile float angle_f_rad;
volatile bool gIsI2cError = false;
#define DEBUG_I2C false
#define DEBUG_I2C false
/* Data sent to the Target */
uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
{
0x03
};
uint8_t gTxPacket[I2C_TX_PACKET_SIZE] = {0x03};
/* Data received from Target */
volatile uint8_t gRxPacket[I2C_RX_PACKET_SIZE] = {0};
@@ -32,184 +30,186 @@ volatile uint32_t gDelayCycles;
/* I2C Target address */
#define I2C_TARGET_ADDRESS (0x06)
void MT6701_iic_read_angel(void)
{
/* Get I2C clock source and clock divider to use for delay cycle calculation */
DL_I2C_getClockConfig(I2C_1_INST, &gI2CclockConfig);
switch(gI2CclockConfig.clockSel)
{
case DL_I2C_CLOCK_BUSCLK:
gClockSelFreq = 32000000;
break;
case DL_I2C_CLOCK_MFCLK:
gClockSelFreq = 4000000;
break;
default:
break;
// void MT6701_iic_read_angel(void) {
// I2C_Start();
// I2C_SendByte(I2C_TARGET_ADDRESS);
// I2C_RecviveAck();
// I2C_SendByte(0X03);
// I2C_RecviveAck();
// I2C_Start();
// I2C_SendByte(I2C_TARGET_ADDRESS | 0x01);
// I2C_RecviveAck();
// gRxPacket[0] = I2C_RecviveData();
// I2C_RecviveAck();
// I2C_Start();
// I2C_SendByte(I2C_TARGET_ADDRESS | 0x01);
// I2C_RecviveAck();
// gRxPacket[1] = I2C_RecviveData();
// I2C_SendAck(1);
// I2C_Stop();
// }
void MT6701_iic_read_angel(void) {
/* Get I2C clock source and clock divider to use for delay cycle
calculation */
DL_I2C_getClockConfig(I2C_1_INST, &gI2CclockConfig);
switch (gI2CclockConfig.clockSel) {
case DL_I2C_CLOCK_BUSCLK:
gClockSelFreq = 32000000;
break;
case DL_I2C_CLOCK_MFCLK:
gClockSelFreq = 4000000;
break;
default:
break;
}
/*
* Calculate number of clock cycles to delay after controller transfer
initiated
* gDelayCycles = 3 I2C functional clock cycles
* gDelayCycles = 3 * I2C clock divider * (CPU clock freq / I2C clock
freq)
*/
gDelayCycles =
(5 * (gI2CclockConfig.divideRatio + 1)) * (CPUCLK_FREQ / gClockSelFreq);
if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
printf("i2c before writing -------\n");
}
/*
* Fill FIFO with data. This example will send a MAX of 8 bytes since it
* doesn't handle the case where FIFO is full
*/
DL_I2C_fillControllerTXFIFO(I2C_1_INST, &gTxPacket[0], I2C_TX_PACKET_SIZE);
/* Wait for I2C to be Idle */
while (
!(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
;
/* Send the packet to the controller.
* This function will send Start + Stop automatically.
*/
DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_TX,
I2C_TX_PACKET_SIZE);
if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
printf("i2c writing done -------\n");
}
/* Workaround for errata I2C_ERR_13 */
delay_cycles(gDelayCycles);
/* Poll until the Controller writes all bytes */
while (DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_BUSY)
;
/* Trap if there was an error */
if (DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_ERROR) {
gIsI2cError = true;
if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
printf("i2c error ------------------------------------------\n");
}
/*
* Calculate number of clock cycles to delay after controller transfer initiated
* gDelayCycles = 3 I2C functional clock cycles
* gDelayCycles = 3 * I2C clock divider * (CPU clock freq / I2C clock freq)
*/
gDelayCycles = (5 * (gI2CclockConfig.divideRatio + 1)) *
(CPUCLK_FREQ / gClockSelFreq);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c before writing -------\n");
/* LED will remain high if there is an error */
__BKPT(0);
return;
}
}
/* Wait for I2C to be Idle */
while (
!(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
;
/*
* Fill FIFO with data. This example will send a MAX of 8 bytes since it
* doesn't handle the case where FIFO is full
*/
DL_I2C_fillControllerTXFIFO(I2C_1_INST, &gTxPacket[0], I2C_TX_PACKET_SIZE);
/* Add delay between transfers */
delay_cycles(1000);
/* Wait for I2C to be Idle */
while(!(
DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
;
if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
printf("i2c before reading -------\n");
}
/* Send the packet to the controller.
* This function will send Start + Stop automatically.
*/
DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_TX, I2C_TX_PACKET_SIZE);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c writing done -------\n");
}
/* Workaround for errata I2C_ERR_13 */
delay_cycles(gDelayCycles);
/* Poll until the Controller writes all bytes */
while(
DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_BUSY)
;
/* Trap if there was an error */
if(DL_I2C_getControllerStatus(I2C_1_INST) &
DL_I2C_CONTROLLER_STATUS_ERROR)
{
gIsI2cError = true;
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c error ------------------------------------------\n");
}
/* LED will remain high if there is an error */
__BKPT(0);
return;
}
/* Wait for I2C to be Idle */
while(!(
DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
;
/* Add delay between transfers */
delay_cycles(1000);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c before reading -------\n");
}
/* Send a read request to Target */
DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_RX, I2C_RX_PACKET_SIZE);
/*
* Receive all bytes from target. LED will remain high if not all bytes
* are received
*/
for(uint8_t i = 0; i < I2C_RX_PACKET_SIZE; i++)
{
while(DL_I2C_isControllerRXFIFOEmpty(I2C_1_INST))
;
gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
}
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c reading done -------\n");
}
/* Send a read request to Target */
DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_RX,
I2C_RX_PACKET_SIZE);
/*
* Receive all bytes from target. LED will remain high if not all bytes
* are received
*/
for (uint8_t i = 0; i < I2C_RX_PACKET_SIZE; i++) {
while (DL_I2C_isControllerRXFIFOEmpty(I2C_1_INST))
;
gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
}
if (DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C) {
printf("i2c reading done -------\n");
}
}
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;
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_rad = 0.0;
//多圈值
float GetAngle(void)
{
volatile float D_Angle_rad = 0.0;
volatile float Angle_rad = GetAngle_NoTrack();
D_Angle_rad = Angle_rad - Last_Angle_rad;
// 多圈值
float GetAngle(void) {
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_rad) > (0.8f * 2 * PI))
{
full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1);
}
if (fabs(D_Angle_rad) > (0.8f * 2 * PI)) {
full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1);
}
Last_Angle_rad = Angle_rad;
Last_Angle_rad = Angle_rad;
return (full_rotations * 2 * PI + Last_Angle_rad);
return (full_rotations * 2 * PI + Last_Angle_rad);
}
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) / 80 * 1e-6f;
}
else
{
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f;
}
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) / 80 * 1e-6f;
} else {
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f;
}
if(dt < 0.0001)
{
dt = 10000;
}
if (dt < 0.0001) {
dt = 10000;
}
float Vel_Angle = GetAngle();
float Vel_Angle = GetAngle();
float dv = Vel_Angle - Vel_Last_Angle;
float dv = Vel_Angle - Vel_Last_Angle;
float velocity = (Vel_Angle - Vel_Last_Angle) / dt;
float velocity = (Vel_Angle - Vel_Last_Angle) / dt;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;
return velocity;
return velocity;
}
void MT6701_get_angle_degree(void)
{
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f = (float)angle * 360 / 16384;
void MT6701_get_angle_degree(void) {
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f = (float)angle * 360 / 16384;
}

72
3rd/soft_i2c.c Normal file
View File

@@ -0,0 +1,72 @@
#include "delay.h"
#include "ti_msp_dl_config.h"
void I2C_W_SCL(uint8_t BitValue) {
DL_GPIO_writePinsVal(SOFT_I2C_PORT, SOFT_I2C_CLK_PIN, BitValue);
}
void I2C_W_SDA(uint8_t BitValue) {
DL_GPIO_writePinsVal(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN, BitValue);
}
// 读取时钟线数据
uint8_t I2C_R_SDA(void) {
uint8_t BitValue;
BitValue = DL_GPIO_readPins(SOFT_I2C_PORT, SOFT_I2C_SDA_PIN);
return BitValue;
}
void I2C_Start(void) {
I2C_W_SCL(1);
I2C_W_SDA(1);
I2C_W_SDA(0);
I2C_W_SCL(0);
}
void I2C_Stop(void) {
I2C_W_SDA(0);
I2C_W_SCL(1);
I2C_W_SDA(1);
}
void I2C_SendByte(uint8_t Byte) {
uint8_t i;
for (i = 0; i < 8; i++) {
I2C_W_SDA(Byte & (0x80 >> i));
I2C_W_SCL(1);
I2C_W_SCL(0);
}
}
uint8_t I2C_RecviveData(void) {
uint8_t i, Byte = 0x00;
I2C_W_SDA(1);
for (i = 0; i < 8; i++) {
I2C_W_SCL(1);
if (I2C_R_SDA() == 1) {
Byte |= (0x80 >> i);
}
I2C_W_SCL(0);
}
return Byte;
}
void I2C_SendAck(uint8_t AckBit) {
I2C_W_SDA(AckBit);
I2C_W_SCL(1);
I2C_W_SCL(0);
}
uint8_t I2C_RecviveAck(void) {
uint8_t AckBit;
I2C_W_SDA(1);
I2C_W_SCL(1);
AckBit = I2C_R_SDA();
I2C_W_SCL(0);
return AckBit;
}

15
3rd/soft_i2c.h Normal file
View File

@@ -0,0 +1,15 @@
#ifndef __SOFT_I2C_H
#define __SOFT_I2C_H
#include <stdint.h>
void I2C_W_SCL(uint8_t BitValue);
void I2C_W_SDA(uint8_t BitValue);
uint8_t I2C_R_SDA(void);
void I2C_Start(void);
void I2C_Stop(void);
void I2C_SendByte(uint8_t Byte);
uint8_t I2C_RecviveData(void);
void I2C_SendAck(uint8_t AckBit);
uint8_t I2C_RecviveAck(void);
#endif /* ti_msp_dl_config_h */