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 I2C_Request* i2c_head = 0;
static uint8_t i2c_busy = 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; static void (*i2c_callback)(uint8_t* buf) = 0;
void imu_pow_init(); 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_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
/*void i2c_enqueue(I2C_Request* req); //void i2c_enqueue(I2C_Request* req);
void i2c_start_next();*/ 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); void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);

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

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

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

36
Source/main.c
View File

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

104
drone.ewp
View File

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

96
drone.ewt
View File

@@ -3491,66 +3491,48 @@
</group> </group>
<group> <group>
<name>Core</name> <name>Core</name>
<group> <file>
<name>Inc</name> <name>$PROJ_DIR$\Source\Core\stm32g431xx.h</name>
<file> </file>
<name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name> <file>
</file> <name>$PROJ_DIR$\Source\Core\stm32g4xx.h</name>
</group> </file>
<group> <file>
<name>Src</name> <name>$PROJ_DIR$\Source\Core\system_stm32g4xx.c</name>
<file> </file>
<name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name> <file>
</file> <name>$PROJ_DIR$\Source\Core\system_stm32g4xx.h</name>
</group> </file>
</group>
<group>
<name>Devices</name>
<file>
<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> <group>
<name>Drivers</name> <name>Drivers</name>
<group> <file>
<name>CMSIS</name> <name>$PROJ_DIR$\Source\Drivers\GPIO.cpp</name>
<group> </file>
<name>Device</name> <file>
<group> <name>$PROJ_DIR$\Source\Drivers\GPIO.h</name>
<name>ST</name> </file>
<group> <file>
<name>STM32G4xx</name> <name>$PROJ_DIR$\Source\Drivers\I2C.cpp</name>
<group> </file>
<name>Include</name> <file>
<file> <name>$PROJ_DIR$\Source\Drivers\I2C.h</name>
<name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g431xx.h</name> </file>
</file>
<file>
<name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g4xx.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>
</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>
</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>
<group> <group>
<name>INS</name> <name>INS</name>