add softi2c , test failed
This commit is contained in:
314
3rd/mt6701.c
314
3rd/mt6701.c
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user