Merge

2025-06-04 09:57:43 +03:00 · 2025-06-04 09:57:43 +03:00 · 6be663c914
commit 6be663c914
parent d935263742 0c51667ffc
60 changed files with 29349 additions and 490 deletions
--- a/controller/fw/bootloader/.clang-tidy
+++ b/controller/fw/bootloader/.clang-tidy
@ -0,0 +1 @@
+Checks: '-*, -misc-definitions-in-headers' 
--- a/controller/fw/bootloader/.clangd
+++ b/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,
+    ]
--- a/controller/fw/bootloader/.gitignore
+++ b/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
--- a/controller/fw/bootloader/README.md
+++ b/controller/fw/bootloader/README.md
@ -0,0 +1,20 @@
+# Встроенное ПО для сервипривода на STM32F446RE
+
+## Для разработки
+
+- [Установить platformio](#introduction)
+```bash
+pip install -U platformio
+```
+- [Скомпилировать проект](#build_project)
+```bash
+platformio run --environment robotroller_reborn
+```
+- [Загрузить прошивку](#upload_project)
+```bash
+platformio run --target upload --environment robotroller_reborn 
+```
+- [Открыть монитор UART](#monitor_port)
+```bash
+platformio device monitor
+```
--- a/controller/fw/bootloader/check_gcc_version.py
+++ b/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}")
--- a/controller/fw/bootloader/cubemx_config.ioc
+++ b/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
--- a/controller/fw/bootloader/gen_compile_commands.py
+++ b/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")
--- a/controller/fw/bootloader/hex_compile.py
+++ b/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))
+) 
--- a/controller/fw/bootloader/include/flash.h
+++ b/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_ */
--- a/controller/fw/bootloader/include/hal_conf_extra.h
+++ b/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>
--- a/controller/fw/bootloader/include/reg_cah.h
+++ b/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_
--- a/controller/fw/bootloader/knowledges/AS5045B_DS000397_2-00.pdf
+++ b/controller/fw/bootloader/knowledges/AS5045B_DS000397_2-00.pdf
--- a/controller/fw/bootloader/knowledges/TI-DRV8313.pdf
+++ b/controller/fw/bootloader/knowledges/TI-DRV8313.pdf
--- a/controller/fw/bootloader/knowledges/motorBoard.pdf
+++ b/controller/fw/bootloader/knowledges/motorBoard.pdf
--- a/controller/fw/bootloader/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf
+++ b/controller/fw/bootloader/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf
--- a/controller/fw/bootloader/lib/AS5045/AS5045.cpp
+++ b/controller/fw/bootloader/lib/AS5045/AS5045.cpp
@ -0,0 +1,44 @@
+#include "AS5045.h"
+
+
+MagneticSensorAS5045::MagneticSensorAS5045(uint16_t as5040_cs, uint16_t as5040_mosi, uint16_t as5040_miso,
+                                           uint16_t as5040_sclk): AS5040_CS_(as5040_cs),
+                                                                  AS5040_MOSI_(as5040_mosi),
+                                                                  AS5040_MISO_(as5040_miso),
+                                                                  AS5040_SCLK_(as5040_sclk),
+                                                                  spi(nullptr),
+                                                                  settings(AS5145SSISettings) {
+}
+
+MagneticSensorAS5045::~MagneticSensorAS5045() = default;
+
+auto MagneticSensorAS5045::init(SPIClass *_spi) -> void {
+    this->spi = _spi;
+    settings = AS5145SSISettings;
+    pinMode(AS5040_CS_, OUTPUT);
+    pinMode(AS5040_MISO_, INPUT);
+    pinMode(AS5040_MOSI_, OUTPUT);
+    pinMode(AS5040_SCLK_, OUTPUT);
+
+    spi->setMISO(AS5040_MISO_);
+    spi->setMOSI(AS5040_MOSI_);
+    spi->setSCLK(AS5040_SCLK_);
+    spi->begin();
+    this->Sensor::init();
+}
+
+float MagneticSensorAS5045::getSensorAngle() {
+    float angle_data = readRawAngleSSI();
+    angle_data = (static_cast<float>(angle_data) / AS5045_CPR) * _2PI;
+    return angle_data;
+}
+
+uint16_t MagneticSensorAS5045::readRawAngleSSI() const {
+    spi->beginTransaction(settings);
+    digitalWrite(AS5040_CS_, LOW);
+    uint16_t value = spi->transfer16(0x0000);
+    digitalWrite(AS5040_CS_, HIGH);
+    spi->endTransaction();
+    delayMicroseconds(30);
+    return (value >> 3) & 0x1FFF;  // TODO(vanyabeat): Add error checking MAGNETIC OWERFLOW and etc.
+}
--- a/controller/fw/bootloader/lib/AS5045/AS5045.h
+++ b/controller/fw/bootloader/lib/AS5045/AS5045.h
@ -0,0 +1,32 @@
+#pragma once
+
+#include "SimpleFOC.h"
+#include "SPI.h"
+
+#ifndef MSBFIRST
+#define MSBFIRST BitOrder::MSBFIRST
+#endif
+
+#define AS5045_BITORDER MSBFIRST
+#define AS5045_CPR 4096.0f
+#define _2PI 6.28318530718f
+
+static SPISettings AS5145SSISettings(1000000, AS5045_BITORDER, SPI_MODE0);
+
+class MagneticSensorAS5045 final: public Sensor {
+ public:
+    MagneticSensorAS5045(uint16_t as5040_cs, uint16_t as5040_mosi, uint16_t as5040_miso, uint16_t as5040_sclk);
+
+    virtual ~MagneticSensorAS5045();
+
+    float getSensorAngle() override;
+
+    virtual void init(SPIClass *_spi = &SPI);
+
+    [[nodiscard]] uint16_t readRawAngleSSI() const;
+
+ private:
+    uint16_t AS5040_CS_, AS5040_MOSI_, AS5040_MISO_, AS5040_SCLK_;
+    SPIClass *spi;
+    SPISettings settings;
+};
--- a/controller/fw/bootloader/lib/AS5045/library.properties
+++ b/controller/fw/bootloader/lib/AS5045/library.properties
@ -0,0 +1,10 @@
+name=AS5045
+version=1.0.1
+author=vanyabeat <vanyabeat@protonmail.com>
+maintainer=vanyabeat <vanyabeat@protonmail.com>
+sentence=Simple library to work with AS5040 and Simple FOC (for Robotroller in Robosemmbler) Fork from https://github.com/runger1101001 
+paragraph=Simple library to work with AS5040 and Simple FOC and simple library intended for hobby comunity to run the AS5040 magnetic sensor using FOC algorithm. It is intended to support as many BLDC/Stepper motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards such as Nucleo and Bluepill, ESP32 and Teensy boards.
+category=Device Control
+url=https://docs.simplefoc.com
+architectures=*
+includes=SimpleFOC.h
--- a/controller/fw/bootloader/lib/DRV8313/DRV8313.cpp
+++ b/controller/fw/bootloader/lib/DRV8313/DRV8313.cpp
@ -0,0 +1,42 @@
+#include "DRV8313.h"
+
+DRV8313Driver::DRV8313Driver(int phA, int phB, int phC, int en1, int en2, int en3, int slp, int rst,
+                             int flt) : BLDCDriver3PWM(phA, phB, phC, en1, en2, en3), slp_pin(slp), rst_pin(rst),
+                                        flt_pin(flt) {
+}
+
+int DRV8313Driver::init() {
+    // Get state from flt pin
+    if (_isset(flt_pin)) {
+        pinMode(flt_pin, INPUT);
+        if (digitalRead(flt_pin) == HIGH) {
+            // if the fault pin is high the driver is in fault state
+            // reset the driver
+            if (_isset(rst_pin)) {
+                pinMode(rst_pin, OUTPUT);
+                digitalWrite(rst_pin, LOW);
+                delay(1);
+                digitalWrite(rst_pin, HIGH);
+                delay(1);
+            }
+        }
+    }
+    return BLDCDriver3PWM::init();
+}
+
+void DRV8313Driver::enable() {
+    // Enable the driver
+    if (_isset(slp_pin)) {
+        pinMode(slp_pin, OUTPUT);
+        digitalWrite(slp_pin, HIGH);
+    }
+    BLDCDriver3PWM::enable();
+}
+
+void DRV8313Driver::disable() {
+    if (_isset(slp_pin)) {
+        pinMode(slp_pin, OUTPUT);
+        digitalWrite(slp_pin, LOW);
+    }
+    BLDCDriver3PWM::disable();
+}
--- a/controller/fw/bootloader/lib/DRV8313/DRV8313.h
+++ b/controller/fw/bootloader/lib/DRV8313/DRV8313.h
@ -0,0 +1,20 @@
+#pragma once
+
+#include "SimpleFOC.h"
+
+class DRV8313Driver : public BLDCDriver3PWM {
+ public:
+    DRV8313Driver(int phA, int phB, int phC, int en1 = NOT_SET, int en2 = NOT_SET, int en3 = NOT_SET, int slp = NOT_SET,
+                  int rst = NOT_SET, int flt = NOT_SET);
+
+    int init() override;
+
+    void enable() override;
+
+    void disable() override;
+
+ private:
+    int slp_pin;
+    int rst_pin;
+    int flt_pin;
+};
--- a/controller/fw/bootloader/lib/DRV8313/library.properties
+++ b/controller/fw/bootloader/lib/DRV8313/library.properties
@ -0,0 +1,10 @@
+name=DRV8313 Simple FOC
+version=1.0.0
+author=vanyabeat <vanyabeat@protonmail.com>
+maintainer=vanyabeat <vanyabeat@protonmail.com>
+sentence=Simple library to work with DRV8313 and Simple FOC (for Robotroller in Robosemmbler)
+paragraph=Simple library to work with DRV8313 and Simple FOC and simple library intended for hobby comunity to run the BLDC and Stepper motor using FOC algorithm. It is intended to support as many BLDC/Stepper motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards such as Nucleo and Bluepill, ESP32 and Teensy boards.
+category=Device Control
+url=https://docs.simplefoc.com
+architectures=*
+includes=SimpleFOC.h
--- a/controller/fw/bootloader/lib/README
+++ b/controller/fw/bootloader/lib/README
@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html
--- a/controller/fw/bootloader/platformio.ini
+++ b/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
--- a/controller/fw/bootloader/src/flash.cpp
+++ b/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;
+}
+
+
--- a/controller/fw/bootloader/src/main.cpp
+++ b/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);
+    }
+}
--- a/controller/fw/bootloader/test/README.md
+++ b/controller/fw/bootloader/test/README.md
@ -0,0 +1,86 @@
+# CAN Communication Scripts
+
+This repository contains Python scripts for testing and interacting with a CAN bus system. These scripts enable sending and receiving CAN messages to control a motor, set angles, and adjust velocities.
+
+## Prerequisites
+
+1. **Python 3.7+** installed on your system.
+2. **`python-can` library** installed. Install it via pip:
+   ```bash
+   pip install python-can
+   ```
+3. **SocketCAN interface** properly configured on your Linux system. The default channel is `can0`.
+
+## Usage
+
+### 1. Receiving CAN Messages
+
+The script `python_can.py` listens to the CAN bus and processes incoming messages.
+
+#### Run:
+```bash
+python3 python_can.py
+```
+
+#### Features:
+- Processes messages with data length 5.
+- Parses the first byte (`flag`) to determine the type:
+  - `'A'`: Angle (float).
+  - `'V'`: Velocity (float).
+  - `'E'`: Enable/disable status (boolean).
+
+### 2. Enabling or Disabling the Motor
+
+The script `python_enable_motor.py` sends commands to enable or disable the motor.
+
+#### Run:
+```bash
+python3 python_enable_motor.py <0|1>
+```
+
+#### Arguments:
+- `0`: Disable the motor.
+- `1`: Enable the motor.
+
+### 3. Sending Target Angle
+
+The script `python_send_angle.py` sends a target angle to the CAN bus.
+
+#### Run:
+```bash
+python3 python_send_angle.py
+```
+
+#### Behavior:
+- Sends a message with a predefined target angle every second.
+- Adjust the target angle in the script (`target_angle` variable).
+
+### 4. Sending Target Velocity
+
+The script `python_send_velocity.py` sends a target velocity to the CAN bus.
+
+#### Run:
+```bash
+python3 python_send_velocity.py
+```
+
+#### Behavior:
+- Sends a message with a predefined target velocity every second.
+- Adjust the target velocity in the script (`target_speed` variable).
+
+## Configuration
+
+### CAN Interface
+The scripts use the following default CAN bus settings:
+- **Channel**: `can0`
+- **Bitrate**: `1 Mbps`
+
+If your configuration differs, update the `Bus()` initialization in the scripts.
+
+## Troubleshooting
+
+1. **"Error initializing CAN bus"**:
+   - Ensure your CAN interface is correctly configured and active:
+     ```bash
+     sudo ip link set can0 up type can bitrate 1000000
+     ```
--- a/controller/fw/bootloader/test/python_can.py
+++ b/controller/fw/bootloader/test/python_can.py
--- a/controller/fw/bootloader/test/python_enable_motor.py
+++ b/controller/fw/bootloader/test/python_enable_motor.py
--- a/controller/fw/bootloader/test/python_send_angle.py
+++ b/controller/fw/bootloader/test/python_send_angle.py
--- a/controller/fw/bootloader/test/python_send_velocity.py
+++ b/controller/fw/bootloader/test/python_send_velocity.py
--- a/controller/fw/bootloader/test/python_test_id.py
+++ b/controller/fw/bootloader/test/python_test_id.py
@ -0,0 +1,73 @@
+import can
+import time
+
+def send_write_read_requests():
+    try:
+        bus = can.interface.Bus(channel='can0', bustype='socketcan')
+
+        # Конфигурация сообщений (ЗАПОЛНИТЕ ВАШИ ЗНАЧЕНИЯ)
+        write_msg = {
+            'arbitration_id': 0x01,          # CAN ID для записи 
+            'data': [0x27, 0xA0, 0xFF, 0x00], # Данные для записи (4 байта) 
+            'description': "Установка id устройства"
+        }
+
+        read_msg = {
+            'arbitration_id': 0x01,          # CAN ID для чтения
+            'data': [0xFF,0x99],                   # Адрес новый + команда запроса данных
+            'description': "Запрос id устройства",
+            'response_id': 0xFF,             # Ожидаемый ID ответа
+            'timeout': 1.0                    # Таймаут ожидания ответа (сек)
+        }
+
+        # 1. Отправка команды записи
+        print("Отправка команды записи...")
+        msg = can.Message(
+            arbitration_id=write_msg['arbitration_id'],
+            data=write_msg['data'],
+            is_extended_id=False
+        )
+        bus.send(msg)
+        print(f"Запись: ID={hex(msg.arbitration_id)}, Данные={list(msg.data)}")
+        
+        # Ждем обработки команды устройством
+        time.sleep(2.0)  
+
+        # 2. Отправка запроса чтения и ожидание ответа
+        print("\nОтправка запроса чтения...")
+        msg = can.Message(
+            arbitration_id=read_msg['arbitration_id'],
+            data=read_msg['data'],
+            is_extended_id=False
+        )
+        bus.send(msg)
+        print(f"Чтение: ID={hex(msg.arbitration_id)}, Команда={list(msg.data)}")
+
+        # Ожидаем ответа
+        start_time = time.time()
+        response_received = False
+        
+        print("\nОжидание ответа...")
+        while (time.time() - start_time) < read_msg['timeout']:
+            response = bus.recv(timeout=0.1)
+            
+            if response and response.arbitration_id == read_msg['response_id']:
+                print(f"\nПолучен ответ: ID={hex(response.arbitration_id)}")
+                print(f"Данные: {list(response.data)}")
+                print(f"Длина: {response.dlc} байт")
+                response_received = True
+                break
+        
+        if not response_received:
+            print("\nОшибка: ответ не получен в течение заданного времени")
+
+    except KeyboardInterrupt:
+        print("\nПрерывание пользователем")
+    except Exception as e:
+        print(f"Ошибка: {str(e)}")
+    finally:
+        bus.shutdown()
+        print("\nCAN соединение закрыто")
+
+if __name__ == '__main__':
+    send_write_read_requests()
--- a/controller/fw/bootloader/test/send_velocity_impulses.py
+++ b/controller/fw/bootloader/test/send_velocity_impulses.py
@ -0,0 +1,35 @@
+import can
+import struct
+import time
+
+# Function to send the target speed
+def send_target_speed(bus, target_speed):
+    msg = can.Message()
+    msg.arbitration_id = 1  # Message ID
+    msg.is_extended_id = False
+    msg.dlc = 5  # Message length
+    msg.data = [ord('V')] + list(struct.pack('<f', target_speed))  # 'V' for the command identifier, followed by the speed in float format
+
+    try:
+        bus.send(msg)
+        print(f"Sent message with target speed: {target_speed} m/s")
+        print(f"Message data: {msg.data}")
+    except can.CanError:
+        print("Message failed to send")
+
+# Main function
+def main():
+    # 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 speed impulses...")
+
+    # Send impulses of target speed from -2 to 2 m/s
+    target_speeds = [-1, 1]
+
+    while True:
+        for speed in target_speeds:
+            send_target_speed(bus, speed)
+            time.sleep(1)  # 1-second delay between messages
+
+if __name__ == '__main__':
+    main()
--- a/controller/fw/embed/.gitignore
+++ b/controller/fw/embed/.gitignore
@ -7,3 +7,4 @@
 .metadata/
 cubemx_config/
 compile_commands.json
