diff --git a/controller/fw/README.md b/controller/fw/README.md new file mode 100644 index 0000000..ff1c0af --- /dev/null +++ b/controller/fw/README.md @@ -0,0 +1,68 @@ +# Встроенное ПО для сервопривода на STM32F446RE + +## Для разработки +- [Установить platformio](#introduction) +```bash +pip install -U platformio +``` +- Установить python3 +```bash +sudo apt install python3 +``` +- Устаноивть st-link +```bash +sudo apt install st-link +``` +### Прошивка делится на два файла один для загрузчика другой для основной прошивки. Чтобы загрузить как описано ниже нужно находится в директории этого проекта. Нужно сделать как для bootloader так и для embed +- [Скомпилировать проект](#build_project) +```bash +platformio run --environment robotroller_reborn +``` +- [Загрузить прошивку](#upload_project) +```bash +platformio run --target upload --environment robotroller_reborn +``` + +## Другой способ прошивки +## Выбор интерфейса прошивки +### Для основной прошивки в директории ./embed +- Если уже есть какя-то основная прошивка, то чтобы перепрошить другую прошивку, добавляем флаг для бутлоадера +```bash +python3 firmw_update_flag.py [адрес устройства] +``` +- Передача прошивки по CAN +```bash +python3 firmware_can.py firmware.hex [адрес устройства] +``` +### St-link(нет адресации можно прошивать только по одному) +```bash +python3 st-link.py firmware.hex +``` +### St-link_full(полная прошивка без адресации) +#### Прошивает и программатор и основную прошивку можно находится как в ./embed, так и в ./bootloader(в директории где есть данный тест в папке test). + +- Если до этого сохраняли адреса и данные, то они останутся даже при полной перепрошивке +- Если бутлоадер не был прошит и FLASH микрокотроллера полностью стерта + +- [Скачать прошивку и бутлоадер в hex формате] +ССЫЛКА + +- [Прошить через программатор] +```bash +python3 st-link_full.py bootloader.hex firmware.hex +``` + +## Работа по CAN +#### Для основной прошивки в директории ./embed +- Установка адреса(если до этого не был установлен адрес, то адрес устройства = 0) +```bash +python3 set_id.py [адрес устройства] +``` + +- Установка PID коэффициентов для угла +```bash +python3 writePID_angle_parametrs.py [адрес устройства] +``` +-Чтение PID коэффициентов для угла +```bash +python3 readPID_angle_parametrs.py [адрес устройства] \ No newline at end of file diff --git a/controller/fw/bootloader/.clang-tidy b/controller/fw/bootloader/.clang-tidy new file mode 100644 index 0000000..3f9824b --- /dev/null +++ b/controller/fw/bootloader/.clang-tidy @@ -0,0 +1 @@ +Checks: '-*, -misc-definitions-in-headers' diff --git a/controller/fw/bootloader/.clangd b/controller/fw/bootloader/.clangd new file mode 100644 index 0000000..05f45f8 --- /dev/null +++ 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, + ] diff --git a/controller/fw/bootloader/.gitignore b/controller/fw/bootloader/.gitignore new file mode 100644 index 0000000..b0e6063 --- /dev/null +++ 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 diff --git a/controller/fw/bootloader/check_gcc_version.py b/controller/fw/bootloader/check_gcc_version.py new file mode 100644 index 0000000..41d1176 --- /dev/null +++ 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}") diff --git a/controller/fw/bootloader/cubemx_config.ioc b/controller/fw/bootloader/cubemx_config.ioc new file mode 100644 index 0000000..619d4c8 --- /dev/null +++ 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 diff --git a/controller/fw/bootloader/gen_compile_commands.py b/controller/fw/bootloader/gen_compile_commands.py new file mode 100644 index 0000000..0537d0b --- /dev/null +++ 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") diff --git a/controller/fw/bootloader/hex_compile.py b/controller/fw/bootloader/hex_compile.py new file mode 100644 index 0000000..65c5917 --- /dev/null +++ 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)) +) \ No newline at end of file diff --git a/controller/fw/bootloader/include/flash.h b/controller/fw/bootloader/include/flash.h new file mode 100644 index 0000000..c3dd762 --- /dev/null +++ b/controller/fw/bootloader/include/flash.h @@ -0,0 +1,86 @@ +#ifndef FLASH_H_ +#define FLASH_H_ +#include "stm32f446xx.h" +#include +#include + + +/* 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_ */ diff --git a/controller/fw/bootloader/include/hal_conf_extra.h b/controller/fw/bootloader/include/hal_conf_extra.h new file mode 100644 index 0000000..3ee6087 --- /dev/null +++ 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 diff --git a/controller/fw/bootloader/include/reg_cah.h b/controller/fw/bootloader/include/reg_cah.h new file mode 100644 index 0000000..d519979 --- /dev/null +++ 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_ diff --git a/controller/fw/bootloader/platformio.ini b/controller/fw/bootloader/platformio.ini new file mode 100644 index 0000000..6daca8f --- /dev/null +++ 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 diff --git a/controller/fw/bootloader/src/flash.cpp b/controller/fw/bootloader/src/flash.cpp new file mode 100644 index 0000000..f3c5ce3 --- /dev/null +++ b/controller/fw/bootloader/src/flash.cpp @@ -0,0 +1,272 @@ +#include "flash.h" +#include +#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(¶m_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, ¶m_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; +} + + diff --git a/controller/fw/bootloader/src/main.cpp b/controller/fw/bootloader/src/main.cpp new file mode 100644 index 0000000..aef034a --- /dev/null +++ b/controller/fw/bootloader/src/main.cpp @@ -0,0 +1,171 @@ +#include "Arduino.h" +#include +#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); + } +} \ No newline at end of file diff --git a/controller/fw/bootloader/test/firmware_can.py b/controller/fw/bootloader/test/firmware_can.py new file mode 100644 index 0000000..37acbc3 --- /dev/null +++ b/controller/fw/bootloader/test/firmware_can.py @@ -0,0 +1,141 @@ +import can +import sys +import time +from intelhex import IntelHex +# Конфигурация +CAN_CHANNEL = 'socketcan' +CAN_INTERFACE = 'can0' +CAN_BITRATE = 1000000 +#ch =int(input("Введите id устройства:")) +ch = int(sys.argv[2]) +BOOT_CAN_ID = (ch * 16) + 1 +DATA_CAN_ID = (ch * 16) + 3 +BOOT_CAN_END = (ch * 16) + 2 +ACK_CAN_ID = 0x05 + +#конфиг для crc16 ibm + + + +def debug_print(msg): + print(f"[DEBUG] {msg}") + +def calculate_crc16(data: bytes) -> int: + crc = 0xFFFF + for byte in data: + crc ^= byte + for _ in range(8): + if crc & 0x0001: + crc = (crc >> 1) ^ 0xA001 + else: + crc >>= 1 + return crc + +def send_firmware(hex_file): + try: + debug_print("Инициализация CAN...") + bus = can.interface.Bus( + channel=CAN_INTERFACE, + bustype=CAN_CHANNEL, + bitrate=CAN_BITRATE + ) + + debug_print("Чтение HEX-файла...") + ih = IntelHex(hex_file) + binary_data = ih.tobinstr() # Исправлено на tobinstr() + fw_size = len(binary_data) + debug_print(f"Размер прошивки: {fw_size} байт") + + # Расчет CRC + debug_print("Расчёт CRC...") + # calculator = Calculator(Crc16.IBM) + fw_crc = calculate_crc16(binary_data) + debug_print(f"CRC: 0x{fw_crc:04X}") + + # Отправка START + start_data = bytearray([0x01]) + start_data += fw_size.to_bytes(4, 'little') + start_data += fw_crc.to_bytes(2, 'little') + + debug_print(f"START: {list(start_data)}") + start_msg = can.Message( + arbitration_id=BOOT_CAN_ID, + data=bytes(start_data), + is_extended_id=False + ) + + try: + bus.send(start_msg) + except can.CanError as e: + debug_print(f"Ошибка отправки START: {str(e)}") + return + + # Ожидание ACK + debug_print("Ожидание ACK...") + ack = wait_for_ack(bus) + if not ack: + debug_print("Таймаут ACK START") + return + debug_print(f"Получен ACK: {list(ack.data)}") + + # Отправка данных + packet_size = 8 + for i in range(0, len(binary_data), packet_size): + chunk = binary_data[i:i+packet_size] + # Дополнение до 8 байт + if len(chunk) < 8: + chunk += b'\xFF' * (8 - len(chunk)) + + debug_print(f"Пакет {i//8}: {list(chunk)}") + data_msg = can.Message( + arbitration_id=DATA_CAN_ID, + data=chunk, + is_extended_id=False + ) + + try: + bus.send(data_msg) + except can.CanError as e: + debug_print(f"Ошибка отправки данных: {str(e)}") + return + + ack = wait_for_ack(bus) + if not ack: + debug_print("Таймаут ACK DATA") + return + + # Финал + debug_print("Отправка FINISH...") + finish_msg = can.Message( + arbitration_id=BOOT_CAN_END, + data=bytes([0xAA]), + is_extended_id=False + ) + bus.send(finish_msg) + + ack = wait_for_ack(bus, timeout=1.0) + if ack and ack.data[0] == 0xAA: + debug_print("Прошивка подтверждена!") + else: + debug_print("Ошибка верификации!") + + except Exception as e: + debug_print(f"Критическая ошибка: {str(e)}") + finally: + bus.shutdown() + +def wait_for_ack(bus, timeout=1.0): + start_time = time.time() + while time.time() - start_time < timeout: + msg = bus.recv(timeout=0.1) # Неблокирующий режим + if msg and msg.arbitration_id == ACK_CAN_ID: + return msg + return None + +if __name__ == "__main__": + import sys + if len(sys.argv) != 3: + print("Использование: sudo python3 can_flasher.py firmware.hex") + sys.exit(1) + + send_firmware(sys.argv[1]) diff --git a/controller/fw/bootloader/test/firmware_update_flag.py b/controller/fw/bootloader/test/firmware_update_flag.py new file mode 100644 index 0000000..748cf6b --- /dev/null +++ b/controller/fw/bootloader/test/firmware_update_flag.py @@ -0,0 +1,70 @@ +import can +import time +import sys +# Конфигурация +CAN_INTERFACE = 'can0' +OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства (по умолчанию) +REG_WRITE = 0x8 # Код команды чтения +REG_ID = 0x55 # Адрес регистра с Firmware Update + +def send_can_message(bus, can_id, data): + """Отправка CAN-сообщения""" + try: + msg = can.Message( + arbitration_id=can_id, + data=data, + is_extended_id=False + ) + bus.send(msg) + print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}") + return True + except can.CanError as e: + print(f"Ошибка CAN: {e}") + return False + + +def validate_crc16(data): + """Расчет CRC16 (MODBUS) для проверки целостности данных""" + crc = 0xFFFF + for byte in data: + crc ^= byte + for _ in range(8): + if crc & 0x0001: + crc = (crc >> 1) ^ 0xA001 + else: + crc >>= 1 + return crc + +# Инициализация CAN-интерфейса +bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan') + +# ======= 1. Запрос текущего ID устройства ======= + +# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ +can_id_read = (OLD_DEVICE_ID << 4) | REG_WRITE + +# Данные для запроса: [регистр, резервный байт] +data_read = [REG_ID, 0x00] + +# Формируем полные данные для расчета CRC: +# - CAN ID разбивается на 2 байта (little-endian) +# - Добавляем данные запроса +full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read + +# Рассчитываем CRC и разбиваем на байты (little-endian) +crc = validate_crc16(full_data_for_crc) +crc_bytes = list(crc.to_bytes(2, 'little')) + +# Собираем итоговый пакет: данные + CRC +packet_read = data_read + crc_bytes + +print("Переход в boot режим", packet_read) +send_can_message(bus, can_id_read, packet_read) + +bus.shutdown() + +if __name__ == "__main__": + import sys + if len(sys.argv) != 2: + print("Использование: python3 firmware_test.py address") + sys.exit(1) diff --git a/controller/fw/bootloader/test/st-link.py b/controller/fw/bootloader/test/st-link.py new file mode 100644 index 0000000..1ada522 --- /dev/null +++ b/controller/fw/bootloader/test/st-link.py @@ -0,0 +1,78 @@ +import subprocess +import os +import sys + +def flash_hex_with_stlink(hex_file_path): + if not os.path.isfile(hex_file_path): + print(f"❌ Файл не найден: {hex_file_path}") + return False + + command = [ + "st-flash", + "--format", "ihex", + "write", + hex_file_path + ] + + try: + print(f"⚡️ Прошиваем {hex_file_path} через ST-Link...") + result = subprocess.run( + command, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + universal_newlines=True, + timeout=30 + ) + + print("▬▬▬ STDOUT ▬▬▬") + print(result.stdout) + + print("▬▬▬ STDERR ▬▬▬") + print(result.stderr) + + if result.returncode == 0: + print("✅ Прошивка успешно завершена!") + + # Добавленный блок сброса + try: + print("🔄 Выполняем сброс устройства...") + reset_result = subprocess.run( + ["st-info", "--reset"], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + universal_newlines=True, + timeout=10 + ) + if reset_result.returncode == 0: + print("♻️ Устройство успешно сброшено!") + else: + print(f"⚠️ Ошибка (код: {reset_result.returncode})") + print("▬▬▬ STDERR сброса ▬▬▬") + print(reset_result.stderr) + except Exception as e: + print(f"⚠️ Ошибка при сбросе: {str(e)}") + + return True + else: + print(f"❌ Ошибка прошивки (код: {result.returncode})") + return False + + except FileNotFoundError: + print("❌ st-flash не найден! Установите stlink-tools.") + return False + except subprocess.TimeoutExpired: + print("❌ Таймаут операции! Проверьте подключение ST-Link.") + return False + except Exception as e: + print(f"❌ Неизвестная ошибка: {str(e)}") + return False + +if __name__ == "__main__": + if len(sys.argv) != 2: + print("Использование: python stlink_flash.py ") + sys.exit(1) + + if flash_hex_with_stlink(sys.argv[1]): + sys.exit(0) + else: + sys.exit(1) diff --git a/controller/fw/bootloader/test/st-link_full.py b/controller/fw/bootloader/test/st-link_full.py new file mode 100644 index 0000000..7652e4f --- /dev/null +++ b/controller/fw/bootloader/test/st-link_full.py @@ -0,0 +1,100 @@ +import subprocess +import os +import sys +import time + +def flash_hex_with_stlink(hex_file_path, component_name): + if not os.path.isfile(hex_file_path): + print(f"❌ Файл {component_name} не найден: {hex_file_path}") + return False + + command = [ + "st-flash", + "--format", "ihex", + "write", + hex_file_path + ] + + try: + print(f"⚡️ Прошиваем {component_name} ({hex_file_path}) через ST-Link...") + result = subprocess.run( + command, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + universal_newlines=True, + timeout=30 + ) + + print("▬▬▬ STDOUT ▬▬▬") + print(result.stdout) + + print("▬▬▬ STDERR ▬▬▬") + print(result.stderr) + + if result.returncode == 0: + print(f"✅ {component_name} успешно прошит!") + return True + else: + print(f"❌ Ошибка прошивки {component_name} (код: {result.returncode})") + return False + + except FileNotFoundError: + print("❌ st-flash не найден! Установите stlink-tools.") + return False + except subprocess.TimeoutExpired: + print(f"❌ Таймаут операции при прошивке {component_name}! Проверьте подключение ST-Link.") + return False + except Exception as e: + print(f"❌ Неизвестная ошибка при прошивке {component_name}: {str(e)}") + return False + +def reset_device(): + try: + print("🔄 Выполняем сброс(перезагрузку) устройства...") + reset_result = subprocess.run( + ["st-info", "--reset"], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + universal_newlines=True, + timeout=10 + ) + if reset_result.returncode == 0: + print("♻️ Устройство успешно сброшено!") + return True + else: + print(f"⚠️ Ошибка при сбросе (код: {reset_result.returncode})") + print("▬▬▬ STDERR сброса ▬▬▬") + print(reset_result.stderr) + return False + except Exception as e: + print(f"⚠️ Ошибка при сбросе: {str(e)}") + return False + +if __name__ == "__main__": + if len(sys.argv) != 3: + print("Использование: python stlink_flash.py ") + 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) diff --git a/controller/fw/embed/.gitignore b/controller/fw/embed/.gitignore index b0e6063..2d22b6c 100644 --- a/controller/fw/embed/.gitignore +++ b/controller/fw/embed/.gitignore @@ -7,3 +7,4 @@ .metadata/ cubemx_config/ compile_commands.json +../embed.rar diff --git a/controller/fw/embed/README.md b/controller/fw/embed/README.md deleted file mode 100644 index 61796e3..0000000 --- a/controller/fw/embed/README.md +++ /dev/null @@ -1,20 +0,0 @@ -# Встроенное ПО для сервипривода на STM32F446RE - -## Для разработки - -- [Установить platformio](#introduction) -```bash -pip install -U platformio -``` -- [Скомпилировать проект](#build_project) -```bash -platformio run --environment robotroller_reborn -``` -- [Загрузить прошивку](#upload_project) -```bash -platformio run --target upload --environment robotroller_reborn -``` -- [Открыть монитор UART](#monitor_port) -```bash -platformio device monitor -``` diff --git a/controller/fw/embed/custom_script.ld b/controller/fw/embed/custom_script.ld new file mode 100644 index 0000000..f7fcbbf --- /dev/null +++ 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 + * + *

