Актуализированы модели CAD, добавлены потерянные версии

Co-authored-by: Vladimir Latukhin <vlatukhin@gmail.com>

controller/fw/README.md

@@ -0,0 +1,68 @@
+# Встроенное ПО для сервопривода на 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 [адрес устройства]

controller/fw/bootloader/.clang-tidy

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

controller/fw/bootloader/.clangd

@@ -0,0 +1,18 @@
+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,
+    ]

controller/fw/bootloader/.gitignore

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

controller/fw/bootloader/check_gcc_version.py

@@ -0,0 +1,19 @@
+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}")

controller/fw/bootloader/cubemx_config.ioc

@@ -0,0 +1,299 @@
+#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

controller/fw/bootloader/gen_compile_commands.py

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

controller/fw/bootloader/hex_compile.py

@@ -0,0 +1,11 @@
+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))
+) 

controller/fw/bootloader/include/flash.h

@@ -0,0 +1,86 @@
+#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_ */

controller/fw/bootloader/include/hal_conf_extra.h

@@ -0,0 +1,38 @@
+#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>

controller/fw/bootloader/include/reg_cah.h

@@ -0,0 +1,38 @@
+#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_

controller/fw/bootloader/platformio.ini

@@ -0,0 +1,31 @@
+; 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

controller/fw/bootloader/src/flash.cpp

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

controller/fw/bootloader/src/main.cpp

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

controller/fw/bootloader/test/firmware_can.py

@@ -0,0 +1,141 @@
+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])

controller/fw/bootloader/test/firmware_update_flag.py

@@ -0,0 +1,70 @@
+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)

controller/fw/bootloader/test/st-link.py

@@ -0,0 +1,78 @@
+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)

controller/fw/bootloader/test/st-link_full.py

@@ -0,0 +1,100 @@
+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)

controller/fw/embed/.gitignore

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

controller/fw/embed/README.md

