Radzhab Bisultanov
d59cf7cd55
Очередная попытка реализовать чтение IMU как в рабочей прошивке оказалась провальной. Поэтому было принято решение перенести проект на C++ и писать его подобно рабочей прошивке. Реализован драйвер для I2C. Добавлены файлы интерфейса IMU и конкретного ICM20948.
378 lines
8.8 KiB
C++
378 lines
8.8 KiB
C++
#include "I2C.h"
|
|
#include "stm32g4xx.h"
|
|
#include "GPIO.h"
|
|
#include <string.h>
|
|
|
|
struct I2C_Data
|
|
{
|
|
I2C_TypeDef* I2C;
|
|
IRQn_Type IRQn;
|
|
|
|
I2C_Request *Head, *Device;
|
|
|
|
unsigned char Index;
|
|
};
|
|
|
|
static I2C_Data I2C1_Data = { I2C1, I2C1_EV_IRQn, 0, 0, 0 };
|
|
|
|
|
|
static void EV_IRQHandler(I2C_Data& I2C)
|
|
{
|
|
if ((I2C.I2C->CR1 & I2C_CR1_TXIE) && (I2C.I2C->ISR & I2C_ISR_TXE))
|
|
{
|
|
const I2C_Request& device = *I2C.Device;
|
|
|
|
I2C.I2C->TXDR = device.Buffer[I2C.Index++];
|
|
if (I2C.Index < device.Write) return;
|
|
I2C.I2C->CR1 = I2C_CR1_TCIE | I2C_CR1_PE;
|
|
I2C.Index = 0;
|
|
return;
|
|
}
|
|
|
|
if ((I2C.I2C->CR1 & I2C_CR1_TCIE) && (I2C.I2C->ISR & I2C_ISR_TC))
|
|
{
|
|
const I2C_Request& device = *I2C.Device;
|
|
|
|
if (device.Read)
|
|
{
|
|
I2C.I2C->CR2 &= ~I2C_CR2_NBYTES_Msk;
|
|
I2C.I2C->CR2 |= I2C_CR2_START | I2C_CR2_RD_WRN | (((unsigned long)device.Read)<<I2C_CR2_NBYTES_Pos);
|
|
I2C.I2C->CR1 = I2C_CR1_RXIE | I2C_CR1_PE;
|
|
}
|
|
else
|
|
{
|
|
I2C.I2C->CR2 |= I2C_CR2_STOP;
|
|
I2C.I2C->CR1 = I2C_CR1_STOPIE | I2C_CR1_PE;
|
|
}
|
|
return;
|
|
}
|
|
|
|
if ((I2C.I2C->CR1 & I2C_CR1_RXIE) && (I2C.I2C->ISR & I2C_ISR_RXNE))
|
|
{
|
|
const I2C_Request& device = *I2C.Device;
|
|
|
|
device.Buffer[I2C.Index++] = I2C.I2C->RXDR;
|
|
if(I2C.Index<device.Read) return;
|
|
I2C.I2C->CR1 = I2C_CR1_STOPIE | I2C_CR1_PE;
|
|
I2C.I2C->CR2 |= I2C_CR2_STOP;
|
|
I2C.Index=0;
|
|
return;
|
|
}
|
|
|
|
if ((I2C.I2C->CR1 & I2C_CR1_STOPIE) && (I2C.I2C->ISR & I2C_ISR_STOPF))
|
|
{
|
|
I2C_Request& device = *I2C.Device;
|
|
|
|
I2C.I2C->ICR = I2C_ICR_STOPCF;
|
|
I2C.I2C->CR1 = I2C_CR1_PE;
|
|
I2C.I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
|
|
unsigned char addr = device.Address;
|
|
device.Address = 0;
|
|
|
|
I2C.Device=device.Next;
|
|
|
|
if(device.CallbackProc) device.CallbackProc(addr, device.Buffer, device.Read);
|
|
}
|
|
|
|
if (I2C.I2C->CR1 == I2C_CR1_PE)
|
|
{
|
|
if(!I2C.Device)
|
|
{
|
|
do
|
|
{
|
|
I2C.Device = (I2C_Request*)__LDREXW((volatile unsigned int*)&I2C.Head);
|
|
} while(__STREXW(0, (volatile unsigned int*)&I2C.Head));
|
|
|
|
if(!I2C.Device) return;
|
|
}
|
|
|
|
const I2C_Request& device = *I2C.Device;
|
|
|
|
if (!(I2C.I2C->CR2 & I2C_CR2_SADD_Msk))
|
|
{
|
|
I2C.I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
|
|
if(device.Write)
|
|
{
|
|
I2C.I2C->CR2 |= I2C_CR2_START | (device.Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)device.Write)<<I2C_CR2_NBYTES_Pos);
|
|
I2C.I2C->CR1 = I2C_CR1_TXIE | I2C_CR1_PE;
|
|
}
|
|
else
|
|
{
|
|
I2C.I2C->CR2 |= I2C_CR2_START | (device.Address << (I2C_CR2_SADD_Pos + 1)) | I2C_CR2_RD_WRN | (((unsigned long)device.Read)<<I2C_CR2_NBYTES_Pos);
|
|
I2C.I2C->CR1 = I2C_CR1_RXIE | I2C_CR1_PE;
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
|
|
extern "C" void I2C1_EV_IRQHandler()
|
|
{
|
|
EV_IRQHandler(I2C1_Data);
|
|
}
|
|
|
|
|
|
static void Init(I2C_TypeDef* I2C, IRQn_Type IRQn)
|
|
{
|
|
//I2C->TIMINGR = 0x00303D5BUL; // 100kHz
|
|
I2C->TIMINGR = 0x10802D9BUL; // 400kHz
|
|
|
|
I2C->CR1 = I2C_CR1_PE;
|
|
|
|
while(I2C->ISR & I2C_ISR_BUSY) { }
|
|
|
|
NVIC_SetPriority(IRQn, 1);
|
|
NVIC_EnableIRQ(IRQn);
|
|
}
|
|
|
|
|
|
static bool I2C_CheckDeviceWhoAmI(I2C_TypeDef* I2C, unsigned char Address, unsigned char WhoAmI_Reg, unsigned char Expected_ID)
|
|
{
|
|
if (I2C->ISR & I2C_ISR_BUSY) return false;
|
|
|
|
I2C->ICR = I2C_ICR_STOPCF | I2C_ICR_NACKCF;
|
|
|
|
uint32_t timeout = 100000;
|
|
|
|
I2C->CR2 = (Address << 1) | (1 << 16) | I2C_CR2_START;
|
|
|
|
while (!(I2C->ISR & I2C_ISR_TXIS))
|
|
{
|
|
if (I2C->ISR & I2C_ISR_NACKF)
|
|
{
|
|
I2C->ICR = I2C_ICR_NACKCF;
|
|
return false;
|
|
}
|
|
if ((timeout--) == 0) return false;
|
|
}
|
|
|
|
I2C->TXDR = WhoAmI_Reg;
|
|
|
|
timeout = 100000;
|
|
while (!(I2C->ISR & I2C_ISR_TC))
|
|
{
|
|
if ((timeout--) == 0) return false;
|
|
}
|
|
|
|
I2C->CR2 = (Address << 1) | (1 << 16) | I2C_CR2_RD_WRN | I2C_CR2_START | I2C_CR2_AUTOEND;
|
|
|
|
timeout = 100000;
|
|
while (!(I2C->ISR & I2C_ISR_RXNE))
|
|
{
|
|
if (I2C->ISR & I2C_ISR_NACKF)
|
|
{
|
|
I2C->ICR = I2C_ICR_NACKCF;
|
|
return false;
|
|
}
|
|
if ((timeout--) == 0) return false;
|
|
}
|
|
|
|
unsigned char received_id = (unsigned char)I2C->RXDR;
|
|
|
|
timeout = 100000;
|
|
while (!(I2C->ISR & I2C_ISR_STOPF))
|
|
{
|
|
if ((timeout--) == 0) return false;
|
|
}
|
|
|
|
I2C->ICR = I2C_ICR_STOPCF;
|
|
I2C->CR2 = 0;
|
|
|
|
if (received_id == Expected_ID)
|
|
{
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
static bool I2C_CheckDevice(I2C_TypeDef* I2C, unsigned char Address)
|
|
{
|
|
if (I2C->ISR & I2C_ISR_BUSY) return false;
|
|
|
|
I2C->ICR = I2C_ICR_STOPCF | I2C_ICR_NACKCF;
|
|
|
|
I2C->CR2 = (Address << 1) | I2C_CR2_AUTOEND | I2C_CR2_START;
|
|
|
|
uint32_t timeout = 100000;
|
|
|
|
while (!(I2C->ISR & I2C_ISR_STOPF))
|
|
{
|
|
if ((timeout--) == 0) return false;
|
|
}
|
|
|
|
bool device_present = false;
|
|
|
|
if (I2C->ISR & I2C_ISR_NACKF)
|
|
{
|
|
I2C->ICR = I2C_ICR_NACKCF;
|
|
device_present = false;
|
|
}
|
|
else device_present = true;
|
|
|
|
I2C->ICR = I2C_ICR_STOPCF;
|
|
I2C->CR2 = 0;
|
|
|
|
return device_present;
|
|
}
|
|
|
|
|
|
void I2C1_Init()
|
|
{
|
|
if (RCC->APB1ENR1 & RCC_APB1ENR1_I2C1EN) return;
|
|
|
|
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
|
|
|
|
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
|
|
|
|
GPIO_InitPin(GPIO_PIN_8 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
|
|
GPIO_InitPin(GPIO_PIN_9 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
|
|
|
|
Init(I2C1, I2C1_EV_IRQn);
|
|
}
|
|
|
|
|
|
static void Stop(I2C_TypeDef* I2C)
|
|
{
|
|
I2C->CR2 |= I2C_CR2_STOP;
|
|
while (!(I2C->ISR & I2C_ISR_STOPF)) { asm volatile("NOP"); }
|
|
I2C->ICR = I2C_ICR_STOPCF;
|
|
I2C->CR2 = 0;
|
|
}
|
|
|
|
|
|
static void Read(I2C_TypeDef* I2C, unsigned char Address, unsigned char* Data, unsigned char Size)
|
|
{
|
|
I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_NBYTES_Msk);
|
|
I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | I2C_CR2_RD_WRN | (((unsigned long)Size)<<I2C_CR2_NBYTES_Pos);
|
|
I2C->CR2 |= I2C_CR2_START;
|
|
|
|
while (Size--)
|
|
{
|
|
while (!(I2C->ISR & I2C_ISR_RXNE)) { }
|
|
*Data++ = I2C->RXDR;
|
|
}
|
|
}
|
|
|
|
|
|
static void Write(I2C_TypeDef* I2C, unsigned char Address, const unsigned char* Data, unsigned char Size)
|
|
{
|
|
I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
|
|
I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)Size)<<I2C_CR2_NBYTES_Pos);
|
|
I2C->CR2 |= I2C_CR2_START;
|
|
|
|
while (I2C->CR2 & I2C_CR2_START) { asm volatile("NOP"); }
|
|
|
|
while (Size--)
|
|
{
|
|
while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
|
|
I2C->TXDR = *Data++;
|
|
}
|
|
|
|
while(!(I2C->ISR & I2C_ISR_TC)) { asm volatile("NOP"); }
|
|
}
|
|
|
|
|
|
static void Write2(I2C_TypeDef* I2C, unsigned char Address, const unsigned char* Data1, unsigned char Size1, const unsigned char* Data2, unsigned char Size2)
|
|
{
|
|
I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
|
|
I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)Size1+Size2)<<I2C_CR2_NBYTES_Pos);
|
|
I2C->CR2 |= I2C_CR2_START;
|
|
|
|
while (Size1--)
|
|
{
|
|
while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
|
|
I2C->TXDR = *Data1++;
|
|
}
|
|
|
|
while (Size2--)
|
|
{
|
|
while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
|
|
I2C->TXDR = *Data2++;
|
|
}
|
|
|
|
while (!(I2C->ISR & I2C_ISR_TC)) { asm volatile("NOP"); }
|
|
}
|
|
|
|
|
|
static bool Trans(I2C_Data& I2C, I2C_Request* Request, unsigned char Address, const unsigned char* Data, unsigned char SizeWrite, unsigned char SizeRead)
|
|
{
|
|
if (Request->Address || SizeWrite>Request->Size || SizeRead>Request->Size || (SizeWrite==0 && SizeRead==0)) return false;
|
|
|
|
if (SizeWrite) memcpy(Request->Buffer, Data, SizeWrite);
|
|
Request->Write = SizeWrite;
|
|
Request->Read = SizeRead;
|
|
|
|
Request->Address = Address;
|
|
|
|
do
|
|
{
|
|
I2C_Request* dev = (I2C_Request*)__LDREXW((volatile unsigned int*)&I2C.Head);
|
|
Request->Next = dev;
|
|
} while (__STREXW((unsigned int)Request, (volatile unsigned int*)&I2C.Head));
|
|
|
|
if (I2C.I2C->CR1 == I2C_CR1_PE) NVIC_SetPendingIRQ(I2C.IRQn);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool I2C1_Trans(I2C_Request* Request, unsigned char Address, const void* Data, unsigned char SizeWrite, unsigned char SizeRead)
|
|
{
|
|
return Trans(I2C1_Data, Request, Address, (unsigned char*)Data, SizeWrite, SizeRead);
|
|
}
|
|
|
|
|
|
void I2C1_Write(unsigned char Address, unsigned char Data)
|
|
{
|
|
Write(I2C1, Address, &Data, 1);
|
|
}
|
|
|
|
|
|
void I2C1_Write(unsigned char Address, const void* Data, unsigned char Size)
|
|
{
|
|
Write(I2C1, Address, (unsigned char*)Data, Size);
|
|
}
|
|
|
|
|
|
void I2C1_Write2(unsigned char Address, const void* Data1, unsigned char Size1, const void* Data2, unsigned char Size2)
|
|
{
|
|
Write2(I2C1, Address, (unsigned char*)Data1, Size1, (unsigned char*)Data2, Size2);
|
|
}
|
|
|
|
|
|
void I2C1_Read(unsigned char Address, void* Data, unsigned char Size)
|
|
{
|
|
Read(I2C1, Address, (unsigned char*)Data, Size);
|
|
}
|
|
|
|
|
|
bool I2C1_CheckDevice(unsigned char Address)
|
|
{
|
|
return I2C_CheckDevice(I2C1, Address);
|
|
}
|
|
|
|
|
|
bool I2C1_CheckDeviceWhoAmI(unsigned char Address, unsigned char WhoAmI_Reg, unsigned char Expected_ID)
|
|
{
|
|
return I2C_CheckDeviceWhoAmI(I2C1, Address, WhoAmI_Reg, Expected_ID);
|
|
}
|
|
|
|
|
|
void I2C1_Stop()
|
|
{
|
|
Stop(I2C1);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|