bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Переход на C++

Browse Source 
Очередная попытка реализовать чтение IMU как в рабочей прошивке оказалась провальной.
Поэтому было принято решение перенести проект на C++ и писать его подобно рабочей прошивке.
Реализован драйвер для I2C.
Добавлены файлы интерфейса IMU и конкретного ICM20948.
This commit is contained in:
Раджаб Бисултанов
2026-04-10 16:54:04 +03:00
parent b62fd39a67
commit d59cf7cd55
20 changed files with 732 additions and 2366 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
Source/BSP/Inc/imu.h
View File

@@ -51,6 +51,20 @@ typedef struct
static I2C_Request* i2c_head = 0;
static uint8_t i2c_busy = 0;*/
/*typedef struct I2C_Request
{
void (*Callback)(uint8_t* data, uint8_t size);
uint8_t* Buffer;
uint8_t Size;
uint8_t Address;
uint8_t Write;
uint8_t Read;
struct I2C_Request* Next;
} I2C_Request;*/
static void (*i2c_callback)(uint8_t* buf) = 0;
void imu_pow_init();
@@ -65,10 +79,10 @@ void imu_tim6_init(const uint16_t freq);
void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
/*void i2c_enqueue(I2C_Request* req);
void i2c_start_next();*/
//void i2c_enqueue(I2C_Request* req);
void i2c_start_next();
void i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*));
void imu_get_async(void (*cb)(uint8_t* data, uint8_t size));
void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);

113
Source/BSP/Src/imu.c
View File

