commit ba3e1161b9b06f084b850b5bb2eb7406f51e3053 Author: Sebastien Vayrette Date: Sat Feb 28 11:58:23 2026 -0500 Balance firmware + USB CDC bug diff --git a/.pio/build/f722/.sconsign39.dblite b/.pio/build/f722/.sconsign39.dblite new file mode 100644 index 0000000..b96eb99 Binary files /dev/null and b/.pio/build/f722/.sconsign39.dblite differ diff --git a/.pio/build/f722/FrameworkCMSISDevice/gcc/startup_stm32f722xx.o b/.pio/build/f722/FrameworkCMSISDevice/gcc/startup_stm32f722xx.o new file mode 100644 index 0000000..988a3cc Binary files /dev/null and b/.pio/build/f722/FrameworkCMSISDevice/gcc/startup_stm32f722xx.o differ diff --git a/.pio/build/f722/FrameworkCMSISDevice/system_stm32f7xx.o b/.pio/build/f722/FrameworkCMSISDevice/system_stm32f7xx.o new file mode 100644 index 0000000..b466d78 Binary files /dev/null and b/.pio/build/f722/FrameworkCMSISDevice/system_stm32f7xx.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal.o new file mode 100644 index 0000000..90bc082 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_adc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_adc.o new file mode 100644 index 0000000..44ce2d3 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_adc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_adc_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_adc_ex.o new file mode 100644 index 0000000..107e14b Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_adc_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_can.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_can.o new file mode 100644 index 0000000..ab9d5d2 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_can.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cec.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cec.o new file mode 100644 index 0000000..d8750d5 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cec.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cortex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cortex.o new file mode 100644 index 0000000..0d7607e Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cortex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_crc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_crc.o new file mode 100644 index 0000000..e58c1b7 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_crc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_crc_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_crc_ex.o new file mode 100644 index 0000000..2e11e33 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_crc_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cryp.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cryp.o new file mode 100644 index 0000000..18d61d0 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cryp.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cryp_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cryp_ex.o new file mode 100644 index 0000000..8baa42d Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_cryp_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dac.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dac.o new file mode 100644 index 0000000..634c7fd Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dac.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dac_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dac_ex.o new file mode 100644 index 0000000..03ff12b Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dac_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dcmi.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dcmi.o new file mode 100644 index 0000000..786adab Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dcmi.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dcmi_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dcmi_ex.o new file mode 100644 index 0000000..0e183be Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dcmi_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dfsdm.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dfsdm.o new file mode 100644 index 0000000..5b9dd48 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dfsdm.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma.o new file mode 100644 index 0000000..9e1cde0 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma2d.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma2d.o new file mode 100644 index 0000000..c184823 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma2d.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma_ex.o new file mode 100644 index 0000000..994fd3c Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dma_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dsi.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dsi.o new file mode 100644 index 0000000..e8c0cc4 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_dsi.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_eth.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_eth.o new file mode 100644 index 0000000..b9bb414 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_eth.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_exti.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_exti.o new file mode 100644 index 0000000..2d585c4 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_exti.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_flash.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_flash.o new file mode 100644 index 0000000..0555039 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_flash.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_flash_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_flash_ex.o new file mode 100644 index 0000000..b5742bb Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_flash_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_gpio.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_gpio.o new file mode 100644 index 0000000..6280210 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_gpio.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hash.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hash.o new file mode 100644 index 0000000..041cc34 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hash.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hash_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hash_ex.o new file mode 100644 index 0000000..02161c7 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hash_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hcd.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hcd.o new file mode 100644 index 0000000..8671a3c Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_hcd.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2c.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2c.o new file mode 100644 index 0000000..fbf2734 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2c.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2c_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2c_ex.o new file mode 100644 index 0000000..a4f5498 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2c_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2s.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2s.o new file mode 100644 index 0000000..cafb4aa Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_i2s.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_irda.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_irda.o new file mode 100644 index 0000000..f79b9db Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_irda.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_iwdg.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_iwdg.o new file mode 100644 index 0000000..f1ee09e Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_iwdg.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_jpeg.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_jpeg.o new file mode 100644 index 0000000..036067e Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_jpeg.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_lptim.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_lptim.o new file mode 100644 index 0000000..3c2dbcc Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_lptim.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_ltdc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_ltdc.o new file mode 100644 index 0000000..a38c178 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_ltdc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_ltdc_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_ltdc_ex.o new file mode 100644 index 0000000..f4b649b Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_ltdc_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_mdios.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_mdios.o new file mode 100644 index 0000000..c8a7b7d Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_mdios.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_mmc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_mmc.o new file mode 100644 index 0000000..84ad917 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_mmc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_nand.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_nand.o new file mode 100644 index 0000000..a6183f9 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_nand.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_nor.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_nor.o new file mode 100644 index 0000000..ace500a Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_nor.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pcd.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pcd.o new file mode 100644 index 0000000..7143514 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pcd.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pcd_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pcd_ex.o new file mode 100644 index 0000000..f52570c Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pcd_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pwr.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pwr.o new file mode 100644 index 0000000..58e595e Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pwr.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pwr_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pwr_ex.o new file mode 100644 index 0000000..f19d8ac Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_pwr_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_qspi.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_qspi.o new file mode 100644 index 0000000..b5a2b52 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_qspi.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rcc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rcc.o new file mode 100644 index 0000000..660b0ca Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rcc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rcc_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rcc_ex.o new file mode 100644 index 0000000..53a4461 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rcc_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rng.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rng.o new file mode 100644 index 0000000..ff6945a Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rng.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rtc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rtc.o new file mode 100644 index 0000000..d1ba7b6 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rtc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rtc_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rtc_ex.o new file mode 100644 index 0000000..99c8106 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_rtc_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sai.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sai.o new file mode 100644 index 0000000..fbb0ef9 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sai.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sai_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sai_ex.o new file mode 100644 index 0000000..5a05a7a Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sai_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sd.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sd.o new file mode 100644 index 0000000..563e2f1 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sd.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sdram.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sdram.o new file mode 100644 index 0000000..44163d8 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sdram.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smartcard.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smartcard.o new file mode 100644 index 0000000..aa3dccd Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smartcard.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smartcard_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smartcard_ex.o new file mode 100644 index 0000000..9900c78 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smartcard_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smbus.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smbus.o new file mode 100644 index 0000000..f62b7e5 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_smbus.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spdifrx.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spdifrx.o new file mode 100644 index 0000000..cad5c8c Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spdifrx.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spi.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spi.o new file mode 100644 index 0000000..4056031 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spi.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spi_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spi_ex.o new file mode 100644 index 0000000..d31d783 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_spi_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sram.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sram.o new file mode 100644 index 0000000..ab6c9b3 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_sram.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_tim.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_tim.o new file mode 100644 index 0000000..51b3311 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_tim.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_tim_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_tim_ex.o new file mode 100644 index 0000000..e1cdce2 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_tim_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_uart.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_uart.o new file mode 100644 index 0000000..a98df6b Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_uart.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_uart_ex.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_uart_ex.o new file mode 100644 index 0000000..b62bd8c Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_uart_ex.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_usart.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_usart.o new file mode 100644 index 0000000..5206940 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_usart.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_wwdg.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_wwdg.o new file mode 100644 index 0000000..913ad7b Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_hal_wwdg.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_adc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_adc.o new file mode 100644 index 0000000..bcf9dc6 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_adc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_crc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_crc.o new file mode 100644 index 0000000..8ad44d1 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_crc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dac.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dac.o new file mode 100644 index 0000000..4b7b332 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dac.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dma.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dma.o new file mode 100644 index 0000000..787f308 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dma.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dma2d.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dma2d.o new file mode 100644 index 0000000..5c5b8b6 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_dma2d.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_exti.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_exti.o new file mode 100644 index 0000000..215d2a4 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_exti.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_fmc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_fmc.o new file mode 100644 index 0000000..839819c Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_fmc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_gpio.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_gpio.o new file mode 100644 index 0000000..42b4b46 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_gpio.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_i2c.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_i2c.o new file mode 100644 index 0000000..a304656 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_i2c.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_lptim.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_lptim.o new file mode 100644 index 0000000..f500722 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_lptim.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_pwr.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_pwr.o new file mode 100644 index 0000000..3fff953 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_pwr.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rcc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rcc.o new file mode 100644 index 0000000..7370fbb Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rcc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rng.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rng.o new file mode 100644 index 0000000..6ee6065 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rng.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rtc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rtc.o new file mode 100644 index 0000000..a1e78d9 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_rtc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_sdmmc.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_sdmmc.o new file mode 100644 index 0000000..2e0a73e Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_sdmmc.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_spi.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_spi.o new file mode 100644 index 0000000..c9cfbd7 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_spi.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_tim.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_tim.o new file mode 100644 index 0000000..704de39 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_tim.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_usart.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_usart.o new file mode 100644 index 0000000..a782c15 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_usart.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_usb.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_usb.o new file mode 100644 index 0000000..9657914 Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_usb.o differ diff --git a/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_utils.o b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_utils.o new file mode 100644 index 0000000..717136d Binary files /dev/null and b/.pio/build/f722/FrameworkHALDriver/Src/stm32f7xx_ll_utils.o differ diff --git a/.pio/build/f722/firmware.bin b/.pio/build/f722/firmware.bin new file mode 100755 index 0000000..1f82f8c Binary files /dev/null and b/.pio/build/f722/firmware.bin differ diff --git a/.pio/build/f722/firmware.elf b/.pio/build/f722/firmware.elf new file mode 100755 index 0000000..a11c4bd Binary files /dev/null and b/.pio/build/f722/firmware.elf differ diff --git a/.pio/build/f722/lib737/USB_CDC/usbd_cdc.o b/.pio/build/f722/lib737/USB_CDC/usbd_cdc.o new file mode 100644 index 0000000..1fbf3fe Binary files /dev/null and b/.pio/build/f722/lib737/USB_CDC/usbd_cdc.o differ diff --git a/.pio/build/f722/lib737/USB_CDC/usbd_cdc_if.o b/.pio/build/f722/lib737/USB_CDC/usbd_cdc_if.o new file mode 100644 index 0000000..71e9635 Binary files /dev/null and b/.pio/build/f722/lib737/USB_CDC/usbd_cdc_if.o differ diff --git a/.pio/build/f722/lib737/USB_CDC/usbd_conf.o b/.pio/build/f722/lib737/USB_CDC/usbd_conf.o new file mode 100644 index 0000000..f11579d Binary files /dev/null and b/.pio/build/f722/lib737/USB_CDC/usbd_conf.o differ diff --git a/.pio/build/f722/lib737/USB_CDC/usbd_core.o b/.pio/build/f722/lib737/USB_CDC/usbd_core.o new file mode 100644 index 0000000..f5a38c0 Binary files /dev/null and b/.pio/build/f722/lib737/USB_CDC/usbd_core.o differ diff --git a/.pio/build/f722/lib737/USB_CDC/usbd_ctlreq.o b/.pio/build/f722/lib737/USB_CDC/usbd_ctlreq.o new file mode 100644 index 0000000..f7e012e Binary files /dev/null and b/.pio/build/f722/lib737/USB_CDC/usbd_ctlreq.o differ diff --git a/.pio/build/f722/lib737/USB_CDC/usbd_desc.o b/.pio/build/f722/lib737/USB_CDC/usbd_desc.o new file mode 100644 index 0000000..9c54bde Binary files /dev/null and b/.pio/build/f722/lib737/USB_CDC/usbd_desc.o differ diff --git a/.pio/build/f722/lib737/USB_CDC/usbd_ioreq.o b/.pio/build/f722/lib737/USB_CDC/usbd_ioreq.o new file mode 100644 index 0000000..70d3279 Binary files /dev/null and b/.pio/build/f722/lib737/USB_CDC/usbd_ioreq.o differ diff --git a/.pio/build/f722/lib737/libUSB_CDC.a b/.pio/build/f722/lib737/libUSB_CDC.a new file mode 100644 index 0000000..e79f38b Binary files /dev/null and b/.pio/build/f722/lib737/libUSB_CDC.a differ diff --git a/.pio/build/f722/libFrameworkCMSISDevice.a b/.pio/build/f722/libFrameworkCMSISDevice.a new file mode 100644 index 0000000..ed9304d Binary files /dev/null and b/.pio/build/f722/libFrameworkCMSISDevice.a differ diff --git a/.pio/build/f722/src/balance.o b/.pio/build/f722/src/balance.o new file mode 100644 index 0000000..212bc7e Binary files /dev/null and b/.pio/build/f722/src/balance.o differ diff --git a/.pio/build/f722/src/bmp280.o b/.pio/build/f722/src/bmp280.o new file mode 100644 index 0000000..37b624d Binary files /dev/null and b/.pio/build/f722/src/bmp280.o differ diff --git a/.pio/build/f722/src/hoverboard.o b/.pio/build/f722/src/hoverboard.o new file mode 100644 index 0000000..5522f21 Binary files /dev/null and b/.pio/build/f722/src/hoverboard.o differ diff --git a/.pio/build/f722/src/icm42688.o b/.pio/build/f722/src/icm42688.o new file mode 100644 index 0000000..eabb04e Binary files /dev/null and b/.pio/build/f722/src/icm42688.o differ diff --git a/.pio/build/f722/src/main.o b/.pio/build/f722/src/main.o new file mode 100644 index 0000000..0c477bd Binary files /dev/null and b/.pio/build/f722/src/main.o differ diff --git a/.pio/build/f722/src/status.o b/.pio/build/f722/src/status.o new file mode 100644 index 0000000..a6a8961 Binary files /dev/null and b/.pio/build/f722/src/status.o differ diff --git a/.pio/build/project.checksum b/.pio/build/project.checksum new file mode 100644 index 0000000..384b04b --- /dev/null +++ b/.pio/build/project.checksum @@ -0,0 +1 @@ +ee8efb31f6b185f16e4d385971f1a0e3291fe5fd \ No newline at end of file diff --git a/flash.sh b/flash.sh new file mode 100755 index 0000000..c6ee599 --- /dev/null +++ b/flash.sh @@ -0,0 +1,13 @@ +#!/bin/bash +# Flash SaltyLab — auto-reboot to DFU if serial port exists +PORT=/dev/cu.usbmodemSALTY0011 +FW=.pio/build/f722/firmware.bin + +if [ -e "$PORT" ]; then + echo "Sending reboot-to-DFU..." + echo -n 'R' > "$PORT" + sleep 2 +fi + +echo "Flashing..." +/opt/homebrew/bin/dfu-util -a 0 -s 0x08000000:leave -D "$FW" diff --git a/include/balance.h b/include/balance.h new file mode 100644 index 0000000..ac30a20 --- /dev/null +++ b/include/balance.h @@ -0,0 +1,46 @@ +#ifndef BALANCE_H +#define BALANCE_H + +#include +#include "icm42688.h" + +/* + * SaltyLab Balance Controller + * + * Complementary filter (gyro + accel) → pitch angle + * PID controller → motor speed command + * Safety: tilt cutoff, arming, watchdog + */ + +typedef enum { + BALANCE_DISARMED = 0, /* Motors off, waiting for arm command */ + BALANCE_ARMED, /* Active balancing */ + BALANCE_TILT_FAULT, /* Tilt exceeded limit, motors killed */ +} balance_state_t; + +typedef struct { + /* State */ + balance_state_t state; + float pitch_deg; /* Current pitch angle (degrees) */ + float pitch_rate; /* Gyro pitch rate (deg/s) */ + + /* PID internals */ + float integral; + float prev_error; + int16_t motor_cmd; /* Output to ESC: -1000..+1000 */ + + /* Tuning */ + float kp, ki, kd; + float setpoint; /* Target pitch angle (degrees) — tune for COG offset */ + + /* Safety */ + float max_tilt; /* Cutoff angle (degrees) */ + int16_t max_speed; /* Speed limit */ +} balance_t; + +void balance_init(balance_t *b); +void balance_update(balance_t *b, const icm42688_data_t *imu, float dt); +void balance_arm(balance_t *b); +void balance_disarm(balance_t *b); + +#endif diff --git a/include/bmp280.h b/include/bmp280.h new file mode 100644 index 0000000..82519a1 --- /dev/null +++ b/include/bmp280.h @@ -0,0 +1,6 @@ +#ifndef BMP280_H +#define BMP280_H +#include +int bmp280_init(void); +void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10); +#endif diff --git a/include/config.h b/include/config.h new file mode 100644 index 0000000..28671cd --- /dev/null +++ b/include/config.h @@ -0,0 +1,143 @@ +#ifndef CONFIG_H +#define CONFIG_H + +// ============================================ +// SaltyLab Balance Bot — MAMBA F722S FC +// Pin assignments from Betaflight: DIAT-MAMBAF722_2022B +// ============================================ + +// --- IMU: MPU6000 (SPI1) --- +// SPI1: PA5=SCK, PA6=MISO, PA7=MOSI +// WHO_AM_I = 0x68 +#define MPU_SPI SPI1 +#define MPU_CS_PORT GPIOA +#define MPU_CS_PIN GPIO_PIN_4 // GYRO_CS 1 +#define MPU_EXTI_PORT GPIOC +#define MPU_EXTI_PIN GPIO_PIN_4 // GYRO_EXTI 1 (data ready IRQ) +#define GYRO_ALIGN CW270 // gyro_1_sensor_align = CW270 + +// --- Barometer: BMP280 or DPS310 (I2C1) --- +#define BARO_I2C I2C1 +#define BARO_SCL_PORT GPIOB +#define BARO_SCL_PIN GPIO_PIN_8 // I2C_SCL 1 +#define BARO_SDA_PORT GPIOB +#define BARO_SDA_PIN GPIO_PIN_9 // I2C_SDA 1 +// Magnetometer also on I2C1 (external, header only) + +// --- LEDs --- +#define LED1_PORT GPIOC +#define LED1_PIN GPIO_PIN_15 // LED 1 (active low) +#define LED2_PORT GPIOC +#define LED2_PIN GPIO_PIN_14 // LED 2 (active low) + +// --- Buzzer --- +#define BEEPER_PORT GPIOB +#define BEEPER_PIN GPIO_PIN_2 // BEEPER 1 +#define BEEPER_INVERTED 1 // beeper_inversion = ON +// beeper_od = OFF (push-pull) + +// --- Battery Monitoring (ADC3) --- +#define ADC_VBAT_PORT GPIOC +#define ADC_VBAT_PIN GPIO_PIN_1 // ADC_BATT 1 +#define ADC_CURR_PORT GPIOC +#define ADC_CURR_PIN GPIO_PIN_3 // ADC_CURR 1 +#define ADC_IBAT_SCALE 115 // ibata_scale + +// --- LED Strip (WS2812) --- +#define LED_STRIP_PORT GPIOB +#define LED_STRIP_PIN GPIO_PIN_3 // LED_STRIP 1 (TIM2_CH2) + +// --- OSD: MAX7456 (SPI2) --- +#define OSD_SPI SPI2 +#define OSD_CS_PORT GPIOB +#define OSD_CS_PIN GPIO_PIN_12 // OSD_CS 1 +// SPI2: PB13=SCK, PB14=MISO, PB15=MOSI + +// --- Blackbox Flash: M25P16 (SPI3) --- +#define FLASH_SPI SPI3 +#define FLASH_CS_PORT GPIOA +#define FLASH_CS_PIN GPIO_PIN_15 // FLASH_CS 1 +// SPI3: PC10=SCK, PC11=MISO, PB5=MOSI + +// --- Motor Outputs (PWM/DShot) --- +#define MOTOR1_PORT GPIOC +#define MOTOR1_PIN GPIO_PIN_8 // TIM8_CH3 +#define MOTOR2_PORT GPIOC +#define MOTOR2_PIN GPIO_PIN_9 // TIM8_CH4 +#define MOTOR3_PORT GPIOA +#define MOTOR3_PIN GPIO_PIN_8 // TIM1_CH1 +#define MOTOR4_PORT GPIOA +#define MOTOR4_PIN GPIO_PIN_9 // TIM1_CH2 +#define MOTOR5_PORT GPIOB +#define MOTOR5_PIN GPIO_PIN_0 // TIM3_CH3 +#define MOTOR6_PORT GPIOB +#define MOTOR6_PIN GPIO_PIN_1 // TIM3_CH4 +#define MOTOR7_PORT GPIOA +#define MOTOR7_PIN GPIO_PIN_10 // TIM1_CH3 +#define MOTOR8_PORT GPIOB +#define MOTOR8_PIN GPIO_PIN_4 // TIM3_CH1 + +// --- UARTs --- +// USART1: PB6=TX, PB7=RX (serial 0, SmartAudio/VTX) +#define UART1_TX_PORT GPIOB +#define UART1_TX_PIN GPIO_PIN_6 +#define UART1_RX_PORT GPIOB +#define UART1_RX_PIN GPIO_PIN_7 + +// USART2: PA2=TX, PA3=RX (serial 1) +#define UART2_TX_PORT GPIOA +#define UART2_TX_PIN GPIO_PIN_2 +#define UART2_RX_PORT GPIOA +#define UART2_RX_PIN GPIO_PIN_3 + +// USART3: PB10=TX, PB11=RX (serial 2, SBUS RX default) +#define UART3_TX_PORT GPIOB +#define UART3_TX_PIN GPIO_PIN_10 +#define UART3_RX_PORT GPIOB +#define UART3_RX_PIN GPIO_PIN_11 + +// UART4: PA0=TX, PA1=RX (serial 3) +#define UART4_TX_PORT GPIOA +#define UART4_TX_PIN GPIO_PIN_0 +#define UART4_RX_PORT GPIOA +#define UART4_RX_PIN GPIO_PIN_1 + +// UART5: PC12=TX, PD2=RX (serial 4) +#define UART5_TX_PORT GPIOC +#define UART5_TX_PIN GPIO_PIN_12 +#define UART5_RX_PORT GPIOD +#define UART5_RX_PIN GPIO_PIN_2 + +// USART6: PC6=TX, PC7=RX (serial 5) +#define UART6_TX_PORT GPIOC +#define UART6_TX_PIN GPIO_PIN_6 +#define UART6_RX_PORT GPIOC +#define UART6_RX_PIN GPIO_PIN_7 + +// --- PINIO (switchable outputs, e.g. VTX power) --- +#define PINIO1_PORT GPIOC +#define PINIO1_PIN GPIO_PIN_2 // pinio_config = 129 (USER1) +#define PINIO2_PORT GPIOC +#define PINIO2_PIN GPIO_PIN_0 // pinio_config = 129 (USER2) + +// --- SaltyLab Assignments --- +// Hoverboard ESC: USART2 (PA2=TX, PA3=RX) or USART3 +// ELRS Receiver: UART4 (PA0=TX, PA1=RX) — CRSF 420000 baud +// Jetson: USART6 (PC6=TX, PC7=RX) +// Debug: UART5 (PC12=TX, PD2=RX) + +// --- PID Tuning --- +#define PID_KP 35.0f +#define PID_KI 1.0f +#define PID_KD 1.0f +#define PID_INTEGRAL_MAX 500.0f +#define PID_LOOP_HZ 1000 + +// --- Safety --- +#define MAX_TILT_DEG 25.0f +#define RC_TIMEOUT_MS 500 +#define ARMING_HOLD_MS 3000 +#define MAX_SPEED_LIMIT 100 +#define WATCHDOG_TIMEOUT_MS 50 + +#endif // CONFIG_H diff --git a/include/crsf.h b/include/crsf.h new file mode 100644 index 0000000..36d495d --- /dev/null +++ b/include/crsf.h @@ -0,0 +1,19 @@ +#ifndef CRSF_H +#define CRSF_H + +#include +#include + +typedef struct { + uint16_t channels[16]; // Raw CRSF values (172-1811) + uint32_t last_rx_ms; // Timestamp of last valid frame + bool armed; // Arm switch state +} CRSFState; + +void crsf_init(void); +void crsf_parse_byte(uint8_t byte); +int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max); + +extern volatile CRSFState crsf_state; + +#endif diff --git a/include/hoverboard.h b/include/hoverboard.h new file mode 100644 index 0000000..43836db --- /dev/null +++ b/include/hoverboard.h @@ -0,0 +1,29 @@ +#ifndef HOVERBOARD_H +#define HOVERBOARD_H + +#include + +/* + * Hoverboard ESC UART protocol (EFeru FOC firmware) + * USART2: PA2=TX, PA3=RX @ 115200 baud + * + * Packet: [0xABCD] [steer:i16] [speed:i16] [checksum:u16] + * Checksum = start ^ steer ^ speed + * Speed range: -1000 to +1000 + * Must send at >=50Hz or ESC times out (TIMEOUT=20 * DELAY_IN_MAIN_LOOP=5ms = 100ms) + */ + +#define HOVERBOARD_START_FRAME 0xABCD +#define HOVERBOARD_BAUD 115200 + +typedef struct __attribute__((packed)) { + uint16_t start; + int16_t steer; + int16_t speed; + uint16_t checksum; +} hoverboard_cmd_t; + +void hoverboard_init(void); +void hoverboard_send(int16_t speed, int16_t steer); + +#endif diff --git a/include/icm42688.h b/include/icm42688.h new file mode 100644 index 0000000..47e7463 --- /dev/null +++ b/include/icm42688.h @@ -0,0 +1,10 @@ +#ifndef ICM42688_H +#define ICM42688_H +#include +typedef struct { + int16_t ax, ay, az, gx, gy, gz, temp_x10; +} icm42688_data_t; +int icm42688_init(void); +void icm42688_read(icm42688_data_t *d); +void icm42688_get_trace(uint8_t *out, int *len); +#endif diff --git a/include/mpu6000.h b/include/mpu6000.h new file mode 100644 index 0000000..3f0bd4f --- /dev/null +++ b/include/mpu6000.h @@ -0,0 +1,17 @@ +#ifndef MPU6000_H +#define MPU6000_H + +#include +#include + +typedef struct { + float pitch; // degrees, filtered + float pitch_rate; // degrees/sec (raw gyro) + float accel_x; // g + float accel_z; // g +} IMUData; + +bool mpu6000_init(void); +void mpu6000_read(IMUData *data); + +#endif diff --git a/include/status.h b/include/status.h new file mode 100644 index 0000000..9badbb6 --- /dev/null +++ b/include/status.h @@ -0,0 +1,7 @@ +#ifndef STATUS_H +#define STATUS_H +#include +void status_init(void); +void status_boot_beep(void); +void status_update(uint32_t tick, int imu_ok, int baro_ok); +#endif diff --git a/lib/USB_CDC/include/usbd_cdc.h b/lib/USB_CDC/include/usbd_cdc.h new file mode 100644 index 0000000..1316976 --- /dev/null +++ b/lib/USB_CDC/include/usbd_cdc.h @@ -0,0 +1,184 @@ +/** + ****************************************************************************** + * @file usbd_cdc.h + * @author MCD Application Team + * @brief header file for the usbd_cdc.c file. + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_CDC_H +#define __USB_CDC_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_ioreq.h" + +/** @addtogroup STM32_USB_DEVICE_LIBRARY + * @{ + */ + +/** @defgroup usbd_cdc + * @brief This file is the Header file for usbd_cdc.c + * @{ + */ + + +/** @defgroup usbd_cdc_Exported_Defines + * @{ + */ +#ifndef CDC_IN_EP +#define CDC_IN_EP 0x81U /* EP1 for data IN */ +#endif /* CDC_IN_EP */ +#ifndef CDC_OUT_EP +#define CDC_OUT_EP 0x01U /* EP1 for data OUT */ +#endif /* CDC_OUT_EP */ +#ifndef CDC_CMD_EP +#define CDC_CMD_EP 0x82U /* EP2 for CDC commands */ +#endif /* CDC_CMD_EP */ + +#ifndef CDC_HS_BINTERVAL +#define CDC_HS_BINTERVAL 0x10U +#endif /* CDC_HS_BINTERVAL */ + +#ifndef CDC_FS_BINTERVAL +#define CDC_FS_BINTERVAL 0x10U +#endif /* CDC_FS_BINTERVAL */ + +/* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */ +#define CDC_DATA_HS_MAX_PACKET_SIZE 512U /* Endpoint IN & OUT Packet size */ +#define CDC_DATA_FS_MAX_PACKET_SIZE 64U /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SIZE 8U /* Control Endpoint Packet size */ + +#define USB_CDC_CONFIG_DESC_SIZ 67U +#define CDC_DATA_HS_IN_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE +#define CDC_DATA_HS_OUT_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE + +#define CDC_DATA_FS_IN_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE +#define CDC_DATA_FS_OUT_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE + +#define CDC_REQ_MAX_DATA_SIZE 0x7U +/*---------------------------------------------------------------------*/ +/* CDC definitions */ +/*---------------------------------------------------------------------*/ +#define CDC_SEND_ENCAPSULATED_COMMAND 0x00U +#define CDC_GET_ENCAPSULATED_RESPONSE 0x01U +#define CDC_SET_COMM_FEATURE 0x02U +#define CDC_GET_COMM_FEATURE 0x03U +#define CDC_CLEAR_COMM_FEATURE 0x04U +#define CDC_SET_LINE_CODING 0x20U +#define CDC_GET_LINE_CODING 0x21U +#define CDC_SET_CONTROL_LINE_STATE 0x22U +#define CDC_SEND_BREAK 0x23U + +/** + * @} + */ + + +/** @defgroup USBD_CORE_Exported_TypesDefinitions + * @{ + */ + +/** + * @} + */ +typedef struct +{ + uint32_t bitrate; + uint8_t format; + uint8_t paritytype; + uint8_t datatype; +} USBD_CDC_LineCodingTypeDef; + +typedef struct _USBD_CDC_Itf +{ + int8_t (* Init)(void); + int8_t (* DeInit)(void); + int8_t (* Control)(uint8_t cmd, uint8_t *pbuf, uint16_t length); + int8_t (* Receive)(uint8_t *Buf, uint32_t *Len); + int8_t (* TransmitCplt)(uint8_t *Buf, uint32_t *Len, uint8_t epnum); +} USBD_CDC_ItfTypeDef; + + +typedef struct +{ + uint32_t data[CDC_DATA_HS_MAX_PACKET_SIZE / 4U]; /* Force 32-bit alignment */ + uint8_t CmdOpCode; + uint8_t CmdLength; + uint8_t *RxBuffer; + uint8_t *TxBuffer; + uint32_t RxLength; + uint32_t TxLength; + + __IO uint32_t TxState; + __IO uint32_t RxState; +} USBD_CDC_HandleTypeDef; + + + +/** @defgroup USBD_CORE_Exported_Macros + * @{ + */ + +/** + * @} + */ + +/** @defgroup USBD_CORE_Exported_Variables + * @{ + */ + +extern USBD_ClassTypeDef USBD_CDC; +#define USBD_CDC_CLASS &USBD_CDC +/** + * @} + */ + +/** @defgroup USB_CORE_Exported_Functions + * @{ + */ +uint8_t USBD_CDC_RegisterInterface(USBD_HandleTypeDef *pdev, + USBD_CDC_ItfTypeDef *fops); + +#ifdef USE_USBD_COMPOSITE +uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff, + uint32_t length, uint8_t ClassId); +uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t ClassId); +#else +uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff, + uint32_t length); +uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev); +#endif /* USE_USBD_COMPOSITE */ +uint8_t USBD_CDC_SetRxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff); +uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev); +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* __USB_CDC_H */ +/** + * @} + */ + +/** + * @} + */ + diff --git a/lib/USB_CDC/include/usbd_cdc_if.h b/lib/USB_CDC/include/usbd_cdc_if.h new file mode 100644 index 0000000..19da466 --- /dev/null +++ b/lib/USB_CDC/include/usbd_cdc_if.h @@ -0,0 +1,14 @@ +#ifndef USBD_CDC_IF_H +#define USBD_CDC_IF_H + +#include "usbd_cdc.h" + +extern USBD_CDC_ItfTypeDef USBD_CDC_fops; + +// Send data over USB CDC +uint8_t CDC_Transmit(uint8_t *buf, uint16_t len); + + +/* Betaflight-style DFU reboot check — call early in main() */ +void checkForBootloader(void); +#endif diff --git a/lib/USB_CDC/include/usbd_conf.h b/lib/USB_CDC/include/usbd_conf.h new file mode 100644 index 0000000..b9f6d3a --- /dev/null +++ b/lib/USB_CDC/include/usbd_conf.h @@ -0,0 +1,27 @@ +#ifndef USBD_CONF_H +#define USBD_CONF_H + +/* Match Betaflight's usbd_conf.h exactly */ +#include "stm32f7xx_hal.h" +#include +#include +#include + +#define USBD_MAX_NUM_INTERFACES 3 +#define USBD_MAX_NUM_CONFIGURATION 1 +#define USBD_MAX_STR_DESC_SIZ 0x100 +#define USBD_SUPPORT_USER_STRING 0 +#define USBD_SELF_POWERED 1 +#define USBD_DEBUG_LEVEL 0 +#define USE_USB_FS + +#define USBD_malloc malloc +#define USBD_free free +#define USBD_memset memset +#define USBD_memcpy memcpy + +#define USBD_UsrLog(...) +#define USBD_ErrLog(...) +#define USBD_DbgLog(...) + +#endif diff --git a/lib/USB_CDC/include/usbd_core.h b/lib/USB_CDC/include/usbd_core.h new file mode 100644 index 0000000..5d8a284 --- /dev/null +++ b/lib/USB_CDC/include/usbd_core.h @@ -0,0 +1,175 @@ +/** + ****************************************************************************** + * @file usbd_core.h + * @author MCD Application Team + * @brief Header file for usbd_core.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USBD_CORE_H +#define __USBD_CORE_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_conf.h" +#include "usbd_def.h" +#include "usbd_ioreq.h" +#include "usbd_ctlreq.h" + +/** @addtogroup STM32_USB_DEVICE_LIBRARY + * @{ + */ + +/** @defgroup USBD_CORE + * @brief This file is the Header file for usbd_core.c file + * @{ + */ + + +/** @defgroup USBD_CORE_Exported_Defines + * @{ + */ +#ifndef USBD_DEBUG_LEVEL +#define USBD_DEBUG_LEVEL 0U +#endif /* USBD_DEBUG_LEVEL */ +/** + * @} + */ + + +/** @defgroup USBD_CORE_Exported_TypesDefinitions + * @{ + */ + + +/** + * @} + */ + + + +/** @defgroup USBD_CORE_Exported_Macros + * @{ + */ + +/** + * @} + */ + +/** @defgroup USBD_CORE_Exported_Variables + * @{ + */ +#define USBD_SOF USBD_LL_SOF +/** + * @} + */ + +/** @defgroup USBD_CORE_Exported_FunctionsPrototype + * @{ + */ +USBD_StatusTypeDef USBD_Init(USBD_HandleTypeDef *pdev, USBD_DescriptorsTypeDef *pdesc, uint8_t id); +USBD_StatusTypeDef USBD_DeInit(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_Start(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_Stop(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_RegisterClass(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass); +#if (USBD_USER_REGISTER_CALLBACK == 1U) +USBD_StatusTypeDef USBD_RegisterDevStateCallback(USBD_HandleTypeDef *pdev, USBD_DevStateCallbackTypeDef pUserCallback); +#endif /* USBD_USER_REGISTER_CALLBACK */ + +#ifdef USE_USBD_COMPOSITE +USBD_StatusTypeDef USBD_RegisterClassComposite(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass, + USBD_CompositeClassTypeDef classtype, uint8_t *EpAddr); + +USBD_StatusTypeDef USBD_UnRegisterClassComposite(USBD_HandleTypeDef *pdev); +uint8_t USBD_CoreGetEPAdd(USBD_HandleTypeDef *pdev, uint8_t ep_dir, uint8_t ep_type, uint8_t ClassId); +#endif /* USE_USBD_COMPOSITE */ + +uint8_t USBD_CoreFindIF(USBD_HandleTypeDef *pdev, uint8_t index); +uint8_t USBD_CoreFindEP(USBD_HandleTypeDef *pdev, uint8_t index); + +USBD_StatusTypeDef USBD_RunTestMode(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_SetClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx); +USBD_StatusTypeDef USBD_ClrClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx); + +USBD_StatusTypeDef USBD_LL_SetupStage(USBD_HandleTypeDef *pdev, uint8_t *psetup); +USBD_StatusTypeDef USBD_LL_DataOutStage(USBD_HandleTypeDef *pdev, uint8_t epnum, uint8_t *pdata); +USBD_StatusTypeDef USBD_LL_DataInStage(USBD_HandleTypeDef *pdev, uint8_t epnum, uint8_t *pdata); + +USBD_StatusTypeDef USBD_LL_Reset(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_LL_SetSpeed(USBD_HandleTypeDef *pdev, USBD_SpeedTypeDef speed); +USBD_StatusTypeDef USBD_LL_Suspend(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_LL_Resume(USBD_HandleTypeDef *pdev); + +USBD_StatusTypeDef USBD_LL_SOF(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum); +USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum); + +USBD_StatusTypeDef USBD_LL_DevConnected(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_LL_DevDisconnected(USBD_HandleTypeDef *pdev); + +/* USBD Low Level Driver */ +USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_LL_DeInit(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_LL_Start(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_LL_Stop(USBD_HandleTypeDef *pdev); + +USBD_StatusTypeDef USBD_LL_OpenEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr, + uint8_t ep_type, uint16_t ep_mps); + +USBD_StatusTypeDef USBD_LL_CloseEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); +USBD_StatusTypeDef USBD_LL_FlushEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); +USBD_StatusTypeDef USBD_LL_StallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); +USBD_StatusTypeDef USBD_LL_ClearStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); +USBD_StatusTypeDef USBD_LL_SetUSBAddress(USBD_HandleTypeDef *pdev, uint8_t dev_addr); + +USBD_StatusTypeDef USBD_LL_Transmit(USBD_HandleTypeDef *pdev, uint8_t ep_addr, + uint8_t *pbuf, uint32_t size); + +USBD_StatusTypeDef USBD_LL_PrepareReceive(USBD_HandleTypeDef *pdev, uint8_t ep_addr, + uint8_t *pbuf, uint32_t size); + +#ifdef USBD_HS_TESTMODE_ENABLE +USBD_StatusTypeDef USBD_LL_SetTestMode(USBD_HandleTypeDef *pdev, uint8_t testmode); +#endif /* USBD_HS_TESTMODE_ENABLE */ + +uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); +uint32_t USBD_LL_GetRxDataSize(USBD_HandleTypeDef *pdev, uint8_t ep_addr); + +void USBD_LL_Delay(uint32_t Delay); + +void *USBD_GetEpDesc(uint8_t *pConfDesc, uint8_t EpAddr); +USBD_DescHeaderTypeDef *USBD_GetNextDesc(uint8_t *pbuf, uint16_t *ptr); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* __USBD_CORE_H */ + +/** + * @} + */ + +/** + * @} + */ + + diff --git a/lib/USB_CDC/include/usbd_ctlreq.h b/lib/USB_CDC/include/usbd_ctlreq.h new file mode 100644 index 0000000..3c4eb16 --- /dev/null +++ b/lib/USB_CDC/include/usbd_ctlreq.h @@ -0,0 +1,101 @@ +/** + ****************************************************************************** + * @file usbd_req.h + * @author MCD Application Team + * @brief Header file for the usbd_req.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_REQUEST_H +#define __USB_REQUEST_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_def.h" + + +/** @addtogroup STM32_USB_DEVICE_LIBRARY + * @{ + */ + +/** @defgroup USBD_REQ + * @brief header file for the usbd_req.c file + * @{ + */ + +/** @defgroup USBD_REQ_Exported_Defines + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_REQ_Exported_Types + * @{ + */ +/** + * @} + */ + + + +/** @defgroup USBD_REQ_Exported_Macros + * @{ + */ +/** + * @} + */ + +/** @defgroup USBD_REQ_Exported_Variables + * @{ + */ +/** + * @} + */ + +/** @defgroup USBD_REQ_Exported_FunctionsPrototype + * @{ + */ + +USBD_StatusTypeDef USBD_StdDevReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +USBD_StatusTypeDef USBD_StdItfReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +USBD_StatusTypeDef USBD_StdEPReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); + +void USBD_CtlError(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +void USBD_ParseSetupRequest(USBD_SetupReqTypedef *req, uint8_t *pdata); +void USBD_GetString(uint8_t *desc, uint8_t *unicode, uint16_t *len); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* __USB_REQUEST_H */ + +/** + * @} + */ + +/** + * @} + */ + + diff --git a/lib/USB_CDC/include/usbd_def.h b/lib/USB_CDC/include/usbd_def.h new file mode 100644 index 0000000..52778b3 --- /dev/null +++ b/lib/USB_CDC/include/usbd_def.h @@ -0,0 +1,523 @@ +/** + ****************************************************************************** + * @file usbd_def.h + * @author MCD Application Team + * @brief General defines for the usb device library + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USBD_DEF_H +#define __USBD_DEF_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_conf.h" + +/** @addtogroup STM32_USBD_DEVICE_LIBRARY + * @{ + */ + +/** @defgroup USB_DEF + * @brief general defines for the usb device library file + * @{ + */ + +/** @defgroup USB_DEF_Exported_Defines + * @{ + */ + +#ifndef NULL +#define NULL 0U +#endif /* NULL */ + +#ifndef USBD_MAX_NUM_INTERFACES +#define USBD_MAX_NUM_INTERFACES 1U +#endif /* USBD_MAX_NUM_CONFIGURATION */ + +#ifndef USBD_MAX_NUM_CONFIGURATION +#define USBD_MAX_NUM_CONFIGURATION 1U +#endif /* USBD_MAX_NUM_CONFIGURATION */ + +#ifdef USE_USBD_COMPOSITE +#ifndef USBD_MAX_SUPPORTED_CLASS +#define USBD_MAX_SUPPORTED_CLASS 4U +#endif /* USBD_MAX_SUPPORTED_CLASS */ +#else +#ifndef USBD_MAX_SUPPORTED_CLASS +#define USBD_MAX_SUPPORTED_CLASS 1U +#endif /* USBD_MAX_SUPPORTED_CLASS */ +#endif /* USE_USBD_COMPOSITE */ + +#ifndef USBD_MAX_CLASS_ENDPOINTS +#define USBD_MAX_CLASS_ENDPOINTS 5U +#endif /* USBD_MAX_CLASS_ENDPOINTS */ + +#ifndef USBD_MAX_CLASS_INTERFACES +#define USBD_MAX_CLASS_INTERFACES 5U +#endif /* USBD_MAX_CLASS_INTERFACES */ + +#ifndef USBD_LPM_ENABLED +#define USBD_LPM_ENABLED 0U +#endif /* USBD_LPM_ENABLED */ + +#ifndef USBD_SELF_POWERED +#define USBD_SELF_POWERED 1U +#endif /*USBD_SELF_POWERED */ + +#ifndef USBD_MAX_POWER +#define USBD_MAX_POWER 0x32U /* 100 mA */ +#endif /* USBD_MAX_POWER */ + +#ifndef USBD_SUPPORT_USER_STRING_DESC +#define USBD_SUPPORT_USER_STRING_DESC 0U +#endif /* USBD_SUPPORT_USER_STRING_DESC */ + +#ifndef USBD_CLASS_USER_STRING_DESC +#define USBD_CLASS_USER_STRING_DESC 0U +#endif /* USBD_CLASS_USER_STRING_DESC */ + +#define USB_LEN_DEV_QUALIFIER_DESC 0x0AU +#define USB_LEN_DEV_DESC 0x12U +#define USB_LEN_CFG_DESC 0x09U +#define USB_LEN_IF_DESC 0x09U +#define USB_LEN_EP_DESC 0x07U +#define USB_LEN_OTG_DESC 0x03U +#define USB_LEN_LANGID_STR_DESC 0x04U +#define USB_LEN_OTHER_SPEED_DESC_SIZ 0x09U + +#define USBD_IDX_LANGID_STR 0x00U +#define USBD_IDX_MFC_STR 0x01U +#define USBD_IDX_PRODUCT_STR 0x02U +#define USBD_IDX_SERIAL_STR 0x03U +#define USBD_IDX_CONFIG_STR 0x04U +#define USBD_IDX_INTERFACE_STR 0x05U + +#define USB_REQ_TYPE_STANDARD 0x00U +#define USB_REQ_TYPE_CLASS 0x20U +#define USB_REQ_TYPE_VENDOR 0x40U +#define USB_REQ_TYPE_MASK 0x60U + +#define USB_REQ_RECIPIENT_DEVICE 0x00U +#define USB_REQ_RECIPIENT_INTERFACE 0x01U +#define USB_REQ_RECIPIENT_ENDPOINT 0x02U +#define USB_REQ_RECIPIENT_MASK 0x03U + +#define USB_REQ_GET_STATUS 0x00U +#define USB_REQ_CLEAR_FEATURE 0x01U +#define USB_REQ_SET_FEATURE 0x03U +#define USB_REQ_SET_ADDRESS 0x05U +#define USB_REQ_GET_DESCRIPTOR 0x06U +#define USB_REQ_SET_DESCRIPTOR 0x07U +#define USB_REQ_GET_CONFIGURATION 0x08U +#define USB_REQ_SET_CONFIGURATION 0x09U +#define USB_REQ_GET_INTERFACE 0x0AU +#define USB_REQ_SET_INTERFACE 0x0BU +#define USB_REQ_SYNCH_FRAME 0x0CU + +#define USB_DESC_TYPE_DEVICE 0x01U +#define USB_DESC_TYPE_CONFIGURATION 0x02U +#define USB_DESC_TYPE_STRING 0x03U +#define USB_DESC_TYPE_INTERFACE 0x04U +#define USB_DESC_TYPE_ENDPOINT 0x05U +#define USB_DESC_TYPE_DEVICE_QUALIFIER 0x06U +#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION 0x07U +#define USB_DESC_TYPE_IAD 0x0BU +#define USB_DESC_TYPE_BOS 0x0FU + +#define USB_CONFIG_REMOTE_WAKEUP 0x02U +#define USB_CONFIG_SELF_POWERED 0x01U + +#define USB_FEATURE_EP_HALT 0x00U +#define USB_FEATURE_REMOTE_WAKEUP 0x01U +#define USB_FEATURE_TEST_MODE 0x02U + +#define USB_DEVICE_CAPABITY_TYPE 0x10U + +#define USB_CONF_DESC_SIZE 0x09U +#define USB_IF_DESC_SIZE 0x09U +#define USB_EP_DESC_SIZE 0x07U +#define USB_IAD_DESC_SIZE 0x08U + +#define USB_HS_MAX_PACKET_SIZE 512U +#define USB_FS_MAX_PACKET_SIZE 64U +#define USB_MAX_EP0_SIZE 64U + +/* Device Status */ +#define USBD_STATE_DEFAULT 0x01U +#define USBD_STATE_ADDRESSED 0x02U +#define USBD_STATE_CONFIGURED 0x03U +#define USBD_STATE_SUSPENDED 0x04U + + +/* EP0 State */ +#define USBD_EP0_IDLE 0x00U +#define USBD_EP0_SETUP 0x01U +#define USBD_EP0_DATA_IN 0x02U +#define USBD_EP0_DATA_OUT 0x03U +#define USBD_EP0_STATUS_IN 0x04U +#define USBD_EP0_STATUS_OUT 0x05U +#define USBD_EP0_STALL 0x06U + +#define USBD_EP_TYPE_CTRL 0x00U +#define USBD_EP_TYPE_ISOC 0x01U +#define USBD_EP_TYPE_BULK 0x02U +#define USBD_EP_TYPE_INTR 0x03U + +#ifdef USE_USBD_COMPOSITE +#define USBD_EP_IN 0x80U +#define USBD_EP_OUT 0x00U +#define USBD_FUNC_DESCRIPTOR_TYPE 0x24U +#define USBD_DESC_SUBTYPE_ACM 0x0FU +#define USBD_DESC_ECM_BCD_LOW 0x00U +#define USBD_DESC_ECM_BCD_HIGH 0x10U +#endif /* USE_USBD_COMPOSITE */ +/** + * @} + */ + + +/** @defgroup USBD_DEF_Exported_TypesDefinitions + * @{ + */ + +typedef struct usb_setup_req +{ + uint8_t bmRequest; + uint8_t bRequest; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; +} USBD_SetupReqTypedef; + +typedef struct +{ + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t wTotalLength; + uint8_t bNumInterfaces; + uint8_t bConfigurationValue; + uint8_t iConfiguration; + uint8_t bmAttributes; + uint8_t bMaxPower; +} __PACKED USBD_ConfigDescTypeDef; + +typedef struct +{ + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t wTotalLength; + uint8_t bNumDeviceCaps; +} USBD_BosDescTypeDef; + +typedef struct +{ + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bEndpointAddress; + uint8_t bmAttributes; + uint16_t wMaxPacketSize; + uint8_t bInterval; +} __PACKED USBD_EpDescTypeDef; + +typedef struct +{ + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; +} USBD_DescHeaderTypeDef; + +struct _USBD_HandleTypeDef; + +typedef struct _Device_cb +{ + uint8_t (*Init)(struct _USBD_HandleTypeDef *pdev, uint8_t cfgidx); + uint8_t (*DeInit)(struct _USBD_HandleTypeDef *pdev, uint8_t cfgidx); + /* Control Endpoints*/ + uint8_t (*Setup)(struct _USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); + uint8_t (*EP0_TxSent)(struct _USBD_HandleTypeDef *pdev); + uint8_t (*EP0_RxReady)(struct _USBD_HandleTypeDef *pdev); + /* Class Specific Endpoints*/ + uint8_t (*DataIn)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum); + uint8_t (*DataOut)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum); + uint8_t (*SOF)(struct _USBD_HandleTypeDef *pdev); + uint8_t (*IsoINIncomplete)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum); + uint8_t (*IsoOUTIncomplete)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum); + + uint8_t *(*GetHSConfigDescriptor)(uint16_t *length); + uint8_t *(*GetFSConfigDescriptor)(uint16_t *length); + uint8_t *(*GetOtherSpeedConfigDescriptor)(uint16_t *length); + uint8_t *(*GetDeviceQualifierDescriptor)(uint16_t *length); +#if (USBD_SUPPORT_USER_STRING_DESC == 1U) + uint8_t *(*GetUsrStrDescriptor)(struct _USBD_HandleTypeDef *pdev, uint8_t index, uint16_t *length); +#endif /* USBD_SUPPORT_USER_STRING_DESC */ + +} USBD_ClassTypeDef; + +/* Following USB Device Speed */ +typedef enum +{ + USBD_SPEED_HIGH = 0U, + USBD_SPEED_FULL = 1U, + USBD_SPEED_LOW = 2U, +} USBD_SpeedTypeDef; + +/* Following USB Device status */ +typedef enum +{ + USBD_OK = 0U, + USBD_BUSY, + USBD_EMEM, + USBD_FAIL, +} USBD_StatusTypeDef; + +/* USB Device descriptors structure */ +typedef struct +{ + uint8_t *(*GetDeviceDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); + uint8_t *(*GetLangIDStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); + uint8_t *(*GetManufacturerStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); + uint8_t *(*GetProductStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); + uint8_t *(*GetSerialStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); + uint8_t *(*GetConfigurationStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); + uint8_t *(*GetInterfaceStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); +#if (USBD_CLASS_USER_STRING_DESC == 1) + uint8_t *(*GetUserStrDescriptor)(USBD_SpeedTypeDef speed, uint8_t idx, uint16_t *length); +#endif /* USBD_CLASS_USER_STRING_DESC */ +#if ((USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1)) + uint8_t *(*GetBOSDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); +#endif /* (USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1) */ +} USBD_DescriptorsTypeDef; + +/* USB Device handle structure */ +typedef struct +{ + uint32_t status; + uint32_t total_length; + uint32_t rem_length; + uint32_t maxpacket; + uint16_t is_used; + uint16_t bInterval; +} USBD_EndpointTypeDef; + +#ifdef USE_USBD_COMPOSITE +typedef enum +{ + CLASS_TYPE_NONE = 0, + CLASS_TYPE_HID = 1, + CLASS_TYPE_CDC = 2, + CLASS_TYPE_MSC = 3, + CLASS_TYPE_DFU = 4, + CLASS_TYPE_CHID = 5, + CLASS_TYPE_AUDIO = 6, + CLASS_TYPE_ECM = 7, + CLASS_TYPE_RNDIS = 8, + CLASS_TYPE_MTP = 9, + CLASS_TYPE_VIDEO = 10, + CLASS_TYPE_PRINTER = 11, + CLASS_TYPE_CCID = 12, +} USBD_CompositeClassTypeDef; + + +/* USB Device handle structure */ +typedef struct +{ + uint8_t add; + uint8_t type; + uint8_t size; + uint8_t is_used; +} USBD_EPTypeDef; + +/* USB Device handle structure */ +typedef struct +{ + USBD_CompositeClassTypeDef ClassType; + uint32_t ClassId; + uint32_t Active; + uint32_t NumEps; + USBD_EPTypeDef Eps[USBD_MAX_CLASS_ENDPOINTS]; + uint8_t *EpAdd; + uint32_t NumIf; + uint8_t Ifs[USBD_MAX_CLASS_INTERFACES]; + uint32_t CurrPcktSze; +} USBD_CompositeElementTypeDef; +#endif /* USE_USBD_COMPOSITE */ + +/* USB Device handle structure */ +typedef struct _USBD_HandleTypeDef +{ + uint8_t id; + uint32_t dev_config; + uint32_t dev_default_config; + uint32_t dev_config_status; + USBD_SpeedTypeDef dev_speed; + USBD_EndpointTypeDef ep_in[16]; + USBD_EndpointTypeDef ep_out[16]; + __IO uint32_t ep0_state; + uint32_t ep0_data_len; + __IO uint8_t dev_state; + __IO uint8_t dev_old_state; + uint8_t dev_address; + uint8_t dev_connection_status; + uint8_t dev_test_mode; + uint32_t dev_remote_wakeup; + uint8_t ConfIdx; + + USBD_SetupReqTypedef request; + USBD_DescriptorsTypeDef *pDesc; + USBD_ClassTypeDef *pClass[USBD_MAX_SUPPORTED_CLASS]; + void *pClassData; + void *pClassDataCmsit[USBD_MAX_SUPPORTED_CLASS]; + void *pUserData[USBD_MAX_SUPPORTED_CLASS]; + void *pData; + void *pBosDesc; + void *pConfDesc; + uint32_t classId; + uint32_t NumClasses; +#ifdef USE_USBD_COMPOSITE + USBD_CompositeElementTypeDef tclasslist[USBD_MAX_SUPPORTED_CLASS]; +#endif /* USE_USBD_COMPOSITE */ +#if (USBD_USER_REGISTER_CALLBACK == 1U) + void (* DevStateCallback)(uint8_t dev_state, uint8_t cfgidx); /*!< User Notification callback */ +#endif /* USBD_USER_REGISTER_CALLBACK */ +} USBD_HandleTypeDef; + +#if (USBD_USER_REGISTER_CALLBACK == 1U) +typedef void (*USBD_DevStateCallbackTypeDef)(uint8_t dev_state, uint8_t cfgidx); /*!< pointer to User callback function */ +#endif /* USBD_USER_REGISTER_CALLBACK */ + +/* USB Device endpoint direction */ +typedef enum +{ + OUT = 0x00, + IN = 0x80, +} USBD_EPDirectionTypeDef; + +typedef enum +{ + NETWORK_CONNECTION = 0x00, + RESPONSE_AVAILABLE = 0x01, + CONNECTION_SPEED_CHANGE = 0x2A +} USBD_CDC_NotifCodeTypeDef; +/** + * @} + */ + + + +/** @defgroup USBD_DEF_Exported_Macros + * @{ + */ +__STATIC_INLINE uint16_t SWAPBYTE(uint8_t *addr) +{ + uint16_t _SwapVal; + uint16_t _Byte1; + uint16_t _Byte2; + uint8_t *_pbuff = addr; + + _Byte1 = *(uint8_t *)_pbuff; + _pbuff++; + _Byte2 = *(uint8_t *)_pbuff; + + _SwapVal = (_Byte2 << 8) | _Byte1; + + return _SwapVal; +} + +#ifndef LOBYTE +#define LOBYTE(x) ((uint8_t)((x) & 0x00FFU)) +#endif /* LOBYTE */ + +#ifndef HIBYTE +#define HIBYTE(x) ((uint8_t)(((x) & 0xFF00U) >> 8U)) +#endif /* HIBYTE */ + +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif /* MIN */ + +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif /* MAX */ + +#if defined ( __GNUC__ ) +#ifndef __weak +#define __weak __attribute__((weak)) +#endif /* __weak */ +#ifndef __packed +#define __packed __attribute__((__packed__)) +#endif /* __packed */ +#endif /* __GNUC__ */ + + +/* In HS mode and when the DMA is used, all variables and data structures dealing + with the DMA during the transaction process should be 4-bytes aligned */ + +#if defined ( __GNUC__ ) && !defined (__CC_ARM) /* GNU Compiler */ +#ifndef __ALIGN_END +#define __ALIGN_END __attribute__ ((aligned (4U))) +#endif /* __ALIGN_END */ +#ifndef __ALIGN_BEGIN +#define __ALIGN_BEGIN +#endif /* __ALIGN_BEGIN */ +#else +#ifndef __ALIGN_END +#define __ALIGN_END +#endif /* __ALIGN_END */ +#ifndef __ALIGN_BEGIN +#if defined (__CC_ARM) /* ARM Compiler */ +#define __ALIGN_BEGIN __align(4U) +#elif defined (__ICCARM__) /* IAR Compiler */ +#define __ALIGN_BEGIN +#endif /* __CC_ARM */ +#endif /* __ALIGN_BEGIN */ +#endif /* __GNUC__ */ + + +/** + * @} + */ + +/** @defgroup USBD_DEF_Exported_Variables + * @{ + */ + +/** + * @} + */ + +/** @defgroup USBD_DEF_Exported_FunctionsPrototype + * @{ + */ + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* __USBD_DEF_H */ + +/** + * @} + */ + +/** + * @} + */ + diff --git a/lib/USB_CDC/include/usbd_desc.h b/lib/USB_CDC/include/usbd_desc.h new file mode 100644 index 0000000..71bd41d --- /dev/null +++ b/lib/USB_CDC/include/usbd_desc.h @@ -0,0 +1,5 @@ +#ifndef USBD_DESC_H +#define USBD_DESC_H +#include "usbd_def.h" +extern USBD_DescriptorsTypeDef SaltyLab_Desc; +#endif diff --git a/lib/USB_CDC/include/usbd_ioreq.h b/lib/USB_CDC/include/usbd_ioreq.h new file mode 100644 index 0000000..bad1e36 --- /dev/null +++ b/lib/USB_CDC/include/usbd_ioreq.h @@ -0,0 +1,113 @@ +/** + ****************************************************************************** + * @file usbd_ioreq.h + * @author MCD Application Team + * @brief Header file for the usbd_ioreq.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USBD_IOREQ_H +#define __USBD_IOREQ_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_def.h" +#include "usbd_core.h" + +/** @addtogroup STM32_USB_DEVICE_LIBRARY + * @{ + */ + +/** @defgroup USBD_IOREQ + * @brief header file for the usbd_ioreq.c file + * @{ + */ + +/** @defgroup USBD_IOREQ_Exported_Defines + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_IOREQ_Exported_Types + * @{ + */ + + +/** + * @} + */ + + + +/** @defgroup USBD_IOREQ_Exported_Macros + * @{ + */ + +/** + * @} + */ + +/** @defgroup USBD_IOREQ_Exported_Variables + * @{ + */ + +/** + * @} + */ + +/** @defgroup USBD_IOREQ_Exported_FunctionsPrototype + * @{ + */ + +USBD_StatusTypeDef USBD_CtlSendData(USBD_HandleTypeDef *pdev, + uint8_t *pbuf, uint32_t len); + +USBD_StatusTypeDef USBD_CtlContinueSendData(USBD_HandleTypeDef *pdev, + uint8_t *pbuf, uint32_t len); + +USBD_StatusTypeDef USBD_CtlPrepareRx(USBD_HandleTypeDef *pdev, + uint8_t *pbuf, uint32_t len); + +USBD_StatusTypeDef USBD_CtlContinueRx(USBD_HandleTypeDef *pdev, + uint8_t *pbuf, uint32_t len); + +USBD_StatusTypeDef USBD_CtlSendStatus(USBD_HandleTypeDef *pdev); +USBD_StatusTypeDef USBD_CtlReceiveStatus(USBD_HandleTypeDef *pdev); + +uint32_t USBD_GetRxCount(USBD_HandleTypeDef *pdev, uint8_t ep_addr); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* __USBD_IOREQ_H */ + +/** + * @} + */ + +/** + * @} + */ + diff --git a/lib/USB_CDC/src/usbd_cdc.c b/lib/USB_CDC/src/usbd_cdc.c new file mode 100644 index 0000000..eb7f105 --- /dev/null +++ b/lib/USB_CDC/src/usbd_cdc.c @@ -0,0 +1,893 @@ +/** + ****************************************************************************** + * @file usbd_cdc.c + * @author MCD Application Team + * @brief This file provides the high layer firmware functions to manage the + * following functionalities of the USB CDC Class: + * - Initialization and Configuration of high and low layer + * - Enumeration as CDC Device (and enumeration for each implemented memory interface) + * - OUT/IN data transfer + * - Command IN transfer (class requests management) + * - Error management + * + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + * @verbatim + * + * =================================================================== + * CDC Class Driver Description + * =================================================================== + * This driver manages the "Universal Serial Bus Class Definitions for Communications Devices + * Revision 1.2 November 16, 2007" and the sub-protocol specification of "Universal Serial Bus + * Communications Class Subclass Specification for PSTN Devices Revision 1.2 February 9, 2007" + * This driver implements the following aspects of the specification: + * - Device descriptor management + * - Configuration descriptor management + * - Enumeration as CDC device with 2 data endpoints (IN and OUT) and 1 command endpoint (IN) + * - Requests management (as described in section 6.2 in specification) + * - Abstract Control Model compliant + * - Union Functional collection (using 1 IN endpoint for control) + * - Data interface class + * + * These aspects may be enriched or modified for a specific user application. + * + * This driver doesn't implement the following aspects of the specification + * (but it is possible to manage these features with some modifications on this driver): + * - Any class-specific aspect relative to communication classes should be managed by user application. + * - All communication classes other than PSTN are not managed + * + * @endverbatim + * + ****************************************************************************** + */ + +/* BSPDependencies +- "stm32xxxxx_{eval}{discovery}{nucleo_144}.c" +- "stm32xxxxx_{eval}{discovery}_io.c" +EndBSPDependencies */ + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_cdc.h" +#include "usbd_ctlreq.h" + + +/** @addtogroup STM32_USB_DEVICE_LIBRARY + * @{ + */ + + +/** @defgroup USBD_CDC + * @brief usbd core module + * @{ + */ + +/** @defgroup USBD_CDC_Private_TypesDefinitions + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_CDC_Private_Defines + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_CDC_Private_Macros + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_CDC_Private_FunctionPrototypes + * @{ + */ + +static uint8_t USBD_CDC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx); +static uint8_t USBD_CDC_DeInit(USBD_HandleTypeDef *pdev, uint8_t cfgidx); +static uint8_t USBD_CDC_Setup(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static uint8_t USBD_CDC_DataIn(USBD_HandleTypeDef *pdev, uint8_t epnum); +static uint8_t USBD_CDC_DataOut(USBD_HandleTypeDef *pdev, uint8_t epnum); +static uint8_t USBD_CDC_EP0_RxReady(USBD_HandleTypeDef *pdev); +#ifndef USE_USBD_COMPOSITE +static uint8_t *USBD_CDC_GetFSCfgDesc(uint16_t *length); +static uint8_t *USBD_CDC_GetHSCfgDesc(uint16_t *length); +static uint8_t *USBD_CDC_GetOtherSpeedCfgDesc(uint16_t *length); +uint8_t *USBD_CDC_GetDeviceQualifierDescriptor(uint16_t *length); +#endif /* USE_USBD_COMPOSITE */ + +#ifndef USE_USBD_COMPOSITE +/* USB Standard Device Descriptor */ +__ALIGN_BEGIN static uint8_t USBD_CDC_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALIGN_END = +{ + USB_LEN_DEV_QUALIFIER_DESC, + USB_DESC_TYPE_DEVICE_QUALIFIER, + 0x00, + 0x02, + 0x00, + 0x00, + 0x00, + 0x40, + 0x01, + 0x00, +}; +#endif /* USE_USBD_COMPOSITE */ +/** + * @} + */ + +/** @defgroup USBD_CDC_Private_Variables + * @{ + */ + + +/* CDC interface class callbacks structure */ +USBD_ClassTypeDef USBD_CDC = +{ + USBD_CDC_Init, + USBD_CDC_DeInit, + USBD_CDC_Setup, + NULL, /* EP0_TxSent */ + USBD_CDC_EP0_RxReady, + USBD_CDC_DataIn, + USBD_CDC_DataOut, + NULL, + NULL, + NULL, +#ifdef USE_USBD_COMPOSITE + NULL, + NULL, + NULL, + NULL, +#else + USBD_CDC_GetHSCfgDesc, + USBD_CDC_GetFSCfgDesc, + USBD_CDC_GetOtherSpeedCfgDesc, + USBD_CDC_GetDeviceQualifierDescriptor, +#endif /* USE_USBD_COMPOSITE */ +}; + +#ifndef USE_USBD_COMPOSITE +/* USB CDC device Configuration Descriptor */ +__ALIGN_BEGIN static uint8_t USBD_CDC_CfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIGN_END = +{ + /* Configuration Descriptor */ + 0x09, /* bLength: Configuration Descriptor size */ + USB_DESC_TYPE_CONFIGURATION, /* bDescriptorType: Configuration */ + USB_CDC_CONFIG_DESC_SIZ, /* wTotalLength */ + 0x00, + 0x02, /* bNumInterfaces: 2 interfaces */ + 0x01, /* bConfigurationValue: Configuration value */ + 0x00, /* iConfiguration: Index of string descriptor + describing the configuration */ +#if (USBD_SELF_POWERED == 1U) + 0xC0, /* bmAttributes: Bus Powered according to user configuration */ +#else + 0x80, /* bmAttributes: Bus Powered according to user configuration */ +#endif /* USBD_SELF_POWERED */ + USBD_MAX_POWER, /* MaxPower (mA) */ + + /*---------------------------------------------------------------------------*/ + + /* Interface Descriptor */ + 0x09, /* bLength: Interface Descriptor size */ + USB_DESC_TYPE_INTERFACE, /* bDescriptorType: Interface */ + /* Interface descriptor type */ + 0x00, /* bInterfaceNumber: Number of Interface */ + 0x00, /* bAlternateSetting: Alternate setting */ + 0x01, /* bNumEndpoints: One endpoint used */ + 0x02, /* bInterfaceClass: Communication Interface Class */ + 0x02, /* bInterfaceSubClass: Abstract Control Model */ + 0x01, /* bInterfaceProtocol: Common AT commands */ + 0x00, /* iInterface */ + + /* Header Functional Descriptor */ + 0x05, /* bLength: Endpoint Descriptor size */ + 0x24, /* bDescriptorType: CS_INTERFACE */ + 0x00, /* bDescriptorSubtype: Header Func Desc */ + 0x10, /* bcdCDC: spec release number */ + 0x01, + + /* Call Management Functional Descriptor */ + 0x05, /* bFunctionLength */ + 0x24, /* bDescriptorType: CS_INTERFACE */ + 0x01, /* bDescriptorSubtype: Call Management Func Desc */ + 0x00, /* bmCapabilities: D0+D1 */ + 0x01, /* bDataInterface */ + + /* ACM Functional Descriptor */ + 0x04, /* bFunctionLength */ + 0x24, /* bDescriptorType: CS_INTERFACE */ + 0x02, /* bDescriptorSubtype: Abstract Control Management desc */ + 0x02, /* bmCapabilities */ + + /* Union Functional Descriptor */ + 0x05, /* bFunctionLength */ + 0x24, /* bDescriptorType: CS_INTERFACE */ + 0x06, /* bDescriptorSubtype: Union func desc */ + 0x00, /* bMasterInterface: Communication class interface */ + 0x01, /* bSlaveInterface0: Data Class Interface */ + + /* Endpoint 2 Descriptor */ + 0x07, /* bLength: Endpoint Descriptor size */ + USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ + CDC_CMD_EP, /* bEndpointAddress */ + 0x03, /* bmAttributes: Interrupt */ + LOBYTE(CDC_CMD_PACKET_SIZE), /* wMaxPacketSize */ + HIBYTE(CDC_CMD_PACKET_SIZE), + CDC_FS_BINTERVAL, /* bInterval */ + /*---------------------------------------------------------------------------*/ + + /* Data class interface descriptor */ + 0x09, /* bLength: Endpoint Descriptor size */ + USB_DESC_TYPE_INTERFACE, /* bDescriptorType: */ + 0x01, /* bInterfaceNumber: Number of Interface */ + 0x00, /* bAlternateSetting: Alternate setting */ + 0x02, /* bNumEndpoints: Two endpoints used */ + 0x0A, /* bInterfaceClass: CDC */ + 0x00, /* bInterfaceSubClass */ + 0x00, /* bInterfaceProtocol */ + 0x00, /* iInterface */ + + /* Endpoint OUT Descriptor */ + 0x07, /* bLength: Endpoint Descriptor size */ + USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ + CDC_OUT_EP, /* bEndpointAddress */ + 0x02, /* bmAttributes: Bulk */ + LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize */ + HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), + 0x00, /* bInterval */ + + /* Endpoint IN Descriptor */ + 0x07, /* bLength: Endpoint Descriptor size */ + USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ + CDC_IN_EP, /* bEndpointAddress */ + 0x02, /* bmAttributes: Bulk */ + LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize */ + HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), + 0x00 /* bInterval */ +}; +#endif /* USE_USBD_COMPOSITE */ + +static uint8_t CDCInEpAdd = CDC_IN_EP; +static uint8_t CDCOutEpAdd = CDC_OUT_EP; +static uint8_t CDCCmdEpAdd = CDC_CMD_EP; + +/** + * @} + */ + +/** @defgroup USBD_CDC_Private_Functions + * @{ + */ + +/** + * @brief USBD_CDC_Init + * Initialize the CDC interface + * @param pdev: device instance + * @param cfgidx: Configuration index + * @retval status + */ +static uint8_t USBD_CDC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx) +{ + UNUSED(cfgidx); + USBD_CDC_HandleTypeDef *hcdc; + + hcdc = (USBD_CDC_HandleTypeDef *)USBD_malloc(sizeof(USBD_CDC_HandleTypeDef)); + + if (hcdc == NULL) + { + pdev->pClassDataCmsit[pdev->classId] = NULL; + return (uint8_t)USBD_EMEM; + } + + (void)USBD_memset(hcdc, 0, sizeof(USBD_CDC_HandleTypeDef)); + + pdev->pClassDataCmsit[pdev->classId] = (void *)hcdc; + pdev->pClassData = pdev->pClassDataCmsit[pdev->classId]; + +#ifdef USE_USBD_COMPOSITE + /* Get the Endpoints addresses allocated for this class instance */ + CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); + CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); + CDCCmdEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_INTR, (uint8_t)pdev->classId); +#endif /* USE_USBD_COMPOSITE */ + + if (pdev->dev_speed == USBD_SPEED_HIGH) + { + /* Open EP IN */ + (void)USBD_LL_OpenEP(pdev, CDCInEpAdd, USBD_EP_TYPE_BULK, + CDC_DATA_HS_IN_PACKET_SIZE); + + pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 1U; + + /* Open EP OUT */ + (void)USBD_LL_OpenEP(pdev, CDCOutEpAdd, USBD_EP_TYPE_BULK, + CDC_DATA_HS_OUT_PACKET_SIZE); + + pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 1U; + + /* Set bInterval for CDC CMD Endpoint */ + pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = CDC_HS_BINTERVAL; + } + else + { + /* Open EP IN */ + (void)USBD_LL_OpenEP(pdev, CDCInEpAdd, USBD_EP_TYPE_BULK, + CDC_DATA_FS_IN_PACKET_SIZE); + + pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 1U; + + /* Open EP OUT */ + (void)USBD_LL_OpenEP(pdev, CDCOutEpAdd, USBD_EP_TYPE_BULK, + CDC_DATA_FS_OUT_PACKET_SIZE); + + pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 1U; + + /* Set bInterval for CMD Endpoint */ + pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = CDC_FS_BINTERVAL; + } + + /* Open Command IN EP */ + (void)USBD_LL_OpenEP(pdev, CDCCmdEpAdd, USBD_EP_TYPE_INTR, CDC_CMD_PACKET_SIZE); + pdev->ep_in[CDCCmdEpAdd & 0xFU].is_used = 1U; + + hcdc->RxBuffer = NULL; + + /* Init physical Interface components */ + ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Init(); + + /* Init Xfer states */ + hcdc->TxState = 0U; + hcdc->RxState = 0U; + + if (hcdc->RxBuffer == NULL) + { + return (uint8_t)USBD_EMEM; + } + + if (pdev->dev_speed == USBD_SPEED_HIGH) + { + /* Prepare Out endpoint to receive next packet */ + (void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer, + CDC_DATA_HS_OUT_PACKET_SIZE); + } + else + { + /* Prepare Out endpoint to receive next packet */ + (void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer, + CDC_DATA_FS_OUT_PACKET_SIZE); + } + + return (uint8_t)USBD_OK; +} + +/** + * @brief USBD_CDC_Init + * DeInitialize the CDC layer + * @param pdev: device instance + * @param cfgidx: Configuration index + * @retval status + */ +static uint8_t USBD_CDC_DeInit(USBD_HandleTypeDef *pdev, uint8_t cfgidx) +{ + UNUSED(cfgidx); + + +#ifdef USE_USBD_COMPOSITE + /* Get the Endpoints addresses allocated for this CDC class instance */ + CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); + CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); + CDCCmdEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_INTR, (uint8_t)pdev->classId); +#endif /* USE_USBD_COMPOSITE */ + + /* Close EP IN */ + (void)USBD_LL_CloseEP(pdev, CDCInEpAdd); + pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 0U; + + /* Close EP OUT */ + (void)USBD_LL_CloseEP(pdev, CDCOutEpAdd); + pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 0U; + + /* Close Command IN EP */ + (void)USBD_LL_CloseEP(pdev, CDCCmdEpAdd); + pdev->ep_in[CDCCmdEpAdd & 0xFU].is_used = 0U; + pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = 0U; + + /* DeInit physical Interface components */ + if (pdev->pClassDataCmsit[pdev->classId] != NULL) + { + ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->DeInit(); + (void)USBD_free(pdev->pClassDataCmsit[pdev->classId]); + pdev->pClassDataCmsit[pdev->classId] = NULL; + pdev->pClassData = NULL; + } + + return (uint8_t)USBD_OK; +} + +/** + * @brief USBD_CDC_Setup + * Handle the CDC specific requests + * @param pdev: instance + * @param req: usb requests + * @retval status + */ +static uint8_t USBD_CDC_Setup(USBD_HandleTypeDef *pdev, + USBD_SetupReqTypedef *req) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; + uint16_t len; + uint8_t ifalt = 0U; + uint16_t status_info = 0U; + USBD_StatusTypeDef ret = USBD_OK; + + if (hcdc == NULL) + { + return (uint8_t)USBD_FAIL; + } + + switch (req->bmRequest & USB_REQ_TYPE_MASK) + { + case USB_REQ_TYPE_CLASS: + if (req->wLength != 0U) + { + if ((req->bmRequest & 0x80U) != 0U) + { + ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(req->bRequest, + (uint8_t *)hcdc->data, + req->wLength); + + len = MIN(CDC_REQ_MAX_DATA_SIZE, req->wLength); + (void)USBD_CtlSendData(pdev, (uint8_t *)hcdc->data, len); + } + else + { + hcdc->CmdOpCode = req->bRequest; + hcdc->CmdLength = (uint8_t)MIN(req->wLength, USB_MAX_EP0_SIZE); + + (void)USBD_CtlPrepareRx(pdev, (uint8_t *)hcdc->data, hcdc->CmdLength); + } + } + else + { + ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(req->bRequest, + (uint8_t *)req, 0U); + } + break; + + case USB_REQ_TYPE_STANDARD: + switch (req->bRequest) + { + case USB_REQ_GET_STATUS: + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + (void)USBD_CtlSendData(pdev, (uint8_t *)&status_info, 2U); + } + else + { + USBD_CtlError(pdev, req); + ret = USBD_FAIL; + } + break; + + case USB_REQ_GET_INTERFACE: + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + (void)USBD_CtlSendData(pdev, &ifalt, 1U); + } + else + { + USBD_CtlError(pdev, req); + ret = USBD_FAIL; + } + break; + + case USB_REQ_SET_INTERFACE: + if (pdev->dev_state != USBD_STATE_CONFIGURED) + { + USBD_CtlError(pdev, req); + ret = USBD_FAIL; + } + break; + + case USB_REQ_CLEAR_FEATURE: + break; + + default: + USBD_CtlError(pdev, req); + ret = USBD_FAIL; + break; + } + break; + + default: + USBD_CtlError(pdev, req); + ret = USBD_FAIL; + break; + } + + return (uint8_t)ret; +} + +/** + * @brief USBD_CDC_DataIn + * Data sent on non-control IN endpoint + * @param pdev: device instance + * @param epnum: endpoint number + * @retval status + */ +static uint8_t USBD_CDC_DataIn(USBD_HandleTypeDef *pdev, uint8_t epnum) +{ + USBD_CDC_HandleTypeDef *hcdc; + PCD_HandleTypeDef *hpcd = (PCD_HandleTypeDef *)pdev->pData; + + if (pdev->pClassDataCmsit[pdev->classId] == NULL) + { + return (uint8_t)USBD_FAIL; + } + + hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; + + if ((pdev->ep_in[epnum & 0xFU].total_length > 0U) && + ((pdev->ep_in[epnum & 0xFU].total_length % hpcd->IN_ep[epnum & 0xFU].maxpacket) == 0U)) + { + /* Update the packet total length */ + pdev->ep_in[epnum & 0xFU].total_length = 0U; + + /* Send ZLP */ + (void)USBD_LL_Transmit(pdev, epnum, NULL, 0U); + } + else + { + hcdc->TxState = 0U; + + if (((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->TransmitCplt != NULL) + { + ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->TransmitCplt(hcdc->TxBuffer, &hcdc->TxLength, epnum); + } + } + + return (uint8_t)USBD_OK; +} + +/** + * @brief USBD_CDC_DataOut + * Data received on non-control Out endpoint + * @param pdev: device instance + * @param epnum: endpoint number + * @retval status + */ +static uint8_t USBD_CDC_DataOut(USBD_HandleTypeDef *pdev, uint8_t epnum) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; + + if (pdev->pClassDataCmsit[pdev->classId] == NULL) + { + return (uint8_t)USBD_FAIL; + } + + /* Get the received data length */ + hcdc->RxLength = USBD_LL_GetRxDataSize(pdev, epnum); + + /* USB data will be immediately processed, this allow next USB traffic being + NAKed till the end of the application Xfer */ + + ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Receive(hcdc->RxBuffer, &hcdc->RxLength); + + return (uint8_t)USBD_OK; +} + +/** + * @brief USBD_CDC_EP0_RxReady + * Handle EP0 Rx Ready event + * @param pdev: device instance + * @retval status + */ +static uint8_t USBD_CDC_EP0_RxReady(USBD_HandleTypeDef *pdev) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; + + if (hcdc == NULL) + { + return (uint8_t)USBD_FAIL; + } + + if ((pdev->pUserData[pdev->classId] != NULL) && (hcdc->CmdOpCode != 0xFFU)) + { + ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(hcdc->CmdOpCode, + (uint8_t *)hcdc->data, + (uint16_t)hcdc->CmdLength); + hcdc->CmdOpCode = 0xFFU; + } + + return (uint8_t)USBD_OK; +} +#ifndef USE_USBD_COMPOSITE +/** + * @brief USBD_CDC_GetFSCfgDesc + * Return configuration descriptor + * @param length : pointer data length + * @retval pointer to descriptor buffer + */ +static uint8_t *USBD_CDC_GetFSCfgDesc(uint16_t *length) +{ + USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP); + USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP); + USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP); + + if (pEpCmdDesc != NULL) + { + pEpCmdDesc->bInterval = CDC_FS_BINTERVAL; + } + + if (pEpOutDesc != NULL) + { + pEpOutDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE; + } + + if (pEpInDesc != NULL) + { + pEpInDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE; + } + + *length = (uint16_t)sizeof(USBD_CDC_CfgDesc); + return USBD_CDC_CfgDesc; +} + +/** + * @brief USBD_CDC_GetHSCfgDesc + * Return configuration descriptor + * @param length : pointer data length + * @retval pointer to descriptor buffer + */ +static uint8_t *USBD_CDC_GetHSCfgDesc(uint16_t *length) +{ + USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP); + USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP); + USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP); + + if (pEpCmdDesc != NULL) + { + pEpCmdDesc->bInterval = CDC_HS_BINTERVAL; + } + + if (pEpOutDesc != NULL) + { + pEpOutDesc->wMaxPacketSize = CDC_DATA_HS_MAX_PACKET_SIZE; + } + + if (pEpInDesc != NULL) + { + pEpInDesc->wMaxPacketSize = CDC_DATA_HS_MAX_PACKET_SIZE; + } + + *length = (uint16_t)sizeof(USBD_CDC_CfgDesc); + return USBD_CDC_CfgDesc; +} + +/** + * @brief USBD_CDC_GetOtherSpeedCfgDesc + * Return configuration descriptor + * @param length : pointer data length + * @retval pointer to descriptor buffer + */ +static uint8_t *USBD_CDC_GetOtherSpeedCfgDesc(uint16_t *length) +{ + USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP); + USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP); + USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP); + + if (pEpCmdDesc != NULL) + { + pEpCmdDesc->bInterval = CDC_FS_BINTERVAL; + } + + if (pEpOutDesc != NULL) + { + pEpOutDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE; + } + + if (pEpInDesc != NULL) + { + pEpInDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE; + } + + *length = (uint16_t)sizeof(USBD_CDC_CfgDesc); + return USBD_CDC_CfgDesc; +} + +/** + * @brief USBD_CDC_GetDeviceQualifierDescriptor + * return Device Qualifier descriptor + * @param length : pointer data length + * @retval pointer to descriptor buffer + */ +uint8_t *USBD_CDC_GetDeviceQualifierDescriptor(uint16_t *length) +{ + *length = (uint16_t)sizeof(USBD_CDC_DeviceQualifierDesc); + + return USBD_CDC_DeviceQualifierDesc; +} +#endif /* USE_USBD_COMPOSITE */ +/** + * @brief USBD_CDC_RegisterInterface + * @param pdev: device instance + * @param fops: CD Interface callback + * @retval status + */ +uint8_t USBD_CDC_RegisterInterface(USBD_HandleTypeDef *pdev, + USBD_CDC_ItfTypeDef *fops) +{ + if (fops == NULL) + { + return (uint8_t)USBD_FAIL; + } + + pdev->pUserData[pdev->classId] = fops; + + return (uint8_t)USBD_OK; +} + + +/** + * @brief USBD_CDC_SetTxBuffer + * @param pdev: device instance + * @param pbuff: Tx Buffer + * @param length: length of data to be sent + * @param ClassId: The Class ID + * @retval status + */ +#ifdef USE_USBD_COMPOSITE +uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, + uint8_t *pbuff, uint32_t length, uint8_t ClassId) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[ClassId]; +#else +uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, + uint8_t *pbuff, uint32_t length) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; +#endif /* USE_USBD_COMPOSITE */ + + if (hcdc == NULL) + { + return (uint8_t)USBD_FAIL; + } + + hcdc->TxBuffer = pbuff; + hcdc->TxLength = length; + + return (uint8_t)USBD_OK; +} + +/** + * @brief USBD_CDC_SetRxBuffer + * @param pdev: device instance + * @param pbuff: Rx Buffer + * @retval status + */ +uint8_t USBD_CDC_SetRxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; + + if (hcdc == NULL) + { + return (uint8_t)USBD_FAIL; + } + + hcdc->RxBuffer = pbuff; + + return (uint8_t)USBD_OK; +} + + +/** + * @brief USBD_CDC_TransmitPacket + * Transmit packet on IN endpoint + * @param pdev: device instance + * @param ClassId: The Class ID + * @retval status + */ +#ifdef USE_USBD_COMPOSITE +uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t ClassId) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[ClassId]; +#else +uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; +#endif /* USE_USBD_COMPOSITE */ + + USBD_StatusTypeDef ret = USBD_BUSY; + +#ifdef USE_USBD_COMPOSITE + /* Get the Endpoints addresses allocated for this class instance */ + CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, ClassId); +#endif /* USE_USBD_COMPOSITE */ + + if (hcdc == NULL) + { + return (uint8_t)USBD_FAIL; + } + + if (hcdc->TxState == 0U) + { + /* Tx Transfer in progress */ + hcdc->TxState = 1U; + + /* Update the packet total length */ + pdev->ep_in[CDCInEpAdd & 0xFU].total_length = hcdc->TxLength; + + /* Transmit next packet */ + (void)USBD_LL_Transmit(pdev, CDCInEpAdd, hcdc->TxBuffer, hcdc->TxLength); + + ret = USBD_OK; + } + + return (uint8_t)ret; +} + +/** + * @brief USBD_CDC_ReceivePacket + * prepare OUT Endpoint for reception + * @param pdev: device instance + * @retval status + */ +uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev) +{ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; + +#ifdef USE_USBD_COMPOSITE + /* Get the Endpoints addresses allocated for this class instance */ + CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); +#endif /* USE_USBD_COMPOSITE */ + + if (pdev->pClassDataCmsit[pdev->classId] == NULL) + { + return (uint8_t)USBD_FAIL; + } + + if (pdev->dev_speed == USBD_SPEED_HIGH) + { + /* Prepare Out endpoint to receive next packet */ + (void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer, + CDC_DATA_HS_OUT_PACKET_SIZE); + } + else + { + /* Prepare Out endpoint to receive next packet */ + (void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer, + CDC_DATA_FS_OUT_PACKET_SIZE); + } + + return (uint8_t)USBD_OK; +} +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + diff --git a/lib/USB_CDC/src/usbd_cdc_if.c b/lib/USB_CDC/src/usbd_cdc_if.c new file mode 100644 index 0000000..d138dff --- /dev/null +++ b/lib/USB_CDC/src/usbd_cdc_if.c @@ -0,0 +1,135 @@ +#include "usbd_cdc_if.h" +#include "stm32f7xx_hal.h" + +extern USBD_HandleTypeDef hUsbDevice; +volatile uint8_t cdc_streaming = 1; /* auto-stream */ +static volatile uint8_t cdc_port_open = 0; /* set when host asserts DTR */ +volatile uint8_t cdc_arm_request = 0; /* set by A command */ +volatile uint8_t cdc_disarm_request = 0; /* set by D command */ + + +static uint8_t UserRxBuffer[256]; +static uint8_t UserTxBuffer[256]; + +/* + * Betaflight-proven DFU reboot: + * 1. Write magic to RTC backup register (persists across soft reset) + * 2. NVIC_SystemReset() — clean hardware reset + * 3. Early startup checks magic, clears it, jumps to system bootloader + * + * The magic check happens in checkForBootloader() called from main.c + * before any peripheral init. + */ +#define BOOTLOADER_MAGIC 0xDEADBEEF + +static void request_bootloader(void) { + /* Betaflight-proven: write magic, disable IRQs, reset. + * checkForBootloader() runs on next boot before anything else. */ + __HAL_RCC_PWR_CLK_ENABLE(); + HAL_PWR_EnableBkUpAccess(); + __HAL_RCC_RTC_ENABLE(); + + /* Write magic to RTC backup register 0 */ + RTC->BKP0R = BOOTLOADER_MAGIC; + + __disable_irq(); + NVIC_SystemReset(); +} + +/* + * Call this VERY early in main(), before HAL_Init(). + * Checks RTC backup register for magic value left by request_bootloader(). + * If found: clear magic, jump to STM32F7 system bootloader at 0x1FF00000. + */ +void checkForBootloader(void) { + /* + * Betaflight-proven bootloader jump for STM32F7. + * Called VERY early, before HAL_Init/caches/clocks. + * At this point only RCC PWR is needed to read RTC backup regs. + */ + + /* Enable backup domain access to read RTC backup register */ + __HAL_RCC_PWR_CLK_ENABLE(); + HAL_PWR_EnableBkUpAccess(); + __HAL_RCC_RTC_ENABLE(); + + uint32_t magic = RTC->BKP0R; + + if (magic != BOOTLOADER_MAGIC) { + return; /* Normal boot */ + } + + /* Write POST marker (Betaflight does this so SystemInit can + * do a second reset if needed — we just clear it) */ + RTC->BKP0R = 0; + + /* Jump to STM32F7 system bootloader at 0x1FF00000. + * Exactly as Betaflight does it — no cache/VTOR/MEMRMP games needed + * because we run before any of that is configured. */ + + __HAL_RCC_SYSCFG_CLK_ENABLE(); + + __set_MSP(*(uint32_t *)0x1FF00000); + ((void (*)(void))(*(uint32_t *)0x1FF00004))(); + + while (1); +} + +static int8_t CDC_Init(void) { + USBD_CDC_SetTxBuffer(&hUsbDevice, UserTxBuffer, 0); + USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer); + USBD_CDC_ReceivePacket(&hUsbDevice); + + /* Reset TxState so CDC_Transmit works after host (re)connects. + * Without this, if transmits happen before host opens port, + * TxState stays BUSY forever since host never ACKs. */ + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)hUsbDevice.pClassData; + if (hcdc) hcdc->TxState = 0; + + return USBD_OK; +} + +static int8_t CDC_DeInit(void) { return USBD_OK; } + +static int8_t CDC_Control(uint8_t cmd, uint8_t *pbuf, uint16_t length) { + (void)pbuf; (void)length; + if (cmd == 0x22) { /* CDC_SET_CONTROL_LINE_STATE — host opened port */ + cdc_port_open = 1; + cdc_streaming = 1; + } + return USBD_OK; +} + +static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) { + if (*len >= 1 && buf[0] == 'S') { + cdc_streaming = !cdc_streaming; /* Toggle streaming */ + } + if (*len >= 1 && buf[0] == 'A') { + cdc_arm_request = 1; /* Arm balance — processed in main loop */ + } + if (*len >= 1 && buf[0] == 'D') { + cdc_disarm_request = 1; /* Disarm — processed in main loop */ + } + if (*len >= 1 && buf[0] == 'R') { + request_bootloader(); /* Sets magic + resets — never returns */ + } + USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer); + USBD_CDC_ReceivePacket(&hUsbDevice); + return USBD_OK; +} + +USBD_CDC_ItfTypeDef USBD_CDC_fops = { CDC_Init, CDC_DeInit, CDC_Control, CDC_Receive }; + +uint8_t CDC_Transmit(uint8_t *buf, uint16_t len) { + + USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)hUsbDevice.pClassData; + if (hcdc == NULL) return USBD_FAIL; + if (hcdc->TxState != 0) { + /* If stuck busy (no host ACK), force reset after a while */ + static uint32_t busy_count = 0; + if (++busy_count > 100) { hcdc->TxState = 0; busy_count = 0; } + return USBD_BUSY; + } + USBD_CDC_SetTxBuffer(&hUsbDevice, buf, len); + return USBD_CDC_TransmitPacket(&hUsbDevice); +} diff --git a/lib/USB_CDC/src/usbd_conf.c b/lib/USB_CDC/src/usbd_conf.c new file mode 100644 index 0000000..fbae848 --- /dev/null +++ b/lib/USB_CDC/src/usbd_conf.c @@ -0,0 +1,97 @@ +/* Taken directly from Betaflight: usbd_conf_stm32f7xx.c */ +#include "stm32f7xx_hal.h" +#include "usbd_core.h" +#include "usbd_desc.h" +#include "usbd_cdc.h" +#include "usbd_conf.h" + +PCD_HandleTypeDef hpcd; + +void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd) +{ + GPIO_InitTypeDef GPIO_InitStruct; + + if (hpcd->Instance == USB_OTG_FS) { + __HAL_RCC_GPIOA_CLK_ENABLE(); + + GPIO_InitStruct.Pin = (GPIO_PIN_11 | GPIO_PIN_12); + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + __HAL_RCC_USB_OTG_FS_CLK_ENABLE(); + + HAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0); + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } +} + +void HAL_PCD_MspDeInit(PCD_HandleTypeDef *hpcd) +{ + if (hpcd->Instance == USB_OTG_FS) { + __HAL_RCC_USB_OTG_FS_CLK_DISABLE(); + __HAL_RCC_SYSCFG_CLK_DISABLE(); + } +} + +void HAL_PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_SetupStage(hpcd->pData, (uint8_t *)hpcd->Setup); } +void HAL_PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_DataOutStage(hpcd->pData, epnum, hpcd->OUT_ep[epnum].xfer_buff); } +void HAL_PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_DataInStage(hpcd->pData, epnum, hpcd->IN_ep[epnum].xfer_buff); } +void HAL_PCD_SOFCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_SOF(hpcd->pData); } +void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd) { + USBD_LL_Reset(hpcd->pData); + USBD_LL_SetSpeed(hpcd->pData, USBD_SPEED_FULL); +} +void HAL_PCD_SuspendCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_Suspend(hpcd->pData); } +void HAL_PCD_ResumeCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_Resume(hpcd->pData); } +void HAL_PCD_ISOOUTIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_IsoOUTIncomplete(hpcd->pData, epnum); } +void HAL_PCD_ISOINIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_IsoINIncomplete(hpcd->pData, epnum); } +void HAL_PCD_ConnectCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_DevConnected(hpcd->pData); } +void HAL_PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_DevDisconnected(hpcd->pData); } + +USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev) +{ + hpcd.Instance = USB_OTG_FS; + hpcd.Init.dev_endpoints = 4; + hpcd.Init.use_dedicated_ep1 = 0; + hpcd.Init.ep0_mps = 0x40; + hpcd.Init.dma_enable = 0; + hpcd.Init.low_power_enable = 0; + hpcd.Init.phy_itface = PCD_PHY_EMBEDDED; + hpcd.Init.Sof_enable = 0; + hpcd.Init.speed = PCD_SPEED_FULL; + hpcd.Init.vbus_sensing_enable = 0; + hpcd.Init.lpm_enable = 0; + + hpcd.pData = pdev; + pdev->pData = &hpcd; + + HAL_PCD_Init(&hpcd); + + HAL_PCDEx_SetRxFiFo(&hpcd, 0x80); + HAL_PCDEx_SetTxFiFo(&hpcd, 0, 0x40); + HAL_PCDEx_SetTxFiFo(&hpcd, 1, 0x80); + + return USBD_OK; +} + +USBD_StatusTypeDef USBD_LL_DeInit(USBD_HandleTypeDef *pdev) { HAL_PCD_DeInit(pdev->pData); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_Start(USBD_HandleTypeDef *pdev) { HAL_PCD_Start(pdev->pData); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_Stop(USBD_HandleTypeDef *pdev) { HAL_PCD_Stop(pdev->pData); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_OpenEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t ep_type, uint16_t ep_mps) { HAL_PCD_EP_Open(pdev->pData, ep_addr, ep_mps, ep_type); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_CloseEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_Close(pdev->pData, ep_addr); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_FlushEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_Flush(pdev->pData, ep_addr); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_StallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_SetStall(pdev->pData, ep_addr); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_ClearStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_ClrStall(pdev->pData, ep_addr); return USBD_OK; } +uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { + PCD_HandleTypeDef *hpcd = pdev->pData; + if ((ep_addr & 0x80) == 0x80) return hpcd->IN_ep[ep_addr & 0x7F].is_stall; + else return hpcd->OUT_ep[ep_addr & 0x7F].is_stall; +} +USBD_StatusTypeDef USBD_LL_SetUSBAddress(USBD_HandleTypeDef *pdev, uint8_t dev_addr) { HAL_PCD_SetAddress(pdev->pData, dev_addr); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_Transmit(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t *pbuf, uint32_t size) { HAL_PCD_EP_Transmit(pdev->pData, ep_addr, pbuf, size); return USBD_OK; } +USBD_StatusTypeDef USBD_LL_PrepareReceive(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t *pbuf, uint32_t size) { HAL_PCD_EP_Receive(pdev->pData, ep_addr, pbuf, size); return USBD_OK; } +uint32_t USBD_LL_GetRxDataSize(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { return HAL_PCD_EP_GetRxCount(pdev->pData, ep_addr); } +void USBD_LL_Delay(uint32_t Delay) { HAL_Delay(Delay); } diff --git a/lib/USB_CDC/src/usbd_core.c b/lib/USB_CDC/src/usbd_core.c new file mode 100644 index 0000000..ad4dd12 --- /dev/null +++ b/lib/USB_CDC/src/usbd_core.c @@ -0,0 +1,1215 @@ +/** + ****************************************************************************** + * @file usbd_core.c + * @author MCD Application Team + * @brief This file provides all the USBD core functions. + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_core.h" + +#ifdef USE_USBD_COMPOSITE +#include "usbd_composite_builder.h" +#endif /* USE_USBD_COMPOSITE */ + +/** @addtogroup STM32_USBD_DEVICE_LIBRARY + * @{ + */ + + +/** @defgroup USBD_CORE + * @brief usbd core module + * @{ + */ + +/** @defgroup USBD_CORE_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_CORE_Private_Defines + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_CORE_Private_Macros + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_CORE_Private_FunctionPrototypes + * @{ + */ + +/** + * @} + */ + +/** @defgroup USBD_CORE_Private_Variables + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_CORE_Private_Functions + * @{ + */ + +/** + * @brief USBD_Init + * Initialize the device stack and load the class driver + * @param pdev: device instance + * @param pdesc: Descriptor structure address + * @param id: Low level core index + * @retval status: USBD Status + */ +USBD_StatusTypeDef USBD_Init(USBD_HandleTypeDef *pdev, + USBD_DescriptorsTypeDef *pdesc, uint8_t id) +{ + USBD_StatusTypeDef ret; + + /* Check whether the USB Host handle is valid */ + if (pdev == NULL) + { +#if (USBD_DEBUG_LEVEL > 1U) + USBD_ErrLog("Invalid Device handle"); +#endif /* (USBD_DEBUG_LEVEL > 1U) */ + return USBD_FAIL; + } + +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Unlink previous class*/ + pdev->pClass[i] = NULL; + pdev->pUserData[i] = NULL; + + /* Set class as inactive */ + pdev->tclasslist[i].Active = 0; + pdev->NumClasses = 0; + pdev->classId = 0; + } +#else + /* Unlink previous class*/ + pdev->pClass[0] = NULL; + pdev->pUserData[0] = NULL; +#endif /* USE_USBD_COMPOSITE */ + + pdev->pConfDesc = NULL; + + /* Assign USBD Descriptors */ + if (pdesc != NULL) + { + pdev->pDesc = pdesc; + } + + /* Set Device initial State */ + pdev->dev_state = USBD_STATE_DEFAULT; + pdev->id = id; + + /* Initialize low level driver */ + ret = USBD_LL_Init(pdev); + + return ret; +} + +/** + * @brief USBD_DeInit + * De-Initialize the device library + * @param pdev: device instance + * @retval status: USBD Status + */ +USBD_StatusTypeDef USBD_DeInit(USBD_HandleTypeDef *pdev) +{ + USBD_StatusTypeDef ret; + + /* Disconnect the USB Device */ + (void)USBD_LL_Stop(pdev); + + /* Set Default State */ + pdev->dev_state = USBD_STATE_DEFAULT; + +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + if (pdev->pClass[i] != NULL) + { + pdev->classId = i; + /* Free Class Resources */ + pdev->pClass[i]->DeInit(pdev, (uint8_t)pdev->dev_config); + } + } + } +#else + /* Free Class Resources */ + if (pdev->pClass[0] != NULL) + { + pdev->pClass[0]->DeInit(pdev, (uint8_t)pdev->dev_config); + } + + pdev->pUserData[0] = NULL; + +#endif /* USE_USBD_COMPOSITE */ + + /* Free Device descriptors resources */ + pdev->pDesc = NULL; + pdev->pConfDesc = NULL; + + /* DeInitialize low level driver */ + ret = USBD_LL_DeInit(pdev); + + return ret; +} + +/** + * @brief USBD_RegisterClass + * Link class driver to Device Core. + * @param pdev: Device Handle + * @param pclass: Class handle + * @retval USBD Status + */ +USBD_StatusTypeDef USBD_RegisterClass(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass) +{ + uint16_t len = 0U; + + if (pclass == NULL) + { +#if (USBD_DEBUG_LEVEL > 1U) + USBD_ErrLog("Invalid Class handle"); +#endif /* (USBD_DEBUG_LEVEL > 1U) */ + return USBD_FAIL; + } + + /* link the class to the USB Device handle */ + pdev->pClass[0] = pclass; + + /* Get Device Configuration Descriptor */ +#ifdef USE_USB_HS + if (pdev->pClass[pdev->classId]->GetHSConfigDescriptor != NULL) + { + pdev->pConfDesc = (void *)pdev->pClass[pdev->classId]->GetHSConfigDescriptor(&len); + } +#else /* Default USE_USB_FS */ + if (pdev->pClass[pdev->classId]->GetFSConfigDescriptor != NULL) + { + pdev->pConfDesc = (void *)pdev->pClass[pdev->classId]->GetFSConfigDescriptor(&len); + } +#endif /* USE_USB_FS */ + + /* Increment the NumClasses */ + pdev->NumClasses++; + + return USBD_OK; +} + +#ifdef USE_USBD_COMPOSITE +/** + * @brief USBD_RegisterClassComposite + * Link class driver to Device Core. + * @param pdev : Device Handle + * @param pclass: Class handle + * @param classtype: Class type + * @param EpAddr: Endpoint Address handle + * @retval USBD Status + */ +USBD_StatusTypeDef USBD_RegisterClassComposite(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass, + USBD_CompositeClassTypeDef classtype, uint8_t *EpAddr) +{ + USBD_StatusTypeDef ret = USBD_OK; + uint16_t len = 0U; + + if ((pdev->classId < USBD_MAX_SUPPORTED_CLASS) && (pdev->NumClasses < USBD_MAX_SUPPORTED_CLASS)) + { + if ((uint32_t)pclass != 0U) + { + /* Link the class to the USB Device handle */ + pdev->pClass[pdev->classId] = pclass; + ret = USBD_OK; + + pdev->tclasslist[pdev->classId].EpAdd = EpAddr; + + /* Call the composite class builder */ + (void)USBD_CMPSIT_AddClass(pdev, pclass, classtype, 0); + + /* Increment the ClassId for the next occurrence */ + pdev->classId ++; + pdev->NumClasses ++; + } + else + { +#if (USBD_DEBUG_LEVEL > 1U) + USBD_ErrLog("Invalid Class handle"); +#endif /* (USBD_DEBUG_LEVEL > 1U) */ + ret = USBD_FAIL; + } + } + + if (ret == USBD_OK) + { + /* Get Device Configuration Descriptor */ +#ifdef USE_USB_HS + pdev->pConfDesc = USBD_CMPSIT.GetHSConfigDescriptor(&len); +#else /* Default USE_USB_FS */ + pdev->pConfDesc = USBD_CMPSIT.GetFSConfigDescriptor(&len); +#endif /* USE_USB_FS */ + } + + return ret; +} + +/** + * @brief USBD_UnRegisterClassComposite + * UnLink all composite class drivers from Device Core. + * @param pdev: Device Handle + * @retval USBD Status + */ +USBD_StatusTypeDef USBD_UnRegisterClassComposite(USBD_HandleTypeDef *pdev) +{ + USBD_StatusTypeDef ret = USBD_FAIL; + uint8_t idx1; + uint8_t idx2; + + /* Unroll all activated classes */ + for (idx1 = 0; idx1 < pdev->NumClasses; idx1++) + { + /* Check if the class correspond to the requested type and if it is active */ + if (pdev->tclasslist[idx1].Active == 1U) + { + /* Set the new class ID */ + pdev->classId = idx1; + + /* Free resources used by the selected class */ + if (pdev->pClass[pdev->classId] != NULL) + { + /* Free Class Resources */ + if (pdev->pClass[pdev->classId]->DeInit(pdev, (uint8_t)pdev->dev_config) != 0U) + { +#if (USBD_DEBUG_LEVEL > 1U) + USBD_ErrLog("Class DeInit didn't succeed!, can't unregister selected class"); +#endif /* (USBD_DEBUG_LEVEL > 1U) */ + + ret = USBD_FAIL; + } + } + + /* Free the class pointer */ + pdev->pClass[pdev->classId] = NULL; + + /* Free the class location in classes table and reset its parameters to zero */ + pdev->tclasslist[pdev->classId].ClassType = CLASS_TYPE_NONE; + pdev->tclasslist[pdev->classId].ClassId = 0U; + pdev->tclasslist[pdev->classId].Active = 0U; + pdev->tclasslist[pdev->classId].NumEps = 0U; + pdev->tclasslist[pdev->classId].NumIf = 0U; + pdev->tclasslist[pdev->classId].CurrPcktSze = 0U; + + for (idx2 = 0U; idx2 < USBD_MAX_CLASS_ENDPOINTS; idx2++) + { + pdev->tclasslist[pdev->classId].Eps[idx2].add = 0U; + pdev->tclasslist[pdev->classId].Eps[idx2].type = 0U; + pdev->tclasslist[pdev->classId].Eps[idx2].size = 0U; + pdev->tclasslist[pdev->classId].Eps[idx2].is_used = 0U; + } + + for (idx2 = 0U; idx2 < USBD_MAX_CLASS_INTERFACES; idx2++) + { + pdev->tclasslist[pdev->classId].Ifs[idx2] = 0U; + } + } + } + + /* Reset the configuration descriptor */ + (void)USBD_CMPST_ClearConfDesc(pdev); + + /* Reset the class ID and number of classes */ + pdev->classId = 0U; + pdev->NumClasses = 0U; + + return ret; +} +#endif /* USE_USBD_COMPOSITE */ + +#if (USBD_USER_REGISTER_CALLBACK == 1U) +/** + * @brief USBD_RegisterDevStateCallback + * @param pdev : Device Handle + * @param pUserCallback: User Callback + * @retval USBD Status + */ +USBD_StatusTypeDef USBD_RegisterDevStateCallback(USBD_HandleTypeDef *pdev, USBD_DevStateCallbackTypeDef pUserCallback) +{ + pdev->DevStateCallback = pUserCallback; + + return USBD_OK; +} +#endif /* USBD_USER_REGISTER_CALLBACK */ + +/** + * @brief USBD_Start + * Start the USB Device Core. + * @param pdev: Device Handle + * @retval USBD Status + */ +USBD_StatusTypeDef USBD_Start(USBD_HandleTypeDef *pdev) +{ +#ifdef USE_USBD_COMPOSITE + pdev->classId = 0U; +#endif /* USE_USBD_COMPOSITE */ + + /* Start the low level driver */ + return USBD_LL_Start(pdev); +} + +/** + * @brief USBD_Stop + * Stop the USB Device Core. + * @param pdev: Device Handle + * @retval USBD Status + */ +USBD_StatusTypeDef USBD_Stop(USBD_HandleTypeDef *pdev) +{ + /* Disconnect USB Device */ + (void)USBD_LL_Stop(pdev); + + /* Free Class Resources */ +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + if (pdev->pClass[i] != NULL) + { + pdev->classId = i; + /* Free Class Resources */ + (void)pdev->pClass[i]->DeInit(pdev, (uint8_t)pdev->dev_config); + } + } + } + + /* Reset the class ID */ + pdev->classId = 0U; +#else + if (pdev->pClass[0] != NULL) + { + (void)pdev->pClass[0]->DeInit(pdev, (uint8_t)pdev->dev_config); + } +#endif /* USE_USBD_COMPOSITE */ + + return USBD_OK; +} + +/** + * @brief USBD_RunTestMode + * Launch test mode process + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_RunTestMode(USBD_HandleTypeDef *pdev) +{ +#ifdef USBD_HS_TESTMODE_ENABLE + USBD_StatusTypeDef ret; + + /* Run USB HS test mode */ + ret = USBD_LL_SetTestMode(pdev, pdev->dev_test_mode); + + return ret; +#else + /* Prevent unused argument compilation warning */ + UNUSED(pdev); + + return USBD_OK; +#endif /* USBD_HS_TESTMODE_ENABLE */ +} + +/** + * @brief USBD_SetClassConfig + * Configure device and start the interface + * @param pdev: device instance + * @param cfgidx: configuration index + * @retval status + */ + +USBD_StatusTypeDef USBD_SetClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx) +{ + USBD_StatusTypeDef ret = USBD_OK; + +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + if (pdev->pClass[i] != NULL) + { + pdev->classId = i; + /* Set configuration and Start the Class*/ + if (pdev->pClass[i]->Init(pdev, cfgidx) != 0U) + { + ret = USBD_FAIL; + } + } + } + } +#else + if (pdev->pClass[0] != NULL) + { + /* Set configuration and Start the Class */ + ret = (USBD_StatusTypeDef)pdev->pClass[0]->Init(pdev, cfgidx); + } +#endif /* USE_USBD_COMPOSITE */ + + return ret; +} + +/** + * @brief USBD_ClrClassConfig + * Clear current configuration + * @param pdev: device instance + * @param cfgidx: configuration index + * @retval status + */ +USBD_StatusTypeDef USBD_ClrClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx) +{ + USBD_StatusTypeDef ret = USBD_OK; + +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + if (pdev->pClass[i] != NULL) + { + pdev->classId = i; + /* Clear configuration and De-initialize the Class process */ + if (pdev->pClass[i]->DeInit(pdev, cfgidx) != 0U) + { + ret = USBD_FAIL; + } + } + } + } +#else + /* Clear configuration and De-initialize the Class process */ + if (pdev->pClass[0]->DeInit(pdev, cfgidx) != 0U) + { + ret = USBD_FAIL; + } +#endif /* USE_USBD_COMPOSITE */ + + return ret; +} + + +/** + * @brief USBD_LL_SetupStage + * Handle the setup stage + * @param pdev: device instance + * @param psetup: setup packet buffer pointer + * @retval status + */ +USBD_StatusTypeDef USBD_LL_SetupStage(USBD_HandleTypeDef *pdev, uint8_t *psetup) +{ + USBD_StatusTypeDef ret; + + USBD_ParseSetupRequest(&pdev->request, psetup); + + pdev->ep0_state = USBD_EP0_SETUP; + + pdev->ep0_data_len = pdev->request.wLength; + + switch (pdev->request.bmRequest & 0x1FU) + { + case USB_REQ_RECIPIENT_DEVICE: + ret = USBD_StdDevReq(pdev, &pdev->request); + break; + + case USB_REQ_RECIPIENT_INTERFACE: + ret = USBD_StdItfReq(pdev, &pdev->request); + break; + + case USB_REQ_RECIPIENT_ENDPOINT: + ret = USBD_StdEPReq(pdev, &pdev->request); + break; + + default: + ret = USBD_LL_StallEP(pdev, (pdev->request.bmRequest & 0x80U)); + break; + } + + return ret; +} + +/** + * @brief USBD_LL_DataOutStage + * Handle data OUT stage + * @param pdev: device instance + * @param epnum: endpoint index + * @param pdata: data pointer + * @retval status + */ +USBD_StatusTypeDef USBD_LL_DataOutStage(USBD_HandleTypeDef *pdev, + uint8_t epnum, uint8_t *pdata) +{ + USBD_EndpointTypeDef *pep; + USBD_StatusTypeDef ret = USBD_OK; + uint8_t idx; + + if (epnum == 0U) + { + pep = &pdev->ep_out[0]; + + if (pdev->ep0_state == USBD_EP0_DATA_OUT) + { + if (pep->rem_length > pep->maxpacket) + { + pep->rem_length -= pep->maxpacket; + + (void)USBD_CtlContinueRx(pdev, pdata, MIN(pep->rem_length, pep->maxpacket)); + } + else + { + /* Find the class ID relative to the current request */ + switch (pdev->request.bmRequest & 0x1FU) + { + case USB_REQ_RECIPIENT_DEVICE: + /* Device requests must be managed by the first instantiated class + (or duplicated by all classes for simplicity) */ + idx = 0U; + break; + + case USB_REQ_RECIPIENT_INTERFACE: + idx = USBD_CoreFindIF(pdev, LOBYTE(pdev->request.wIndex)); + break; + + case USB_REQ_RECIPIENT_ENDPOINT: + idx = USBD_CoreFindEP(pdev, LOBYTE(pdev->request.wIndex)); + break; + + default: + /* Back to the first class in case of doubt */ + idx = 0U; + break; + } + + if (idx < USBD_MAX_SUPPORTED_CLASS) + { + /* Setup the class ID and route the request to the relative class function */ + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + if (pdev->pClass[idx]->EP0_RxReady != NULL) + { + pdev->classId = idx; + pdev->pClass[idx]->EP0_RxReady(pdev); + } + } + } + + (void)USBD_CtlSendStatus(pdev); + } + } + } + else + { + /* Get the class index relative to this interface */ + idx = USBD_CoreFindEP(pdev, (epnum & 0x7FU)); + + if (((uint16_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) + { + /* Call the class data out function to manage the request */ + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + if (pdev->pClass[idx]->DataOut != NULL) + { + pdev->classId = idx; + ret = (USBD_StatusTypeDef)pdev->pClass[idx]->DataOut(pdev, epnum); + } + } + if (ret != USBD_OK) + { + return ret; + } + } + } + + return USBD_OK; +} + +/** + * @brief USBD_LL_DataInStage + * Handle data in stage + * @param pdev: device instance + * @param epnum: endpoint index + * @param pdata: data pointer + * @retval status + */ +USBD_StatusTypeDef USBD_LL_DataInStage(USBD_HandleTypeDef *pdev, + uint8_t epnum, uint8_t *pdata) +{ + USBD_EndpointTypeDef *pep; + USBD_StatusTypeDef ret; + uint8_t idx; + + if (epnum == 0U) + { + pep = &pdev->ep_in[0]; + + if (pdev->ep0_state == USBD_EP0_DATA_IN) + { + if (pep->rem_length > pep->maxpacket) + { + pep->rem_length -= pep->maxpacket; + + (void)USBD_CtlContinueSendData(pdev, pdata, pep->rem_length); + + /* Prepare endpoint for premature end of transfer */ + (void)USBD_LL_PrepareReceive(pdev, 0U, NULL, 0U); + } + else + { + /* last packet is MPS multiple, so send ZLP packet */ + if ((pep->maxpacket == pep->rem_length) && + (pep->total_length >= pep->maxpacket) && + (pep->total_length < pdev->ep0_data_len)) + { + (void)USBD_CtlContinueSendData(pdev, NULL, 0U); + pdev->ep0_data_len = 0U; + + /* Prepare endpoint for premature end of transfer */ + (void)USBD_LL_PrepareReceive(pdev, 0U, NULL, 0U); + } + else + { + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + if (pdev->pClass[0]->EP0_TxSent != NULL) + { + pdev->classId = 0U; + pdev->pClass[0]->EP0_TxSent(pdev); + } + } + (void)USBD_LL_StallEP(pdev, 0x80U); + (void)USBD_CtlReceiveStatus(pdev); + } + } + } + + if (pdev->dev_test_mode != 0U) + { + (void)USBD_RunTestMode(pdev); + pdev->dev_test_mode = 0U; + } + } + else + { + /* Get the class index relative to this interface */ + idx = USBD_CoreFindEP(pdev, ((uint8_t)epnum | 0x80U)); + + if (((uint16_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) + { + /* Call the class data out function to manage the request */ + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + if (pdev->pClass[idx]->DataIn != NULL) + { + pdev->classId = idx; + ret = (USBD_StatusTypeDef)pdev->pClass[idx]->DataIn(pdev, epnum); + + if (ret != USBD_OK) + { + return ret; + } + } + } + } + } + + return USBD_OK; +} + +/** + * @brief USBD_LL_Reset + * Handle Reset event + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_LL_Reset(USBD_HandleTypeDef *pdev) +{ + USBD_StatusTypeDef ret = USBD_OK; + + /* Upon Reset call user call back */ + pdev->dev_state = USBD_STATE_DEFAULT; + pdev->ep0_state = USBD_EP0_IDLE; + pdev->dev_config = 0U; + pdev->dev_remote_wakeup = 0U; + pdev->dev_test_mode = 0U; + +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + if (pdev->pClass[i] != NULL) + { + pdev->classId = i; + /* Clear configuration and De-initialize the Class process*/ + + if (pdev->pClass[i]->DeInit != NULL) + { + if (pdev->pClass[i]->DeInit(pdev, (uint8_t)pdev->dev_config) != USBD_OK) + { + ret = USBD_FAIL; + } + } + } + } + } +#else + + if (pdev->pClass[0] != NULL) + { + if (pdev->pClass[0]->DeInit != NULL) + { + if (pdev->pClass[0]->DeInit(pdev, (uint8_t)pdev->dev_config) != USBD_OK) + { + ret = USBD_FAIL; + } + } + } +#endif /* USE_USBD_COMPOSITE */ + + /* Open EP0 OUT */ + (void)USBD_LL_OpenEP(pdev, 0x00U, USBD_EP_TYPE_CTRL, USB_MAX_EP0_SIZE); + pdev->ep_out[0x00U & 0xFU].is_used = 1U; + + pdev->ep_out[0].maxpacket = USB_MAX_EP0_SIZE; + + /* Open EP0 IN */ + (void)USBD_LL_OpenEP(pdev, 0x80U, USBD_EP_TYPE_CTRL, USB_MAX_EP0_SIZE); + pdev->ep_in[0x80U & 0xFU].is_used = 1U; + + pdev->ep_in[0].maxpacket = USB_MAX_EP0_SIZE; + + return ret; +} + +/** + * @brief USBD_LL_SetSpeed + * Handle Reset event + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_LL_SetSpeed(USBD_HandleTypeDef *pdev, + USBD_SpeedTypeDef speed) +{ + pdev->dev_speed = speed; + + return USBD_OK; +} + +/** + * @brief USBD_LL_Suspend + * Handle Suspend event + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_LL_Suspend(USBD_HandleTypeDef *pdev) +{ + if (pdev->dev_state != USBD_STATE_SUSPENDED) + { + pdev->dev_old_state = pdev->dev_state; + } + + pdev->dev_state = USBD_STATE_SUSPENDED; + + return USBD_OK; +} + +/** + * @brief USBD_LL_Resume + * Handle Resume event + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_LL_Resume(USBD_HandleTypeDef *pdev) +{ + if (pdev->dev_state == USBD_STATE_SUSPENDED) + { + pdev->dev_state = pdev->dev_old_state; + } + + return USBD_OK; +} + +/** + * @brief USBD_LL_SOF + * Handle SOF event + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_LL_SOF(USBD_HandleTypeDef *pdev) +{ + /* The SOF event can be distributed for all classes that support it */ + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + if (pdev->pClass[i] != NULL) + { + if (pdev->pClass[i]->SOF != NULL) + { + pdev->classId = i; + (void)pdev->pClass[i]->SOF(pdev); + } + } + } + } +#else + if (pdev->pClass[0] != NULL) + { + if (pdev->pClass[0]->SOF != NULL) + { + (void)pdev->pClass[0]->SOF(pdev); + } + } +#endif /* USE_USBD_COMPOSITE */ + } + + return USBD_OK; +} + +/** + * @brief USBD_LL_IsoINIncomplete + * Handle iso in incomplete event + * @param pdev: device instance + * @param epnum: Endpoint number + * @retval status + */ +USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, + uint8_t epnum) +{ + if (pdev->pClass[pdev->classId] == NULL) + { + return USBD_FAIL; + } + + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + if (pdev->pClass[pdev->classId]->IsoINIncomplete != NULL) + { + (void)pdev->pClass[pdev->classId]->IsoINIncomplete(pdev, epnum); + } + } + + return USBD_OK; +} + +/** + * @brief USBD_LL_IsoOUTIncomplete + * Handle iso out incomplete event + * @param pdev: device instance + * @param epnum: Endpoint number + * @retval status + */ +USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, + uint8_t epnum) +{ + if (pdev->pClass[pdev->classId] == NULL) + { + return USBD_FAIL; + } + + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + if (pdev->pClass[pdev->classId]->IsoOUTIncomplete != NULL) + { + (void)pdev->pClass[pdev->classId]->IsoOUTIncomplete(pdev, epnum); + } + } + + return USBD_OK; +} + +/** + * @brief USBD_LL_DevConnected + * Handle device connection event + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_LL_DevConnected(USBD_HandleTypeDef *pdev) +{ + /* Prevent unused argument compilation warning */ + UNUSED(pdev); + + return USBD_OK; +} + +/** + * @brief USBD_LL_DevDisconnected + * Handle device disconnection event + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_LL_DevDisconnected(USBD_HandleTypeDef *pdev) +{ + USBD_StatusTypeDef ret = USBD_OK; + + /* Free Class Resources */ + pdev->dev_state = USBD_STATE_DEFAULT; + +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + if (pdev->pClass[i] != NULL) + { + pdev->classId = i; + /* Clear configuration and De-initialize the Class process*/ + if (pdev->pClass[i]->DeInit(pdev, (uint8_t)pdev->dev_config) != 0U) + { + ret = USBD_FAIL; + } + } + } + } +#else + if (pdev->pClass[0] != NULL) + { + if (pdev->pClass[0]->DeInit(pdev, (uint8_t)pdev->dev_config) != 0U) + { + ret = USBD_FAIL; + } + } +#endif /* USE_USBD_COMPOSITE */ + + return ret; +} + +/** + * @brief USBD_CoreFindIF + * return the class index relative to the selected interface + * @param pdev: device instance + * @param index : selected interface number + * @retval index of the class using the selected interface number. OxFF if no class found. + */ +uint8_t USBD_CoreFindIF(USBD_HandleTypeDef *pdev, uint8_t index) +{ +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + /* Parse all interfaces listed in the current class */ + for (uint32_t j = 0U; j < pdev->tclasslist[i].NumIf; j++) + { + /* Check if requested Interface matches the current class interface */ + if (pdev->tclasslist[i].Ifs[j] == index) + { + if (pdev->pClass[i]->Setup != NULL) + { + return (uint8_t)i; + } + } + } + } + } + + return 0xFFU; +#else + UNUSED(pdev); + UNUSED(index); + + return 0x00U; +#endif /* USE_USBD_COMPOSITE */ +} + +/** + * @brief USBD_CoreFindEP + * return the class index relative to the selected endpoint + * @param pdev: device instance + * @param index : selected endpoint number + * @retval index of the class using the selected endpoint number. 0xFF if no class found. + */ +uint8_t USBD_CoreFindEP(USBD_HandleTypeDef *pdev, uint8_t index) +{ +#ifdef USE_USBD_COMPOSITE + /* Parse the table of classes in use */ + for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) + { + /* Check if current class is in use */ + if ((pdev->tclasslist[i].Active) == 1U) + { + /* Parse all endpoints listed in the current class */ + for (uint32_t j = 0U; j < pdev->tclasslist[i].NumEps; j++) + { + /* Check if requested endpoint matches the current class endpoint */ + if (pdev->tclasslist[i].Eps[j].add == index) + { + if (pdev->pClass[i]->Setup != NULL) + { + return (uint8_t)i; + } + } + } + } + } + + return 0xFFU; +#else + UNUSED(pdev); + UNUSED(index); + + return 0x00U; +#endif /* USE_USBD_COMPOSITE */ +} + +#ifdef USE_USBD_COMPOSITE +/** + * @brief USBD_CoreGetEPAdd + * Get the endpoint address relative to a selected class + * @param pdev: device instance + * @param ep_dir: USBD_EP_IN or USBD_EP_OUT + * @param ep_type: USBD_EP_TYPE_CTRL, USBD_EP_TYPE_ISOC, USBD_EP_TYPE_BULK or USBD_EP_TYPE_INTR + * @param ClassId: The Class ID + * @retval Address of the selected endpoint or 0xFFU if no endpoint found. + */ +uint8_t USBD_CoreGetEPAdd(USBD_HandleTypeDef *pdev, uint8_t ep_dir, uint8_t ep_type, uint8_t ClassId) +{ + uint8_t idx; + + /* Find the EP address in the selected class table */ + for (idx = 0; idx < pdev->tclasslist[ClassId].NumEps; idx++) + { + if (((pdev->tclasslist[ClassId].Eps[idx].add & USBD_EP_IN) == ep_dir) && \ + (pdev->tclasslist[ClassId].Eps[idx].type == ep_type) && \ + (pdev->tclasslist[ClassId].Eps[idx].is_used != 0U)) + { + return (pdev->tclasslist[ClassId].Eps[idx].add); + } + } + + /* If reaching this point, then no endpoint was found */ + return 0xFFU; +} +#endif /* USE_USBD_COMPOSITE */ + +/** + * @brief USBD_GetEpDesc + * This function return the Endpoint descriptor + * @param pdev: device instance + * @param pConfDesc: pointer to Bos descriptor + * @param EpAddr: endpoint address + * @retval pointer to video endpoint descriptor + */ +void *USBD_GetEpDesc(uint8_t *pConfDesc, uint8_t EpAddr) +{ + USBD_DescHeaderTypeDef *pdesc = (USBD_DescHeaderTypeDef *)(void *)pConfDesc; + USBD_ConfigDescTypeDef *desc = (USBD_ConfigDescTypeDef *)(void *)pConfDesc; + USBD_EpDescTypeDef *pEpDesc = NULL; + uint16_t ptr; + + if (desc->wTotalLength > desc->bLength) + { + ptr = desc->bLength; + + while (ptr < desc->wTotalLength) + { + pdesc = USBD_GetNextDesc((uint8_t *)pdesc, &ptr); + + if (pdesc->bDescriptorType == USB_DESC_TYPE_ENDPOINT) + { + pEpDesc = (USBD_EpDescTypeDef *)(void *)pdesc; + + if (pEpDesc->bEndpointAddress == EpAddr) + { + break; + } + else + { + pEpDesc = NULL; + } + } + } + } + + return (void *)pEpDesc; +} + +/** + * @brief USBD_GetNextDesc + * This function return the next descriptor header + * @param buf: Buffer where the descriptor is available + * @param ptr: data pointer inside the descriptor + * @retval next header + */ +USBD_DescHeaderTypeDef *USBD_GetNextDesc(uint8_t *pbuf, uint16_t *ptr) +{ + USBD_DescHeaderTypeDef *pnext = (USBD_DescHeaderTypeDef *)(void *)pbuf; + + *ptr += pnext->bLength; + pnext = (USBD_DescHeaderTypeDef *)(void *)(pbuf + pnext->bLength); + + return (pnext); +} + +/** + * @} + */ + + +/** + * @} + */ + + +/** + * @} + */ + diff --git a/lib/USB_CDC/src/usbd_ctlreq.c b/lib/USB_CDC/src/usbd_ctlreq.c new file mode 100644 index 0000000..fa6ffcf --- /dev/null +++ b/lib/USB_CDC/src/usbd_ctlreq.c @@ -0,0 +1,1058 @@ +/** + ****************************************************************************** + * @file usbd_req.c + * @author MCD Application Team + * @brief This file provides the standard USB requests following chapter 9. + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_ctlreq.h" +#include "usbd_ioreq.h" + +#ifdef USE_USBD_COMPOSITE +#include "usbd_composite_builder.h" +#endif /* USE_USBD_COMPOSITE */ + +/** @addtogroup STM32_USBD_STATE_DEVICE_LIBRARY + * @{ + */ + + +/** @defgroup USBD_REQ + * @brief USB standard requests module + * @{ + */ + +/** @defgroup USBD_REQ_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_REQ_Private_Defines + * @{ + */ +#ifndef USBD_MAX_STR_DESC_SIZ +#define USBD_MAX_STR_DESC_SIZ 64U +#endif /* USBD_MAX_STR_DESC_SIZ */ +/** + * @} + */ + + +/** @defgroup USBD_REQ_Private_Macros + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_REQ_Private_Variables + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_REQ_Private_FunctionPrototypes + * @{ + */ +static void USBD_GetDescriptor(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static void USBD_SetAddress(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static USBD_StatusTypeDef USBD_SetConfig(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static void USBD_GetConfig(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static void USBD_GetStatus(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static void USBD_SetFeature(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static void USBD_ClrFeature(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static uint8_t USBD_GetLen(uint8_t *buf); + +/** + * @} + */ + + +/** @defgroup USBD_REQ_Private_Functions + * @{ + */ + + +/** + * @brief USBD_StdDevReq + * Handle standard usb device requests + * @param pdev: device instance + * @param req: usb request + * @retval status + */ +USBD_StatusTypeDef USBD_StdDevReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + USBD_StatusTypeDef ret = USBD_OK; + + switch (req->bmRequest & USB_REQ_TYPE_MASK) + { + case USB_REQ_TYPE_CLASS: + case USB_REQ_TYPE_VENDOR: + ret = (USBD_StatusTypeDef)pdev->pClass[pdev->classId]->Setup(pdev, req); + break; + + case USB_REQ_TYPE_STANDARD: + switch (req->bRequest) + { + case USB_REQ_GET_DESCRIPTOR: + USBD_GetDescriptor(pdev, req); + break; + + case USB_REQ_SET_ADDRESS: + USBD_SetAddress(pdev, req); + break; + + case USB_REQ_SET_CONFIGURATION: + ret = USBD_SetConfig(pdev, req); + break; + + case USB_REQ_GET_CONFIGURATION: + USBD_GetConfig(pdev, req); + break; + + case USB_REQ_GET_STATUS: + USBD_GetStatus(pdev, req); + break; + + case USB_REQ_SET_FEATURE: + USBD_SetFeature(pdev, req); + break; + + case USB_REQ_CLEAR_FEATURE: + USBD_ClrFeature(pdev, req); + break; + + default: + USBD_CtlError(pdev, req); + break; + } + break; + + default: + USBD_CtlError(pdev, req); + break; + } + + return ret; +} + +/** + * @brief USBD_StdItfReq + * Handle standard usb interface requests + * @param pdev: device instance + * @param req: usb request + * @retval status + */ +USBD_StatusTypeDef USBD_StdItfReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + USBD_StatusTypeDef ret = USBD_OK; + uint8_t idx; + + switch (req->bmRequest & USB_REQ_TYPE_MASK) + { + case USB_REQ_TYPE_CLASS: + case USB_REQ_TYPE_VENDOR: + case USB_REQ_TYPE_STANDARD: + switch (pdev->dev_state) + { + case USBD_STATE_DEFAULT: + case USBD_STATE_ADDRESSED: + case USBD_STATE_CONFIGURED: + + if (LOBYTE(req->wIndex) <= USBD_MAX_NUM_INTERFACES) + { + /* Get the class index relative to this interface */ + idx = USBD_CoreFindIF(pdev, LOBYTE(req->wIndex)); + if (((uint8_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) + { + /* Call the class data out function to manage the request */ + if (pdev->pClass[idx]->Setup != NULL) + { + pdev->classId = idx; + ret = (USBD_StatusTypeDef)(pdev->pClass[idx]->Setup(pdev, req)); + } + else + { + /* should never reach this condition */ + ret = USBD_FAIL; + } + } + else + { + /* No relative interface found */ + ret = USBD_FAIL; + } + + if ((req->wLength == 0U) && (ret == USBD_OK)) + { + (void)USBD_CtlSendStatus(pdev); + } + } + else + { + USBD_CtlError(pdev, req); + } + break; + + default: + USBD_CtlError(pdev, req); + break; + } + break; + + default: + USBD_CtlError(pdev, req); + break; + } + + return ret; +} + +/** + * @brief USBD_StdEPReq + * Handle standard usb endpoint requests + * @param pdev: device instance + * @param req: usb request + * @retval status + */ +USBD_StatusTypeDef USBD_StdEPReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + USBD_EndpointTypeDef *pep; + uint8_t ep_addr; + uint8_t idx; + USBD_StatusTypeDef ret = USBD_OK; + + ep_addr = LOBYTE(req->wIndex); + + switch (req->bmRequest & USB_REQ_TYPE_MASK) + { + case USB_REQ_TYPE_CLASS: + case USB_REQ_TYPE_VENDOR: + /* Get the class index relative to this endpoint */ + idx = USBD_CoreFindEP(pdev, ep_addr); + if (((uint8_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) + { + pdev->classId = idx; + /* Call the class data out function to manage the request */ + if (pdev->pClass[idx]->Setup != NULL) + { + ret = (USBD_StatusTypeDef)pdev->pClass[idx]->Setup(pdev, req); + } + } + break; + + case USB_REQ_TYPE_STANDARD: + switch (req->bRequest) + { + case USB_REQ_SET_FEATURE: + switch (pdev->dev_state) + { + case USBD_STATE_ADDRESSED: + if ((ep_addr != 0x00U) && (ep_addr != 0x80U)) + { + (void)USBD_LL_StallEP(pdev, ep_addr); + (void)USBD_LL_StallEP(pdev, 0x80U); + } + else + { + USBD_CtlError(pdev, req); + } + break; + + case USBD_STATE_CONFIGURED: + if (req->wValue == USB_FEATURE_EP_HALT) + { + if ((ep_addr != 0x00U) && (ep_addr != 0x80U) && (req->wLength == 0x00U)) + { + (void)USBD_LL_StallEP(pdev, ep_addr); + } + } + (void)USBD_CtlSendStatus(pdev); + + break; + + default: + USBD_CtlError(pdev, req); + break; + } + break; + + case USB_REQ_CLEAR_FEATURE: + + switch (pdev->dev_state) + { + case USBD_STATE_ADDRESSED: + if ((ep_addr != 0x00U) && (ep_addr != 0x80U)) + { + (void)USBD_LL_StallEP(pdev, ep_addr); + (void)USBD_LL_StallEP(pdev, 0x80U); + } + else + { + USBD_CtlError(pdev, req); + } + break; + + case USBD_STATE_CONFIGURED: + if (req->wValue == USB_FEATURE_EP_HALT) + { + if ((ep_addr & 0x7FU) != 0x00U) + { + (void)USBD_LL_ClearStallEP(pdev, ep_addr); + } + (void)USBD_CtlSendStatus(pdev); + + /* Get the class index relative to this interface */ + idx = USBD_CoreFindEP(pdev, ep_addr); + if (((uint8_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) + { + pdev->classId = idx; + /* Call the class data out function to manage the request */ + if (pdev->pClass[idx]->Setup != NULL) + { + ret = (USBD_StatusTypeDef)(pdev->pClass[idx]->Setup(pdev, req)); + } + } + } + break; + + default: + USBD_CtlError(pdev, req); + break; + } + break; + + case USB_REQ_GET_STATUS: + switch (pdev->dev_state) + { + case USBD_STATE_ADDRESSED: + if ((ep_addr != 0x00U) && (ep_addr != 0x80U)) + { + USBD_CtlError(pdev, req); + break; + } + pep = ((ep_addr & 0x80U) == 0x80U) ? &pdev->ep_in[ep_addr & 0x7FU] : \ + &pdev->ep_out[ep_addr & 0x7FU]; + + pep->status = 0x0000U; + + (void)USBD_CtlSendData(pdev, (uint8_t *)&pep->status, 2U); + break; + + case USBD_STATE_CONFIGURED: + if ((ep_addr & 0x80U) == 0x80U) + { + if (pdev->ep_in[ep_addr & 0xFU].is_used == 0U) + { + USBD_CtlError(pdev, req); + break; + } + } + else + { + if (pdev->ep_out[ep_addr & 0xFU].is_used == 0U) + { + USBD_CtlError(pdev, req); + break; + } + } + + pep = ((ep_addr & 0x80U) == 0x80U) ? &pdev->ep_in[ep_addr & 0x7FU] : \ + &pdev->ep_out[ep_addr & 0x7FU]; + + if ((ep_addr == 0x00U) || (ep_addr == 0x80U)) + { + pep->status = 0x0000U; + } + else if (USBD_LL_IsStallEP(pdev, ep_addr) != 0U) + { + pep->status = 0x0001U; + } + else + { + pep->status = 0x0000U; + } + + (void)USBD_CtlSendData(pdev, (uint8_t *)&pep->status, 2U); + break; + + default: + USBD_CtlError(pdev, req); + break; + } + break; + + default: + USBD_CtlError(pdev, req); + break; + } + break; + + default: + USBD_CtlError(pdev, req); + break; + } + + return ret; +} + + +/** + * @brief USBD_GetDescriptor + * Handle Get Descriptor requests + * @param pdev: device instance + * @param req: usb request + * @retval None + */ +static void USBD_GetDescriptor(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + uint16_t len = 0U; + uint8_t *pbuf = NULL; + uint8_t err = 0U; + + switch (req->wValue >> 8) + { +#if ((USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1U)) + case USB_DESC_TYPE_BOS: + if (pdev->pDesc->GetBOSDescriptor != NULL) + { + pbuf = pdev->pDesc->GetBOSDescriptor(pdev->dev_speed, &len); + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; +#endif /* (USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1U) */ + case USB_DESC_TYPE_DEVICE: + pbuf = pdev->pDesc->GetDeviceDescriptor(pdev->dev_speed, &len); + break; + + case USB_DESC_TYPE_CONFIGURATION: + if (pdev->dev_speed == USBD_SPEED_HIGH) + { +#ifdef USE_USBD_COMPOSITE + if ((uint8_t)(pdev->NumClasses) > 0U) + { + pbuf = (uint8_t *)USBD_CMPSIT.GetHSConfigDescriptor(&len); + } + else +#endif /* USE_USBD_COMPOSITE */ + { + pbuf = (uint8_t *)pdev->pClass[0]->GetHSConfigDescriptor(&len); + } + pbuf[1] = USB_DESC_TYPE_CONFIGURATION; + } + else + { +#ifdef USE_USBD_COMPOSITE + if ((uint8_t)(pdev->NumClasses) > 0U) + { + pbuf = (uint8_t *)USBD_CMPSIT.GetFSConfigDescriptor(&len); + } + else +#endif /* USE_USBD_COMPOSITE */ + { + pbuf = (uint8_t *)pdev->pClass[0]->GetFSConfigDescriptor(&len); + } + pbuf[1] = USB_DESC_TYPE_CONFIGURATION; + } + break; + + case USB_DESC_TYPE_STRING: + switch ((uint8_t)(req->wValue)) + { + case USBD_IDX_LANGID_STR: + if (pdev->pDesc->GetLangIDStrDescriptor != NULL) + { + pbuf = pdev->pDesc->GetLangIDStrDescriptor(pdev->dev_speed, &len); + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; + + case USBD_IDX_MFC_STR: + if (pdev->pDesc->GetManufacturerStrDescriptor != NULL) + { + pbuf = pdev->pDesc->GetManufacturerStrDescriptor(pdev->dev_speed, &len); + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; + + case USBD_IDX_PRODUCT_STR: + if (pdev->pDesc->GetProductStrDescriptor != NULL) + { + pbuf = pdev->pDesc->GetProductStrDescriptor(pdev->dev_speed, &len); + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; + + case USBD_IDX_SERIAL_STR: + if (pdev->pDesc->GetSerialStrDescriptor != NULL) + { + pbuf = pdev->pDesc->GetSerialStrDescriptor(pdev->dev_speed, &len); + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; + + case USBD_IDX_CONFIG_STR: + if (pdev->pDesc->GetConfigurationStrDescriptor != NULL) + { + pbuf = pdev->pDesc->GetConfigurationStrDescriptor(pdev->dev_speed, &len); + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; + + case USBD_IDX_INTERFACE_STR: + if (pdev->pDesc->GetInterfaceStrDescriptor != NULL) + { + pbuf = pdev->pDesc->GetInterfaceStrDescriptor(pdev->dev_speed, &len); + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; + + default: +#if (USBD_SUPPORT_USER_STRING_DESC == 1U) + pbuf = NULL; + + for (uint32_t idx = 0U; (idx < pdev->NumClasses); idx++) + { + if (pdev->pClass[idx]->GetUsrStrDescriptor != NULL) + { + pdev->classId = idx; + pbuf = pdev->pClass[idx]->GetUsrStrDescriptor(pdev, LOBYTE(req->wValue), &len); + + if (pbuf == NULL) /* This means that no class recognized the string index */ + { + continue; + } + else + { + break; + } + } + } +#endif /* USBD_SUPPORT_USER_STRING_DESC */ + +#if (USBD_CLASS_USER_STRING_DESC == 1U) + if (pdev->pDesc->GetUserStrDescriptor != NULL) + { + pbuf = pdev->pDesc->GetUserStrDescriptor(pdev->dev_speed, LOBYTE(req->wValue), &len); + } + else + { + USBD_CtlError(pdev, req); + err++; + } +#endif /* USBD_SUPPORT_USER_STRING_DESC */ + +#if ((USBD_CLASS_USER_STRING_DESC == 0U) && (USBD_SUPPORT_USER_STRING_DESC == 0U)) + USBD_CtlError(pdev, req); + err++; +#endif /* (USBD_CLASS_USER_STRING_DESC == 0U) && (USBD_SUPPORT_USER_STRING_DESC == 0U) */ + break; + } + break; + + case USB_DESC_TYPE_DEVICE_QUALIFIER: + if (pdev->dev_speed == USBD_SPEED_HIGH) + { +#ifdef USE_USBD_COMPOSITE + if ((uint8_t)(pdev->NumClasses) > 0U) + { + pbuf = (uint8_t *)USBD_CMPSIT.GetDeviceQualifierDescriptor(&len); + } + else +#endif /* USE_USBD_COMPOSITE */ + { + pbuf = (uint8_t *)pdev->pClass[0]->GetDeviceQualifierDescriptor(&len); + } + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; + + case USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION: + if (pdev->dev_speed == USBD_SPEED_HIGH) + { +#ifdef USE_USBD_COMPOSITE + if ((uint8_t)(pdev->NumClasses) > 0U) + { + pbuf = (uint8_t *)USBD_CMPSIT.GetOtherSpeedConfigDescriptor(&len); + } + else +#endif /* USE_USBD_COMPOSITE */ + { + pbuf = (uint8_t *)pdev->pClass[0]->GetOtherSpeedConfigDescriptor(&len); + } + pbuf[1] = USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION; + } + else + { + USBD_CtlError(pdev, req); + err++; + } + break; + + default: + USBD_CtlError(pdev, req); + err++; + break; + } + + if (err != 0U) + { + return; + } + + if (req->wLength != 0U) + { + if (len != 0U) + { + len = MIN(len, req->wLength); + (void)USBD_CtlSendData(pdev, pbuf, len); + } + else + { + USBD_CtlError(pdev, req); + } + } + else + { + (void)USBD_CtlSendStatus(pdev); + } +} + + +/** + * @brief USBD_SetAddress + * Set device address + * @param pdev: device instance + * @param req: usb request + * @retval None + */ +static void USBD_SetAddress(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + uint8_t dev_addr; + + if ((req->wIndex == 0U) && (req->wLength == 0U) && (req->wValue < 128U)) + { + dev_addr = (uint8_t)(req->wValue) & 0x7FU; + + if (pdev->dev_state == USBD_STATE_CONFIGURED) + { + USBD_CtlError(pdev, req); + } + else + { + pdev->dev_address = dev_addr; + (void)USBD_LL_SetUSBAddress(pdev, dev_addr); + (void)USBD_CtlSendStatus(pdev); + + if (dev_addr != 0U) + { + pdev->dev_state = USBD_STATE_ADDRESSED; + } + else + { + pdev->dev_state = USBD_STATE_DEFAULT; + } + } + } + else + { + USBD_CtlError(pdev, req); + } +} + +/** + * @brief USBD_SetConfig + * Handle Set device configuration request + * @param pdev: device instance + * @param req: usb request + * @retval status + */ +static USBD_StatusTypeDef USBD_SetConfig(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + USBD_StatusTypeDef ret = USBD_OK; + static uint8_t cfgidx; + + cfgidx = (uint8_t)(req->wValue); + + if (cfgidx > USBD_MAX_NUM_CONFIGURATION) + { + USBD_CtlError(pdev, req); + return USBD_FAIL; + } + + switch (pdev->dev_state) + { + case USBD_STATE_ADDRESSED: + if (cfgidx != 0U) + { + pdev->dev_config = cfgidx; + + ret = USBD_SetClassConfig(pdev, cfgidx); + + if (ret != USBD_OK) + { + USBD_CtlError(pdev, req); + pdev->dev_state = USBD_STATE_ADDRESSED; + } + else + { + (void)USBD_CtlSendStatus(pdev); + pdev->dev_state = USBD_STATE_CONFIGURED; + +#if (USBD_USER_REGISTER_CALLBACK == 1U) + if (pdev->DevStateCallback != NULL) + { + pdev->DevStateCallback(USBD_STATE_CONFIGURED, cfgidx); + } +#endif /* USBD_USER_REGISTER_CALLBACK */ + } + } + else + { + (void)USBD_CtlSendStatus(pdev); + } + break; + + case USBD_STATE_CONFIGURED: + if (cfgidx == 0U) + { + pdev->dev_state = USBD_STATE_ADDRESSED; + pdev->dev_config = cfgidx; + (void)USBD_ClrClassConfig(pdev, cfgidx); + (void)USBD_CtlSendStatus(pdev); + } + else if (cfgidx != pdev->dev_config) + { + /* Clear old configuration */ + (void)USBD_ClrClassConfig(pdev, (uint8_t)pdev->dev_config); + + /* set new configuration */ + pdev->dev_config = cfgidx; + + ret = USBD_SetClassConfig(pdev, cfgidx); + + if (ret != USBD_OK) + { + USBD_CtlError(pdev, req); + (void)USBD_ClrClassConfig(pdev, (uint8_t)pdev->dev_config); + pdev->dev_state = USBD_STATE_ADDRESSED; + } + else + { + (void)USBD_CtlSendStatus(pdev); + } + } + else + { + (void)USBD_CtlSendStatus(pdev); + } + break; + + default: + USBD_CtlError(pdev, req); + (void)USBD_ClrClassConfig(pdev, cfgidx); + ret = USBD_FAIL; + break; + } + + return ret; +} + +/** + * @brief USBD_GetConfig + * Handle Get device configuration request + * @param pdev: device instance + * @param req: usb request + * @retval None + */ +static void USBD_GetConfig(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + if (req->wLength != 1U) + { + USBD_CtlError(pdev, req); + } + else + { + switch (pdev->dev_state) + { + case USBD_STATE_DEFAULT: + case USBD_STATE_ADDRESSED: + pdev->dev_default_config = 0U; + (void)USBD_CtlSendData(pdev, (uint8_t *)&pdev->dev_default_config, 1U); + break; + + case USBD_STATE_CONFIGURED: + (void)USBD_CtlSendData(pdev, (uint8_t *)&pdev->dev_config, 1U); + break; + + default: + USBD_CtlError(pdev, req); + break; + } + } +} + +/** + * @brief USBD_GetStatus + * Handle Get Status request + * @param pdev: device instance + * @param req: usb request + * @retval None + */ +static void USBD_GetStatus(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + switch (pdev->dev_state) + { + case USBD_STATE_DEFAULT: + case USBD_STATE_ADDRESSED: + case USBD_STATE_CONFIGURED: + if (req->wLength != 0x2U) + { + USBD_CtlError(pdev, req); + break; + } + +#if (USBD_SELF_POWERED == 1U) + pdev->dev_config_status = USB_CONFIG_SELF_POWERED; +#else + pdev->dev_config_status = 0U; +#endif /* USBD_SELF_POWERED */ + + if (pdev->dev_remote_wakeup != 0U) + { + pdev->dev_config_status |= USB_CONFIG_REMOTE_WAKEUP; + } + + (void)USBD_CtlSendData(pdev, (uint8_t *)&pdev->dev_config_status, 2U); + break; + + default: + USBD_CtlError(pdev, req); + break; + } +} + + +/** + * @brief USBD_SetFeature + * Handle Set device feature request + * @param pdev: device instance + * @param req: usb request + * @retval None + */ +static void USBD_SetFeature(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + if (req->wValue == USB_FEATURE_REMOTE_WAKEUP) + { + pdev->dev_remote_wakeup = 1U; + (void)USBD_CtlSendStatus(pdev); + } + else if (req->wValue == USB_FEATURE_TEST_MODE) + { + pdev->dev_test_mode = (uint8_t)(req->wIndex >> 8); + (void)USBD_CtlSendStatus(pdev); + } + else + { + USBD_CtlError(pdev, req); + } +} + + +/** + * @brief USBD_ClrFeature + * Handle clear device feature request + * @param pdev: device instance + * @param req: usb request + * @retval None + */ +static void USBD_ClrFeature(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + switch (pdev->dev_state) + { + case USBD_STATE_DEFAULT: + case USBD_STATE_ADDRESSED: + case USBD_STATE_CONFIGURED: + if (req->wValue == USB_FEATURE_REMOTE_WAKEUP) + { + pdev->dev_remote_wakeup = 0U; + (void)USBD_CtlSendStatus(pdev); + } + break; + + default: + USBD_CtlError(pdev, req); + break; + } +} + + +/** + * @brief USBD_ParseSetupRequest + * Copy buffer into setup structure + * @param req: usb request + * @param pdata: setup data pointer + * @retval None + */ +void USBD_ParseSetupRequest(USBD_SetupReqTypedef *req, uint8_t *pdata) +{ + uint8_t *pbuff = pdata; + + req->bmRequest = *(uint8_t *)(pbuff); + + pbuff++; + req->bRequest = *(uint8_t *)(pbuff); + + pbuff++; + req->wValue = SWAPBYTE(pbuff); + + pbuff++; + pbuff++; + req->wIndex = SWAPBYTE(pbuff); + + pbuff++; + pbuff++; + req->wLength = SWAPBYTE(pbuff); +} + + +/** + * @brief USBD_CtlError + * Handle USB low level Error + * @param pdev: device instance + * @param req: usb request + * @retval None + */ +void USBD_CtlError(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + UNUSED(req); + + (void)USBD_LL_StallEP(pdev, 0x80U); + (void)USBD_LL_StallEP(pdev, 0U); +} + + +/** + * @brief USBD_GetString + * Convert Ascii string into unicode one + * @param desc : descriptor buffer + * @param unicode : Formatted string buffer (unicode) + * @param len : descriptor length + * @retval None + */ +void USBD_GetString(uint8_t *desc, uint8_t *unicode, uint16_t *len) +{ + uint8_t idx = 0U; + uint8_t *pdesc; + + if (desc == NULL) + { + return; + } + + pdesc = desc; + *len = MIN(USBD_MAX_STR_DESC_SIZ, ((uint16_t)USBD_GetLen(pdesc) * 2U) + 2U); + + unicode[idx] = *(uint8_t *)len; + idx++; + unicode[idx] = USB_DESC_TYPE_STRING; + idx++; + + while (*pdesc != (uint8_t)'\0') + { + unicode[idx] = *pdesc; + pdesc++; + idx++; + + unicode[idx] = 0U; + idx++; + } +} + + +/** + * @brief USBD_GetLen + * return the string length + * @param buf : pointer to the ascii string buffer + * @retval string length + */ +static uint8_t USBD_GetLen(uint8_t *buf) +{ + uint8_t len = 0U; + uint8_t *pbuff = buf; + + while (*pbuff != (uint8_t)'\0') + { + len++; + pbuff++; + } + + return len; +} +/** + * @} + */ + + +/** + * @} + */ + + +/** + * @} + */ + diff --git a/lib/USB_CDC/src/usbd_desc.c b/lib/USB_CDC/src/usbd_desc.c new file mode 100644 index 0000000..3c467cd --- /dev/null +++ b/lib/USB_CDC/src/usbd_desc.c @@ -0,0 +1,63 @@ +#include "usbd_core.h" +#include "usbd_desc.h" +#include "usbd_conf.h" + +#define USBD_VID 0x0483 /* STMicroelectronics */ +#define USBD_PID_FS 0x5740 /* CDC Virtual COM Port */ +#define USBD_LANGID_STRING 0x0409 /* English US */ +#define USBD_MFR_STRING "SaltyLab" +#define USBD_PRODUCT_STRING "SaltyLab IMU" +#define USBD_SERIAL_STRING "SALTY001" + +static uint8_t *USBD_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); + +USBD_DescriptorsTypeDef SaltyLab_Desc = { + USBD_DeviceDescriptor, + USBD_LangIDStrDescriptor, + USBD_ManufacturerStrDescriptor, + USBD_ProductStrDescriptor, + USBD_SerialStrDescriptor, + USBD_ConfigStrDescriptor, + USBD_InterfaceStrDescriptor, +}; + +static uint8_t USBD_DeviceDesc[USB_LEN_DEV_DESC] = { + 0x12, USB_DESC_TYPE_DEVICE, 0x00, 0x02, 0x02, 0x02, 0x00, + 64, LOBYTE(USBD_VID), HIBYTE(USBD_VID), LOBYTE(USBD_PID_FS), HIBYTE(USBD_PID_FS), + 0x00, 0x02, 1, 2, 3, 1 +}; + +static uint8_t USBD_LangIDDesc[USB_LEN_LANGID_STR_DESC] = { + USB_LEN_LANGID_STR_DESC, USB_DESC_TYPE_STRING, + LOBYTE(USBD_LANGID_STRING), HIBYTE(USBD_LANGID_STRING) +}; + +static uint8_t USBD_StrDesc[USBD_MAX_STR_DESC_SIZ]; + +static uint8_t *USBD_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { + (void)speed; *length = sizeof(USBD_DeviceDesc); return USBD_DeviceDesc; +} +static uint8_t *USBD_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { + (void)speed; *length = sizeof(USBD_LangIDDesc); return USBD_LangIDDesc; +} +static uint8_t *USBD_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { + USBD_GetString((uint8_t *)USBD_MFR_STRING, USBD_StrDesc, length); return USBD_StrDesc; +} +static uint8_t *USBD_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { + USBD_GetString((uint8_t *)USBD_PRODUCT_STRING, USBD_StrDesc, length); return USBD_StrDesc; +} +static uint8_t *USBD_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { + USBD_GetString((uint8_t *)USBD_SERIAL_STRING, USBD_StrDesc, length); return USBD_StrDesc; +} +static uint8_t *USBD_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { + USBD_GetString((uint8_t *)"CDC Config", USBD_StrDesc, length); return USBD_StrDesc; +} +static uint8_t *USBD_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { + USBD_GetString((uint8_t *)"CDC Interface", USBD_StrDesc, length); return USBD_StrDesc; +} diff --git a/lib/USB_CDC/src/usbd_ioreq.c b/lib/USB_CDC/src/usbd_ioreq.c new file mode 100644 index 0000000..e8a44db --- /dev/null +++ b/lib/USB_CDC/src/usbd_ioreq.c @@ -0,0 +1,224 @@ +/** + ****************************************************************************** + * @file usbd_ioreq.c + * @author MCD Application Team + * @brief This file provides the IO requests APIs for control endpoints. + ****************************************************************************** + * @attention + * + * Copyright (c) 2015 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_ioreq.h" + +/** @addtogroup STM32_USB_DEVICE_LIBRARY + * @{ + */ + + +/** @defgroup USBD_IOREQ + * @brief control I/O requests module + * @{ + */ + +/** @defgroup USBD_IOREQ_Private_TypesDefinitions + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_IOREQ_Private_Defines + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_IOREQ_Private_Macros + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_IOREQ_Private_Variables + * @{ + */ + +/** + * @} + */ + + +/** @defgroup USBD_IOREQ_Private_FunctionPrototypes + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_IOREQ_Private_Functions + * @{ + */ + +/** + * @brief USBD_CtlSendData + * send data on the ctl pipe + * @param pdev: device instance + * @param buff: pointer to data buffer + * @param len: length of data to be sent + * @retval status + */ +USBD_StatusTypeDef USBD_CtlSendData(USBD_HandleTypeDef *pdev, + uint8_t *pbuf, uint32_t len) +{ + /* Set EP0 State */ + pdev->ep0_state = USBD_EP0_DATA_IN; + pdev->ep_in[0].total_length = len; + +#ifdef USBD_AVOID_PACKET_SPLIT_MPS + pdev->ep_in[0].rem_length = 0U; +#else + pdev->ep_in[0].rem_length = len; +#endif /* USBD_AVOID_PACKET_SPLIT_MPS */ + + /* Start the transfer */ + (void)USBD_LL_Transmit(pdev, 0x00U, pbuf, len); + + return USBD_OK; +} + +/** + * @brief USBD_CtlContinueSendData + * continue sending data on the ctl pipe + * @param pdev: device instance + * @param buff: pointer to data buffer + * @param len: length of data to be sent + * @retval status + */ +USBD_StatusTypeDef USBD_CtlContinueSendData(USBD_HandleTypeDef *pdev, + uint8_t *pbuf, uint32_t len) +{ + /* Start the next transfer */ + (void)USBD_LL_Transmit(pdev, 0x00U, pbuf, len); + + return USBD_OK; +} + +/** + * @brief USBD_CtlPrepareRx + * receive data on the ctl pipe + * @param pdev: device instance + * @param buff: pointer to data buffer + * @param len: length of data to be received + * @retval status + */ +USBD_StatusTypeDef USBD_CtlPrepareRx(USBD_HandleTypeDef *pdev, + uint8_t *pbuf, uint32_t len) +{ + /* Set EP0 State */ + pdev->ep0_state = USBD_EP0_DATA_OUT; + pdev->ep_out[0].total_length = len; + +#ifdef USBD_AVOID_PACKET_SPLIT_MPS + pdev->ep_out[0].rem_length = 0U; +#else + pdev->ep_out[0].rem_length = len; +#endif /* USBD_AVOID_PACKET_SPLIT_MPS */ + + /* Start the transfer */ + (void)USBD_LL_PrepareReceive(pdev, 0U, pbuf, len); + + return USBD_OK; +} + +/** + * @brief USBD_CtlContinueRx + * continue receive data on the ctl pipe + * @param pdev: device instance + * @param buff: pointer to data buffer + * @param len: length of data to be received + * @retval status + */ +USBD_StatusTypeDef USBD_CtlContinueRx(USBD_HandleTypeDef *pdev, + uint8_t *pbuf, uint32_t len) +{ + (void)USBD_LL_PrepareReceive(pdev, 0U, pbuf, len); + + return USBD_OK; +} + +/** + * @brief USBD_CtlSendStatus + * send zero lzngth packet on the ctl pipe + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_CtlSendStatus(USBD_HandleTypeDef *pdev) +{ + /* Set EP0 State */ + pdev->ep0_state = USBD_EP0_STATUS_IN; + + /* Start the transfer */ + (void)USBD_LL_Transmit(pdev, 0x00U, NULL, 0U); + + return USBD_OK; +} + +/** + * @brief USBD_CtlReceiveStatus + * receive zero lzngth packet on the ctl pipe + * @param pdev: device instance + * @retval status + */ +USBD_StatusTypeDef USBD_CtlReceiveStatus(USBD_HandleTypeDef *pdev) +{ + /* Set EP0 State */ + pdev->ep0_state = USBD_EP0_STATUS_OUT; + + /* Start the transfer */ + (void)USBD_LL_PrepareReceive(pdev, 0U, NULL, 0U); + + return USBD_OK; +} + +/** + * @brief USBD_GetRxCount + * returns the received data length + * @param pdev: device instance + * @param ep_addr: endpoint address + * @retval Rx Data blength + */ +uint32_t USBD_GetRxCount(USBD_HandleTypeDef *pdev, uint8_t ep_addr) +{ + return USBD_LL_GetRxDataSize(pdev, ep_addr); +} + +/** + * @} + */ + + +/** + * @} + */ + + +/** + * @} + */ + diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..82ebd17 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,18 @@ +[env:f722] +platform = ststm32 +board = nucleo_f722ze +framework = stm32cube +upload_protocol = dfu +upload_command = /opt/homebrew/bin/dfu-util -a 0 -s 0x08000000:leave -D $SOURCE +monitor_speed = 115200 +board_build.mcu = stm32f722ret6 +board_build.f_cpu = 216000000L +build_flags = + -DSTM32F722xx + -DUSE_HAL_DRIVER + -DHSE_VALUE=8000000U + -DUSE_USB_FS + -I include + -Os + -Wl,--defsym,_Min_Heap_Size=0x2000 + -Wl,--defsym,_Min_Stack_Size=0x1000 diff --git a/src/balance.c b/src/balance.c new file mode 100644 index 0000000..8659c0b --- /dev/null +++ b/src/balance.c @@ -0,0 +1,113 @@ +#include "balance.h" +#include "config.h" +#include + +/* MPU6000 raw to physical units (default ±2g, ±250°/s) */ +#define ACCEL_SCALE (1.0f / 16384.0f) /* LSB to g (±2g range) */ +#define GYRO_SCALE (1.0f / 131.0f) /* LSB to °/s (±250°/s range) */ + +/* Complementary filter coefficient — 0.98 trusts gyro, 0.02 corrects with accel */ +#define COMP_ALPHA 0.98f + +void balance_init(balance_t *b) { + b->state = BALANCE_DISARMED; + b->pitch_deg = 0.0f; + b->pitch_rate = 0.0f; + b->integral = 0.0f; + b->prev_error = 0.0f; + b->motor_cmd = 0; + + /* Default PID — conservative starting point */ + b->kp = PID_KP; + b->ki = PID_KI; + b->kd = PID_KD; + b->setpoint = 0.0f; + + /* Safety defaults from config */ + b->max_tilt = MAX_TILT_DEG; + b->max_speed = MAX_SPEED_LIMIT; +} + +void balance_update(balance_t *b, const icm42688_data_t *imu, float dt) { + if (dt <= 0.0f || dt > 0.1f) return; /* Sanity check dt */ + + /* Convert raw IMU to physical units */ + float ax = imu->ax * ACCEL_SCALE; + float ay = imu->ay * ACCEL_SCALE; + float az = imu->az * ACCEL_SCALE; + + /* + * Gyro axis for pitch depends on mounting orientation. + * MPU6000 on MAMBA F722S with CW270 alignment: + * Pitch rate = gx axis (adjust sign if needed during testing) + */ + float gyro_pitch_rate = imu->gx * GYRO_SCALE; + b->pitch_rate = gyro_pitch_rate; + + /* Accelerometer pitch angle (atan2 of forward/down axes) + * With CW270: pitch = atan2(ax, az) + * Adjust axes based on actual mounting during testing */ + float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265f); + + /* Complementary filter */ + b->pitch_deg = COMP_ALPHA * (b->pitch_deg + gyro_pitch_rate * dt) + + (1.0f - COMP_ALPHA) * accel_pitch; + + /* Safety: tilt cutoff */ + if (b->state == BALANCE_ARMED) { + if (fabsf(b->pitch_deg) > b->max_tilt) { + b->state = BALANCE_TILT_FAULT; + b->motor_cmd = 0; + b->integral = 0.0f; + return; + } + } + + if (b->state != BALANCE_ARMED) { + b->motor_cmd = 0; + b->integral = 0.0f; + b->prev_error = 0.0f; + return; + } + + /* PID */ + float error = b->setpoint - b->pitch_deg; + + /* Proportional */ + float p_term = b->kp * error; + + /* Integral with anti-windup */ + b->integral += error * dt; + if (b->integral > PID_INTEGRAL_MAX) b->integral = PID_INTEGRAL_MAX; + if (b->integral < -PID_INTEGRAL_MAX) b->integral = -PID_INTEGRAL_MAX; + float i_term = b->ki * b->integral; + + /* Derivative (on measurement to avoid setpoint kick) */ + float d_term = b->kd * (-gyro_pitch_rate); /* Use gyro directly, cleaner than differencing */ + + /* Sum and clamp */ + float output = p_term + i_term + d_term; + if (output > (float)b->max_speed) output = (float)b->max_speed; + if (output < -(float)b->max_speed) output = -(float)b->max_speed; + + b->motor_cmd = (int16_t)output; + b->prev_error = error; +} + +void balance_arm(balance_t *b) { + if (b->state == BALANCE_DISARMED) { + /* Only arm if roughly upright */ + if (fabsf(b->pitch_deg) < 10.0f) { + b->integral = 0.0f; + b->prev_error = 0.0f; + b->motor_cmd = 0; + b->state = BALANCE_ARMED; + } + } +} + +void balance_disarm(balance_t *b) { + b->state = BALANCE_DISARMED; + b->motor_cmd = 0; + b->integral = 0.0f; +} diff --git a/src/bmp280.c b/src/bmp280.c new file mode 100644 index 0000000..3035147 --- /dev/null +++ b/src/bmp280.c @@ -0,0 +1,107 @@ +/* BMP280 barometer driver — I2C1 */ +#include "stm32f7xx_hal.h" +#include "bmp280.h" + +static I2C_HandleTypeDef hi2c1; +#define BMP280_ADDR (0x76 << 1) /* SDO to GND = 0x76, to VCC = 0x77 */ + +/* Calibration data */ +static uint16_t dig_T1; +static int16_t dig_T2, dig_T3; +static uint16_t dig_P1; +static int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9; +static int32_t t_fine; + +static uint8_t i2c_read(uint8_t reg) { + uint8_t val = 0; + HAL_I2C_Mem_Read(&hi2c1, BMP280_ADDR, reg, 1, &val, 1, 100); + return val; +} + +static void i2c_read_burst(uint8_t reg, uint8_t *buf, uint8_t len) { + HAL_I2C_Mem_Read(&hi2c1, BMP280_ADDR, reg, 1, buf, len, 100); +} + +static void i2c_write(uint8_t reg, uint8_t val) { + HAL_I2C_Mem_Write(&hi2c1, BMP280_ADDR, reg, 1, &val, 1, 100); +} + +int bmp280_init(void) { + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_I2C1_CLK_ENABLE(); + + GPIO_InitTypeDef gpio = {0}; + gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9; /* PB8=SCL, PB9=SDA */ + gpio.Mode = GPIO_MODE_AF_OD; + gpio.Pull = GPIO_PULLUP; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + gpio.Alternate = GPIO_AF4_I2C1; + HAL_GPIO_Init(GPIOB, &gpio); + + hi2c1.Instance = I2C1; + hi2c1.Init.Timing = 0x20404768; /* 100kHz @ 54MHz APB1 */ + hi2c1.Init.OwnAddress1 = 0; + hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + if (HAL_I2C_Init(&hi2c1) != HAL_OK) return -1; + + HAL_Delay(10); + + /* Check chip ID */ + uint8_t id = i2c_read(0xD0); + if (id != 0x58 && id != 0x60) return -(int)id; /* 0x58=BMP280, 0x60=BME280 */ + + /* Read calibration */ + uint8_t cal[26]; + i2c_read_burst(0x88, cal, 26); + dig_T1 = (uint16_t)(cal[1] << 8 | cal[0]); + dig_T2 = (int16_t)(cal[3] << 8 | cal[2]); + dig_T3 = (int16_t)(cal[5] << 8 | cal[4]); + dig_P1 = (uint16_t)(cal[7] << 8 | cal[6]); + dig_P2 = (int16_t)(cal[9] << 8 | cal[8]); + dig_P3 = (int16_t)(cal[11] << 8 | cal[10]); + dig_P4 = (int16_t)(cal[13] << 8 | cal[12]); + dig_P5 = (int16_t)(cal[15] << 8 | cal[14]); + dig_P6 = (int16_t)(cal[17] << 8 | cal[16]); + dig_P7 = (int16_t)(cal[19] << 8 | cal[18]); + dig_P8 = (int16_t)(cal[21] << 8 | cal[20]); + dig_P9 = (int16_t)(cal[23] << 8 | cal[22]); + + /* Config: normal mode, 16x oversampling temp+press, 0.5ms standby */ + i2c_write(0xF5, 0x00); /* config: standby=0.5ms, filter=off */ + i2c_write(0xF4, 0xB7); /* ctrl_meas: osrs_t=x16, osrs_p=x16, normal mode */ + + HAL_Delay(50); + return (int)id; +} + +void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) { + uint8_t buf[6]; + i2c_read_burst(0xF7, buf, 6); + + int32_t adc_P = (int32_t)((buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4)); + int32_t adc_T = (int32_t)((buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4)); + + /* Temperature compensation (from BMP280 datasheet) */ + int32_t var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11; + int32_t var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1))) >> 12) * ((int32_t)dig_T3)) >> 14; + t_fine = var1 + var2; + *temp_x10 = (int16_t)((t_fine * 5 + 128) >> 8); /* 0.1°C units */ + + /* Pressure compensation */ + int64_t v1 = ((int64_t)t_fine) - 128000; + int64_t v2 = v1 * v1 * (int64_t)dig_P6; + v2 = v2 + ((v1 * (int64_t)dig_P5) << 17); + v2 = v2 + (((int64_t)dig_P4) << 35); + v1 = ((v1 * v1 * (int64_t)dig_P3) >> 8) + ((v1 * (int64_t)dig_P2) << 12); + v1 = (((((int64_t)1) << 47) + v1)) * ((int64_t)dig_P1) >> 33; + if (v1 == 0) { *pressure_pa = 0; return; } + int64_t p = 1048576 - adc_P; + p = (((p << 31) - v2) * 3125) / v1; + v1 = (((int64_t)dig_P9) * (p >> 13) * (p >> 13)) >> 25; + v2 = (((int64_t)dig_P8) * p) >> 19; + *pressure_pa = (int32_t)((p + v1 + v2) >> 8) + (((int64_t)dig_P7) << 4); + *pressure_pa /= 256; /* Pa */ +} diff --git a/src/hoverboard.c b/src/hoverboard.c new file mode 100644 index 0000000..572a3bc --- /dev/null +++ b/src/hoverboard.c @@ -0,0 +1,41 @@ +#include "hoverboard.h" +#include "config.h" +#include "stm32f7xx_hal.h" + +static UART_HandleTypeDef huart2; + +void hoverboard_init(void) { + /* Enable clocks */ + __HAL_RCC_USART2_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + + /* PA2=TX, PA3=RX, AF7 for USART2 */ + GPIO_InitTypeDef gpio = {0}; + gpio.Pin = GPIO_PIN_2 | GPIO_PIN_3; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_PULLUP; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + gpio.Alternate = GPIO_AF7_USART2; + HAL_GPIO_Init(GPIOA, &gpio); + + /* USART2 config */ + huart2.Instance = USART2; + huart2.Init.BaudRate = HOVERBOARD_BAUD; + huart2.Init.WordLength = UART_WORDLENGTH_8B; + huart2.Init.StopBits = UART_STOPBITS_1; + huart2.Init.Parity = UART_PARITY_NONE; + huart2.Init.Mode = UART_MODE_TX_RX; + huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart2.Init.OverSampling = UART_OVERSAMPLING_16; + HAL_UART_Init(&huart2); +} + +void hoverboard_send(int16_t speed, int16_t steer) { + hoverboard_cmd_t cmd; + cmd.start = HOVERBOARD_START_FRAME; + cmd.steer = steer; + cmd.speed = speed; + cmd.checksum = cmd.start ^ cmd.steer ^ cmd.speed; + + HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5); +} diff --git a/src/icm42688.c b/src/icm42688.c new file mode 100644 index 0000000..744d1b9 --- /dev/null +++ b/src/icm42688.c @@ -0,0 +1,175 @@ +/* MPU6000 + ICM-42688-P dual driver — auto-detects based on WHO_AM_I */ +#include "stm32f7xx_hal.h" +#include "config.h" +#include "icm42688.h" + +static SPI_HandleTypeDef hspi1; +static uint8_t imu_type = 0; /* 0=unknown, 1=MPU6000, 2=ICM42688 */ + +/* MPU6000 registers */ +#define MPU_REG_SMPLRT_DIV 0x19 +#define MPU_REG_CONFIG 0x1A +#define MPU_REG_GYRO_CONFIG 0x1B +#define MPU_REG_ACCEL_CONFIG 0x1C +#define MPU_REG_ACCEL_XOUT_H 0x3B +#define MPU_REG_PWR_MGMT_1 0x6B +#define MPU_REG_PWR_MGMT_2 0x6C +#define MPU_REG_WHO_AM_I 0x75 +#define MPU6000_WHO 0x68 + +/* ICM-42688-P registers */ +#define ICM_REG_DEVICE_CONFIG 0x11 +#define ICM_REG_TEMP_DATA1 0x1D +#define ICM_REG_PWR_MGMT0 0x4E +#define ICM_REG_GYRO_CONFIG0 0x4F +#define ICM_REG_ACCEL_CONFIG0 0x50 +#define ICM_REG_WHO_AM_I 0x75 +#define ICM_REG_BANK_SEL 0x76 +#define ICM42688_WHO 0x47 + +static void cs_low(void) { HAL_GPIO_WritePin(MPU_CS_PORT, MPU_CS_PIN, GPIO_PIN_RESET); } +static void cs_high(void) { HAL_GPIO_WritePin(MPU_CS_PORT, MPU_CS_PIN, GPIO_PIN_SET); } + +static void wreg(uint8_t reg, uint8_t val) { + uint8_t tx[2] = { reg & 0x7F, val }; + uint8_t rx[2]; + cs_low(); + HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100); + cs_high(); + HAL_Delay(1); +} + +static uint8_t rreg(uint8_t reg) { + uint8_t tx[2] = { reg | 0x80, 0x00 }; + uint8_t rx[2] = {0, 0}; + cs_low(); + HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100); + cs_high(); + return rx[1]; +} + +static uint8_t trace[16]; +static int trace_idx = 0; +static void tr(uint8_t v) { if (trace_idx < 16) trace[trace_idx++] = v; } + +static int init_mpu6000(void) { + /* Reset */ + wreg(MPU_REG_PWR_MGMT_1, 0x80); + HAL_Delay(100); + + /* Wake up, use PLL with X gyro ref */ + wreg(MPU_REG_PWR_MGMT_1, 0x01); + HAL_Delay(10); + + /* Sample rate = 1kHz (divider=0) */ + wreg(MPU_REG_SMPLRT_DIV, 0x00); + + /* DLPF = 42Hz (config=3) */ + wreg(MPU_REG_CONFIG, 0x03); + + /* Gyro: ±2000°/s (FS_SEL=3) */ + wreg(MPU_REG_GYRO_CONFIG, 0x18); + + /* Accel: ±16g (AFS_SEL=3) */ + wreg(MPU_REG_ACCEL_CONFIG, 0x18); + + /* Enable all axes */ + wreg(MPU_REG_PWR_MGMT_2, 0x00); + HAL_Delay(50); + + /* Verify */ + uint8_t pwr = rreg(MPU_REG_PWR_MGMT_1); + tr(pwr); /* Should be 0x01 */ + + return (pwr == 0x01) ? 0 : -200 - (int)pwr; +} + +int icm42688_init(void) { + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_SPI1_CLK_ENABLE(); + + GPIO_InitTypeDef gpio = {0}; + gpio.Pin = GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + gpio.Alternate = GPIO_AF5_SPI1; + HAL_GPIO_Init(GPIOA, &gpio); + + /* CS on PA4 for MAMBA */ + gpio.Pin = MPU_CS_PIN; + gpio.Mode = GPIO_MODE_OUTPUT_PP; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + gpio.Pull = GPIO_PULLUP; + HAL_GPIO_Init(MPU_CS_PORT, &gpio); + cs_high(); + + SCB_DisableDCache(); /* Avoid SPI DCache issues on F7 */ + + hspi1.Instance = SPI1; + hspi1.Init.Mode = SPI_MODE_MASTER; + hspi1.Init.Direction = SPI_DIRECTION_2LINES; + hspi1.Init.DataSize = SPI_DATASIZE_8BIT; + hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH; + hspi1.Init.CLKPhase = SPI_PHASE_2EDGE; + hspi1.Init.NSS = SPI_NSS_SOFT; + hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; + hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB; + hspi1.Init.TIMode = SPI_TIMODE_DISABLE; + hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + if (HAL_SPI_Init(&hspi1) != HAL_OK) return -1; + + HAL_Delay(200); + /* Wake from sleep first - MPU6000 needs this before WHO_AM_I */ + wreg(0x6B, 0x80); /* Reset */ + HAL_Delay(100); + wreg(0x6B, 0x01); /* Wake, PLL */ + HAL_Delay(50); + + uint8_t who = rreg(MPU_REG_WHO_AM_I); + tr(who); /* trace[0] */ + + int ret; + if (who == MPU6000_WHO) { + imu_type = 1; + ret = init_mpu6000(); + } else if (who == ICM42688_WHO) { + imu_type = 2; + ret = -99; /* TODO: ICM init */ + } else { + ret = -who; + } + + /* Speed up SPI for reads */ + hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; + HAL_SPI_Init(&hspi1); + + tr((uint8_t)imu_type); /* trace[last] */ + return ret; +} + +void icm42688_read(icm42688_data_t *d) { + if (imu_type == 1) { + /* MPU6000: ACCEL_XOUT_H (0x3B) → 14 bytes: accel(6)+temp(2)+gyro(6) */ + uint8_t tx[15], rx[15]; + tx[0] = MPU_REG_ACCEL_XOUT_H | 0x80; + for (int i = 1; i < 15; i++) tx[i] = 0x00; + cs_low(); + HAL_SPI_TransmitReceive(&hspi1, tx, rx, 15, 100); + cs_high(); + + d->ax = (int16_t)((rx[1] << 8) | rx[2]); + d->ay = (int16_t)((rx[3] << 8) | rx[4]); + d->az = (int16_t)((rx[5] << 8) | rx[6]); + int16_t temp_raw = (int16_t)((rx[7] << 8) | rx[8]); + d->temp_x10 = (int16_t)((temp_raw + 12421) / 34); /* MPU6000 formula */ + d->gx = (int16_t)((rx[9] << 8) | rx[10]); + d->gy = (int16_t)((rx[11] << 8) | rx[12]); + d->gz = (int16_t)((rx[13] << 8) | rx[14]); + } +} + +void icm42688_get_trace(uint8_t *out, int *len) { + *len = trace_idx; + for (int i = 0; i < trace_idx; i++) out[i] = trace[i]; +} diff --git a/src/main.c b/src/main.c new file mode 100644 index 0000000..d359c20 --- /dev/null +++ b/src/main.c @@ -0,0 +1,141 @@ +#include "stm32f7xx_hal.h" +#include "usbd_core.h" +#include "usbd_cdc.h" +#include "usbd_cdc_if.h" +#include "usbd_desc.h" +#include "icm42688.h" +#include "bmp280.h" +#include "balance.h" +#include "hoverboard.h" +#include "config.h" +#include "status.h" +#include +#include + +extern volatile uint8_t cdc_streaming; /* set by S command in CDC RX */ +extern volatile uint8_t cdc_arm_request; /* set by A command */ +extern volatile uint8_t cdc_disarm_request; /* set by D command */ + +USBD_HandleTypeDef hUsbDevice; + +static void SystemClock_Config(void) { + RCC_OscInitTypeDef osc = {0}; + RCC_ClkInitTypeDef clk = {0}; + RCC_PeriphCLKInitTypeDef pclk = {0}; + __HAL_RCC_PWR_CLK_ENABLE(); + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); + osc.OscillatorType = RCC_OSCILLATORTYPE_HSE; + osc.HSEState = RCC_HSE_ON; + osc.PLL.PLLState = RCC_PLL_ON; + osc.PLL.PLLSource = RCC_PLLSOURCE_HSE; + osc.PLL.PLLM = 8; osc.PLL.PLLN = 432; osc.PLL.PLLP = 2; osc.PLL.PLLQ = 9; + if (HAL_RCC_OscConfig(&osc) != HAL_OK) { + osc.OscillatorType = RCC_OSCILLATORTYPE_HSI; + osc.HSIState = RCC_HSI_ON; osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + osc.HSEState = RCC_HSE_OFF; osc.PLL.PLLSource = RCC_PLLSOURCE_HSI; osc.PLL.PLLM = 16; + if (HAL_RCC_OscConfig(&osc) != HAL_OK) while (1); + } + HAL_PWREx_EnableOverDrive(); + pclk.PeriphClockSelection = RCC_PERIPHCLK_CLK48 | RCC_PERIPHCLK_I2C1; + pclk.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP; + pclk.PLLSAI.PLLSAIN = 384; pclk.PLLSAI.PLLSAIQ = 7; pclk.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8; + pclk.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; + if (HAL_RCCEx_PeriphCLKConfig(&pclk) != HAL_OK) while (1); + clk.ClockType = RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + clk.AHBCLKDivider = RCC_SYSCLK_DIV1; + clk.APB1CLKDivider = RCC_HCLK_DIV4; clk.APB2CLKDivider = RCC_HCLK_DIV2; + HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7); +} + +extern PCD_HandleTypeDef hpcd; +void OTG_FS_IRQHandler(void) { HAL_PCD_IRQHandler(&hpcd); } +void SysTick_Handler(void) { HAL_IncTick(); } + +int main(void) { + SCB_EnableICache(); + checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */ + HAL_Init(); + SystemClock_Config(); + + /* USB CDC */ + USBD_Init(&hUsbDevice, &SaltyLab_Desc, 0); + USBD_RegisterClass(&hUsbDevice, &USBD_CDC); + USBD_CDC_RegisterInterface(&hUsbDevice, &USBD_CDC_fops); + USBD_Start(&hUsbDevice); + + status_init(); + HAL_Delay(3000); /* Wait for USB host to enumerate */ + + /* Init IMU */ + int imu_ret = icm42688_init(); + + /* Init hoverboard ESC UART */ + hoverboard_init(); + + /* Init balance controller */ + balance_t bal; + balance_init(&bal); + + char buf[256]; + int len; + + icm42688_data_t imu; + uint32_t send_tick = 0; + uint32_t balance_tick = 0; + uint32_t esc_tick = 0; + const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */ + + while (1) { + if (imu_ret == 0) icm42688_read(&imu); + + uint32_t now = HAL_GetTick(); + + /* Handle arm/disarm requests from USB */ + if (cdc_arm_request) { + cdc_arm_request = 0; + balance_arm(&bal); + } + if (cdc_disarm_request) { + cdc_disarm_request = 0; + balance_disarm(&bal); + } + + /* Balance PID at 1kHz */ + if (imu_ret == 0 && now - balance_tick >= 1) { + balance_tick = now; + balance_update(&bal, &imu, dt); + } + + /* Send to hoverboard ESC at 50Hz (every 20ms) + * Both wheels get same speed for balance, steer=0 for now */ + if (now - esc_tick >= 20) { + esc_tick = now; + if (bal.state == BALANCE_ARMED) { + hoverboard_send(bal.motor_cmd, 0); + } else { + hoverboard_send(0, 0); /* Always send to prevent ESC timeout */ + } + } + + /* USB telemetry at 50Hz (only when streaming enabled via S command) */ + if (cdc_streaming && now - send_tick >= 20) { + send_tick = now; + if (imu_ret == 0) { + len = snprintf(buf, sizeof(buf), + "{\"ax\":%d,\"ay\":%d,\"az\":%d,\"gx\":%d,\"gy\":%d,\"gz\":%d," + "\"p\":%d,\"m\":%d,\"s\":%d}\n", + imu.ax, imu.ay, imu.az, imu.gx, imu.gy, imu.gz, + (int)(bal.pitch_deg * 10), /* pitch x10 for precision without %f */ + (int)bal.motor_cmd, + (int)bal.state); + } else { + len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret); + } + CDC_Transmit((uint8_t *)buf, len); + } + + status_update(now, (imu_ret == 0), (bal.state == BALANCE_ARMED)); + HAL_Delay(1); + } +} diff --git a/src/main.c.bak b/src/main.c.bak new file mode 100644 index 0000000..5f66b9f --- /dev/null +++ b/src/main.c.bak @@ -0,0 +1,147 @@ +#include "stm32f7xx_hal.h" +#include "usbd_core.h" +#include "usbd_cdc.h" +#include "usbd_cdc_if.h" +#include "usbd_desc.h" +#include "icm42688.h" +#include "bmp280.h" +#include "balance.h" +#include "hoverboard.h" +#include "config.h" +#include "status.h" +#include +#include + +extern volatile uint8_t cdc_streaming; /* set by S command in CDC RX */ +extern volatile uint8_t cdc_arm_request; /* set by A command */ +extern volatile uint8_t cdc_disarm_request; /* set by D command */ + +USBD_HandleTypeDef hUsbDevice; + +static void SystemClock_Config(void) { + RCC_OscInitTypeDef osc = {0}; + RCC_ClkInitTypeDef clk = {0}; + RCC_PeriphCLKInitTypeDef pclk = {0}; + __HAL_RCC_PWR_CLK_ENABLE(); + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); + osc.OscillatorType = RCC_OSCILLATORTYPE_HSE; + osc.HSEState = RCC_HSE_ON; + osc.PLL.PLLState = RCC_PLL_ON; + osc.PLL.PLLSource = RCC_PLLSOURCE_HSE; + osc.PLL.PLLM = 8; osc.PLL.PLLN = 432; osc.PLL.PLLP = 2; osc.PLL.PLLQ = 9; + if (HAL_RCC_OscConfig(&osc) != HAL_OK) { + osc.OscillatorType = RCC_OSCILLATORTYPE_HSI; + osc.HSIState = RCC_HSI_ON; osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + osc.HSEState = RCC_HSE_OFF; osc.PLL.PLLSource = RCC_PLLSOURCE_HSI; osc.PLL.PLLM = 16; + if (HAL_RCC_OscConfig(&osc) != HAL_OK) while (1); + } + HAL_PWREx_EnableOverDrive(); + pclk.PeriphClockSelection = RCC_PERIPHCLK_CLK48 | RCC_PERIPHCLK_I2C1; + pclk.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP; + pclk.PLLSAI.PLLSAIN = 384; pclk.PLLSAI.PLLSAIQ = 7; pclk.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8; + pclk.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; + if (HAL_RCCEx_PeriphCLKConfig(&pclk) != HAL_OK) while (1); + clk.ClockType = RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + clk.AHBCLKDivider = RCC_SYSCLK_DIV1; + clk.APB1CLKDivider = RCC_HCLK_DIV4; clk.APB2CLKDivider = RCC_HCLK_DIV2; + HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7); +} + +extern PCD_HandleTypeDef hpcd; +void OTG_FS_IRQHandler(void) { HAL_PCD_IRQHandler(&hpcd); } +void SysTick_Handler(void) { HAL_IncTick(); } + +int main(void) { + SCB_EnableICache(); + checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */ + HAL_Init(); + SystemClock_Config(); + + /* USB CDC */ + USBD_Init(&hUsbDevice, &SaltyLab_Desc, 0); + USBD_RegisterClass(&hUsbDevice, &USBD_CDC); + USBD_CDC_RegisterInterface(&hUsbDevice, &USBD_CDC_fops); + USBD_Start(&hUsbDevice); + + status_init(); + HAL_Delay(1000); + + /* Init IMU */ + int imu_ret = icm42688_init(); + + /* Init hoverboard ESC UART */ + hoverboard_init(); + + /* Init balance controller */ + balance_t bal; + balance_init(&bal); + + /* Send init status over USB */ + uint8_t tr[16]; int tlen; + icm42688_get_trace(tr, &tlen); + char buf[256]; + int len = snprintf(buf, sizeof(buf), + "{\"init\":1,\"imu\":%d,\"who\":%d}\n", + imu_ret, tr[0]); + CDC_Transmit((uint8_t *)buf, len); + + icm42688_data_t imu; + uint32_t send_tick = 0; + uint32_t balance_tick = 0; + uint32_t esc_tick = 0; + const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */ + + while (1) { + if (imu_ret == 0) icm42688_read(&imu); + + uint32_t now = HAL_GetTick(); + + /* Handle arm/disarm requests from USB */ + if (cdc_arm_request) { + cdc_arm_request = 0; + balance_arm(&bal); + } + if (cdc_disarm_request) { + cdc_disarm_request = 0; + balance_disarm(&bal); + } + + /* Balance PID at 1kHz */ + if (imu_ret == 0 && now - balance_tick >= 1) { + balance_tick = now; + balance_update(&bal, &imu, dt); + } + + /* Send to hoverboard ESC at 50Hz (every 20ms) + * Both wheels get same speed for balance, steer=0 for now */ + if (now - esc_tick >= 20) { + esc_tick = now; + if (bal.state == BALANCE_ARMED) { + hoverboard_send(bal.motor_cmd, 0); + } else { + hoverboard_send(0, 0); /* Always send to prevent ESC timeout */ + } + } + + /* USB telemetry at 50Hz (only when streaming enabled via S command) */ + if (cdc_streaming && now - send_tick >= 20) { + send_tick = now; + if (imu_ret == 0) { + len = snprintf(buf, sizeof(buf), + "{\"ax\":%d,\"ay\":%d,\"az\":%d,\"gx\":%d,\"gy\":%d,\"gz\":%d," + "\"p\":%d,\"m\":%d,\"s\":%d}\n", + imu.ax, imu.ay, imu.az, imu.gx, imu.gy, imu.gz, + (int)(bal.pitch_deg * 10), /* pitch x10 for precision without %f */ + (int)bal.motor_cmd, + (int)bal.state); + } else { + len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret); + } + CDC_Transmit((uint8_t *)buf, len); + } + + status_update(now, (imu_ret == 0), (bal.state == BALANCE_ARMED)); + HAL_Delay(1); + } +} diff --git a/src/status.c b/src/status.c new file mode 100644 index 0000000..58dd31b --- /dev/null +++ b/src/status.c @@ -0,0 +1,67 @@ +#include "stm32f7xx_hal.h" +#include "config.h" +#include "status.h" + +void status_init(void) { + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + + GPIO_InitTypeDef gpio = {0}; + gpio.Mode = GPIO_MODE_OUTPUT_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_LOW; + + /* LED1 - PC15 */ + gpio.Pin = LED1_PIN; + HAL_GPIO_Init(LED1_PORT, &gpio); + + /* LED2 - PC14 */ + gpio.Pin = LED2_PIN; + HAL_GPIO_Init(LED2_PORT, &gpio); + + /* Buzzer - PB2 (inverted: HIGH = off) */ + gpio.Pin = BEEPER_PIN; + HAL_GPIO_Init(BEEPER_PORT, &gpio); + + /* Start with both LEDs ON (active low) */ + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); + + /* Buzzer off */ + HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); +} + +void status_boot_beep(void) { + /* Two quick beeps */ + HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET); + HAL_Delay(80); + HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); + HAL_Delay(80); + HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET); + HAL_Delay(80); + HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); +} + +void status_update(uint32_t tick, int imu_ok, int baro_ok) { + if (imu_ok) { + /* Slow blink LED1 at 1Hz */ + if ((tick / 500) % 2) + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); /* ON */ + else + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_SET); /* OFF */ + + /* LED2: quick flash every 5s if baro OK */ + if (baro_ok && (tick % 5000) < 100) + HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); + else + HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET); + } else { + /* Fast blink LED1 at 5Hz = error */ + if ((tick / 100) % 2) + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); + else + HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_SET); + /* LED2 solid ON */ + HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); + } +} diff --git a/ui/index.html b/ui/index.html new file mode 100644 index 0000000..83630c4 --- /dev/null +++ b/ui/index.html @@ -0,0 +1,375 @@ + + + + + +SaltyLab Balance Bot + + + +
+

⚡ SALTYLAB DISARMED

+
PITCH --°
+
ROLL --°
+
MOTOR --
+
ACCEL --
+
GYRO --
+
HZ --
+
+ + + +
+
WebSerial ready
+
+ +
+
+ PITCH + +
+
+ MOTOR + +
+
+ + + + +