@@ -1,20 +0,0 @@
-# Встроенное ПО для сервипривода на 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
-```

controller/fw/embed/custom_script.ld

@@ -0,0 +1,178 @@
+/**
+ ******************************************************************************
+ * @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) }
+}

controller/fw/embed/hex_compile.py

@@ -0,0 +1,10 @@
+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")
+) 

controller/fw/embed/include/config.h

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

controller/fw/embed/include/flash.h

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

controller/fw/embed/include/process_can.h

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

controller/fw/embed/include/reg_cah.h

@@ -37,4 +37,12 @@
 #define MOTOR_ANGLE             0x72
 #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_

controller/fw/embed/platformio.ini

@@ -1,15 +1,3 @@
-; 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
@@ -18,11 +6,19 @@ upload_protocol = stlink
 debug_tool = stlink
 monitor_speed = 19200
 monitor_parity = N
+
+board_upload.offset_address = 0x08008000
+board_build.ldscript = ${PROJECT_DIR}/custom_script.ld
+
 build_flags = 
-	-DSTM32F446xx
+    -D STM32F446xx
-	-D HAL_CAN_MODULE_ENABLED
+    -D HAL_CAN_MODULE_ENABLED
-	-D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
+    -D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
+
 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

controller/fw/embed/src/config.cpp

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

controller/fw/embed/src/flash.cpp

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

controller/fw/embed/src/main.cpp

@@ -1,4 +1,3 @@
-// clang-format off
 #include "Arduino.h"
 #include "stm32f446xx.h"
 #include <SimpleFOC.h>
@@ -6,21 +5,23 @@
 #include <AS5045.h>
 #include <DRV8313.h>
 #include <cstring>
+#include <iostream>
 #include <iterator>
 #include "common/base_classes/FOCMotor.h"
 #include "hal_conf_extra.h"
 #include "wiring_analog.h"
 #include "wiring_constants.h"
 // clang-format on
+
 #include "reg_cah.h"
 #include "flash.h"
-
+#include "config.h"
+#include "process_can.h"
 
 void SysTick_Handler(void) {
   HAL_IncTick();
 }
 
-
 STM32_CAN Can(CAN2, DEF);
 /*      for FLASH         */
 uint32_t flash_flag;
@@ -29,18 +30,14 @@ uint32_t flash_error;
 FLASH_EraseInitTypeDef pEraseInit;
 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;
 
-volatile float kt = 0.1;  //for torgue calculation
+/* bool for test CAN    */
+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;
 MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
@@ -53,430 +50,66 @@ DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER,
 LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
                                   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;
 
-void doMotor(char *cmd) {
+ volatile uint16_t msg_id;
-  command.motor(&motor, cmd);
+ volatile uint16_t id_x;
-  digitalWrite(PC10, !digitalRead(PC10));
+ volatile uint8_t msg_ch;
-  delayMicroseconds(2);
+ volatile uint8_t crc_h;
-}
+ volatile uint8_t crc_l;
-
+
-void CAN2_RX0_IRQHandler() {
+  void setup(){
-  // Пустая функция, но прерывание не приведет к Default Handler
+  SCB->VTOR = (volatile uint32_t)0x08008004;
-}
+  
-
+  Serial.setRx(HARDWARE_SERIAL_RX_PIN);
-void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
+  Serial.setTx(HARDWARE_SERIAL_TX_PIN);
-               DRV8313Driver *driver, LowsideCurrentSense *current_sense,
+  Serial.begin(115200);
-               Commander *commander, CommandCallback callback) {
+
-  encoder->init(&spi);
+  pinMode(PC11, OUTPUT);
-
+  pinMode(PC10,OUTPUT);
-  driver->pwm_frequency = 20000;
+  GPIOC->ODR &= ~GPIO_ODR_OD10;
-  driver->voltage_power_supply = 24;
+  // Can.enableMBInterrupts();
-  driver->voltage_limit = 24;
+  Can.begin();
-  driver->init();
+  Can.setBaudRate(1000000);
-
+  // Настройка прерываний CAN
-  current_sense->linkDriver(driver);
+  CAN2->IER |= CAN_IER_FMPIE0;
-  current_sense->init();
+  flash_rec = load_params();    //for update write_ptr
-
+  if(flash_rec[firmw].value == FIRMWARE_FLAG) NVIC_SystemReset();  //if in flash go to the bootloader
-  motor->linkSensor(encoder);
+  
-  motor->linkDriver(driver);
+  // Initialize FOC system
-  motor->linkCurrentSense(current_sense);
+  setup_foc(&encoder, &motor, &driver, &current_sense,flash_rec);
-  motor->useMonitoring(Serial);
+  
-  motor->monitor_downsample = 5000;  // default 0
+  CAN2->IER |= CAN_IER_FMPIE0 |  // Сообщение в FIFO0
-  motor->controller = MotionControlType::angle;
+  CAN_IER_FFIE0 |   // FIFO0 full
-  motor->torque_controller = TorqueControlType::voltage;
+  CAN_IER_FOVIE0;    // FIFO0 overflow
-  motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
+
-
+
-  //  PID start
+  // Default motor configuration
-  motor->PID_velocity.P = 0.75;
+  GPIOC->ODR |= GPIO_ODR_OD11;  //set LED
-  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;
-  
-  else if(l == 1)
-    data_send = (float)(d * 10);
-
-  else
-    data_send = (float)(d * 100);
-  if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp;
-  else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki;
-  else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd;
-  send_can_with_id_crc(id,param_pid,&data_send,sizeof(data_send));
-}
-
-void setup_id(uint8_t my_id) {
-  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, чтобы выбрать, что делать
-  
-  
-  /*          Вычисление CRC          */
-  // Объединение старшего и младшего байтов для получения полученного CRC
-  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;
-  }
-
-
-  /*          0x691  
-  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.controller = MotionControlType::torque;
   __enable_irq();
-  }
+  
+}
+
+
 
 void loop() {
- foc_step(&motor, &command);
+  __enable_irq();
+  foc_step(&motor);
   CAN_message_t msg;
-  GPIOC->ODR ^= GPIO_ODR_OD11;
+  
-  delay(500);
+  // Process incoming CAN messages
   while (Can.read(msg)) {
     listen_can(msg);
+    CAN_GET = true;
+  }
+  /*  If receive data from  CAN */
+  if(CAN_GET) {
+
+    CAN_GET = false;
   }
 }
+

controller/fw/embed/src/process_can.cpp

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

controller/fw/embed/test/firmware_can.py

@@ -0,0 +1,141 @@
+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])

controller/fw/embed/test/firmware_update_flag.py

@@ -0,0 +1,70 @@
+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)

controller/fw/embed/test/pid_p.py

@@ -0,0 +1,103 @@
+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()

controller/fw/embed/test/python_can.py

@@ -1,47 +0,0 @@
-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()

controller/fw/embed/test/python_send_angle.py

@@ -1,37 +0,0 @@
-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()

controller/fw/embed/test/readPID_angle_parametrs.py

@@ -0,0 +1,126 @@
+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()

controller/fw/embed/test/read_angle.py

@@ -0,0 +1,98 @@
+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()

controller/fw/embed/test/read_id.py

@@ -0,0 +1,108 @@
+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)

controller/fw/embed/test/send_angle.py

@@ -0,0 +1,67 @@
+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()

controller/fw/embed/test/set_id.py

@@ -0,0 +1,124 @@
+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)

controller/fw/embed/test/st-link.py

@@ -0,0 +1,78 @@
+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)

controller/fw/embed/test/st-link_full.py

@@ -0,0 +1,100 @@
+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)

controller/fw/embed/test/writePID_angle_parametrs.py

@@ -0,0 +1,95 @@
+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()

controller/fw/embed/test/write_pidP.py

@@ -0,0 +1,122 @@
+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()