@@ -4,6 +4,11 @@
static uint8_t i2c_buf[16];
static uint8_t i2c_index = 0;*/
static I2C_Request* i2c_head = 0;
static I2C_Request* i2c_current = 0;
static uint8_t imu_buffer[16];
void imu_pow_init()
{
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
@@ -107,33 +112,23 @@ void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
I2C1->ICR |= I2C_ICR_STOPCF;
}
/*void i2c_enqueue(I2C_Request* req)
static void i2c_enqueue(I2C_Request* req)
{
req->next = 0;
__disable_irq();
if (!i2c_head)
{
req->Next = i2c_head;
i2c_head = req;
}
else
{
I2C_Request* cur = i2c_head;
while (cur->next) cur = cur->next;
cur->next = req;
}
__enable_irq();
// если I2C свободен — стартуем
// если I2C свободен — запускаем
if (!i2c_busy)
{
NVIC_SetPendingIRQ(I2C1_EV_IRQn);
}
}
void i2c_start_next()
static void i2c_start_next()
{
if (!i2c_head)
{
@@ -143,27 +138,8 @@ void i2c_start_next()
i2c_busy = 1;
current_req = i2c_head;
i2c_head = i2c_head->next;
i2c_index = 0;
// сначала пишем регистр
I2C1->CR2 = (current_req->addr << 1) |
(1 << I2C_CR2_NBYTES_Pos) |
I2C_CR2_START;
}*/
void i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*))
{
if (i2c_busy) return; // ❗ ВАЖНО
i2c_busy = 1;
i2c_addr = addr;
i2c_reg = reg;
i2c_len = len;
i2c_callback = cb;
i2c_current = i2c_head;
i2c_head = i2c_head->Next;
I2C1->CR1 |= I2C_CR1_TXIE |
I2C_CR1_RXIE |
@@ -173,11 +149,28 @@ void i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*)
NVIC_EnableIRQ(I2C1_EV_IRQn);
// старт записи регистра
I2C1->CR2 = (addr << 1) |
I2C1->CR2 = (i2c_current->Address << 1) |
(1 << I2C_CR2_NBYTES_Pos) |
I2C_CR2_START;
}
void imu_get_async(void (*cb)(uint8_t* data, uint8_t size))
{
static I2C_Request req;
req.Callback = cb;
req.Buffer = imu_buffer;
req.Size = sizeof(imu_buffer);
req.Address = ICM_ADDR;
req.Write = 1;
req.Read = 12;
imu_buffer[0] = 0x2D; // регистр
i2c_enqueue(&req);
}
void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
{
i2c_wait_idle(I2C1);
@@ -197,50 +190,58 @@ void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
void I2C1_EV_IRQHandler()
{
static int test_irq = 0;
test_irq++;
uint32_t isr = I2C1->ISR;
// TXIS — отправляем регистр
if (isr & I2C_ISR_TXIS)
if (!i2c_current)
{
I2C1->TXDR = i2c_reg;
i2c_start_next();
return;
}
// TC — запускаем чтение
static uint8_t index = 0;
// TXIS
if (isr & I2C_ISR_TXIS)
{
I2C1->TXDR = i2c_current->Buffer[0];
}
// TC → старт чтения
else if (isr & I2C_ISR_TC)
{
I2C1->CR2 = (i2c_addr << 1) |
I2C1->CR2 = (i2c_current->Address << 1) |
I2C_CR2_RD_WRN |
(i2c_len << I2C_CR2_NBYTES_Pos) |
(i2c_current->Read << I2C_CR2_NBYTES_Pos) |
I2C_CR2_AUTOEND |
I2C_CR2_START;
}
// RXNE — читаем байты
// RXNE
else if (isr & I2C_ISR_RXNE)
{
static uint8_t index = 0;
i2c_buf[index++] = I2C1->RXDR;
if (index >= i2c_len)
i2c_current->Buffer[index++] = I2C1->RXDR;
if (index >= i2c_current->Read)
index = 0;
}
// STOP — завершение
// STOP
else if (isr & I2C_ISR_STOPF)
{
I2C1->ICR |= I2C_ICR_STOPCF;
i2c_busy = 0;
if (i2c_current->Callback)
i2c_current->Callback(i2c_current->Buffer, i2c_current->Read);
I2C1->CR1 |= I2C_CR1_TXIE |
I2C_CR1_RXIE |
I2C_CR1_TCIE |
I2C_CR1_STOPIE;
i2c_current = 0;
NVIC_EnableIRQ(I2C1_EV_IRQn);
if (i2c_callback)
i2c_callback(i2c_buf);
i2c_start_next();
}
}

0
Source/Drivers/CMSIS/Device/ST/STM32G4xx/Include/stm32g431xx.h → Source/Core/stm32g431xx.h
View File

0
Source/Drivers/CMSIS/Device/ST/STM32G4xx/Include/stm32g4xx.h → Source/Core/stm32g4xx.h
View File

0
Source/Core/Src/system_stm32g4xx.c → Source/Core/system_stm32g4xx.c
View File

0
Source/Core/Inc/system_stm32g4xx.h → Source/Core/system_stm32g4xx.h
View File

1
Source/Devices/ICM20948.cpp Normal file
View File

@@ -0,0 +1 @@
#include "ICM20948.h"

8
Source/Devices/ICM20948.h Normal file
View File

@@ -0,0 +1,8 @@
#pragma once
#ifndef ICM20948_H
#define ICM20948_H
#endif

2
Source/Devices/IIMU.cpp Normal file
View File

@@ -0,0 +1,2 @@
#include "IIMU.h"

9
Source/Devices/IIMU.h Normal file
View File

@@ -0,0 +1,9 @@
#pragma once
#ifndef IIMU_H
#define IIMU_H
#endif

2124
Source/Drivers/CMSIS/Include/core_cm4.h
View File

File diff suppressed because it is too large Load Diff

35
Source/Drivers/GPIO.cpp Normal file
View File

@@ -0,0 +1,35 @@
#include "GPIO.H"
#include "stm32g4xx.h"
void GPIO_InitPin(unsigned long Pin)
{
unsigned long port = (Pin & 0x000000F0UL) >> 4;
GPIO_TypeDef* gpio = (GPIO_TypeDef*)(((unsigned char*)GPIOA) + (port * 0x0400));
unsigned long rcc = 1UL << port;
unsigned long pin = Pin & 0x0000000FUL;
unsigned long af = (Pin & 0x0F000000UL) >> 24;
unsigned long pupd = (Pin & 0x00F00000UL) >> 20;
unsigned long ospeed = (Pin & 0x000F0000UL) >> 16;
unsigned long mode = (Pin & 0x0000F000UL) >> 12;
unsigned long otype = (Pin & 0x00000100UL) >> 8;
unsigned long set = (Pin & 0x00000200UL) >> 9;
if (!(RCC->AHB2ENR & rcc)) RCC->AHB2ENR |= rcc;
gpio->AFR[pin >> 3] &= ~(0x0000000FUL << ((pin & 0x07) * 4));
gpio->AFR[pin >> 3] |= af << ((pin & 0x07) * 4);
gpio->OSPEEDR &= ~(0x00000003UL << (pin * 2));
gpio->OSPEEDR |= ospeed << (pin * 2);
gpio->OTYPER &= ~(0x00000001UL << pin);
gpio->OTYPER |= otype << pin;
gpio->PUPDR &= ~(0x00000003UL << (pin * 2));
gpio->PUPDR |= pupd << (pin * 2);
gpio->BSRR = 1 << (set ? pin : pin+16);
gpio->MODER &= ~(0x00000003UL << (pin * 2));
gpio->MODER |= mode << (pin * 2);
}

74
Source/Drivers/GPIO.h Normal file
View File

@@ -0,0 +1,74 @@
#pragma once
#ifndef GPIO_H
#define GPIO_H
#define GPIO_PIN_0 0x00000000UL
#define GPIO_PIN_1 0x00000001UL
#define GPIO_PIN_2 0x00000002UL
#define GPIO_PIN_3 0x00000003UL
#define GPIO_PIN_4 0x00000004UL
#define GPIO_PIN_5 0x00000005UL
#define GPIO_PIN_6 0x00000006UL
#define GPIO_PIN_7 0x00000007UL
#define GPIO_PIN_8 0x00000008UL
#define GPIO_PIN_9 0x00000009UL
#define GPIO_PIN_10 0x0000000AUL
#define GPIO_PIN_11 0x0000000BUL
#define GPIO_PIN_12 0x0000000CUL
#define GPIO_PIN_13 0x0000000DUL
#define GPIO_PIN_14 0x0000000EUL
#define GPIO_PIN_15 0x0000000FUL
#define GPIO_PORT_A 0x00000000UL
#define GPIO_PORT_B 0x00000010UL
#define GPIO_PORT_C 0x00000020UL
#define GPIO_PORT_D 0x00000030UL
#define GPIO_PORT_E 0x00000040UL
#define GPIO_PORT_F 0x00000070UL
#define GPIO_PORT_G 0x00000080UL
#define GPIO_PORT_H 0x00000090UL
#define GPIO_PORT_I 0x000000A0UL
#define GPIO_PORT_J 0x000000B0UL
#define GPIO_PORT_K 0x000000C0UL
#define GPIO_PUSHPULL 0x00000000UL
#define GPIO_OPENDRAIN 0x00000100UL
#define GPIO_RESET 0x00000000UL
#define GPIO_SET 0x00000200UL
#define GPIO_INPUT 0x00000000UL
#define GPIO_OUTPUT 0x00001000UL
#define GPIO_ALTER 0x00002000UL
#define GPIO_ANALOG 0x00003000UL
#define GPIO_OSPEED_LOW 0x00000000UL
#define GPIO_OSPEED_MEDIUM 0x00010000UL
#define GPIO_OSPEED_FAST 0x00020000UL
#define GPIO_OSPEED_HIGH 0x00030000UL
#define GPIO_NOPUPD 0x00000000UL
#define GPIO_PULLUP 0x00100000UL
#define GPIO_PULLDOWN 0x00200000UL
#define GPIO_AF0 0x00000000UL
#define GPIO_AF1 0x01000000UL
#define GPIO_AF2 0x02000000UL
#define GPIO_AF3 0x03000000UL
#define GPIO_AF4 0x04000000UL
#define GPIO_AF5 0x05000000UL
#define GPIO_AF6 0x06000000UL
#define GPIO_AF7 0x07000000UL
#define GPIO_AF8 0x08000000UL
#define GPIO_AF9 0x09000000UL
#define GPIO_AF10 0x0A000000UL
#define GPIO_AF11 0x0B000000UL
#define GPIO_AF12 0x0C000000UL
#define GPIO_AF13 0x0D000000UL
#define GPIO_AF14 0x0E000000UL
#define GPIO_AF15 0x0F000000UL
void GPIO_InitPin(unsigned long Pin);
#endif

19
Source/Drivers/HAL_M/GPIO/Inc/HAL_GPIO.h
View File

@@ -1,19 +0,0 @@
#ifndef HAL_GPIO
#define HAL_GPIO
#include "stm32g431xx.h"
// I/O MODES
#define INPUT_MODE ((uint32_t) 0x00) // input mode
#define OUTPUT_MODE ((uint32_t) 0x01) // general purpose output mode
#define ALT_FUNC_MODE ((uint32_t) 0x02) // alternative function mode
#define ANALOG_MODE ((uint32_t) 0x03) // alanog mode (reset state)
// CLOCK ENABLING
#define GPIO_CLOCK_EN_GPIOA (RCC->AHB2ENR |= (1 << 0))
#define GPIO_CLOCK_EN_GPIOB (RCC->AHB2ENR |= (1 << 1))
#define GPIO_CLOCK_EN_GPIOC (RCC->AHB2ENR |= (1 << 2))
#endif

3
Source/Drivers/HAL_M/GPIO/Src/HAL_GPIO.c
View File

@@ -1,3 +0,0 @@
#include "GPIO/Inc/HAL_GPIO.h"

377
Source/Drivers/I2C.cpp Normal file
View File

@@ -0,0 +1,377 @@
#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);
}

