robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

Compare commits

base: robossembler:main
Branches Tags
robossembler:main
robossembler:Prec_reducer_version_4
robossembler:process_can
robossembler:repository_synchronization
robossembler:updated-hardware-interface
robossembler:pcb-0.4.2-production-ci-fix
robossembler:pcb-0.4.1-production
robossembler:pcb-0.4-production
..
compare: robossembler:repository_synchronization
Branches Tags
robossembler:Prec_reducer_version_4
robossembler:process_can
robossembler:main
robossembler:repository_synchronization
robossembler:updated-hardware-interface
robossembler:pcb-0.4.2-production-ci-fix
robossembler:pcb-0.4.1-production
robossembler:pcb-0.4-production

1 commit
main ... repository

Author SHA1 Message Date
Vladimir Latukhin
5161aa61c9 synchronization_03.06.2025 2025-06-03 16:12:31 +03:00
48 changed files with 594 additions and 3426 deletions
Download patch file Download diff file Expand all files Collapse all files

68
controller/fw/README.md
View file

@ -1,68 +0,0 @@
# Встроенное ПО для сервопривода на STM32F446RE
## Для разработки
- [Установить platformio](#introduction)
```bash
pip install -U platformio
```
- Установить python3
```bash
sudo apt install python3
```
- Устаноивть st-link
```bash
sudo apt install st-link
```
### Прошивка делится на два файла один для загрузчика другой для основной прошивки. Чтобы загрузить как описано ниже нужно находится в директории этого проекта. Нужно сделать как для bootloader так и для embed
- [Скомпилировать проект](#build_project)
```bash
platformio run --environment robotroller_reborn
```
- [Загрузить прошивку](#upload_project)
```bash
platformio run --target upload --environment robotroller_reborn
```
## Другой способ прошивки
## Выбор интерфейса прошивки
### Для основной прошивки в директории ./embed
- Если уже есть какя-то основная прошивка, то чтобы перепрошить другую прошивку, добавляем флаг для бутлоадера
```bash
python3 firmw_update_flag.py [адрес устройства]
```
- Передача прошивки по CAN
```bash
python3 firmware_can.py firmware.hex [адрес устройства]
```
### St-link(нет адресации можно прошивать только по одному)
```bash
python3 st-link.py firmware.hex
```
### St-link_full(полная прошивка без адресации)
#### Прошивает и программатор и основную прошивку можно находится как в ./embed, так и в ./bootloader(в директории где есть данный тест в папке test).
- Если до этого сохраняли адреса и данные, то они останутся даже при полной перепрошивке
- Если бутлоадер не был прошит и FLASH микрокотроллера полностью стерта
- [Скачать прошивку и бутлоадер в hex формате]
ССЫЛКА
- [Прошить через программатор]
```bash
python3 st-link_full.py bootloader.hex firmware.hex
```
## Работа по CAN
#### Для основной прошивки в директории ./embed
- Установка адреса(если до этого не был установлен адрес, то адрес устройства = 0)
```bash
python3 set_id.py [адрес устройства]
```
- Установка PID коэффициентов для угла
```bash
python3 writePID_angle_parametrs.py [адрес устройства]
```
-Чтение PID коэффициентов для угла
```bash
python3 readPID_angle_parametrs.py [адрес устройства]

1
controller/fw/bootloader/.clang-tidy
View file

@ -1 +0,0 @@
Checks: '-*, -misc-definitions-in-headers'

18
controller/fw/bootloader/.clangd
View file

@ -1,18 +0,0 @@
CompileFlags:
Add:
[
# -mlong-calls,
-DSSIZE_MAX,
-DLWIP_NO_UNISTD_H=1,
-Dssize_t=long,
-D_SSIZE_T_DECLARED,
]
Remove:
[
-fno-tree-switch-conversion,
-mtext-section-literals,
-mlongcalls,
-fstrict-volatile-bitfields,
-free,
-fipa-pta,
]

9
controller/fw/bootloader/.gitignore vendored
View file

@ -1,9 +0,0 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch
.cache/
.metadata/
cubemx_config/
compile_commands.json

19
controller/fw/bootloader/check_gcc_version.py
View file

@ -1,19 +0,0 @@
Import("env")
# Получаем путь к компилятору из окружения PlatformIO
gcc_path = env.subst("$CC")
# Выполняем команду для получения версии компилятора
import subprocess
try:
result = subprocess.run([gcc_path, "--version"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True)
if result.returncode == 0:
print(f"GCC version: {result.stdout}")
else:
print(f"Failed to get GCC version: {result.stderr}")
except Exception as e:
print(f"Error while getting GCC version: {e}")
# Дополнительно проверяем путь к компилятору
print(f"Compiler path: {gcc_path}")

299
controller/fw/bootloader/cubemx_config.ioc
View file

@ -1,299 +0,0 @@
#MicroXplorer Configuration settings - do not modify
ADC2.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_15
ADC2.Channel-5\#ChannelRegularConversion=ADC_CHANNEL_8
ADC2.Channel-6\#ChannelRegularConversion=ADC_CHANNEL_9
ADC2.EOCSelection=ADC_EOC_SEQ_CONV
ADC2.IPParameters=Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,NbrOfConversionFlag,InjNumberOfConversion,NbrOfConversion,Rank-5\#ChannelRegularConversion,Channel-5\#ChannelRegularConversion,SamplingTime-5\#ChannelRegularConversion,Rank-6\#ChannelRegularConversion,Channel-6\#ChannelRegularConversion,SamplingTime-6\#ChannelRegularConversion,EOCSelection
ADC2.InjNumberOfConversion=0
ADC2.NbrOfConversion=3
ADC2.NbrOfConversionFlag=1
ADC2.Rank-1\#ChannelRegularConversion=1
ADC2.Rank-5\#ChannelRegularConversion=2
ADC2.Rank-6\#ChannelRegularConversion=3
ADC2.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
ADC2.SamplingTime-5\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
ADC2.SamplingTime-6\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
FREERTOS.IPParameters=Tasks01,configENABLE_FPU,configTIMER_TASK_PRIORITY
FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
FREERTOS.configENABLE_FPU=1
FREERTOS.configTIMER_TASK_PRIORITY=1
File.Version=6
GPIO.groupedBy=Group By Peripherals
KeepUserPlacement=false
Mcu.CPN=STM32F446RET6
Mcu.Family=STM32F4
Mcu.IP0=ADC2
Mcu.IP1=FREERTOS
Mcu.IP2=NVIC
Mcu.IP3=RCC
Mcu.IP4=SPI2
Mcu.IP5=SYS
Mcu.IP6=TIM1
Mcu.IP7=TIM3
Mcu.IP8=TIM5
Mcu.IP9=USART1
Mcu.IPNb=10
Mcu.Name=STM32F446R(C-E)Tx
Mcu.Package=LQFP64
Mcu.Pin0=PC1
Mcu.Pin1=PC5
Mcu.Pin10=PC9
Mcu.Pin11=PA8
Mcu.Pin12=PA9
Mcu.Pin13=PA10
Mcu.Pin14=PA11
Mcu.Pin15=PA12
Mcu.Pin16=PA13
Mcu.Pin17=PA14
Mcu.Pin18=PC10
Mcu.Pin19=PC11
Mcu.Pin2=PB0
Mcu.Pin20=PC12
Mcu.Pin21=PD2
Mcu.Pin22=PB6
Mcu.Pin23=PB7
Mcu.Pin24=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin25=VP_SYS_VS_tim2
Mcu.Pin26=VP_TIM1_VS_ClockSourceINT
Mcu.Pin27=VP_TIM3_VS_ClockSourceINT
Mcu.Pin28=VP_TIM5_VS_ClockSourceINT
Mcu.Pin3=PB1
Mcu.Pin4=PB10
Mcu.Pin5=PB14
Mcu.Pin6=PB15
Mcu.Pin7=PC6
Mcu.Pin8=PC7
Mcu.Pin9=PC8
Mcu.PinsNb=29
Mcu.ThirdPartyNb=0
Mcu.UserConstants=
Mcu.UserName=STM32F446RETx
MxCube.Version=6.5.0
MxDb.Version=DB.6.0.50
NVIC.ADC_IRQn=true\:5\:0\:true\:true\:true\:1\:true\:true\:true\:true
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.ForceEnableDMAVector=true
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:false\:false
NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
NVIC.SPI2_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false\:false
NVIC.SavedPendsvIrqHandlerGenerated=true
NVIC.SavedSvcallIrqHandlerGenerated=true
NVIC.SavedSystickIrqHandlerGenerated=true
NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false
NVIC.TIM2_IRQn=true\:15\:0\:true\:false\:true\:false\:false\:true\:true
NVIC.TIM3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.TimeBase=TIM2_IRQn
NVIC.TimeBaseIP=TIM2
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
PA10.Signal=S_TIM1_CH3
PA11.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
PA11.GPIO_Label=EN_U
PA11.GPIO_PuPd=GPIO_PULLDOWN
PA11.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PA11.Locked=true
PA11.Signal=GPIO_Output
PA12.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
PA12.GPIO_Label=EN_V
PA12.GPIO_PuPd=GPIO_PULLDOWN
PA12.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PA12.Locked=true
PA12.Signal=GPIO_Output
PA13.Mode=Serial_Wire
PA13.Signal=SYS_JTMS-SWDIO
PA14.Mode=Serial_Wire
PA14.Signal=SYS_JTCK-SWCLK
PA8.Signal=S_TIM1_CH1
PA9.Signal=S_TIM1_CH2
PB0.GPIOParameters=GPIO_Label
PB0.GPIO_Label=SENSE2
PB0.Locked=true
PB0.Signal=ADCx_IN8
PB1.GPIOParameters=GPIO_Label
PB1.GPIO_Label=SENSE1
PB1.Locked=true
PB1.Signal=ADCx_IN9
PB10.Locked=true
PB10.Mode=Full_Duplex_Master
PB10.Signal=SPI2_SCK
PB14.Locked=true
PB14.Mode=Full_Duplex_Master
PB14.Signal=SPI2_MISO
PB15.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label
PB15.GPIO_Label=AS5045_CS
PB15.GPIO_PuPd=GPIO_PULLUP
PB15.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PB15.Locked=true
PB15.PinState=GPIO_PIN_SET
PB15.Signal=GPIO_Output
PB6.Mode=Asynchronous
PB6.Signal=USART1_TX
PB7.Mode=Asynchronous
PB7.Signal=USART1_RX
PC1.Mode=Full_Duplex_Master
PC1.Signal=SPI2_MOSI
PC10.GPIOParameters=GPIO_Label
PC10.GPIO_Label=LED1
PC10.Locked=true
PC10.Signal=GPIO_Output
PC11.GPIOParameters=GPIO_Label
PC11.GPIO_Label=LED2
PC11.Locked=true
PC11.Signal=GPIO_Output
PC12.GPIOParameters=GPIO_Label
PC12.GPIO_Label=LED3
PC12.Locked=true
PC12.Signal=GPIO_Output
PC5.GPIOParameters=GPIO_Label
PC5.GPIO_Label=SENSE3
PC5.Locked=true
PC5.Signal=ADCx_IN15
PC6.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
PC6.GPIO_Label=EN_W
PC6.GPIO_PuPd=GPIO_PULLDOWN
PC6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
PC6.Locked=true
PC6.Signal=GPIO_Output
PC7.GPIOParameters=GPIO_Label
PC7.GPIO_Label=DRV_FAULT
PC7.Locked=true
PC7.Signal=GPIO_Input
PC8.GPIOParameters=GPIO_Label
PC8.GPIO_Label=DRV_RESET
PC8.Locked=true
PC8.Signal=GPIO_Output
PC9.GPIOParameters=GPIO_Label
PC9.GPIO_Label=DRV_SLEEP
PC9.Locked=true
PC9.Signal=GPIO_Output
PD2.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
PD2.GPIO_Label=spi1_cs
PD2.GPIO_PuPd=GPIO_PULLDOWN
PD2.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PD2.Locked=true
PD2.Signal=GPIO_Output
PinOutPanel.RotationAngle=0
ProjectManager.AskForMigrate=true
ProjectManager.BackupPrevious=false
ProjectManager.CompilerOptimize=6
ProjectManager.ComputerToolchain=false
ProjectManager.CoupleFile=true
ProjectManager.CustomerFirmwarePackage=
ProjectManager.DefaultFWLocation=true
ProjectManager.DeletePrevious=true
ProjectManager.DeviceId=STM32F446RETx
ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.27.1
ProjectManager.FreePins=false
ProjectManager.HalAssertFull=false
ProjectManager.HeapSize=0x200
ProjectManager.KeepUserCode=true
ProjectManager.LastFirmware=true
ProjectManager.LibraryCopy=1
ProjectManager.MainLocation=Src
ProjectManager.NoMain=false
ProjectManager.PreviousToolchain=STM32CubeIDE
ProjectManager.ProjectBuild=false
ProjectManager.ProjectFileName=cubemx_config.ioc
ProjectManager.ProjectName=cubemx_config
ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x400
ProjectManager.TargetToolchain=Other Toolchains (GPDSC)
ProjectManager.ToolChainLocation=
ProjectManager.UnderRoot=false
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_TIM1_Init-TIM1-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,5-MX_SPI2_Init-SPI2-false-HAL-true,6-MX_TIM3_Init-TIM3-false-HAL-true,7-MX_ADC2_Init-ADC2-false-HAL-true,8-MX_TIM5_Init-TIM5-false-HAL-true
RCC.AHBFreq_Value=180000000
RCC.APB1CLKDivider=RCC_HCLK_DIV4
RCC.APB1Freq_Value=45000000
RCC.APB1TimFreq_Value=90000000
RCC.APB2CLKDivider=RCC_HCLK_DIV2
RCC.APB2Freq_Value=90000000
RCC.APB2TimFreq_Value=180000000
RCC.CECFreq_Value=32786.88524590164
RCC.CortexFreq_Value=180000000
RCC.FCLKCortexFreq_Value=180000000
RCC.FMPI2C1Freq_Value=45000000
RCC.FamilyName=M
RCC.HCLKFreq_Value=180000000
RCC.HSE_VALUE=8000000
RCC.I2S1Freq_Value=96000000
RCC.I2S2Freq_Value=96000000
RCC.IPParameters=AHBFreq_Value,APB1CLKDivider,APB1Freq_Value,APB1TimFreq_Value,APB2CLKDivider,APB2Freq_Value,APB2TimFreq_Value,CECFreq_Value,CortexFreq_Value,FCLKCortexFreq_Value,FMPI2C1Freq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,I2S1Freq_Value,I2S2Freq_Value,MCO2PinFreq_Value,PLLCLKFreq_Value,PLLI2SPCLKFreq_Value,PLLI2SQCLKFreq_Value,PLLI2SRCLKFreq_Value,PLLI2SoutputFreq_Value,PLLM,PLLN,PLLQCLKFreq_Value,PLLRCLKFreq_Value,PLLSAIPCLKFreq_Value,PLLSAIQCLKFreq_Value,PLLSAIoutputFreq_Value,PWRFreq_Value,SAIAFreq_Value,SAIBFreq_Value,SDIOFreq_Value,SPDIFRXFreq_Value,SYSCLKFreq_VALUE,SYSCLKSource,USBFreq_Value,VCOI2SInputFreq_Value,VCOI2SOutputFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VCOSAIInputFreq_Value,VCOSAIOutputFreq_Value
RCC.MCO2PinFreq_Value=180000000
RCC.PLLCLKFreq_Value=180000000
RCC.PLLI2SPCLKFreq_Value=96000000
RCC.PLLI2SQCLKFreq_Value=96000000
RCC.PLLI2SRCLKFreq_Value=96000000
RCC.PLLI2SoutputFreq_Value=96000000
RCC.PLLM=8
RCC.PLLN=180
RCC.PLLQCLKFreq_Value=180000000
RCC.PLLRCLKFreq_Value=180000000
RCC.PLLSAIPCLKFreq_Value=96000000
RCC.PLLSAIQCLKFreq_Value=96000000
RCC.PLLSAIoutputFreq_Value=96000000
RCC.PWRFreq_Value=180000000
RCC.SAIAFreq_Value=96000000
RCC.SAIBFreq_Value=96000000
RCC.SDIOFreq_Value=180000000
RCC.SPDIFRXFreq_Value=180000000
RCC.SYSCLKFreq_VALUE=180000000
RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK
RCC.USBFreq_Value=180000000
RCC.VCOI2SInputFreq_Value=1000000
RCC.VCOI2SOutputFreq_Value=192000000
RCC.VCOInputFreq_Value=2000000
RCC.VCOOutputFreq_Value=360000000
RCC.VCOSAIInputFreq_Value=1000000
RCC.VCOSAIOutputFreq_Value=192000000
SH.ADCx_IN15.0=ADC2_IN15,IN15
SH.ADCx_IN15.ConfNb=1
SH.ADCx_IN8.0=ADC2_IN8,IN8
SH.ADCx_IN8.ConfNb=1
SH.ADCx_IN9.0=ADC2_IN9,IN9
SH.ADCx_IN9.ConfNb=1
SH.S_TIM1_CH1.0=TIM1_CH1,PWM Generation1 CH1
SH.S_TIM1_CH1.ConfNb=1
SH.S_TIM1_CH2.0=TIM1_CH2,PWM Generation2 CH2
SH.S_TIM1_CH2.ConfNb=1
SH.S_TIM1_CH3.0=TIM1_CH3,PWM Generation3 CH3
SH.S_TIM1_CH3.ConfNb=1
SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_64
SPI2.CLKPhase=SPI_PHASE_1EDGE
SPI2.CLKPolarity=SPI_POLARITY_LOW
SPI2.CalculateBaudRate=703.125 KBits/s
SPI2.DataSize=SPI_DATASIZE_16BIT
SPI2.Direction=SPI_DIRECTION_2LINES
SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,CLKPhase,BaudRatePrescaler,CLKPolarity
SPI2.Mode=SPI_MODE_MASTER
SPI2.VirtualType=VM_MASTER
TIM1.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
TIM1.BreakState=TIM_BREAK_DISABLE
TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
TIM1.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
TIM1.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
TIM1.CounterMode=TIM_COUNTERMODE_CENTERALIGNED1
TIM1.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,TIM_MasterOutputTrigger,AutoReloadPreload,BreakState,OffStateRunMode,OffStateIDLEMode,CounterMode,Period
TIM1.OffStateIDLEMode=TIM_OSSI_DISABLE
TIM1.OffStateRunMode=TIM_OSSR_DISABLE
TIM1.Period=2399
TIM1.TIM_MasterOutputTrigger=TIM_TRGO_RESET
TIM3.IPParameters=Period,Prescaler
TIM3.Period=99
TIM3.Prescaler=89
USART1.IPParameters=VirtualMode
USART1.VirtualMode=VM_ASYNC
VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
VP_SYS_VS_tim2.Mode=TIM2
VP_SYS_VS_tim2.Signal=SYS_VS_tim2
VP_TIM1_VS_ClockSourceINT.Mode=Internal
VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT
VP_TIM3_VS_ClockSourceINT.Mode=Internal
VP_TIM3_VS_ClockSourceINT.Signal=TIM3_VS_ClockSourceINT
VP_TIM5_VS_ClockSourceINT.Mode=Internal
VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT
board=custom

8
controller/fw/bootloader/gen_compile_commands.py
View file

@ -1,8 +0,0 @@
import os
Import("env")
# include toolchain paths
env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True)
# override compilation DB path
env.Replace(COMPILATIONDB_PATH="compile_commands.json")

11
controller/fw/bootloader/hex_compile.py
View file

@ -1,11 +0,0 @@
Import("env")
hex_name = "bootloader.hex"
# Custom HEX from ELF
env.AddPostAction(
"$BUILD_DIR/${PROGNAME}.elf",
env.VerboseAction(" ".join([
"$OBJCOPY", "-O", "ihex", "-R", ".eeprom",
"$BUILD_DIR/${PROGNAME}.elf", "$BUILD_DIR/{}".format(hex_name)
]), "Building $BUILD_DIR/{}".format(hex_name))
)

86
controller/fw/bootloader/include/flash.h
View file

@ -1,86 +0,0 @@
#ifndef FLASH_H_
#define FLASH_H_
#include "stm32f446xx.h"
#include <stdio.h>
#include <stdlib.h>
/* no padding for this struct, beacuse storing 8 bytes*/
typedef struct{
uint8_t data_id; // data_id = id register of can
uint8_t data_type;
uint16_t crc;
uint32_t value;
// uint32_t write_ptr_now;
}FLASH_RECORD;
enum {
addr_id = 0,
pid_p = 1,
pid_i,
pid_d,
firmw,
foc_id,
angl,
vel
};
/* for saved in FLASH float data*/
union{
uint32_t i;
float f;
}conv_float_to_int;
#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct
// Flash sectors for STM32F407
#define APP_ADDRESS 0x08008000
#define UPDATE_FLAG 0xDEADBEEF // flag forz update firmware
#define BOOT_CAN_ID 0x01 // CAN ID bootloader
#define BOOT_CAN_END 0x02 // CAN ID end of transfer
#define DATA_CAN_ID 0x03 // CAN ID packet data
#define ACK_CAN_ID 0x05 // CAN ID acknowledge
#define MAX_FW_SIZE 0x3FFF // Max size firmware = 256 kB
#define PARAM_COUNT 5 // count data in flash
#define SECTOR_6 0x08040000 // 128KB
#define SECTOR_6_END (SECTOR_6 + 128 * 1024) // sector 6 end
// Flash keys for unlocking flash memory
#define BYTE32 0
#define BYTE8 1
//FLASH SET ONE PROGRAMM WORD
#define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1
#define FLASH_32BYTE \
FLASH->CR = (FLASH->CR & ~FLASH_CR_PSIZE) | (0x2 << FLASH_CR_PSIZE_Pos)
// Flash command bits
#define FLASH_LOCK FLASH->CR |= FLASH_CR_LOCK
#define FLASH_UNLOCK FLASH->KEYR = FLASH_KEY1; FLASH->KEYR = FLASH_KEY2
// Flash status flags
#define FLASH_BUSY (FLASH->SR & FLASH_SR_BSY)
#define FLASH_ERROR (FLASH->SR & (FLASH_SR_WRPERR | FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR))
//for bootloader
typedef void(*pFunction)(void);
/* for start addr in FLASH */
static uint32_t write_ptr = SECTOR_6;
static uint32_t ptr_fl = APP_ADDRESS;
// Function prototypes
void flash_unlock(void);
void flash_lock(void);
void erase_sector(uint8_t sector);
void flash_program_word(uint32_t address, uint32_t data,uint32_t byte_len);
uint8_t flash_read_word(uint32_t address);
FLASH_RECORD* load_params();
void compact_page();
void flash_read(uint32_t addr,FLASH_RECORD* ptr);
uint16_t validate_crc16(uint8_t *data,uint32_t length);
void flash_write(uint32_t addr, FLASH_RECORD* record);
void write_flash_page(const uint8_t* data, uint16_t len);
void erase_flash_pages();
void write_param(uint8_t param_id,uint32_t val);
uint16_t calc_crc_struct(FLASH_RECORD* res);
#endif /* FLASH_H_ */

38
controller/fw/bootloader/include/hal_conf_extra.h
View file

@ -1,38 +0,0 @@
#pragma once
#pragma region "Motor and sensor setup"
#define LED1 PC10
#define LED2 PC11
#define HARDWARE_SERIAL_RX_PIN PB7
#define HARDWARE_SERIAL_TX_PIN PB6
#define AS5045_CS PB15
#define AS5045_MISO PB14
#define AS5045_MOSI PC1
#define AS5045_SCLK PB10
#define CURRENT_SENSOR_1 PB1
#define CURRENT_SENSOR_2 PB0
#define CURRENT_SENSOR_3 PC5
#define TIM1_CH1 PA8
#define TIM1_CH2 PA9
#define TIM1_CH3 PA10
#define EN_W_GATE_DRIVER PC6
#define EN_U_GATE_DRIVER PA11
#define EN_V_GATE_DRIVER PA12
#define SLEEP_DRIVER PC9
#define RESET_DRIVER PC8
#define FAULT_DRIVER PC7
#define POLE_PAIRS 14
#define CAN2_TX PB13
#define CAN2_RX PB12
#define CAN1_TX PB9
#define CAN1_RX PB8
#define GM6208_RESISTANCE 31
#define OWN_RESISTANCE 26
#pragma endregion
#if !defined(HAL_CAN_MODULE_ENABLED)
#define HAL_CAN_MODULE_ENABLED
#endif
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_can.h"
#include <STM32_CAN.h>

38
controller/fw/bootloader/include/reg_cah.h
View file

@ -1,38 +0,0 @@
#ifndef REG_CAH_H_
#define REG_CAH_H_
#define APP_ADDR 0x0800400 // 16KB - Application
#define ADDR_VAR 0x8040000
#define REG_READ 0x07
#define REG_WRITE 0x08
/* Startup ID device */
#define START_ID 0x00
/* CAN REGISTER ID */
#define REG_ID 0x01
#define REG_BAUDRATE 0x02
#define REG_MOTOR_POSPID_Kp 0x30
#define REG_MOTOR_POSPID_Ki 0x31
#define REG_MOTOR_POSPID_Kd 0x32
#define REG_MOTOR_VELPID_Kp 0x40
#define REG_MOTOR_VELPID_Ki 0x41
#define REG_MOTOR_VELPID_Kd 0x42
#define REG_MOTOR_IMPPID_Kp 0x50
#define REG_MOTOR_IMPPID_Kd 0x51
#define REG_RESET 0x88
#define REG_LED_BLINK 0x8B
#define FOC_STATE 0x60
#define MOTOR_VELOCITY 0x70
#define MOTOR_ENABLED 0x71
#define MOTOR_ANGLE 0x72
#endif // REG_CAH_H_

31
controller/fw/bootloader/platformio.ini
View file

@ -1,31 +0,0 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[platformio]
[env:robotroller_reborn]
platform = ststm32
board = genericSTM32F446RE
framework = arduino
upload_protocol = stlink
debug_tool = stlink
monitor_speed = 19200
monitor_parity = N
build_flags =
-DSTM32F446xx
-D HAL_CAN_MODULE_ENABLED
-D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
lib_deps =
askuric/Simple FOC@^2.3.4
pazi88/STM32_CAN@^1.1.2
extra_scripts =
pre:gen_compile_commands.py
post:hex_compile.py

272
controller/fw/bootloader/src/flash.cpp
View file

@ -1,272 +0,0 @@
#include "flash.h"
#include <stdbool.h>
#include "hal_conf_extra.h"
void flash_unlock(){
// Check if flash is locked
if(!(FLASH->CR & FLASH_CR_LOCK)) {
return; // Already unlocked
}
// Write flash key sequence to unlock
FLASH->KEYR = 0x45670123; // First key
FLASH->KEYR = 0xCDEF89AB; // Second key
}
void flash_lock() {
if(FLASH->CR & FLASH_CR_LOCK) {
return; // Already locked
}
FLASH->CR |= FLASH_CR_LOCK; // Lock flash memory
}
void erase_sector(uint8_t sector){
// Wait if flash is busy
while(FLASH_BUSY);
// Check if flash is locked and unlock if needed
if(FLASH->CR & FLASH_CR_LOCK) {
flash_unlock();
}
// Set sector erase bit and sector number
FLASH->CR |= FLASH_CR_SER;
FLASH->CR &= ~FLASH_CR_SNB;
FLASH->CR |= (sector << FLASH_CR_SNB_Pos) & FLASH_CR_SNB_Msk;
// Start erase
FLASH->CR |= FLASH_CR_STRT;
// Wait for erase to complete
while(FLASH_BUSY);
// Clear sector erase bit
FLASH->CR &= ~FLASH_CR_SER;
}
void flash_program_word(uint32_t address,uint32_t data,uint32_t byte_len){
// Wait if flash is busy
while(FLASH_BUSY);
// Check if flash is locked and unlock if needed
if(FLASH->CR & FLASH_CR_LOCK) {
flash_unlock();
}
// Set program bit 32bit programm size and Write data to address
if(byte_len == 1) {
FLASH_8BYTE;
FLASH->CR |= FLASH_CR_PG;
*(volatile uint8_t*)address = (uint8_t)data;
} else {
FLASH_32BYTE;
FLASH->CR |= FLASH_CR_PG;
*(volatile uint32_t*)address = data;
}
// Wait for programming to complete
while(FLASH_BUSY);
// Clear program bit
FLASH->CR &= ~FLASH_CR_PG;
}
void flash_write(uint32_t addr, FLASH_RECORD* record){
uint32_t* data = (uint32_t*)record;
uint32_t size = FLASH_RECORD_SIZE / 4; //count words in struct
// Wait if flash is busy
while(FLASH_BUSY);
// Check if flash is locked and unlock if needed
if(FLASH->CR & FLASH_CR_LOCK) {
flash_unlock();
}
// Set program bit and write data to flash
FLASH_32BYTE;
FLASH->CR |= FLASH_CR_PG;
for(int i = 0;i < size;i++){
*(volatile uint32_t*)(addr + (i * 4)) = data[i];
}
// Clear program bit
FLASH->CR &= ~FLASH_CR_PG;
write_ptr = addr + (size * 4); //increase variable storing addr
flash_lock();
}
uint8_t flash_read_word(uint32_t address){
// Check if address is valid
if(address < FLASH_BASE || address > FLASH_END) {
return 0;
}
// Read byte from flash memory
return *((volatile uint8_t*)address);
}
// Wait if flash
// bool validata_crc(FLASH_RECORD* crc){
// return crc->crc == 0x6933? true : false;
// }
uint16_t validate_crc16(uint8_t *data, uint32_t length) {
uint16_t crc = 0xFFFF; // start value for CRC MODBUS
while (length--) {
crc ^= *data++; // XOR
for (uint8_t i = 0; i < 8; i++) {
if (crc & 0x0001) {
crc = (crc >> 1) ^ 0xA001; // polynome 0x8005 (reverse)
} else {
crc >>= 1;
}
}
}
return crc;
}
uint16_t calc_crc_struct(FLASH_RECORD* res){
uint8_t arr_res[FLASH_RECORD_SIZE - 2];
uint16_t crc_res;
/* sorting data without CRC */
arr_res[0] = res->data_id;
arr_res[1] = res->data_type;
/* from 32 to 8 bit */
for(int i = 0;i < 4;i++)
arr_res[i + 2] = (uint8_t)(res->value >> i * 8);
crc_res = validate_crc16(arr_res,FLASH_RECORD_SIZE - 2);
return crc_res;
}
/* read struct from FLASH */
void flash_read(uint32_t addr,FLASH_RECORD* ptr){
uint8_t* flash_ptr = (uint8_t*)addr;
uint8_t* dest = (uint8_t*)ptr;
for(int i = 0;i < FLASH_RECORD_SIZE;i++)
dest[i] = flash_ptr[i];
}
void compact_page(){
FLASH_RECORD latest[PARAM_COUNT] = {0};
for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_6_END;i += FLASH_RECORD_SIZE) {
FLASH_RECORD rec;
flash_read(i,&rec);
uint16_t calculated_crc = calc_crc_struct(&rec);
if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) {
// if the crc does not match, we check further
latest[rec.data_id] = rec;
}
else
// if
continue;
}
erase_sector(6);
write_ptr = SECTOR_6; // Сброс на начало
for (int i = 0; i < PARAM_COUNT; i++) {
if (latest[i].data_id != 0xFF) {
// alignment
if (write_ptr % 4 != 0) {
write_ptr += (4 - (write_ptr % 4));
}
flash_write(write_ptr, &latest[i]);
}
}
}
void write_param(uint8_t param_id, uint32_t val) {
FLASH_RECORD param_flash;
// __disable_irq(); // Interrupt off
param_flash.data_id = param_id;
param_flash.value = val;
param_flash.data_type = sizeof(uint8_t);
param_flash.crc = calc_crc_struct(&param_flash);
// check alignment
if (write_ptr % 8 != 0) {
write_ptr += (8 - (write_ptr % 8));
}
// check buffer overflow
if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) {
compact_page(); // after compact_page update
// alignment
if (write_ptr % 8 != 0) {
write_ptr += (8 - (write_ptr % 8));
}
}
flash_write(write_ptr, &param_flash); //inside the function, the write_ptr pointer is automatically incremented by the size of the structure
// __enable_irq(); // Interrupt on
}
void write_flash_page(const uint8_t* data, uint16_t len) { // Добавлен const
flash_unlock();
uint32_t word = 0;
for (uint16_t i = 0; i < len; i += 4) {
memcpy(&word, &data[i], 4); // Безопасное копирование
HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, ptr_fl + i, word);
}
ptr_fl += len;
flash_lock();
}
void erase_flash_pages() {
FLASH_EraseInitTypeDef erase;
erase.TypeErase = FLASH_TYPEERASE_SECTORS;
erase.Sector = FLASH_SECTOR_2;
erase.NbSectors = 4;
erase.VoltageRange = FLASH_VOLTAGE_RANGE_3;
uint32_t error;
flash_unlock();
HAL_FLASHEx_Erase(&erase, &error);
flash_lock();
}
FLASH_RECORD* load_params(){
__disable_irq();
static FLASH_RECORD latest[PARAM_COUNT] = {0};
FLASH_RECORD res;
for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) {
flash_read(addr,&res);
uint16_t calculated_crc = calc_crc_struct(&res);
if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT) continue;
else{
latest[res.data_id] = res;
write_ptr = addr + FLASH_RECORD_SIZE;
}
}
__enable_irq();
return latest;
}

171
controller/fw/bootloader/src/main.cpp
View file

@ -1,171 +0,0 @@
#include "Arduino.h"
#include <STM32_CAN.h>
#include "flash.h"
STM32_CAN Can(CAN2, DEF);
volatile bool fw_update = false;
volatile bool app_valid = false;
volatile uint32_t fw_size = 0;
volatile uint16_t fw_crc = 0;
volatile uint32_t jump;
static FLASH_RECORD *flash_record = {0};
static uint32_t ptr_flash;
volatile uint32_t msg_id;
volatile uint16_t id_x;
volatile uint8_t msg_ch;
// Прототипы функций
void jump_to_app();
void process_can_message(const CAN_message_t &msg);
void erase_flash_pages();
bool verify_firmware();
void send_ack(uint8_t status);
bool is_app_valid();
void setup() {
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
Serial.begin(115200);
Can.begin();
Can.setBaudRate(1000000);
TIM_TypeDef *Instance = TIM2;
HardwareTimer *SendTimer = new HardwareTimer(Instance);
SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
SendTimer->resume();
Can.setFilter(0, 0, STD);
// Настройка GPIO
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
GPIOC->MODER |= GPIO_MODER_MODE10_0 | GPIO_MODER_MODE11_0;
GPIOC->ODR &= ~GPIO_ODR_OD11;
GPIOC->ODR |= GPIO_ODR_OD10;
flash_record = load_params();
if(flash_record[firmw].value == UPDATE_FLAG) {
fw_update = true;
for(int i = 0; i < 5;i++){
GPIOC->ODR ^= GPIO_ODR_OD10; // Indecate message
delay(100);
}
// write_param(firmw,0); //reset flasg
erase_flash_pages();
}
else{
// for st-link update, because he doesnt reset flag_update
if(is_app_valid()) jump_to_app(); //firmware exist
else fw_update = true; //firmware doesnt exist, but we in bootloader
}
GPIOC->ODR |= GPIO_ODR_OD10;
}
void process_can_message(const CAN_message_t &msg) {
msg_id = msg.id;
/* 0x697
69 - slave addr
7 || 8 - REG_READ or REG_WRITE */
id_x = (msg_id >> 4) & 0xFFFF; // saved address
msg_ch = msg_id & 0xF; // saved id
if(id_x == flash_record[addr_id].value){
switch(msg_ch) {
case BOOT_CAN_ID:
if(msg.buf[0] == 0x01) { // start transfer
fw_size = *(uint32_t*)&msg.buf[1]; //size of firmware
fw_crc = *(uint16_t*)&msg.buf[5]; //crc
ptr_flash = APP_ADDRESS;
send_ack(0x01);
}
break;
case DATA_CAN_ID: // Data packet
if(ptr_flash < (APP_ADDRESS + fw_size)) {
write_flash_page((const uint8_t*)msg.buf, msg.len);
ptr_flash += msg.len;
send_ack(0x02);
}
break;
case BOOT_CAN_END: // End of transfer
if(verify_firmware()) {
send_ack(0xAA);
write_param(firmw,0); //reset flag set 0
fw_update = false; //reset flag
// erase_sector(7);
delay(500);
NVIC_SystemReset();
} else {
send_ack(0x55);
erase_flash_pages(); //if error
}
break;
}
}
}
void jump_to_app() {
__disable_irq();
jump = *(volatile uint32_t*)(APP_ADDRESS + 4);
void (*app_entry)(void);
app_entry = (void (*)(void))jump;
for (uint32_t i = 0; i < 8; i++) {
NVIC->ICPR[i] = 0xFFFFFFFF;
}
__set_MSP(*(volatile uint32_t*)APP_ADDRESS);
// SCB->VTOR = (uint32_t)0x08008004;
app_entry();
}
bool verify_firmware() {
uint16_t calculated_crc = 0;
calculated_crc = validate_crc16((uint8_t*)APP_ADDRESS,fw_size);
return (calculated_crc == fw_crc);
}
void send_ack(uint8_t status) {
CAN_message_t ack;
ack.id = ACK_CAN_ID;
ack.len = 1;
ack.buf[0] = status;
Can.write(ack);
}
bool is_app_valid() {
volatile uint32_t* app_vector = (volatile uint32_t*)APP_ADDRESS;
// Check stack pointer
bool sp_valid = (app_vector[0] >= 0x20000000) &&
(app_vector[0] <= (0x20000000 + 128*1024)); // Для STM32 с 128K RAM
// check reset_handler
bool pc_valid = (app_vector[1] >= 0x08000000) &&
(app_vector[1] <= (0x08000000 + 1024*1024)); // Для 1MB Flash
// check two words on reset value
bool not_erased = (app_vector[0] != 0xFFFFFFFF) &&
(app_vector[1] != 0xFFFFFFFF);
return sp_valid && pc_valid && not_erased;
}
void loop() {
if(fw_update) {
CAN_message_t msg;
while(Can.read(msg))
process_can_message(msg);
}
}

141
controller/fw/bootloader/test/firmware_can.py
View file

@ -1,141 +0,0 @@
import can
import sys
import time
from intelhex import IntelHex
# Конфигурация
CAN_CHANNEL = 'socketcan'
CAN_INTERFACE = 'can0'
CAN_BITRATE = 1000000
#ch =int(input("Введите id устройства:"))
ch = int(sys.argv[2])
BOOT_CAN_ID = (ch * 16) + 1
DATA_CAN_ID = (ch * 16) + 3
BOOT_CAN_END = (ch * 16) + 2
ACK_CAN_ID = 0x05
#конфиг для crc16 ibm
def debug_print(msg):
print(f"[DEBUG] {msg}")
def calculate_crc16(data: bytes) -> int:
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_firmware(hex_file):
try:
debug_print("Инициализация CAN...")
bus = can.interface.Bus(
channel=CAN_INTERFACE,
bustype=CAN_CHANNEL,
bitrate=CAN_BITRATE
)
debug_print("Чтение HEX-файла...")
ih = IntelHex(hex_file)
binary_data = ih.tobinstr() # Исправлено на tobinstr()
fw_size = len(binary_data)
debug_print(f"Размер прошивки: {fw_size} байт")
# Расчет CRC
debug_print("Расчёт CRC...")
# calculator = Calculator(Crc16.IBM)
fw_crc = calculate_crc16(binary_data)
debug_print(f"CRC: 0x{fw_crc:04X}")
# Отправка START
start_data = bytearray([0x01])
start_data += fw_size.to_bytes(4, 'little')
start_data += fw_crc.to_bytes(2, 'little')
debug_print(f"START: {list(start_data)}")
start_msg = can.Message(
arbitration_id=BOOT_CAN_ID,
data=bytes(start_data),
is_extended_id=False
)
try:
bus.send(start_msg)
except can.CanError as e:
debug_print(f"Ошибка отправки START: {str(e)}")
return
# Ожидание ACK
debug_print("Ожидание ACK...")
ack = wait_for_ack(bus)
if not ack:
debug_print("Таймаут ACK START")
return
debug_print(f"Получен ACK: {list(ack.data)}")
# Отправка данных
packet_size = 8
for i in range(0, len(binary_data), packet_size):
chunk = binary_data[i:i+packet_size]
# Дополнение до 8 байт
if len(chunk) < 8:
chunk += b'\xFF' * (8 - len(chunk))
debug_print(f"Пакет {i//8}: {list(chunk)}")
data_msg = can.Message(
arbitration_id=DATA_CAN_ID,
data=chunk,
is_extended_id=False
)
try:
bus.send(data_msg)
except can.CanError as e:
debug_print(f"Ошибка отправки данных: {str(e)}")
return
ack = wait_for_ack(bus)
if not ack:
debug_print("Таймаут ACK DATA")
return
# Финал
debug_print("Отправка FINISH...")
finish_msg = can.Message(
arbitration_id=BOOT_CAN_END,
data=bytes([0xAA]),
is_extended_id=False
)
bus.send(finish_msg)
ack = wait_for_ack(bus, timeout=1.0)
if ack and ack.data[0] == 0xAA:
debug_print("Прошивка подтверждена!")
else:
debug_print("Ошибка верификации!")
except Exception as e:
debug_print(f"Критическая ошибка: {str(e)}")
finally:
bus.shutdown()
def wait_for_ack(bus, timeout=1.0):
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1) # Неблокирующий режим
if msg and msg.arbitration_id == ACK_CAN_ID:
return msg
return None
if __name__ == "__main__":
import sys
if len(sys.argv) != 3:
print("Использование: sudo python3 can_flasher.py firmware.hex")
sys.exit(1)
send_firmware(sys.argv[1])

70
controller/fw/bootloader/test/firmware_update_flag.py
View file

@ -1,70 +0,0 @@
import can
import time
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства (по умолчанию)
REG_WRITE = 0x8 # Код команды чтения
REG_ID = 0x55 # Адрес регистра с Firmware Update
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def validate_crc16(data):
"""Расчет CRC16 (MODBUS) для проверки целостности данных"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
# Инициализация CAN-интерфейса
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# ======= 1. Запрос текущего ID устройства =======
# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
can_id_read = (OLD_DEVICE_ID << 4) | REG_WRITE
# Данные для запроса: [регистр, резервный байт]
data_read = [REG_ID, 0x00]
# Формируем полные данные для расчета CRC:
# - CAN ID разбивается на 2 байта (little-endian)
# - Добавляем данные запроса
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
# Рассчитываем CRC и разбиваем на байты (little-endian)
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))
# Собираем итоговый пакет: данные + CRC
packet_read = data_read + crc_bytes
print("Переход в boot режим", packet_read)
send_can_message(bus, can_id_read, packet_read)
bus.shutdown()
if __name__ == "__main__":
import sys
if len(sys.argv) != 2:
print("Использование: python3 firmware_test.py address")
sys.exit(1)

78
controller/fw/bootloader/test/st-link.py
View file

@ -1,78 +0,0 @@
import subprocess
import os
import sys
def flash_hex_with_stlink(hex_file_path):
if not os.path.isfile(hex_file_path):
print(f"❌ Файл не найден: {hex_file_path}")
return False
command = [
"st-flash",
"--format", "ihex",
"write",
hex_file_path
]
try:
print(f"⚡️ Прошиваем {hex_file_path} через ST-Link...")
result = subprocess.run(
command,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
universal_newlines=True,
timeout=30
)
print("▬▬▬ STDOUT ▬▬▬")
print(result.stdout)
print("▬▬▬ STDERR ▬▬▬")
print(result.stderr)
if result.returncode == 0:
print("✅ Прошивка успешно завершена!")
# Добавленный блок сброса
try:
print("🔄 Выполняем сброс устройства...")
reset_result = subprocess.run(
["st-info", "--reset"],
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
universal_newlines=True,
timeout=10
)
if reset_result.returncode == 0:
print("♻️ Устройство успешно сброшено!")
else:
print(f"⚠️ Ошибка (код: {reset_result.returncode})")
print("▬▬▬ STDERR сброса ▬▬▬")
print(reset_result.stderr)
except Exception as e:
print(f"⚠️ Ошибка при сбросе: {str(e)}")
return True
else:
print(f"❌ Ошибка прошивки (код: {result.returncode})")
return False
except FileNotFoundError:
print("❌ st-flash не найден! Установите stlink-tools.")
return False
except subprocess.TimeoutExpired:
print("❌ Таймаут операции! Проверьте подключение ST-Link.")
return False
except Exception as e:
print(f"❌ Неизвестная ошибка: {str(e)}")
return False
if __name__ == "__main__":
if len(sys.argv) != 2:
print("Использование: python stlink_flash.py <firmware.hex>")
sys.exit(1)
if flash_hex_with_stlink(sys.argv[1]):
sys.exit(0)
else:
sys.exit(1)

100
controller/fw/bootloader/test/st-link_full.py
View file

@ -1,100 +0,0 @@
import subprocess
import os
import sys
import time
def flash_hex_with_stlink(hex_file_path, component_name):
if not os.path.isfile(hex_file_path):
print(f"❌ Файл {component_name} не найден: {hex_file_path}")
return False
command = [
"st-flash",
"--format", "ihex",
"write",
hex_file_path
]
try:
print(f"⚡️ Прошиваем {component_name} ({hex_file_path}) через ST-Link...")
result = subprocess.run(
command,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
universal_newlines=True,
timeout=30
)
print("▬▬▬ STDOUT ▬▬▬")
print(result.stdout)
print("▬▬▬ STDERR ▬▬▬")
print(result.stderr)
if result.returncode == 0:
print(f"✅ {component_name} успешно прошит!")
return True
else:
print(f"❌ Ошибка прошивки {component_name} (код: {result.returncode})")
return False
except FileNotFoundError:
print("❌ st-flash не найден! Установите stlink-tools.")
return False
except subprocess.TimeoutExpired:
print(f"❌ Таймаут операции при прошивке {component_name}! Проверьте подключение ST-Link.")
return False
except Exception as e:
print(f"❌ Неизвестная ошибка при прошивке {component_name}: {str(e)}")
return False
def reset_device():
try:
print("🔄 Выполняем сброс(перезагрузку) устройства...")
reset_result = subprocess.run(
["st-info", "--reset"],
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
universal_newlines=True,
timeout=10
)
if reset_result.returncode == 0:
print("♻️ Устройство успешно сброшено!")
return True
else:
print(f"⚠️ Ошибка при сбросе (код: {reset_result.returncode})")
print("▬▬▬ STDERR сброса ▬▬▬")
print(reset_result.stderr)
return False
except Exception as e:
print(f"⚠️ Ошибка при сбросе: {str(e)}")
return False
if __name__ == "__main__":
if len(sys.argv) != 3:
print("Использование: python stlink_flash.py <bootloader.hex> <application.hex>")
print("Пример: python stlink_flash.py bootloader.hex firmware.hex")
sys.exit(1)
bootloader_path = sys.argv[1]
app_path = sys.argv[2]
# Прошиваем сначала бутлоадер
if not flash_hex_with_stlink(bootloader_path, "Bootloader"):
print("\n💥 Ошибка прошивки бутлоадера!")
sys.exit(1)
# Сбрасываем устройство после прошивки бутлоадера
reset_device()
time.sleep(1) # Короткая пауза
# Прошиваем основное приложение
if not flash_hex_with_stlink(app_path, "Application"):
print("\n💥 Ошибка прошивки основного приложения!")
sys.exit(1)
# Финальный сброс устройства
reset_device()
print("\n🎉 Все компоненты успешно прошиты!")
sys.exit(0)

1
controller/fw/embed/.gitignore vendored
View file

@ -7,4 +7,3 @@
.metadata/ .metadata/
cubemx_config/ cubemx_config/
compile_commands.json compile_commands.json
../embed.rar

20
controller/fw/embed/README.md Normal file
View file

@ -0,0 +1,20 @@
# Встроенное ПО для сервипривода на STM32F446RE
## Для разработки
- [Установить platformio](#introduction)
```bash
pip install -U platformio
```
- [Скомпилировать проект](#build_project)
```bash
platformio run --environment robotroller_reborn
```
- [Загрузить прошивку](#upload_project)
```bash
platformio run --target upload --environment robotroller_reborn
```
- [Открыть монитор UART](#monitor_port)
```bash
platformio device monitor
```

178
controller/fw/embed/custom_script.ld
View file

@ -1,178 +0,0 @@
/**
******************************************************************************
* @file LinkerScript.ld
* @author Auto-generated by STM32CubeIDE
* @brief Linker script for STM32F446RCTx Device from STM32F4 series
* 256Kbytes FLASH
* 128Kbytes RAM
*
* Set heap size, stack size and stack location according
* to application requirements.
*
* Set memory bank area and size if external memory is used
******************************************************************************
* @attention
*
* <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
* opensource.org/licenses/BSD-3-Clause
*
******************************************************************************
*/
/* Entry Point */
ENTRY(Reset_Handler)
/* Highest address of the user mode stack */
_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of "RAM" Ram type memory */
_Min_Heap_Size = 0x200; /* required amount of heap */
_Min_Stack_Size = 0x400; /* required amount of stack */
/* Memories definition */
MEMORY
{
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = LD_MAX_DATA_SIZE
FLASH (rx) : ORIGIN = 0x8000000 + 0x8000, LENGTH = 512K - 0x8000
}
/* Sections */
SECTIONS
{
/* The startup code into "FLASH" Rom type memory */
.isr_vector :
{
. = ALIGN(4);
KEEP(*(.isr_vector)) /* Startup code */
. = ALIGN(4);
} >FLASH
/* The program code and other data into "FLASH" Rom type memory */
.text :
{
. = ALIGN(4);
*(.text) /* .text sections (code) */
*(.text*) /* .text* sections (code) */
*(.glue_7) /* glue arm to thumb code */
*(.glue_7t) /* glue thumb to arm code */
*(.eh_frame)
KEEP (*(.init))
KEEP (*(.fini))
. = ALIGN(4);
_etext = .; /* define a global symbols at end of code */
} >FLASH
/* Constant data into "FLASH" Rom type memory */
.rodata :
{
. = ALIGN(4);
*(.rodata) /* .rodata sections (constants, strings, etc.) */
*(.rodata*) /* .rodata* sections (constants, strings, etc.) */
. = ALIGN(4);
} >FLASH
.ARM.extab (READONLY) : {
. = ALIGN(4);
*(.ARM.extab* .gnu.linkonce.armextab.*)
. = ALIGN(4);
} >FLASH
.ARM (READONLY) : {
. = ALIGN(4);
__exidx_start = .;
*(.ARM.exidx*)
__exidx_end = .;
. = ALIGN(4);
} >FLASH
.preinit_array (READONLY) :
{
. = ALIGN(4);
PROVIDE_HIDDEN (__preinit_array_start = .);
KEEP (*(.preinit_array*))
PROVIDE_HIDDEN (__preinit_array_end = .);
. = ALIGN(4);
} >FLASH
.init_array (READONLY) :
{
. = ALIGN(4);
PROVIDE_HIDDEN (__init_array_start = .);
KEEP (*(SORT(.init_array.*)))
KEEP (*(.init_array*))
PROVIDE_HIDDEN (__init_array_end = .);
. = ALIGN(4);
} >FLASH
.fini_array (READONLY) :
{
. = ALIGN(4);
PROVIDE_HIDDEN (__fini_array_start = .);
KEEP (*(SORT(.fini_array.*)))
KEEP (*(.fini_array*))
PROVIDE_HIDDEN (__fini_array_end = .);
. = ALIGN(4);
} >FLASH
/* Used by the startup to initialize data */
_sidata = LOADADDR(.data);
/* Initialized data sections into "RAM" Ram type memory */
.data :
{
. = ALIGN(4);
_sdata = .; /* create a global symbol at data start */
*(.data) /* .data sections */
*(.data*) /* .data* sections */
*(.RamFunc) /* .RamFunc sections */
*(.RamFunc*) /* .RamFunc* sections */
. = ALIGN(4);
_edata = .; /* define a global symbol at data end */
} >RAM AT> FLASH
/* Uninitialized data section into "RAM" Ram type memory */
. = ALIGN(4);
.bss :
{
/* This is used by the startup in order to initialize the .bss section */
_sbss = .; /* define a global symbol at bss start */
__bss_start__ = _sbss;
*(.bss)
*(.bss*)
*(COMMON)
. = ALIGN(4);
_ebss = .; /* define a global symbol at bss end */
__bss_end__ = _ebss;
} >RAM
/* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */
._user_heap_stack :
{
. = ALIGN(8);
PROVIDE ( end = . );
PROVIDE ( _end = . );
. = . + _Min_Heap_Size;
. = . + _Min_Stack_Size;
. = ALIGN(8);
} >RAM
/* Remove information from the compiler libraries */
/DISCARD/ :
{
libc.a ( * )
libm.a ( * )
libgcc.a ( * )
}
.ARM.attributes 0 : { *(.ARM.attributes) }
}

2
controller/fw/embed/gen_compile_commands.py
View file

@ -5,4 +5,4 @@ Import("env")
env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True) env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True)
# override compilation DB path # override compilation DB path
env.Replace(COMPILATIONDB_PATH="compile_commands.json") env.Replace(COMPILATIONDB_PATH="compile_commands.json")

10
controller/fw/embed/hex_compile.py
View file

@ -1,10 +0,0 @@
Import("env")
# Custom HEX from ELF
env.AddPostAction(
"$BUILD_DIR/${PROGNAME}.elf",
env.VerboseAction(" ".join([
"$OBJCOPY", "-O", "ihex", "-R", ".eeprom",
"$BUILD_DIR/${PROGNAME}.elf", "$BUILD_DIR/${PROGNAME}.hex"
]), "Building $BUILD_DIR/${PROGNAME}.hex")
)

31
controller/fw/embed/include/config.h
View file

@ -1,31 +0,0 @@
#pragma once
#include "Arduino.h"
#include <AS5045.h>
#include <DRV8313.h>
#include <SimpleFOC.h>
#include <STM32_CAN.h>
#include "flash.h"
extern STM32_CAN Can;
extern SPIClass spi;
extern MagneticSensorAS5045 encoder;
extern BLDCMotor motor;
extern DRV8313Driver driver;
extern LowsideCurrentSense current_sense;
extern Commander command;
struct MotorControlInputs {
float target_angle = 0.0;
float target_velocity = 0.0;
bool motor_enabled = false;
bool foc_state = false;
};
extern MotorControlInputs motor_control_inputs;
void doMotor(char *cmd);
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
FLASH_RECORD* pid_data);
void foc_step(BLDCMotor *motor);

36
controller/fw/embed/include/flash.h
View file

@ -4,36 +4,27 @@
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
/* for addr in FLASH */
/* no padding for this struct, beacuse storing 8 bytes*/ /* for addr in FLASH */
typedef struct{ typedef struct{
uint8_t data_id; // data_id = id register of can uint8_t data_id; // data_id = id register of can
uint8_t data_type; uint8_t value;
uint16_t crc; uint16_t crc;
uint32_t value;
// uint32_t write_ptr_now; // uint32_t write_ptr_now;
}FLASH_RECORD; }FLASH_RECORD;
enum { enum {
addr_id = 0, addr_id = 0,
pid_p = 1, foc_id = 1,
angl = 2,
vel = 3,
pid_p = 4,
pid_i, pid_i,
pid_d, pid_d
firmw,
foc_id,
angl,
vel
}; };
/* for saved in FLASH float data*/
union{
uint32_t i;
float f;
}conv_float_to_int;
#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct #define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct
#define PARAM_COUNT 5 // count data in flash #define PARAM_COUNT 4 // count data in flash
#define FIRMWARE_FLAG (uint32_t)0xDEADBEEF
// Flash sectors for STM32F407 // Flash sectors for STM32F407
#define SECTOR_2 0x08008000 // 16KB #define SECTOR_2 0x08008000 // 16KB
@ -49,7 +40,7 @@ union{
// Flash keys for unlocking flash memory // Flash keys for unlocking flash memory
#define BYTE32 0 #define BYTE32 0
#define BYTE8 1 #define BYTE8 1
#define UPDATE_FLAG 0xDEADBEEF // Unique 32bit value #define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение
//FLASH SET ONE PROGRAMM WORD //FLASH SET ONE PROGRAMM WORD
#define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1 #define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1
#define FLASH_32BYTE \ #define FLASH_32BYTE \
@ -63,24 +54,21 @@ union{
// Flash status flags // Flash status flags
#define FLASH_BUSY (FLASH->SR & FLASH_SR_BSY) #define FLASH_BUSY (FLASH->SR & FLASH_SR_BSY)
#define FLASH_ERROR (FLASH->SR & (FLASH_SR_WRPERR | FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR)) #define FLASH_ERROR (FLASH->SR & (FLASH_SR_WRPERR | FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR))
static uint32_t write_ptr = SECTOR_6;
//for bootloader //for bootloader
typedef void(*pFunction)(void); typedef void(*pFunction)(void);
// Function prototypes // Function prototypes
void flash_unlock(void); void flash_unlock(void);
void flash_lock(void); void flash_lock(void);
void erase_sector(uint8_t sector); void erase_sector(uint8_t sector);
void flash_program_word(uint32_t address, uint32_t data,uint32_t byte_len); void flash_program_word(uint32_t address, uint32_t data,uint32_t byte_len);
uint8_t flash_read_word(uint32_t address); uint8_t flash_read_word(uint32_t address);
void write_param(uint8_t param_id, uint8_t val);
FLASH_RECORD* load_params(); FLASH_RECORD* load_params();
void compact_page(); void compact_page();
void flash_read(uint32_t addr,FLASH_RECORD* ptr); void flash_read(uint32_t addr,FLASH_RECORD* ptr);
uint16_t validate_crc16(uint8_t *data,uint32_t length); uint16_t validate_crc16(uint8_t *data,uint32_t length);
void flash_write(uint32_t addr, FLASH_RECORD* record); void flash_write(uint32_t addr, FLASH_RECORD* record);
bool validaate_crc(FLASH_RECORD* crc); bool validaate_crc(FLASH_RECORD* crc);
void write_param(uint8_t param_id,uint32_t val);
#endif /* FLASH_H_ */ #endif /* FLASH_H_ */

27
controller/fw/embed/include/process_can.h
View file

@ -1,27 +0,0 @@
#pragma once
#include "config.h"
#include "STM32_CAN.h"
#include "flash.h"
#include "reg_cah.h"
extern FLASH_RECORD *flash_rec;
extern volatile uint16_t msg_id;
extern volatile uint16_t id_x;
extern volatile uint8_t msg_ch;
extern volatile uint8_t crc_h;
extern volatile uint8_t crc_l;
void send_velocity();
void send_angle();
void send_motor_enabled();
void send_motor_enabled();
void send_id();
void firmware_update();
void send_pid_angle(uint8_t param_pid);
// void send_motor_torque();
void send_pid(uint8_t param_pid);
void setup_id(uint8_t my_id);
void setup_angle(float target_angle);
void setup_pid_angle(uint8_t param_pid, float data);
void listen_can(const CAN_message_t &msg);

8
controller/fw/embed/include/reg_cah.h
View file

@ -37,12 +37,4 @@
#define MOTOR_ANGLE 0x72 #define MOTOR_ANGLE 0x72
#define MOTOR_TORQUE 0x73 #define MOTOR_TORQUE 0x73
#define FIRMWARE_UPDATE 0x55
//For send
#define CAN_MSG_MAX_LEN 7
#define CRC_SIZE 2
#define ID_SIZE sizeof(uint8_t)
#endif // REG_CAH_H_ #endif // REG_CAH_H_

32
controller/fw/embed/platformio.ini
View file

@ -1,3 +1,15 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[platformio]
[env:robotroller_reborn] [env:robotroller_reborn]
platform = ststm32 platform = ststm32
board = genericSTM32F446RE board = genericSTM32F446RE
@ -6,19 +18,11 @@ upload_protocol = stlink
debug_tool = stlink debug_tool = stlink
monitor_speed = 19200 monitor_speed = 19200
monitor_parity = N monitor_parity = N
board_upload.offset_address = 0x08008000
board_build.ldscript = ${PROJECT_DIR}/custom_script.ld
build_flags = build_flags =
-D STM32F446xx -DSTM32F446xx
-D HAL_CAN_MODULE_ENABLED -D HAL_CAN_MODULE_ENABLED
-D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH -D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
lib_deps = lib_deps =
askuric/Simple FOC@^2.3.4 askuric/Simple FOC@^2.3.4
pazi88/STM32_CAN@^1.1.2 pazi88/STM32_CAN@^1.1.2
extra_scripts = pre:gen_compile_commands.py
extra_scripts =
pre:gen_compile_commands.py
post:hex_compile.py

75
controller/fw/embed/src/config.cpp
View file

@ -1,75 +0,0 @@
#include "config.h"
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
FLASH_RECORD* pid_data) {
encoder->init(&spi);
/* convert data from flash int value to float*/
conv_float_to_int.i = pid_data[pid_p].value;
float p = conv_float_to_int.f;
conv_float_to_int.i = pid_data[pid_i].value;
float i = conv_float_to_int.f;
conv_float_to_int.i = pid_data[pid_d].value;
float d = conv_float_to_int.f;
// Driver configuration
driver->pwm_frequency = 20000;
driver->voltage_power_supply = 24;
driver->voltage_limit = 24;
driver->init();
// Current sense initialization
current_sense->linkDriver(driver);
current_sense->init();
// Motor configuration
motor->linkSensor(encoder);
motor->linkDriver(driver);
motor->linkCurrentSense(current_sense);
motor->controller = MotionControlType::angle;
motor->torque_controller = TorqueControlType::voltage;
motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
// PID Configuration
motor->PID_velocity.P = 0.5f;
motor->PID_velocity.I = 2.0f;
motor->PID_velocity.D = 0.0f;
motor->LPF_velocity.Tf = 0.01f;
motor->P_angle.P = p;
motor->P_angle.I = i;
motor->P_angle.D = d;
motor->LPF_angle.Tf = 0.02f;
// Motor limits
motor->velocity_limit = 40; // Speed limit in rad/s (382 rpm)
motor->voltage_limit = 24;
motor->current_limit = 0.5;
motor->sensor_direction = Direction::CCW;
motor->init();
motor->initFOC();
}
void foc_step(BLDCMotor *motor) {
if (motor_control_inputs.target_velocity != 0 ||
motor->controller == MotionControlType::velocity) {
if (motor->controller != MotionControlType::velocity) {
motor->controller = MotionControlType::velocity;
}
motor->target = motor_control_inputs.target_velocity;
} else {
if (motor->controller != MotionControlType::angle) {
motor->controller = MotionControlType::angle;
}
motor->target = motor_control_inputs.target_angle;
}
motor->loopFOC();
motor->move();
}

108
controller/fw/embed/src/flash.cpp
View file

@ -2,8 +2,10 @@
#include <stdbool.h> #include <stdbool.h>
#include "hal_conf_extra.h" #include "hal_conf_extra.h"
static uint32_t write_ptr = SECTOR_6;
void flash_unlock(){ void flash_unlock(){
// Check if flash is locked // Check if flash is locked
if(!(FLASH->CR & FLASH_CR_LOCK)) { if(!(FLASH->CR & FLASH_CR_LOCK)) {
return; // Already unlocked return; // Already unlocked
@ -92,12 +94,12 @@ void flash_write(uint32_t addr, FLASH_RECORD* record){
FLASH->CR |= FLASH_CR_PG; FLASH->CR |= FLASH_CR_PG;
for(int i = 0;i < size;i++){ for(int i = 0;i < size;i++){
*(volatile uint32_t*)(addr + (i * 4)) = data[i]; *(volatile uint32_t*)(addr + i) = data[i];
write_ptr++;
} }
// Clear program bit // Clear program bit
FLASH->CR &= ~FLASH_CR_PG; FLASH->CR &= ~FLASH_CR_PG;
write_ptr = addr + (size * 4); //increase variable storing addr
flash_lock(); flash_lock();
} }
@ -113,53 +115,29 @@ uint8_t flash_read_word(uint32_t address){
} }
// Wait if flash // Wait if flash
// bool validata_crc(FLASH_RECORD* crc){ bool validata_crc(FLASH_RECORD* crc){
// return crc->crc == 0x6933? true : false; return crc->crc == 0x6933? true : false;
// } }
uint16_t validate_crc16(uint8_t *data, uint32_t length) { uint16_t validate_crc16(uint8_t *data, uint32_t length) {
uint16_t crc = 0xFFFF; // start value for CRC MODBUS uint16_t crc = 0xFFFF; // Начальное значение для MODBUS
while (length--) { while (length--) {
crc ^= *data++; // XOR crc ^= *data++; // XOR с очередным байтом данных
for (uint8_t i = 0; i < 8; i++) { for (uint8_t i = 0; i < 8; i++) {
if (crc & 0x0001) { if (crc & 0x0001) {
crc = (crc >> 1) ^ 0xA001; // polynome 0x8005 (reverse) crc = (crc >> 1) ^ 0xA001; // Полином 0x8005 (reverse)
} else { } else {
crc >>= 1; crc >>= 1;
} }
} }
} }
return crc; return crc; // Возвращаем вычисленный CRC
} }
uint16_t calc_crc_struct(FLASH_RECORD* res){
uint8_t arr_res[FLASH_RECORD_SIZE - 2];
uint16_t crc_res;
/* sorting data without CRC */
arr_res[0] = res->data_id;
arr_res[1] = res->data_type;
/* from 32 to 8 bit */
for(int i = 0;i < 4;i++)
arr_res[i + 2] = (uint8_t)(res->value >> i * 8);
crc_res = validate_crc16(arr_res,FLASH_RECORD_SIZE - 2);
return crc_res;
}
void disable_flash_protection() {
HAL_FLASH_Unlock();
__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR);
HAL_FLASH_Lock();
}
/* read struct from FLASH */ /* read struct from FLASH */
void flash_read(uint32_t addr,FLASH_RECORD* ptr){ void flash_read(uint32_t addr,FLASH_RECORD* ptr){
disable_flash_protection();
uint8_t* flash_ptr = (uint8_t*)addr; uint8_t* flash_ptr = (uint8_t*)addr;
uint8_t* dest = (uint8_t*)ptr; uint8_t* dest = (uint8_t*)ptr;
for(int i = 0;i < FLASH_RECORD_SIZE;i++) for(int i = 0;i < FLASH_RECORD_SIZE;i++)
@ -171,14 +149,14 @@ void compact_page(){
for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_7;i += FLASH_RECORD_SIZE) { for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_7;i += FLASH_RECORD_SIZE) {
FLASH_RECORD rec; FLASH_RECORD rec;
flash_read(i,&rec); flash_read(i,&rec);
uint16_t calculated_crc = calc_crc_struct(&rec); uint16_t calculated_crc = validate_crc16((uint8_t*)&rec, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC
if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) { if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) {
// if the crc does not match, we check further // Если CRC совпадает и ID параметра валидный, сохраняем последнее значение
latest[rec.data_id] = rec; latest[rec.data_id] = rec;
} }
else else
// if //Если не совпадает продолжить читать флэш
continue; continue;
} }
@ -186,7 +164,7 @@ void compact_page(){
write_ptr = SECTOR_6; // Сброс на начало write_ptr = SECTOR_6; // Сброс на начало
for (int i = 0; i < PARAM_COUNT; i++) { for (int i = 0; i < PARAM_COUNT; i++) {
if (latest[i].data_id != 0xFF) { if (latest[i].data_id != 0xFF) {
// alignment // Выравнивание перед каждой записью
if (write_ptr % 4 != 0) { if (write_ptr % 4 != 0) {
write_ptr += (4 - (write_ptr % 4)); write_ptr += (4 - (write_ptr % 4));
} }
@ -196,56 +174,48 @@ void compact_page(){
} }
} }
void write_param(uint8_t param_id, uint32_t val) {
FLASH_RECORD param_flash; void write_param(uint8_t param_id, uint8_t val) {
// __disable_irq(); // Interrupt off FLASH_RECORD param_flash = {param_id, val};
param_flash.data_id = param_id; // __disable_irq(); // Запрещаем прерывания на время всей операции
param_flash.value = val;
param_flash.data_type = sizeof(uint8_t); param_flash.crc = validate_crc16((uint8_t*)&param_flash,sizeof(param_flash) - 2);//Нахождение CRC для данных, хранящихся во флэш памяти
param_flash.crc = calc_crc_struct(&param_flash);
// check alignment // Проверка выравнивания ДО проверки границ сектора кратного 4
if (write_ptr % 8 != 0) { if (write_ptr % 4 != 0) {
write_ptr += (8 - (write_ptr % 8)); write_ptr += (4 - (write_ptr % 4));
} }
// check buffer overflow // Проверка переполнения с учётом выравнивания
if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) { if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) {
compact_page(); // after compact_page update compact_page(); // После compact_page write_ptr обновляется
// alignment // Повторно выравниваем после функции. То есть сколько не хватает для кратности
if (write_ptr % 8 != 0) { if (write_ptr % 4 != 0) {
write_ptr += (8 - (write_ptr % 8)); write_ptr += (4 - (write_ptr % 4));
} }
} }
flash_write(write_ptr, &param_flash); //inside the function, the write_ptr pointer is automatically incremented by the size of the structure flash_write(write_ptr, &param_flash); //внутри функции итак автоматические инкрементируется указатель write_ptr на размер структуры
// __enable_irq(); // Interrupt on // __enable_irq(); // Разрешаем прерывания
} }
FLASH_RECORD* load_params(){ FLASH_RECORD* load_params(){
__disable_irq(); __disable_irq();
static FLASH_RECORD latest[PARAM_COUNT] = {0}; static FLASH_RECORD latest[PARAM_COUNT] = {0};
FLASH_RECORD res; FLASH_RECORD res;
for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) { for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) {
flash_read(addr,&res); flash_read(addr,&res);
/* провекра CRC */
uint16_t calculated_crc = validate_crc16((uint8_t*)&res, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC
uint16_t calculated_crc = calc_crc_struct(&res); if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT)
if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT) continue; continue;
else{
else{
latest[res.data_id] = res; latest[res.data_id] = res;
write_ptr = addr + FLASH_RECORD_SIZE;
}
} }
write_ptr = addr + FLASH_RECORD_SIZE;
}
__enable_irq(); __enable_irq();
return latest; return latest;
} }

473
controller/fw/embed/src/main.cpp
View file

@ -1,3 +1,4 @@
// clang-format off
#include "Arduino.h" #include "Arduino.h"
#include "stm32f446xx.h" #include "stm32f446xx.h"
#include <SimpleFOC.h> #include <SimpleFOC.h>
@ -5,23 +6,21 @@
#include <AS5045.h> #include <AS5045.h>
#include <DRV8313.h> #include <DRV8313.h>
#include <cstring> #include <cstring>
#include <iostream>
#include <iterator> #include <iterator>
#include "common/base_classes/FOCMotor.h" #include "common/base_classes/FOCMotor.h"
#include "hal_conf_extra.h" #include "hal_conf_extra.h"
#include "wiring_analog.h" #include "wiring_analog.h"
#include "wiring_constants.h" #include "wiring_constants.h"
// clang-format on // clang-format on
#include "reg_cah.h" #include "reg_cah.h"
#include "flash.h" #include "flash.h"
#include "config.h"
#include "process_can.h"
void SysTick_Handler(void) { void SysTick_Handler(void) {
HAL_IncTick(); HAL_IncTick();
} }
STM32_CAN Can(CAN2, DEF); STM32_CAN Can(CAN2, DEF);
/* for FLASH */ /* for FLASH */
uint32_t flash_flag; uint32_t flash_flag;
@ -30,14 +29,18 @@ uint32_t flash_error;
FLASH_EraseInitTypeDef pEraseInit; FLASH_EraseInitTypeDef pEraseInit;
uint32_t SectorError; uint32_t SectorError;
volatile uint16_t msg_id;
volatile uint16_t id_x;
volatile uint8_t msg_ch;
volatile uint8_t crc_h;
volatile uint8_t crc_l;
/* bool for test CAN */ volatile float kt = 0.1; //for torgue calculation
volatile bool CAN_GET = false;
volatile float kt = 0.1; // Torque calculation constant
FLASH_RECORD* flash_rec;
static FLASH_RECORD* flash_rec;
static FLASH_RECORD flash_buf[PARAM_COUNT];
static CAN_message_t CAN_TX_msg;
static CAN_message_t CAN_inMsg;
SPIClass spi; SPIClass spi;
MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK); MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
@ -50,66 +53,430 @@ DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER,
LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1, LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
CURRENT_SENSOR_2, CURRENT_SENSOR_3); CURRENT_SENSOR_2, CURRENT_SENSOR_3);
// Commander command(Serial); Commander command(Serial);
struct MotorControlInputs {
float target_angle = 0.0;
float target_velocity = 0.0;
bool motor_enabled = false;
bool foc_state = false;
};
MotorControlInputs motor_control_inputs; MotorControlInputs motor_control_inputs;
volatile uint16_t msg_id; void doMotor(char *cmd) {
volatile uint16_t id_x; command.motor(&motor, cmd);
volatile uint8_t msg_ch; digitalWrite(PC10, !digitalRead(PC10));
volatile uint8_t crc_h; delayMicroseconds(2);
volatile uint8_t crc_l; }
void setup(){ void CAN2_RX0_IRQHandler() {
SCB->VTOR = (volatile uint32_t)0x08008004; // Пустая функция, но прерывание не приведет к Default Handler
}
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
Commander *commander, CommandCallback callback) {
encoder->init(&spi);
driver->pwm_frequency = 20000;
driver->voltage_power_supply = 24;
driver->voltage_limit = 24;
driver->init();
current_sense->linkDriver(driver);
current_sense->init();
motor->linkSensor(encoder);
motor->linkDriver(driver);
motor->linkCurrentSense(current_sense);
motor->useMonitoring(Serial);
motor->monitor_downsample = 5000; // default 0
motor->controller = MotionControlType::angle;
motor->torque_controller = TorqueControlType::voltage;
motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
// PID start
motor->PID_velocity.P = 0.75;
motor->PID_velocity.I = 20;
motor->LPF_velocity.Tf = 0.005;
motor->P_angle.P = 0.5;
motor->LPF_angle.Tf = 0.001;
// PID end
motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm)
motor->voltage_limit = 24;
motor->current_limit = 0.5;
motor->sensor_direction = Direction::CCW;
motor->init();
motor->initFOC();
}
void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) {
// Создаем сообщение
CAN_message_t msg;
msg.id = id;
msg.len = 8; // или как в протоколе
msg.buf[0] = message_type;
memcpy(&msg.buf[1], data, data_length);
// Формируем массив для CRC, включающий ID и все данные
size_t crc_data_size = sizeof(msg.id) + data_length;
uint8_t crc_data[crc_data_size];
// Копируем ID
memcpy(crc_data, &msg.id, sizeof(msg.id));
// Копируем все байты data
memcpy(crc_data + sizeof(msg.id), data, data_length);
// Расчет CRC
uint16_t crc_value = validate_crc16(crc_data, crc_data_size);
// Вставляем CRC в буфер
msg.buf[6] = crc_value & 0xFF;
msg.buf[7] = (crc_value >> 8) & 0xFF;
// Отправляем
Can.write(msg);
__NOP();
}
void send_velocity() {
float current_velocity = motor.shaftVelocity();
flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL
// Обработка ошибки: запись в лог, сигнализация и т.д.
return;
}
uint8_t value = flash_rec[vel].value;
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'V',&value,sizeof(value));
}
void send_angle() {
float current_angle = motor.shaftAngle();
flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL
// Обработка ошибки: запись в лог, сигнализация и т.д.
return;
}
// uint8_t value = flash_rec[angl].value;
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'A',&current_angle,sizeof(current_angle));
}
void send_motor_enabled() {
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
CAN_TX_msg.id = id;
CAN_TX_msg.buf[0] = 'E';
memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled,
sizeof(motor_control_inputs.motor_enabled));
Can.write(CAN_TX_msg);
}
void send_foc_state() {
/* data for reading of firmware */
flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL
// Обработка ошибки: запись в лог, сигнализация и т.д.
return;
}
uint8_t value = flash_rec[foc_id].value;
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'F',&value,sizeof(value));
}
void send_id() {
/* data for reading of firmware */
flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL
// Обработка ошибки: запись в лог, сигнализация и т.д.
return;
}
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'I',&id,sizeof(id));
__NOP();
}
void send_motor_torque() {
float i_q = motor.current.q; // Ток по оси q (А)
float torque = kt * i_q; // Расчет момента
torque *= 100;
flash_rec = load_params();
CAN_TX_msg.id = flash_rec->value;
CAN_TX_msg.buf[0] = 'T';
CAN_TX_msg.len = 5;
memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
Can.write(CAN_TX_msg);
}
void send_pid(uint8_t param_pid){
flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL
return;
}
uint8_t id = flash_rec[addr_id].value;
uint8_t d = flash_rec[param_pid].value;
uint8_t data_send = 0;
int l = 0;
while(d /= 10)
l++;
if(l >= 2)
data_send = (float)d;
Serial.setRx(HARDWARE_SERIAL_RX_PIN); else if(l == 1)
Serial.setTx(HARDWARE_SERIAL_TX_PIN); data_send = (float)(d * 10);
Serial.begin(115200);
pinMode(PC11, OUTPUT); else
pinMode(PC10,OUTPUT); data_send = (float)(d * 100);
GPIOC->ODR &= ~GPIO_ODR_OD10; if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp;
// Can.enableMBInterrupts(); else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki;
Can.begin(); else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd;
Can.setBaudRate(1000000); send_can_with_id_crc(id,param_pid,&data_send,sizeof(data_send));
// Настройка прерываний CAN }
CAN2->IER |= CAN_IER_FMPIE0;
flash_rec = load_params(); //for update write_ptr void setup_id(uint8_t my_id) {
if(flash_rec[firmw].value == FIRMWARE_FLAG) NVIC_SystemReset(); //if in flash go to the bootloader write_param(addr_id,my_id);
// send_id();
}
void setup_angle(float target_angle) {
// float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы
motor.enable(); // Включаем мотор если он отключен
motor.controller = MotionControlType::angle;
motor.move(target_angle);
}
void setup_pid_angle(uint8_t param_pid, float data){
switch (param_pid)
{
case pid_p:
motor.P_angle.P = data;
break;
case pid_i:
motor.P_angle.I = data;
break;
case pid_d:
motor.P_angle.D = data;
break;
default:
break;
}
uint8_t check = uint8_t(data);
uint8_t data_save = 0;
if(check != 0)
if(check /= 10)
data_save = check;
else
data_save = (uint8_t)(data * 10);
else
data_save = (uint8_t)(data * 100);
write_param(param_pid,data_save);
}
void listen_can(const CAN_message_t &msg) {
msg_id = msg.id;
msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать
id_x = (msg_id >> 4) & 0x7FF; //получение адреса устройства страшие 2 бита msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать
// Initialize FOC system
setup_foc(&encoder, &motor, &driver, &current_sense,flash_rec);
CAN2->IER |= CAN_IER_FMPIE0 | // Сообщение в FIFO0 /* Вычисление CRC */
CAN_IER_FFIE0 | // FIFO0 full // Объединение старшего и младшего байтов для получения полученного CRC
CAN_IER_FOVIE0; // FIFO0 overflow uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
uint8_t data[10] = {0}; //буфер хранения сообщения и расчета его CRC для проверки
// Копируем ID сообщения в буфер данных для расчета CRC 2 байта
memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
// Копируем данные сообщения в буфер (без байтов CRC)
memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
// Рассчитываем CRC для полученных данных
uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
// Проверяем совпадение CRC
if (calculated_crc != received_crc) {
// Несовпадение CRC, игнорируем сообщение
return;
}
// Default motor configuration /* 0x691
GPIOC->ODR |= GPIO_ODR_OD11; //set LED 69 - адрес устройства
1 - что делать дальше с данными */
if(id_x == flash_rec->value){
if(msg_ch == REG_WRITE){
switch(msg.buf[0]) {
case REG_ID:
/* setup new id */
setup_id(msg.buf[1]);
break;
case REG_LED_BLINK:
for (int i = 0; i < 10; i++) {
GPIOC->ODR ^= GPIO_ODR_OD10;
delay(100);
}
break;
case MOTOR_ANGLE:
memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
sizeof(motor_control_inputs.target_angle));
setup_angle(motor_control_inputs.target_angle);
break;
case REG_MOTOR_POSPID_Kp:
setup_pid_angle(pid_p,msg.buf[1]);
break;
case REG_MOTOR_POSPID_Ki:
setup_pid_angle(pid_i,msg.buf[1]);
break;
case REG_MOTOR_POSPID_Kd:
setup_pid_angle(pid_d,msg.buf[1]);
break;
case MOTOR_ENABLED:
if (msg.buf[1] == 1) {
motor.enable();
motor_control_inputs.motor_enabled = 1;
} else {
motor.disable();
motor_control_inputs.motor_enabled = 0;
}
default:
break;
}
}
else if (msg_ch == REG_READ) {
switch (msg.buf[0]) {
case REG_ID:
send_id();
break;
case MOTOR_VELOCITY:
send_velocity();
break;
case MOTOR_ANGLE:
send_angle();
break;
case MOTOR_ENABLED:
send_motor_enabled();
break;
case MOTOR_TORQUE:
send_motor_torque();
break;
case FOC_STATE:
send_foc_state();
break;
case REG_MOTOR_POSPID_Kp:
send_pid(pid_p);
break;
case REG_MOTOR_POSPID_Ki:
send_pid(pid_i);
break;
case REG_MOTOR_POSPID_Kd:
send_pid(pid_d);
break;
default:
break;
}
}
}
}
volatile uint32_t ipsr_value = 0;
void foc_step(BLDCMotor *motor, Commander *commander) {
if (motor_control_inputs.target_velocity != 0 ||
motor->controller == MotionControlType::velocity) {
if (motor->controller != MotionControlType::velocity) {
motor->controller = MotionControlType::velocity;
}
motor->target = motor_control_inputs.target_velocity;
} else {
if (motor->controller != MotionControlType::angle) {
motor->controller = MotionControlType::angle;
}
motor->target = motor_control_inputs.target_angle;
}
motor->loopFOC();
motor->move();
motor->monitor();
commander->run();
}
void setup(){
/* bias for vector int */
// __set_MSP(*(volatile uint32_t*)0x08008000);
// SCB->VTOR = (volatile uint32_t)0x08008000;
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
Serial.begin(115200);
pinMode(PC11, OUTPUT);
pinMode(PC10,OUTPUT);
GPIOC->ODR &= ~GPIO_ODR_OD10;
// Setup thermal sensor pin
// pinMode(TH1, INPUT_ANALOG);
Can.begin();
Can.setBaudRate(1000000);
TIM_TypeDef *Instance = TIM2;
HardwareTimer *SendTimer = new HardwareTimer(Instance);
// SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
// SendTimer->attachInterrupt(send_data);
// SendTimer->resume();
flash_rec = load_params();
for(int i = 0;i < PARAM_COUNT;i++)
flash_buf[i] = flash_rec[i];
setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
GPIOC->ODR |= GPIO_ODR_OD11;
motor.torque_controller = TorqueControlType::foc_current; motor.torque_controller = TorqueControlType::foc_current;
motor.controller = MotionControlType::torque; motor.controller = MotionControlType::torque;
__enable_irq(); __enable_irq();
}
}
void loop() { void loop() {
__enable_irq(); foc_step(&motor, &command);
foc_step(&motor);
CAN_message_t msg; CAN_message_t msg;
GPIOC->ODR ^= GPIO_ODR_OD11;
// Process incoming CAN messages delay(500);
while (Can.read(msg)) { while (Can.read(msg)) {
listen_can(msg); listen_can(msg);
CAN_GET = true;
}
/* If receive data from CAN */
if(CAN_GET) {
CAN_GET = false;
} }
} }

245
controller/fw/embed/src/process_can.cpp
View file

@ -1,245 +0,0 @@
#include "process_can.h"
static CAN_message_t CAN_TX_msg;
static CAN_message_t CAN_inMsg;
template <typename T>
void send_can_with_id_crc(uint8_t id, uint8_t message_type, T* data) {
// Create CAN message
CAN_message_t msg_l;
msg_l.id = id;
// msg_l.len = 8; // Protocol-defined message length
memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
memcpy(&msg_l.buf[1], data, sizeof(T));
// Prepare CRC calculation buffer (ID + data)
uint8_t crc_data[CAN_MSG_MAX_LEN];
// Copy message ID
memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
// Copy all data bytes
memcpy(crc_data + 1, msg_l.buf, 6);
// Calculate CRC
uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
// Insert CRC into buffer
// memcpy(&msg_l.buf[6], &crc_value, sizeof(uint16_t));
msg_l.buf[6] = crc_value & 0xFF;
msg_l.buf[7] = (crc_value >> 8) & 0xFF;
// Send message
Can.write(msg_l);
}
void send_velocity() {
float current_velocity = motor.shaftVelocity();
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
return;
}
float value = flash_rec[vel].value;
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'V',&value);
}
void send_angle() {
float current_angle = motor.shaftAngle();
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
return;
}
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'A',&current_angle);
}
void send_motor_enabled() {
/* Firmware data reading */
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
return;
}
uint8_t value = motor_control_inputs.motor_enabled; //copy current motor state
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'M',&value);
}
void send_id() {
/* Firmware data reading */
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
return;
}
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'I',&id);
}
// void send_motor_torque() {
// float i_q = motor.current.q; // Q-axis current (A)
// float torque = kt * i_q; // Torque calculation
// torque *= 100;
// CAN_TX_msg.id = flash_rec->value;
// CAN_TX_msg.buf[0] = 'T';
// CAN_TX_msg.len = 5;
// memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
// Can.write(CAN_TX_msg);
// }
void send_pid_angle(uint8_t param_pid){
if (flash_rec == nullptr) { // Null check
return;
}
uint8_t id = flash_rec[addr_id].value;
conv_float_to_int.i = flash_rec[param_pid].value;
uint32_t data = conv_float_to_int.i;
switch(param_pid){
case pid_p:
param_pid = REG_MOTOR_POSPID_Kp;
break;
case pid_i:
param_pid = REG_MOTOR_POSPID_Ki;
break;
case pid_d:
param_pid = REG_MOTOR_POSPID_Kd;
break;
}
send_can_with_id_crc(id,param_pid,&data);
}
void setup_id(uint8_t my_id) {
write_param(addr_id,my_id);
}
void firmware_update(){
write_param(firmw,FIRMWARE_FLAG);
NVIC_SystemReset();
}
void setup_angle(float target_angle) {
motor.enable(); // Enable motor if disabled
// motor.controller = MotionControlType::angle;
motor_control_inputs.target_angle = target_angle;
// motor.move(target_angle);
}
// void setup_pid_angle(uint8_t param_pid, uint32_t data){
// conv_float_to_int.f = data;
// switch (param_pid) {
// case pid_p:
// motor.P_angle.P = conv_float_to_int.f;
// break;
// case pid_i:
// motor.P_angle.I = conv_float_to_int.f;
// break;
// case pid_d:
// motor.P_angle.D = conv_float_to_int.f;
// break;
// default:
// break;
// }
// write_param(param_pid,data);
// }
void listen_can(const CAN_message_t &msg) {
msg_id = msg.id;
msg_ch = msg_id & 0xF; // Extract message channel
uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address
/* CRC Calculation */
uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
uint8_t data[10] = {0}; // Message buffer for CRC verification
// Copy message ID (2 bytes)
memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
// Copy message data (excluding CRC bytes)
memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
// Calculate CRC
uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
// Verify CRC match
if (calculated_crc != received_crc) {
return; // Ignore message on CRC mismatch
}
flash_rec = load_params();
/* Message Structure: 0x691
69 - Device address
1 - Action code */
if(id_x == flash_rec[addr_id].value){
if(msg_ch == REG_WRITE){
switch(msg.buf[0]) {
case REG_ID:
setup_id(msg.buf[1]);
break;
case REG_LED_BLINK:
for (int i = 0; i < 10; i++) {
GPIOC->ODR ^= GPIO_ODR_OD10;
delay(100);
}
break;
case MOTOR_ANGLE:
memcpy(&motor_control_inputs.target_angle, &msg.buf[1],
sizeof(motor_control_inputs.target_angle));
setup_angle(motor_control_inputs.target_angle);
break;
case REG_MOTOR_POSPID_Kp:
memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float));
conv_float_to_int.f = motor.P_angle.P;
write_param(pid_p,conv_float_to_int.i);
break;
case REG_MOTOR_POSPID_Ki:
memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float));
conv_float_to_int.f = motor.P_angle.I;
write_param(pid_i,conv_float_to_int.i);
break;
case REG_MOTOR_POSPID_Kd:
memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float));
conv_float_to_int.f = motor.P_angle.D;
write_param(pid_d,conv_float_to_int.i);
break;
case FIRMWARE_UPDATE:
firmware_update();
break;
case MOTOR_ENABLED:
if (msg.buf[1] == 1) {
motor.enable();
motor_control_inputs.motor_enabled = 1;
} else {
motor.disable();
motor_control_inputs.motor_enabled = 0;
}
default:
break;
}
}
else if (msg_ch == REG_READ) {
switch (msg.buf[0]) {
case REG_ID: send_id(); break;
case MOTOR_VELOCITY: send_velocity(); break;
case MOTOR_ANGLE: send_angle(); break;
case MOTOR_ENABLED: send_motor_enabled(); break;
// case MOTOR_TORQUE: send_motor_torque(); break;
// case FOC_STATE: send_foc_state(); break;
case REG_MOTOR_POSPID_Kp: send_pid_angle(pid_p); break;
case REG_MOTOR_POSPID_Ki: send_pid_angle(pid_i); break;
case REG_MOTOR_POSPID_Kd: send_pid_angle(pid_d); break;
default: break;
}
}
}
}

141
controller/fw/embed/test/firmware_can.py
View file

@ -1,141 +0,0 @@
import can
import sys
import time
from intelhex import IntelHex
# Конфигурация
CAN_CHANNEL = 'socketcan'
CAN_INTERFACE = 'can0'
CAN_BITRATE = 1000000
#ch =int(input("Введите id устройства:"))
ch = int(sys.argv[2])
BOOT_CAN_ID = (ch * 16) + 1
DATA_CAN_ID = (ch * 16) + 3
BOOT_CAN_END = (ch * 16) + 2
ACK_CAN_ID = 0x05
#конфиг для crc16 ibm
def debug_print(msg):
print(f"[DEBUG] {msg}")
def calculate_crc16(data: bytes) -> int:
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_firmware(hex_file):
try:
debug_print("Инициализация CAN...")
bus = can.interface.Bus(
channel=CAN_INTERFACE,
bustype=CAN_CHANNEL,
bitrate=CAN_BITRATE
)
debug_print("Чтение HEX-файла...")
ih = IntelHex(hex_file)
binary_data = ih.tobinstr() # Исправлено на tobinstr()
fw_size = len(binary_data)
debug_print(f"Размер прошивки: {fw_size} байт")
# Расчет CRC
debug_print("Расчёт CRC...")
# calculator = Calculator(Crc16.IBM)
fw_crc = calculate_crc16(binary_data)
debug_print(f"CRC: 0x{fw_crc:04X}")
# Отправка START
start_data = bytearray([0x01])
start_data += fw_size.to_bytes(4, 'little')
start_data += fw_crc.to_bytes(2, 'little')
debug_print(f"START: {list(start_data)}")
start_msg = can.Message(
arbitration_id=BOOT_CAN_ID,
data=bytes(start_data),
is_extended_id=False
)
try:
bus.send(start_msg)
except can.CanError as e:
debug_print(f"Ошибка отправки START: {str(e)}")
return
# Ожидание ACK
debug_print("Ожидание ACK...")
ack = wait_for_ack(bus)
if not ack:
debug_print("Таймаут ACK START")
return
debug_print(f"Получен ACK: {list(ack.data)}")
# Отправка данных
packet_size = 8
for i in range(0, len(binary_data), packet_size):
chunk = binary_data[i:i+packet_size]
# Дополнение до 8 байт
if len(chunk) < 8:
chunk += b'\xFF' * (8 - len(chunk))
debug_print(f"Пакет {i//8}: {list(chunk)}")
data_msg = can.Message(
arbitration_id=DATA_CAN_ID,
data=chunk,
is_extended_id=False
)
try:
bus.send(data_msg)
except can.CanError as e:
debug_print(f"Ошибка отправки данных: {str(e)}")
return
ack = wait_for_ack(bus)
if not ack:
debug_print("Таймаут ACK DATA")
return
# Финал
debug_print("Отправка FINISH...")
finish_msg = can.Message(
arbitration_id=BOOT_CAN_END,
data=bytes([0xAA]),
is_extended_id=False
)
bus.send(finish_msg)
ack = wait_for_ack(bus, timeout=1.0)
if ack and ack.data[0] == 0xAA:
debug_print("Прошивка подтверждена!")
else:
debug_print("Ошибка верификации!")
except Exception as e:
debug_print(f"Критическая ошибка: {str(e)}")
finally:
bus.shutdown()
def wait_for_ack(bus, timeout=1.0):
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1) # Неблокирующий режим
if msg and msg.arbitration_id == ACK_CAN_ID:
return msg
return None
if __name__ == "__main__":
import sys
if len(sys.argv) != 3:
print("Использование: sudo python3 can_flasher.py firmware.hex")
sys.exit(1)
send_firmware(sys.argv[1])

70
controller/fw/embed/test/firmware_update_flag.py
View file

@ -1,70 +0,0 @@
import can
import time
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства (по умолчанию)
REG_WRITE = 0x8 # Код команды чтения
REG_ID = 0x55 # Адрес регистра с Firmware Update
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def validate_crc16(data):
"""Расчет CRC16 (MODBUS) для проверки целостности данных"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
# Инициализация CAN-интерфейса
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# ======= 1. Запрос текущего ID устройства =======
# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
can_id_read = (OLD_DEVICE_ID << 4) | REG_WRITE
# Данные для запроса: [регистр, резервный байт]
data_read = [REG_ID, 0x00]
# Формируем полные данные для расчета CRC:
# - CAN ID разбивается на 2 байта (little-endian)
# - Добавляем данные запроса
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
# Рассчитываем CRC и разбиваем на байты (little-endian)
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))
# Собираем итоговый пакет: данные + CRC
packet_read = data_read + crc_bytes
print("Переход в boot режим", packet_read)
send_can_message(bus, can_id_read, packet_read)
bus.shutdown()
if __name__ == "__main__":
import sys
if len(sys.argv) != 2:
print("Использование: python3 firmware_test.py address")
sys.exit(1)

103
controller/fw/embed/test/pid_p.py
View file

@ -1,103 +0,0 @@
import can
import time
import struct
# Конфигурация
CAN_INTERFACE = 'can0'
OLD_DEVICE_ID = 0x00 # Текущий ID устройства (по умолчанию)
REG_READ = 0x7 # Код команды чтения
REG_ID = 0x30 # Адрес регистра с REG_PMOTOR_POSPID_Kp устройства
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def receive_response(bus, timeout=1.0):
"""Ожидание ответа от устройства"""
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1)
if msg:
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
return msg
print("[Ошибка] Таймаут")
return None
def validate_crc16(data):
"""Расчет CRC16 (MODBUS) для проверки целостности данных"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
# Инициализация CAN-интерфейса
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# ======= 1. Запрос текущего ID устройства =======
# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
# Данные для запроса: [регистр, резервный байт]
data_read = [REG_ID, 0x00]
# Формируем полные данные для расчета CRC:
# - CAN ID разбивается на 2 байта (little-endian)
# - Добавляем данные запроса
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
# Рассчитываем CRC и разбиваем на байты (little-endian)
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))
# Собираем итоговый пакет: данные + CRC
packet_read = data_read + crc_bytes
print("Запрос на чтение ID:", packet_read)
send_can_message(bus, can_id_read, packet_read)
# ======= 2. Получение и проверка ответа =======
response = receive_response(bus)
if response:
data = response.data
if len(data) < 4:
print("Слишком короткий ответ")
# Проверяем минимальную длину ответа (данные + CRC)
else:
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
#buff with id and data without CRC
full_data = list(id_bytes) + list(data[:-2])
print(f"Received full_data: {list(full_data)}")
received_crc = int.from_bytes(data[-2:], byteorder='little')
#calc CRC
calc_crc = validate_crc16(full_data)
print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}")
if received_crc == calc_crc:
# Если CRC совпадает, проверяем структуру ответа:
kp_value = struct.unpack('<f', bytes(data[1:5]))[0]
print(f"Текущий Kp устройства: {kp_value:.3f}")
else:
print("Ошибка: CRC не совпадает")
else:
print("Устройство не ответило")
# Завершаем работу с шиной
bus.shutdown()

47
controller/fw/embed/test/python_can.py Normal file
View file

@ -0,0 +1,47 @@
import can
import struct
import time
def process_can_message(msg):
if msg.dlc == 5: # Check the message length
print(f"Received message with ID: {msg.arbitration_id}")
print(f"Data: {msg.data}")
# The first byte determines the data type (flag)
flag = chr(msg.data[0])
if flag == 'A': # Angle
angle_bytes = msg.data[1:5]
angle = struct.unpack('<f', bytes(angle_bytes))[0]
print(f"Angle: {angle} degrees")
elif flag == 'V': # Velocity
velocity_bytes = msg.data[1:5]
velocity = struct.unpack('<f', bytes(velocity_bytes))[0]
print(f"Velocity: {velocity} rad/s")
elif flag == 'E' and msg.dlc >= 2: # Enable/Disable
enabled = msg.data[1] # Expecting 1 byte (0 or 1)
print(f"Enabled: {bool(enabled)}")
else:
print(f"Unknown flag: {flag}")
else:
print(f"Received message with unexpected length: {msg.dlc}")
def receive_can_messages():
try:
# Connect to the CAN bus
bus = can.interface.Bus(channel='can0', bustype='socketcan')
print("Waiting for messages on the CAN bus...")
while True:
msg = bus.recv()
if msg:
process_can_message(msg)
except KeyboardInterrupt:
print("\nExiting program...")
except Exception as e:
print(f"Error: {e}")
if __name__ == '__main__':
receive_can_messages()

0
controller/fw/embed/test/enable_motor.py → controller/fw/embed/test/python_enable_motor.py
View file

37
controller/fw/embed/test/python_send_angle.py Normal file
View file

@ -0,0 +1,37 @@
import can
import struct
import time
import argparse
# Function to send the target angle
def send_target_angle(bus, target_angle):
msg = can.Message()
msg.arbitration_id = 1 # Message ID
msg.is_extended_id = False
msg.dlc = 5 # Message length
msg.data = [ord('A')] + list(struct.pack('<f', target_angle)) # 'A' for the command identifier, followed by the angle in float format
try:
bus.send(msg)
print(f"Sent message with target angle: {target_angle} degrees")
print(f"Message data: {msg.data}")
except can.CanError:
print("Message failed to send")
# Main function
def main():
parser = argparse.ArgumentParser(description="Send target angles over CAN bus.")
parser.add_argument("--angle", type=float, required=True, help="Target angle to send over the CAN bus")
args = parser.parse_args()
target_angle = args.angle
# CAN interface setup
bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
print("CAN bus initialized, sending target angles...")
# Loop to send messages
send_target_angle(bus, target_angle)
if __name__ == '__main__':
main()

0
controller/fw/embed/test/send_velocity.py → controller/fw/embed/test/python_send_velocity.py
View file

126
controller/fw/embed/test/readPID_angle_parametrs.py
View file

@ -1,126 +0,0 @@
import can
import time
import struct
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = int(sys.argv[1]) # ID ADDR for servo
REG_READ = 0x7 # Код команды чтения
REG_MOTOR_POSPID_Kp = 0x30
REG_MOTOR_POSPID_Ki = 0x31
REG_MOTOR_POSPID_Kd = 0x32
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def validate_crc16(data):
"""Расчет CRC16 (MODBUS)"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_read_request(bus, device_id, register):
"""Отправка запроса на чтение регистра"""
can_id = (device_id << 4) | REG_READ
data_part = [register, 0x00]
# Расчет CRC для CAN ID (2 байта) + данные
full_data_for_crc = list(can_id.to_bytes(2, 'little')) + data_part
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))
# Формирование итогового пакета
packet = data_part + crc_bytes
send_can_message(bus, can_id, packet)
def receive_pid_response(bus, timeout=1.0):
"""Получение и проверка ответа с PID-значением"""
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1)
if msg and msg.arbitration_id == DEVICE_ID:
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
if len(msg.data) < 8:
print("Ошибка: Слишком короткий ответ")
return None
# Извлечение данных и CRC
data = msg.data
received_crc = int.from_bytes(data[-2:], byteorder='little')
# Подготовка данных для проверки CRC
id_bytes = msg.arbitration_id.to_bytes(1, 'little')
full_data = list(id_bytes) + list(data[:-2])
# Проверка CRC
calc_crc = validate_crc16(full_data)
if calc_crc != received_crc:
print(f"Ошибка CRC: ожидалось 0x{calc_crc:04X}, получено 0x{received_crc:04X}")
return None
# Извлечение float значения
try:
value = struct.unpack('<f', bytes(data[1:5]))[0]
return value
except struct.error:
print("Ошибка распаковки float")
return None
print("Таймаут ожидания ответа")
return None
def main():
"""Основная логика чтения PID-коэффициентов"""
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
try:
# Чтение коэффициентов с задержкой
print("\nЧтение Kp...")
send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Kp)
kp = receive_pid_response(bus)
if kp is not None:
print(f"Текущий Kp: {kp:.3f}")
time.sleep(1)
print("\nЧтение Ki...")
send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Ki)
ki = receive_pid_response(bus)
if ki is not None:
print(f"Текущий Ki: {ki:.3f}")
time.sleep(1)
print("\nЧтение Kd...")
send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Kd)
kd = receive_pid_response(bus)
if kd is not None:
print(f"Текущий Kd: {kd:.3f}")
finally:
bus.shutdown()
if __name__ == "__main__":
if len(sys.argv) != 2:
print("Используйте python3 read_pid.py addr")
sys.exit(1)
main()

98
controller/fw/embed/test/read_angle.py
View file

@ -1,98 +0,0 @@
import can
import struct
import time
import argparse
# Константы
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x27 # ID ADDR for servo
REG_WRITE = 0x7
REG_POS = 0x72 # MOTOR+ANGLE = 0x72
def validate_crc16(data):
# Calculate CRC16
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def receive_response(bus, timeout=1.0):
"""Ожидание ответа от устройства"""
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1)
if msg:
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
return msg
print("[Ошибка] Таймаут")
return None
def send_target_angle(bus):
# ID and cmd
arbitration_id = (DEVICE_ID << 4) | REG_WRITE
id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
# cmd + parametrs
data_write = [REG_POS]
full_data_for_crc = id_bytes + data_write
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
# Full packet
packet = data_write + crc_bytes
msg = can.Message(
arbitration_id=arbitration_id,
is_extended_id=False,
data=packet
)
bus.send(msg)
response = receive_response(bus)
if response:
data = response.data
if len(data) < 4:
print("Слишком короткий ответ")
# Проверяем минимальную длину ответа (данные + CRC)
else:
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
#buff with id and data without CRC
full_data = list(id_bytes) + list(data[:-2])
print(f"Received full_data: {list(full_data)}")
received_crc = int.from_bytes(data[-2:], byteorder='little')
#calc CRC
calc_crc = validate_crc16(full_data)
print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}")
if received_crc == calc_crc:
# Если CRC совпадает, проверяем структуру ответа:
velocity = struct.unpack('<f', bytes(data[1:5]))[0]
print(f"Угол: {velocity}")
else:
print("Ошибка: CRC не совпадает")
else:
print("Устройство не ответило")
def main():
# Инициализация CAN
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
print("CAN шина инициализирована.")
send_target_angle(bus)
bus.shutdown()
if __name__ == '__main__':
main()

108
controller/fw/embed/test/read_id.py
View file

@ -1,108 +0,0 @@
import can
import time
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства (по умолчанию)
REG_READ = 0x7 # Код команды чтения
REG_ID = 0x01 # Адрес регистра с ID устройства
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def receive_response(bus, timeout=1.0):
"""Ожидание ответа от устройства"""
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1)
if msg:
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
return msg
print("[Ошибка] Таймаут")
return None
def validate_crc16(data):
"""Расчет CRC16 (MODBUS) для проверки целостности данных"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
# Инициализация CAN-интерфейса
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# ======= 1. Запрос текущего ID устройства =======
# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
# Данные для запроса: [регистр, резервный байт]
data_read = [REG_ID, 0x00]
# Формируем полные данные для расчета CRC:
# - CAN ID разбивается на 2 байта (little-endian)
# - Добавляем данные запроса
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
# Рассчитываем CRC и разбиваем на байты (little-endian)
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))
# Собираем итоговый пакет: данные + CRC
packet_read = data_read + crc_bytes
print("Запрос на чтение ID:", packet_read)
send_can_message(bus, can_id_read, packet_read)
# ======= 2. Получение и проверка ответа =======
response = receive_response(bus)
if response:
data = response.data
if len(data) < 4:
print("Слишком короткий ответ")
# Проверяем минимальную длину ответа (данные + CRC)
else:
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
#buff with id and data without CRC
full_data = list(id_bytes) + list(data[:-2])
print(f"Received full_data: {list(full_data)}")
received_crc = int.from_bytes(data[-2:], byteorder='little')
#calc CRC
calc_crc = validate_crc16(full_data)
print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}")
if received_crc == calc_crc:
# Если CRC совпадает, проверяем структуру ответа:
print(f"Текущий ID устройства: 0x{data[1]:02X}")
else:
print("Ошибка: CRC не совпадает")
else:
print("Устройство не ответило")
# Завершаем работу с шиной
bus.shutdown()
if __name__ == "__main__":
import sys
if len(sys.argv) != 2:
print("Использование: python3 can_flasher.py address")
sys.exit(1)

67
controller/fw/embed/test/send_angle.py
View file

@ -1,67 +0,0 @@
from can.interface import Bus
import can
import struct
import time
import argparse
# Константы
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x27 # ID ADDR for servo
REG_WRITE = 0x8
REG_POS = 0x72 # MOTOR+ANGLE = 0x72
def validate_crc16(data):
# Calculate CRC16
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_target_angle(bus, target_angle):
# ID and cmd
arbitration_id = (DEVICE_ID << 4) | REG_WRITE
id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
# cmd + parametrs
data_write = [REG_POS] + list(struct.pack('<f', target_angle))
full_data_for_crc = id_bytes + data_write
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
# Full packet
packet = data_write + crc_bytes
msg = can.Message(
arbitration_id=arbitration_id,
is_extended_id=False,
data=packet
)
try:
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{arbitration_id:03X}, Угол: {target_angle} rad, Данные: {list(msg.data)}")
except can.CanError:
print("Ошибка отправки сообщения")
def main():
parser = argparse.ArgumentParser(description="Отправка угла позиции по CAN.")
parser.add_argument("--angle", type=float, required=True, help="Угол (в градусах)")
args = parser.parse_args()
# Инициализация CAN
bus = Bus(channel=CAN_INTERFACE, bustype='socketcan')
print("CAN шина инициализирована.")
send_target_angle(bus, args.angle)
bus.shutdown()
if __name__ == '__main__':
main()

124
controller/fw/embed/test/set_id.py
View file

@ -1,124 +0,0 @@
import can
import time
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
OLD_DEVICE_ID = int(sys.argv[1])
NEW_DEVICE_ID = int(sys.argv[2])
REG_WRITE = 0x8
REG_READ = 0x7
REG_ID = 0x1
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def receive_response(bus, timeout=1.0):
"""Ожидание ответа"""
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1)
if msg:
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
return msg
print("[Ошибка] Таймаут")
return None
def validate_crc16(data):
"""Функция расчета CRC16 (MODBUS)"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
# Инициализация
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# ======= 1. Отправляем команду изменить ID =======
# Весь буфер: id + команда + параметры
OLD_WITH_REG = (OLD_DEVICE_ID << 4) | REG_WRITE
id_bytes = list(OLD_WITH_REG.to_bytes(2, byteorder='little'))
# Важные части сообщения: address (id), команда, параметры
data_write = [REG_ID, NEW_DEVICE_ID] # команда изменить ID
# Полностью собираем массив для CRC (включая id и команду)
full_data_for_crc = id_bytes + data_write
# Расчет CRC по всему пакету
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
# Итоговый пакет: команда + параметры + CRC
packet_write = data_write + crc_bytes
print("Отправляем: команда изменить ID + CRC:", packet_write)
# Отправляем с `OLD_DEVICE_ID` в качестве адреса
send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write)
time.sleep(1.0)
# ======= 2. Запрашиваем текущий ID (используем новый адрес) =======
# Теперь для запроса используем **уже новый id**
NEW_WITH_REG = (NEW_DEVICE_ID << 4) | REG_READ
current_id_bytes = list(NEW_WITH_REG.to_bytes(2, byteorder='little'))
data_read = [REG_ID, 0x00]
full_data_for_crc = current_id_bytes + data_read
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
packet_read = data_read + crc_bytes
print("Запрос на чтение ID + CRC (после смены):", packet_read)
send_can_message(bus, (NEW_DEVICE_ID << 4) | REG_READ, packet_read)
# ======= 3. Получение и проверка ответа =======
response = receive_response(bus)
if response:
data = response.data
if len(data) < 4:
print("Ответ слишком короткий")
else:
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
#buff with id and data without CRC
full_data = list(id_bytes) + list(data[:-2])
print(f"Received full_data: {list(full_data)}")
received_crc = int.from_bytes(data[-2:], byteorder='little')
#calc CRC
calc_crc = validate_crc16(full_data)
if received_crc == calc_crc:
if data[0] == ord('I') and data[1] == NEW_DEVICE_ID:
print(f"\nУСПЕХ! ID устройства изменен на 0x{NEW_DEVICE_ID:02X}")
else:
print(f"Некорректный ответ: {list(data)}")
else:
print("CRC не совпадает, данные повреждены.")
else:
print("Нет ответа от устройства.")
bus.shutdown()
if __name__ == "__main__":
import sys
if len(sys.argv) != 3:
print("Использование: python3 can_flasher.py old_addr new addr")
sys.exit(1)

78
controller/fw/embed/test/st-link.py
View file

@ -1,78 +0,0 @@
import subprocess
import os
import sys
def flash_hex_with_stlink(hex_file_path):
if not os.path.isfile(hex_file_path):
print(f"❌ Файл не найден: {hex_file_path}")
return False
command = [
"st-flash",
"--format", "ihex",
"write",
hex_file_path
]
try:
print(f"⚡️ Прошиваем {hex_file_path} через ST-Link...")
result = subprocess.run(
command,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
universal_newlines=True,
timeout=30
)
print("▬▬▬ STDOUT ▬▬▬")
print(result.stdout)
print("▬▬▬ STDERR ▬▬▬")
print(result.stderr)
if result.returncode == 0:
print("✅ Прошивка успешно завершена!")
# Добавленный блок сброса
try:
print("🔄 Выполняем сброс устройства...")
reset_result = subprocess.run(
["st-info", "--reset"],
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
universal_newlines=True,
timeout=10
)
if reset_result.returncode == 0:
print("♻️ Устройство успешно сброшено!")
else:
print(f"⚠️ Ошибка (код: {reset_result.returncode})")
print("▬▬▬ STDERR сброса ▬▬▬")
print(reset_result.stderr)
except Exception as e:
print(f"⚠️ Ошибка при сбросе: {str(e)}")
return True
else:
print(f"❌ Ошибка прошивки (код: {result.returncode})")
return False
except FileNotFoundError:
print("❌ st-flash не найден! Установите stlink-tools.")
return False
except subprocess.TimeoutExpired:
print("❌ Таймаут операции! Проверьте подключение ST-Link.")
return False
except Exception as e:
print(f"❌ Неизвестная ошибка: {str(e)}")
return False
if __name__ == "__main__":
if len(sys.argv) != 2:
print("Использование: python stlink_flash.py <firmware.hex>")
sys.exit(1)
if flash_hex_with_stlink(sys.argv[1]):
sys.exit(0)
else:
sys.exit(1)

100
controller/fw/embed/test/st-link_full.py
View file

@ -1,100 +0,0 @@
import subprocess
import os
import sys
import time
def flash_hex_with_stlink(hex_file_path, component_name):
if not os.path.isfile(hex_file_path):
print(f"❌ Файл {component_name} не найден: {hex_file_path}")
return False
command = [
"st-flash",
"--format", "ihex",
"write",
hex_file_path
]
try:
print(f"⚡️ Прошиваем {component_name} ({hex_file_path}) через ST-Link...")
result = subprocess.run(
command,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
universal_newlines=True,
timeout=30
)
print("▬▬▬ STDOUT ▬▬▬")
print(result.stdout)
print("▬▬▬ STDERR ▬▬▬")
print(result.stderr)
if result.returncode == 0:
print(f"✅ {component_name} успешно прошит!")
return True
else:
print(f"❌ Ошибка прошивки {component_name} (код: {result.returncode})")
return False
except FileNotFoundError:
print("❌ st-flash не найден! Установите stlink-tools.")
return False
except subprocess.TimeoutExpired:
print(f"❌ Таймаут операции при прошивке {component_name}! Проверьте подключение ST-Link.")
return False
except Exception as e:
print(f"❌ Неизвестная ошибка при прошивке {component_name}: {str(e)}")
return False
def reset_device():
try:
print("🔄 Выполняем сброс(перезагрузку) устройства...")
reset_result = subprocess.run(
["st-info", "--reset"],
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
universal_newlines=True,
timeout=10
)
if reset_result.returncode == 0:
print("♻️ Устройство успешно сброшено!")
return True
else:
print(f"⚠️ Ошибка при сбросе (код: {reset_result.returncode})")
print("▬▬▬ STDERR сброса ▬▬▬")
print(reset_result.stderr)
return False
except Exception as e:
print(f"⚠️ Ошибка при сбросе: {str(e)}")
return False
if __name__ == "__main__":
if len(sys.argv) != 3:
print("Использование: python stlink_flash.py <bootloader.hex> <application.hex>")
print("Пример: python stlink_flash.py bootloader.hex firmware.hex")
sys.exit(1)
bootloader_path = sys.argv[1]
app_path = sys.argv[2]
# Прошиваем сначала бутлоадер
if not flash_hex_with_stlink(bootloader_path, "Bootloader"):
print("\n💥 Ошибка прошивки бутлоадера!")
sys.exit(1)
# Сбрасываем устройство после прошивки бутлоадера
reset_device()
time.sleep(1) # Короткая пауза
# Прошиваем основное приложение
if not flash_hex_with_stlink(app_path, "Application"):
print("\n💥 Ошибка прошивки основного приложения!")
sys.exit(1)
# Финальный сброс устройства
reset_device()
print("\n🎉 Все компоненты успешно прошиты!")
sys.exit(0)

95
controller/fw/embed/test/writePID_angle_parametrs.py
View file

@ -1,95 +0,0 @@
import can
import time
import struct
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = int(sys.argv[1]) # ID ADDR for servo
REG_WRITE = 0x8 # Код команды записи
REG_MOTOR_POSPID_Kp = 0x30
REG_MOTOR_POSPID_Ki = 0x31
REG_MOTOR_POSPID_Kd = 0x32
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def validate_crc16(data):
"""Расчет CRC16 (MODBUS) для проверки целостности данных"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_pid_value(bus, device_id, reg, value):
"""Отправка коэффициента PID на устройство"""
# Формируем CAN ID для записи: (device_id << 4) | REG_WRITE
can_id_write = (device_id << 4) | REG_WRITE
# Упаковываем значение в байты (little-endian)
float_bytes = struct.pack('<f', value)
# Формируем часть данных (регистр + значение)
data_part = [reg] + list(float_bytes)
# Полные данные для расчета CRC: CAN ID + данные
full_data_for_crc = list(can_id_write.to_bytes(2, 'little')) + data_part
# Рассчитываем CRC и разбиваем на байты (little-endian)
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))
# Собираем итоговый пакет данных
can_data = data_part + crc_bytes
# Отправляем сообщение
send_can_message(bus, can_id_write, can_data)
def main():
# Запрос коэффициентов у пользователя
try:
p = float(input("Введите коэффициент P: "))
i = float(input("Введите коэффициент I: "))
d = float(input("Введите коэффициент D: "))
except ValueError:
print("Ошибка: Введите числовые значения.")
return
# Инициализация CAN-интерфейса
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
try:
# Отправка коэффициентов с задержкой
send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Kp, p)
time.sleep(1)
send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Ki, i)
time.sleep(1)
send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Kd, d)
finally:
# Завершение работы с шиной
bus.shutdown()
if __name__ == "__main__":
if len(sys.argv) != 2:
print("Используйте python3 pid_set.py addr")
sys.exit(1)
main()

122
controller/fw/embed/test/write_pidP.py
View file

@ -1,122 +0,0 @@
import can
import time
import struct
# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x00
SET_PID_P = 3.6
REG_WRITE = 0x8
REG_READ = 0x7
REG_ID = 0x30 #REG_MOTOR_POSPID_Kp
PID_P = 0x01
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def receive_response(bus, timeout=1.0):
print("Ожидание ответа")
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1)
if msg:
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
return msg
print("[Ошибка] Таймаут")
return None
def validate_crc16(data):
"""Функция расчета CRC16 (MODBUS)"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
# Инициализация
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# Перевод float -> hex -> int
result = (struct.unpack('<I',struct.pack('<f', float(SET_PID_P)))[0])
result_bytes = result.to_bytes(4, byteorder='little')
# ======= 1. Отправляем команду изменить ID =======
# Весь буфер: id + команда + параметры
OLD_WITH_REG = (DEVICE_ID << 4) | REG_WRITE
id_bytes = list(OLD_WITH_REG.to_bytes(2, byteorder='little'))
# Важные части сообщения: address (id), команда, параметры
data_write = [REG_ID] + list(result_bytes) # команда изменить PID_P
# Полностью собираем массив для CRC (включая id и команду)
full_data_for_crc = id_bytes + data_write
# Расчет CRC по всему пакету
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
# Итоговый пакет: команда + параметры + CRC
packet_write = data_write + crc_bytes
print("Отправляем: команда изменить PID_p + CRC:", packet_write)
# Отправляем с `OLD_DEVICE_ID` в качестве адреса
send_can_message(bus, (DEVICE_ID << 4) | REG_WRITE, packet_write)
time.sleep(1.0)
# ======= 2. Запрашиваем текущий ID (используем новый адрес) =======
# Теперь для запроса используем **уже новый id**
NEW_WITH_REG = (DEVICE_ID << 4) | REG_READ
current_id_bytes = list(NEW_WITH_REG.to_bytes(2, byteorder='little'))
data_read = [REG_ID, 0x00]
full_data_for_crc = current_id_bytes + data_read
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
packet_read = data_read + crc_bytes
print("Запрос на чтение ID + CRC (после смены):", packet_read)
send_can_message(bus, (DEVICE_ID << 4) | REG_READ, packet_read)
# ======= 3. Получение и проверка ответа =======
response = receive_response(bus)
if response:
data = response.data
if len(data) < 4:
print("Ответ слишком короткий")
else:
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
#buff with id and data without CRC
full_data = list(id_bytes) + list(data[:-2])
print(f"Received full_data: {list(full_data)}")
received_crc = int.from_bytes(data[-2:], byteorder='little')
#calc CRC
calc_crc = validate_crc16(full_data)
if received_crc == calc_crc:
if data[0] == int(REG_ID):
kp_val = struct.unpack('<f', bytes(data[1:5]))[0]
print(f"\nУСПЕХ! PID_P = {kp_val:.3f}")
else:
print(f"Некорректный ответ: {list(data)}")
else:
print("CRC не совпадает, данные повреждены.")
else:
print("Нет ответа от устройства.")
bus.shutdown()