+../embed.rar
--- a/controller/fw/embed/README.md
+++ b/controller/fw/embed/README.md
@ -18,3 +18,52 @@ platformio run --target upload --environment robotroller_reborn
 ```bash
 platformio device monitor
 ```
+
+## Для прошивки
+
+- [Установить python3]
+```bash
+sudo apt install python3
+```
+- [Устаноивть st-link]
+```bash
+sudo apt install st-link
+```
+## Выбор интерфейса прошивки
+### CAN
+- Если уже есть какя-то основная прошивка, то чтобы перепрошить другую прошивку, добавляем флаг для бутлоадера
+```bash
+python3 firmw_update_flag.py [адрес устройства]
+```
+- Передача прошивки по CAN
+```bash
+python3 firmware_can.py firmware.hex [адрес устройства]
+```
+### St-link(нет адресации можно прошивать только по одному)
+```bash
+python3 st-link.py firmware.hex
+``` 
+
+- Если бутлоадер не был прошит и FLASH микрокотроллера полностью стерта
+
+- [Скачать прошивку и бутлоадер в hex формате]
+ССЫЛКА
+
+- [Прошить через программатор]
+```bash
+python3 st-link_full.py bootloader.hex firmware.hex
+```
+
+## Работа по CAN
+- Установка адреса(если до этого не был установлен адрес, то адрес устройства = 0)
+```bash
+python3 set_id.py [адрес устройства]
+```
+
+- Установка PID коэффициентов для угла
+```bash 
+python3 writePID_angle_parametrs.py [адрес устройства]
+```
+-Чтение PID коэффициентов для угла
+```bash
+python3 readPID_angle_parametrs.py [адрес устройства]
--- a/controller/fw/embed/custom_script.ld
+++ b/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) }
+}
--- a/controller/fw/embed/hex_compile.py
+++ b/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")
+) 
--- a/controller/fw/embed/include/config.h
+++ b/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);
--- a/controller/fw/embed/include/flash.h
+++ b/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,
-  angl      = 2,
-  vel       = 3,
-  pid_p     = 4,  
+  pid_p     = 1, 
  pid_i,
-  pid_d
+  pid_d,
+  firmw,
+  foc_id,
+  angl,
+  vel
 };

-#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD)    //size flash struct
-#define PARAM_COUNT 4             // count data in flash
+/*  for saved in FLASH float data*/
+union{
+    uint32_t i;
+    float f;
+}conv_float_to_int;

+#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD)    //size flash struct
+#define 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_ */
--- a/controller/fw/embed/include/process_can.h
+++ b/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);
--- a/controller/fw/embed/include/reg_cah.h
+++ b/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_
--- a/controller/fw/embed/platformio.ini
+++ b/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 = 
    -D STM32F446xx
    -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
+
+extra_scripts = 
+    pre:gen_compile_commands.py
+    post:hex_compile.py
--- a/controller/fw/embed/src/config.cpp
+++ b/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();
+}
--- a/controller/fw/embed/src/flash.cpp
+++ b/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];
-        write_ptr++;
+        *(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();
 }

@ -115,29 +113,53 @@ uint8_t flash_read_word(uint32_t address){

 }
    // Wait if flash
-bool validata_crc(FLASH_RECORD* crc){
-    return crc->crc == 0x6933? true : false;
-}
+// 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; // Начальное значение для 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) {
-    FLASH_RECORD param_flash = {param_id, val};
-    // __disable_irq(); // Запрещаем прерывания на время всей операции
-
-    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 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(); // После compact_page write_ptr обновляется
-        // Повторно выравниваем после функции. То есть сколько не хватает для кратности
-        if (write_ptr % 4 != 0) {
-            write_ptr += (4 - (write_ptr % 4));
+        compact_page(); // after compact_page update
+        // alignment
+        if (write_ptr % 8 != 0) {
+            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) 
-            continue;
+
+        
+        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;
 }
+
+
--- a/controller/fw/embed/src/main.cpp
+++ b/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,16 @@ 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

-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;
+/* bool for test CAN    */
+volatile bool CAN_GET = false;
+
+
+volatile float kt = 0.1;  // Torque calculation constant
+
+FLASH_RECORD* flash_rec;
+

 SPIClass spi;
 MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
@ -53,398 +52,19 @@ 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);
-
-struct MotorControlInputs {
-  float target_angle = 0.0;
-  float target_velocity = 0.0;
-  bool motor_enabled = false;
-  bool foc_state = false;
-};
+// Commander command(Serial);

 MotorControlInputs motor_control_inputs;

-void doMotor(char *cmd) {
-  command.motor(&motor, cmd);
-  digitalWrite(PC10, !digitalRead(PC10));
-  delayMicroseconds(2);
-}
-
-void CAN2_RX0_IRQHandler() {
-  // Пустая функция, но прерывание не приведет к Default Handler
-}
-
-void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
-               DRV8313Driver *driver, LowsideCurrentSense *current_sense,
-               Commander *commander, CommandCallback callback) {
-  encoder->init(&spi);
-
-  driver->pwm_frequency = 20000;
-  driver->voltage_power_supply = 24;
-  driver->voltage_limit = 24;
-  driver->init();
-
-  current_sense->linkDriver(driver);
-  current_sense->init();
-
-  motor->linkSensor(encoder);
-  motor->linkDriver(driver);
-  motor->linkCurrentSense(current_sense);
-  motor->useMonitoring(Serial);
-  motor->monitor_downsample = 5000;  // default 0
-  motor->controller = MotionControlType::angle;
-  motor->torque_controller = TorqueControlType::voltage;
-  motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
-
-  //  PID start
-  motor->PID_velocity.P = 0.75;
-  motor->PID_velocity.I = 20;
-  motor->LPF_velocity.Tf = 0.005;
-  motor->P_angle.P = 0.5;
-  motor->LPF_angle.Tf = 0.001;
-  //  PID end
-
-  motor->velocity_limit = 40;  // Ограничение по скорости вращения rad/s (382 rpm)
-  motor->voltage_limit = 24;
-  motor->current_limit = 0.5;
-
-  motor->sensor_direction = Direction::CCW;
-  motor->init();
-  motor->initFOC();
-}
-
-void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) {
-  // Создаем сообщение
-  CAN_message_t msg;
-  msg.id = id;
-  msg.len = 8; // или как в протоколе
-  msg.buf[0] = message_type;
-  memcpy(&msg.buf[1], data, data_length);
-
-  // Формируем массив для CRC, включающий ID и все данные
-  size_t crc_data_size = sizeof(msg.id) + data_length;
-  uint8_t crc_data[crc_data_size];
-
-  // Копируем ID
-  memcpy(crc_data, &msg.id, sizeof(msg.id));
-  // Копируем все байты data
-  memcpy(crc_data + sizeof(msg.id), data, data_length);
-
-  // Расчет CRC
-  uint16_t crc_value = validate_crc16(crc_data, crc_data_size);
-
-  // Вставляем CRC в буфер
-  msg.buf[6] = crc_value & 0xFF;
-  msg.buf[7] = (crc_value >> 8) & 0xFF;
-
-  // Отправляем
-  Can.write(msg);
-  __NOP();
-}
-
-
-
-void send_velocity() {
-  float current_velocity = motor.shaftVelocity();
-  flash_rec = load_params();
-  if (flash_rec == nullptr) { // Проверка на NULL
-      // Обработка ошибки: запись в лог, сигнализация и т.д.
-      return;
-  }
-  uint8_t value = flash_rec[vel].value;
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'V',&value,sizeof(value));
-}
-
-void send_angle() {
-  float current_angle = motor.shaftAngle();
-
-  flash_rec = load_params();
-  if (flash_rec == nullptr) { // Проверка на NULL
-      // Обработка ошибки: запись в лог, сигнализация и т.д.
-      return;
-  }
-  // uint8_t value = flash_rec[angl].value;
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'A',&current_angle,sizeof(current_angle));
-}
-
-void send_motor_enabled() {
-  uint8_t id = *(volatile uint8_t*)ADDR_VAR;
-  CAN_TX_msg.id = id;
-  CAN_TX_msg.buf[0] = 'E';
-  memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled,
-         sizeof(motor_control_inputs.motor_enabled));
-  Can.write(CAN_TX_msg);
-}
-
-void send_foc_state() {
-    /*          data for reading of firmware      */
-    flash_rec = load_params();
-    if (flash_rec == nullptr) { // Проверка на NULL
-        // Обработка ошибки: запись в лог, сигнализация и т.д.
-        return;
-    }
-
-  uint8_t value = flash_rec[foc_id].value;
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'F',&value,sizeof(value));
-}
-
-void send_id() {
-  /*          data for reading of firmware      */
-  flash_rec = load_params();
-  if (flash_rec == nullptr) { // Проверка на NULL
-      // Обработка ошибки: запись в лог, сигнализация и т.д.
-      return;
-  }
-  uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'I',&id,sizeof(id));
-  __NOP();
-}
-
-void send_motor_torque() {
-  float i_q = motor.current.q; // Ток по оси q (А)
-  float torque = kt * i_q; // Расчет момента
-  torque *= 100;
-  flash_rec = load_params();
-  CAN_TX_msg.id = flash_rec->value;
-  CAN_TX_msg.buf[0] = 'T';
-  CAN_TX_msg.len = 5;
-  memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
-  Can.write(CAN_TX_msg);
-}
-
-
-void send_pid(uint8_t param_pid){
-  flash_rec = load_params();
-  if (flash_rec == nullptr) { // Проверка на NULL
-    return;
-  }
-  uint8_t id = flash_rec[addr_id].value;
-  uint8_t d = flash_rec[param_pid].value;
-  uint8_t data_send = 0;
-  int l = 0;
-  while(d /= 10)
-    l++;
-  if(l >= 2)
-    data_send = (float)d;
-  
-  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();
-}
-
-
-
+ 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;

  void setup(){
-  /*   bias for vector int    */
-  // __set_MSP(*(volatile uint32_t*)0x08008000);
-  // SCB->VTOR = (volatile uint32_t)0x08008000;
+  SCB->VTOR = (volatile uint32_t)0x08008004;
+  
  Serial.setRx(HARDWARE_SERIAL_RX_PIN);
  Serial.setTx(HARDWARE_SERIAL_TX_PIN);
  Serial.begin(115200);
@ -452,31 +72,46 @@ Serial.begin(115200);
  pinMode(PC11, OUTPUT);
  pinMode(PC10,OUTPUT);
  GPIOC->ODR &= ~GPIO_ODR_OD10;
-// Setup thermal sensor pin
-// pinMode(TH1, INPUT_ANALOG);
+  // Can.enableMBInterrupts();
  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;
+  // Настройка прерываний CAN
+  CAN2->IER |= CAN_IER_FMPIE0;
+  flash_rec = load_params();    //for update write_ptr
+  if(flash_rec[firmw].value == FIRMWARE_FLAG) NVIC_SystemReset();  //if in flash go to the bootloader
+  
+  // Initialize FOC system
+  setup_foc(&encoder, &motor, &driver, &current_sense,flash_rec);
+  
+  CAN2->IER |= CAN_IER_FMPIE0 |  // Сообщение в FIFO0
+  CAN_IER_FFIE0 |   // FIFO0 full
+  CAN_IER_FOVIE0;    // FIFO0 overflow
+
+
+  // Default motor configuration
+  GPIOC->ODR |= GPIO_ODR_OD11;  //set LED
  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;
  }
 }
+
--- a/controller/fw/embed/src/process_can.cpp
+++ b/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;
+      }
+    }
+  }
+}
--- a/controller/fw/embed/test/enable_motor.py
+++ b/controller/fw/embed/test/enable_motor.py
@ -0,0 +1,54 @@
+import can
+import sys
+
+# Function to send the motor enable/disable command
+def send_motor_enable(bus, enable):
+    """
+    Sends a command to enable or disable the motor.
+
+    :param bus: The CAN bus
+    :param enable: 1 to enable the motor, 0 to disable it
+    """
+    msg = can.Message()
+    msg.arbitration_id = 1  # Message ID
+    msg.is_extended_id = False
+    msg.dlc = 2  # Message length (flag + 1 byte of data)
+    msg.data = [ord('E'), enable]  # 'E' for the command, followed by 0 or 1
+
+    try:
+        bus.send(msg)
+        state = "enabled" if enable else "disabled"
+        print(f"Sent message to {state} motor")
+        print(f"Message data: {msg.data}")
+    except can.CanError as e:
+        print(f"Message failed to send: {e}")
+        sys.exit(1)  # Exit the program on failure
+
+def main():
+    # CAN interface setup
+    bus = None  # Define outside the try block for proper shutdown
+    try:
+        bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
+        print("CAN bus initialized.")
+
+        # Ensure the state is passed via arguments
+        if len(sys.argv) != 2 or sys.argv[1] not in ['0', '1']:
+            print("Usage: python3 script_name.py <0|1>")
+            print("0 - Disable motor, 1 - Enable motor")
+            sys.exit(1)
+
+        enable = int(sys.argv[1])
+        send_motor_enable(bus, enable)
+
+    except Exception as e:
+        print(f"Error initializing CAN bus: {e}")
+        sys.exit(1)
+
+    finally:
+        # Ensure the bus is properly shut down
+        if bus is not None:
+            bus.shutdown()
+            print("CAN bus shut down.")
+
+if __name__ == '__main__':
+    main()
--- a/controller/fw/embed/test/firmware_can.py
+++ b/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])
--- a/controller/fw/embed/test/firmware_update_flag.py
+++ b/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)
--- a/controller/fw/embed/test/pid_p.py
+++ b/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()
--- a/controller/fw/embed/test/readPID_angle_parametrs.py
+++ b/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()
--- a/controller/fw/embed/test/read_angle.py
+++ b/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()
--- a/controller/fw/embed/test/read_id.py
+++ b/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)
--- a/controller/fw/embed/test/send_angle.py
+++ b/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()
--- a/controller/fw/embed/test/send_velocity.py
+++ b/controller/fw/embed/test/send_velocity.py
@ -0,0 +1,76 @@
+import can
+import struct
+import time
+import sys
+
+# Function to send the target speed
+def send_target_speed(bus, target_speed):
+    msg = can.Message()
+    msg.arbitration_id = 1  # Message ID
+    msg.is_extended_id = False
+    msg.dlc = 5  # Message length
+    msg.data = bytearray([ord('V')] + list(struct.pack('<f', target_speed)))  # 'V' for the command identifier, followed by the speed in float format
+
+    try:
+        bus.send(msg)
+        print(f"Sent message with target speed: {target_speed} rad/s")
+    except can.CanError:
+        print("Message failed to send")
+
+# Function to send the motor enable/disable command
+def send_motor_enable(bus, enable):
+    """
+    Sends a command to enable or disable the motor.
+    
+    :param bus: The CAN bus
+    :param enable: 1 to enable the motor, 0 to disable it
+    """
+    msg = can.Message()
+    msg.arbitration_id = 1  # Message ID
+    msg.is_extended_id = False
+    msg.dlc = 2  # Message length (flag + 1 byte of data)
+    msg.data = bytearray([ord('E'), enable])  # 'E' for the command, followed by 0 or 1
+
+    try:
+        bus.send(msg)
+        state = "enabled" if enable else "disabled"
+        print(f"Sent message to {state} motor")
+    except can.CanError as e:
+        print(f"Message failed to send: {e}")
+        sys.exit(1)  # Exit the program on failure
+
+    send_target_speed(bus,0.0)
+
+def main():
+    # CAN interface setup
+    bus = None  # Define outside the try block for proper shutdown
+    try:
+        bus = can.interface.Bus(channel='COM4', bustype='slcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
+        print("CAN bus initialized.")
+
+        while True:
+            user_input = input("Enter target speed: ")
+            if user_input.lower() == 'exit':
+                print("Exiting...")
+                break
+            try:
+                target_speed = float(user_input)
+                send_target_speed(bus, target_speed)
+            except ValueError:
+                print("Invalid input. Please enter a valid number.")
+
+        # Disable motor before exiting
+        send_motor_enable(bus, 0)
+        print("Motor disabled.")
+    
+    except Exception as e:
+        print(f"Error initializing1 CAN bus: {e}")
+        sys.exit(1)
+
+    finally:
+        if bus is not None:
+            bus.shutdown()
+            print("CAN bus shut down.")
+
+if __name__ == '__main__':
+    main()
--- a/controller/fw/embed/test/set_id.py
+++ b/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)
--- a/controller/fw/embed/test/st-link.py
+++ b/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)
--- a/controller/fw/embed/test/st-link_full.py
+++ b/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)
--- a/controller/fw/embed/test/writePID_angle_parametrs.py
+++ b/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()
--- a/controller/fw/embed/test/write_pidP.py
+++ b/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()