29
Source/Drivers/I2C.h Normal file
View File

@@ -0,0 +1,29 @@
#pragma once
#ifndef I2C_H
#define I2C_H
struct I2C_Request
{
void (*CallbackProc)(unsigned char Address, const unsigned char* Data, unsigned char Size);
unsigned char* Buffer;
unsigned char Size;
unsigned char Address;
unsigned char Write;
unsigned char Read;
I2C_Request* Next;
};
void I2C1_Init();
bool I2C1_Trans(I2C_Request* Request, unsigned char Address, const void* Data, unsigned char SizeWrite, unsigned char SizeRead);
void I2C1_Write(unsigned char Address, unsigned char Data);
void I2C1_Write(unsigned char Address, const void* Data, unsigned char Size);
void I2C1_Write2(unsigned char Address, const void* Data1, unsigned char Size1, const void* Data2, unsigned char Size2);
void I2C1_Read(unsigned char Address, void* Data, unsigned char Size);
bool I2C1_CheckDevice(unsigned char Address);
bool I2C1_CheckDeviceWhoAmI(unsigned char Address, unsigned char WhoAmI_Reg, unsigned char Expected_ID);
void I2C1_Stop();
#endif

18
Source/main.c
View File

@@ -25,7 +25,7 @@ control_channels_t ctrl_chs;
Vector3 euler;
void imu_callback(uint8_t* buf);
void imu_callback(uint8_t* buf, uint8_t size);
void delay_ms(uint32_t ms);
int main(void)
@@ -91,15 +91,15 @@ int main(void)
}
}
void imu_callback(uint8_t* buf)
void imu_callback(uint8_t* buf, uint8_t size)
{
imu_raw.ax = (buf[0] << 8) | buf[1];
imu_raw.ay = (buf[2] << 8) | buf[3];
imu_raw.az = (buf[4] << 8) | buf[5];
imu_raw.ax = Rev16((buf[0] << 8) | buf[1]);
imu_raw.ay = Rev16((buf[2] << 8) | buf[3]);
imu_raw.az = Rev16((buf[4] << 8) | buf[5]);
imu_raw.gx = (buf[6] << 8) | buf[7];
imu_raw.gy = (buf[8] << 8) | buf[9];
imu_raw.gz = (buf[10] << 8) | buf[11];
imu_raw.gx = Rev16((buf[6] << 8) | buf[7]);
imu_raw.gy = Rev16((buf[8] << 8) | buf[9]);
imu_raw.gz = Rev16((buf[10] << 8) | buf[11]);
imu_process_raw(&imu, &imu_raw, &allowed_calib);
@@ -122,7 +122,7 @@ void TIM6_DAC_IRQHandler()
{
TIM6->SR &= ~TIM_SR_UIF;
i2c_read_async(ICM_ADDR, 0x2D, 12, imu_callback);
imu_get_async(imu_callback);
control_update_flag = 1;
}
}