© Copyright (c) 2020 STMicroelectronics. + * All rights reserved.

+ * + * 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) } +} diff --git a/controller/fw/embed/gen_compile_commands.py b/controller/fw/embed/gen_compile_commands.py index 0537d0b..9fc1948 100644 --- a/controller/fw/embed/gen_compile_commands.py +++ b/controller/fw/embed/gen_compile_commands.py @@ -5,4 +5,4 @@ Import("env") env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True) # override compilation DB path -env.Replace(COMPILATIONDB_PATH="compile_commands.json") +env.Replace(COMPILATIONDB_PATH="compile_commands.json") \ No newline at end of file diff --git a/controller/fw/embed/hex_compile.py b/controller/fw/embed/hex_compile.py new file mode 100644 index 0000000..157a1e4 --- /dev/null +++ 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") +) \ No newline at end of file diff --git a/controller/fw/embed/include/config.h b/controller/fw/embed/include/config.h new file mode 100644 index 0000000..9746177 --- /dev/null +++ b/controller/fw/embed/include/config.h @@ -0,0 +1,31 @@ +#pragma once +#include "Arduino.h" +#include +#include +#include +#include +#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); diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index f15b6ea..01ec21b 100644 --- a/controller/fw/embed/include/flash.h +++ b/controller/fw/embed/include/flash.h @@ -4,27 +4,36 @@ #include #include - /* 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_ */ diff --git a/controller/fw/embed/include/process_can.h b/controller/fw/embed/include/process_can.h new file mode 100644 index 0000000..90e4c66 --- /dev/null +++ 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); \ No newline at end of file diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index dbbfdc7..68f2405 100644 --- 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_ diff --git a/controller/fw/embed/platformio.ini b/controller/fw/embed/platformio.ini index a759816..a15e7c9 100644 --- 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 = - -DSTM32F446xx - -D HAL_CAN_MODULE_ENABLED - -D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH + -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 + askuric/Simple FOC@^2.3.4 + pazi88/STM32_CAN@^1.1.2 + +extra_scripts = + pre:gen_compile_commands.py + post:hex_compile.py diff --git a/controller/fw/embed/src/config.cpp b/controller/fw/embed/src/config.cpp new file mode 100644 index 0000000..1a5c610 --- /dev/null +++ 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(); +} diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp index f22bc51..1e68b28 100644 --- a/controller/fw/embed/src/flash.cpp +++ b/controller/fw/embed/src/flash.cpp @@ -2,10 +2,8 @@ #include #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, uint8_t val) { - FLASH_RECORD param_flash = {param_id, val}; - // __disable_irq(); // Запрещаем прерывания на время всей операции - - param_flash.crc = validate_crc16((uint8_t*)¶m_flash,sizeof(param_flash) - 2);//Нахождение CRC для данных, хранящихся во флэш памяти +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(¶m_flash); - // Проверка выравнивания ДО проверки границ сектора кратного 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, ¶m_flash); //внутри функции итак автоматические инкрементируется указатель write_ptr на размер структуры + flash_write(write_ptr, ¶m_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; - else{ + + + 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; + } } - write_ptr = addr + FLASH_RECORD_SIZE; -} __enable_irq(); return latest; -} \ No newline at end of file +} + + diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 8dd8e66..79eede0 100644 --- 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 @@ -6,21 +5,23 @@ #include #include #include +#include #include #include "common/base_classes/FOCMotor.h" #include "hal_conf_extra.h" #include "wiring_analog.h" #include "wiring_constants.h" // clang-format on + #include "reg_cah.h" #include "flash.h" - +#include "config.h" +#include "process_can.h" void SysTick_Handler(void) { HAL_IncTick(); } - STM32_CAN Can(CAN2, DEF); /* for FLASH */ uint32_t flash_flag; @@ -29,18 +30,14 @@ uint32_t flash_error; FLASH_EraseInitTypeDef pEraseInit; uint32_t SectorError; -volatile uint16_t msg_id; -volatile uint16_t id_x; -volatile uint8_t msg_ch; -volatile uint8_t crc_h; -volatile uint8_t crc_l; -volatile float kt = 0.1; //for torgue calculation +/* bool for test CAN */ +volatile bool CAN_GET = false; + +volatile float kt = 0.1; // Torque calculation constant + +FLASH_RECORD* flash_rec; -static FLASH_RECORD* flash_rec; -static FLASH_RECORD flash_buf[PARAM_COUNT]; -static CAN_message_t CAN_TX_msg; -static CAN_message_t CAN_inMsg; SPIClass spi; MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK); @@ -53,430 +50,66 @@ DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER, LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1, CURRENT_SENSOR_2, CURRENT_SENSOR_3); -Commander command(Serial); - -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); -} + 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 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',¤t_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; + void setup(){ + SCB->VTOR = (volatile uint32_t)0x08008004; - else if(l == 1) - data_send = (float)(d * 10); + Serial.setRx(HARDWARE_SERIAL_RX_PIN); + Serial.setTx(HARDWARE_SERIAL_TX_PIN); + Serial.begin(115200); - 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, чтобы выбрать, что делать + pinMode(PC11, OUTPUT); + pinMode(PC10,OUTPUT); + GPIOC->ODR &= ~GPIO_ODR_OD10; + // Can.enableMBInterrupts(); + Can.begin(); + Can.setBaudRate(1000000); + // Настройка прерываний 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, ¤t_sense,flash_rec); - /* Вычисление 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; - } + CAN2->IER |= CAN_IER_FMPIE0 | // Сообщение в FIFO0 + CAN_IER_FFIE0 | // FIFO0 full + CAN_IER_FOVIE0; // FIFO0 overflow - /* 0x691 - 69 - адрес устройства - 1 - что делать дальше с данными */ - - if(id_x == flash_rec->value){ - if(msg_ch == REG_WRITE){ - switch(msg.buf[0]) { - case REG_ID: - /* setup new id */ - setup_id(msg.buf[1]); - break; - - case REG_LED_BLINK: - for (int i = 0; i < 10; i++) { - GPIOC->ODR ^= GPIO_ODR_OD10; - delay(100); - } - break; - - case MOTOR_ANGLE: - memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1], - sizeof(motor_control_inputs.target_angle)); - setup_angle(motor_control_inputs.target_angle); - break; - - case REG_MOTOR_POSPID_Kp: - setup_pid_angle(pid_p,msg.buf[1]); - break; - - case REG_MOTOR_POSPID_Ki: - setup_pid_angle(pid_i,msg.buf[1]); - break; - - case REG_MOTOR_POSPID_Kd: - setup_pid_angle(pid_d,msg.buf[1]); - break; - - case MOTOR_ENABLED: - if (msg.buf[1] == 1) { - motor.enable(); - motor_control_inputs.motor_enabled = 1; - } else { - motor.disable(); - motor_control_inputs.motor_enabled = 0; - } - - default: - break; - } - } - else if (msg_ch == REG_READ) { - switch (msg.buf[0]) { - case REG_ID: - send_id(); - break; - case MOTOR_VELOCITY: - send_velocity(); - break; - - case MOTOR_ANGLE: - send_angle(); - break; - - case MOTOR_ENABLED: - send_motor_enabled(); - break; - - case MOTOR_TORQUE: - send_motor_torque(); - break; - - case FOC_STATE: - send_foc_state(); - break; - - case REG_MOTOR_POSPID_Kp: - send_pid(pid_p); - break; - - case REG_MOTOR_POSPID_Ki: - send_pid(pid_i); - break; - - case REG_MOTOR_POSPID_Kd: - send_pid(pid_d); - break; - - - - default: - break; - } - } -} -} - - - - -volatile uint32_t ipsr_value = 0; - - -void foc_step(BLDCMotor *motor, Commander *commander) { - if (motor_control_inputs.target_velocity != 0 || - motor->controller == MotionControlType::velocity) { - if (motor->controller != MotionControlType::velocity) { - motor->controller = MotionControlType::velocity; - } - motor->target = motor_control_inputs.target_velocity; - - } else { - if (motor->controller != MotionControlType::angle) { - motor->controller = MotionControlType::angle; - } - motor->target = motor_control_inputs.target_angle; - } - - motor->loopFOC(); - motor->move(); - motor->monitor(); - commander->run(); -} - - - - -void setup(){ - /* bias for vector int */ - // __set_MSP(*(volatile uint32_t*)0x08008000); - // SCB->VTOR = (volatile uint32_t)0x08008000; -Serial.setRx(HARDWARE_SERIAL_RX_PIN); -Serial.setTx(HARDWARE_SERIAL_TX_PIN); -Serial.begin(115200); - -pinMode(PC11, OUTPUT); -pinMode(PC10,OUTPUT); -GPIOC->ODR &= ~GPIO_ODR_OD10; -// Setup thermal sensor pin -// pinMode(TH1, INPUT_ANALOG); -Can.begin(); -Can.setBaudRate(1000000); -TIM_TypeDef *Instance = TIM2; -HardwareTimer *SendTimer = new HardwareTimer(Instance); -// SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz -// SendTimer->attachInterrupt(send_data); -// SendTimer->resume(); - flash_rec = load_params(); - for(int i = 0;i < PARAM_COUNT;i++) - flash_buf[i] = flash_rec[i]; - setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); - GPIOC->ODR |= GPIO_ODR_OD11; + // 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; } } + diff --git a/controller/fw/embed/src/process_can.cpp b/controller/fw/embed/src/process_can.cpp new file mode 100644 index 0000000..08ad180 --- /dev/null +++ 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 +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',¤t_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; + } + } + } +} diff --git a/controller/fw/embed/test/python_enable_motor.py b/controller/fw/embed/test/enable_motor.py similarity index 100% rename from controller/fw/embed/test/python_enable_motor.py rename to controller/fw/embed/test/enable_motor.py diff --git a/controller/fw/embed/test/firmware_can.py b/controller/fw/embed/test/firmware_can.py new file mode 100644 index 0000000..37acbc3 --- /dev/null +++ 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]) diff --git a/controller/fw/embed/test/firmware_update_flag.py b/controller/fw/embed/test/firmware_update_flag.py new file mode 100644 index 0000000..748cf6b --- /dev/null +++ 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) diff --git a/controller/fw/embed/test/pid_p.py b/controller/fw/embed/test/pid_p.py new file mode 100644 index 0000000..ee3b444 --- /dev/null +++ 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('= 2: # Enable/Disable - enabled = msg.data[1] # Expecting 1 byte (0 or 1) - print(f"Enabled: {bool(enabled)}") - else: - print(f"Unknown flag: {flag}") - else: - print(f"Received message with unexpected length: {msg.dlc}") - -def receive_can_messages(): - try: - # Connect to the CAN bus - bus = can.interface.Bus(channel='can0', bustype='socketcan') - - print("Waiting for messages on the CAN bus...") - - while True: - msg = bus.recv() - if msg: - process_can_message(msg) - - except KeyboardInterrupt: - print("\nExiting program...") - except Exception as e: - print(f"Error: {e}") - -if __name__ == '__main__': - receive_can_messages() diff --git a/controller/fw/embed/test/python_send_angle.py b/controller/fw/embed/test/python_send_angle.py deleted file mode 100644 index dc23d9b..0000000 --- a/controller/fw/embed/test/python_send_angle.py +++ /dev/null @@ -1,37 +0,0 @@ -import can -import struct -import time -import argparse - -# Function to send the target angle -def send_target_angle(bus, target_angle): - msg = can.Message() - msg.arbitration_id = 1 # Message ID - msg.is_extended_id = False - msg.dlc = 5 # Message length - msg.data = [ord('A')] + list(struct.pack('> 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('> 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('> 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) diff --git a/controller/fw/embed/test/send_angle.py b/controller/fw/embed/test/send_angle.py new file mode 100644 index 0000000..11e1d86 --- /dev/null +++ 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('> 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) diff --git a/controller/fw/embed/test/st-link.py b/controller/fw/embed/test/st-link.py new file mode 100644 index 0000000..1ada522 --- /dev/null +++ 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 ") + sys.exit(1) + + if flash_hex_with_stlink(sys.argv[1]): + sys.exit(0) + else: + sys.exit(1) diff --git a/controller/fw/embed/test/st-link_full.py b/controller/fw/embed/test/st-link_full.py new file mode 100644 index 0000000..7652e4f --- /dev/null +++ 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 ") + 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) diff --git a/controller/fw/embed/test/writePID_angle_parametrs.py b/controller/fw/embed/test/writePID_angle_parametrs.py new file mode 100644 index 0000000..14ab26f --- /dev/null +++ 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('> 1) ^ 0xA001 + else: + crc >>= 1 + return crc + +# Инициализация +bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan') +# Перевод float -> hex -> int +result = (struct.unpack('