76
drone.ewp
View File

@@ -356,10 +356,8 @@
</option>
<option>
<name>CCIncludePath2</name>
<state>$PROJ_DIR$\Source\Drivers\CMSIS\Device\ST\STM32G4xx\Include</state>
<state>$PROJ_DIR$\Source\Drivers\CMSIS\Include</state>
<state>$PROJ_DIR$\Source\Drivers\HAL_M</state>
<state>$PROJ_DIR$\Source\Core\Inc</state>
<state>$PROJ_DIR$\Source\Drivers</state>
<state>$PROJ_DIR$\Source\Core</state>
<state>$PROJ_DIR$\Source\BSP\Inc</state>
<state>$PROJ_DIR$\Source\Control\Inc</state>
<state>$PROJ_DIR$\Source\INS\geometry</state>
@@ -404,7 +402,7 @@
</option>
<option>
<name>IccLang</name>
<state>2</state>
<state>1</state>
</option>
<option>
<name>IccCDialect</name>
@@ -2308,67 +2306,49 @@
</group>
<group>
<name>Core</name>
<group>
<name>Inc</name>
<file>
<name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name>
<name>$PROJ_DIR$\Source\Core\stm32g431xx.h</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Core\stm32g4xx.h</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Core\system_stm32g4xx.c</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Core\system_stm32g4xx.h</name>
</file>
</group>
<group>
<name>Src</name>
<name>Devices</name>
<file>
<name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name>
<name>$PROJ_DIR$\Source\Devices\ICM20948.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Devices\ICM20948.h</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Devices\IIMU.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Devices\IIMU.h</name>
</file>
</group>
</group>
<group>
<name>Drivers</name>
<group>
<name>CMSIS</name>
<group>
<name>Device</name>
<group>
<name>ST</name>
<group>
<name>STM32G4xx</name>
<group>
<name>Include</name>
<file>
<name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g431xx.h</name>
<name>$PROJ_DIR$\Source\Drivers\GPIO.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g4xx.h</name>
<name>$PROJ_DIR$\Source\Drivers\GPIO.h</name>
</file>
</group>
</group>
</group>
</group>
<group>
<name>Include</name>
<file>
<name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Include\core_cm4.h</name>
<name>$PROJ_DIR$\Source\Drivers\I2C.cpp</name>
</file>
</group>
</group>
<group>
<name>HAL_M</name>
<group>
<name>GPIO</name>
<group>
<name>Inc</name>
<file>
<name>$PROJ_DIR$\Source\Drivers\HAL_M\GPIO\Inc\HAL_GPIO.h</name>
<name>$PROJ_DIR$\Source\Drivers\I2C.h</name>
</file>
</group>
<group>
<name>Src</name>
<file>
<name>$PROJ_DIR$\Source\Drivers\HAL_M\GPIO\Src\HAL_GPIO.c</name>
</file>
</group>
</group>
</group>
</group>
<group>
<name>INS</name>
<group>

68
drone.ewt
View File

@@ -3491,67 +3491,49 @@
</group>
<group>
<name>Core</name>
<group>
<name>Inc</name>
<file>
<name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name>
<name>$PROJ_DIR$\Source\Core\stm32g431xx.h</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Core\stm32g4xx.h</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Core\system_stm32g4xx.c</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Core\system_stm32g4xx.h</name>
</file>
</group>
<group>
<name>Src</name>
<name>Devices</name>
<file>
<name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name>
<name>$PROJ_DIR$\Source\Devices\ICM20948.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Devices\ICM20948.h</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Devices\IIMU.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\Source\Devices\IIMU.h</name>
</file>
</group>
</group>
<group>
<name>Drivers</name>
<group>
<name>CMSIS</name>
<group>
<name>Device</name>
<group>
<name>ST</name>
<group>
<name>STM32G4xx</name>
<group>
<name>Include</name>
<file>
<name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g431xx.h</name>
<name>$PROJ_DIR$\Source\Drivers\GPIO.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g4xx.h</name>
<name>$PROJ_DIR$\Source\Drivers\GPIO.h</name>
</file>
</group>
</group>
</group>
</group>
<group>
<name>Include</name>
<file>
<name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Include\core_cm4.h</name>
<name>$PROJ_DIR$\Source\Drivers\I2C.cpp</name>
</file>
</group>
</group>
<group>
<name>HAL_M</name>
<group>
<name>GPIO</name>
<group>
<name>Inc</name>
<file>
<name>$PROJ_DIR$\Source\Drivers\HAL_M\GPIO\Inc\HAL_GPIO.h</name>
<name>$PROJ_DIR$\Source\Drivers\I2C.h</name>
</file>
</group>
<group>
<name>Src</name>
<file>
<name>$PROJ_DIR$\Source\Drivers\HAL_M\GPIO\Src\HAL_GPIO.c</name>
</file>
</group>
</group>
</group>
</group>
<group>
<name>INS</name>
<group>