├── .gitignore ├── .gitattributes ├── Misc └── GD32F303RET6_PINMUX.png ├── .settings ├── stm32cubeide.project.prefs ├── language.settings.xml └── org.eclipse.cdt.core.prefs ├── Drivers ├── CMSIS │ ├── Device │ │ └── ST │ │ │ └── STM32F4xx │ │ │ ├── LICENSE.txt │ │ │ └── Include │ │ │ └── system_stm32f4xx.h │ └── Include │ │ ├── cmsis_version.h │ │ └── tz_context.h └── STM32F4xx_HAL_Driver │ ├── LICENSE.txt │ ├── Inc │ ├── stm32f4xx_hal_flash_ramfunc.h │ └── stm32f4xx_hal_dma_ex.h │ └── Src │ └── stm32f4xx_hal_flash_ramfunc.c ├── README.md ├── Core ├── Inc │ ├── math_ops.h │ ├── structs.h │ ├── position_sensor.h │ ├── fsm.h │ ├── gpio.h │ ├── preference_writer.h │ ├── tim.h │ ├── usart.h │ ├── calibration.h │ ├── adc.h │ ├── spi.h │ ├── systick.h │ ├── stm32f4xx_it.h │ ├── flash_writer.h │ ├── can.h │ ├── gd32f30x_libopt.h │ ├── user_config.h │ ├── main.h │ ├── gd32f30x_it.h │ ├── foc.h │ ├── lookup.h │ └── hw_config.h └── Src │ ├── preference_writer.c │ ├── math_ops.c │ ├── stm32f4xx_hal_msp.c │ ├── flash_writer.c │ ├── systick.c │ ├── sysmem.c │ ├── usart.c │ ├── syscalls.c │ ├── gpio.c │ ├── drv8323.c │ ├── calibration.c │ └── position_sensor.c ├── LICENSE ├── Firmware ├── CMSIS │ └── GD │ │ └── GD32F30x │ │ └── Include │ │ └── system_gd32f30x.h └── GD32F30x_standard_peripheral │ ├── Include │ ├── gd32f30x_crc.h │ ├── gd32f30x_wwdgt.h │ ├── gd32f30x_misc.h │ ├── gd32f30x_fwdgt.h │ └── gd32f30x_rtc.h │ └── Source │ ├── gd32f30x_crc.c │ ├── gd32f30x_wwdgt.c │ ├── gd32f30x_fwdgt.c │ └── gd32f30x_dbg.c ├── GD32F303RETx_FLASH.ld ├── STM32F446RETx_FLASH.ld ├── STM32F446RETX_RAM.ld ├── STM32F446RETX_FLASH.ld └── Makefile /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | GD32F30x_Firmware_Library* 3 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /Misc/GD32F303RET6_PINMUX.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zbwu/athena_motorcontrol/HEAD/Misc/GD32F303RET6_PINMUX.png -------------------------------------------------------------------------------- /.settings/stm32cubeide.project.prefs: -------------------------------------------------------------------------------- 1 | 2F62501ED4689FB349E356AB974DBE57=DE78DC8D1D979D5B4D10CF844311F3C5 2 | 8DF89ED150041C4CBC7CB9A9CAA90856=DE78DC8D1D979D5B4D10CF844311F3C5 3 | DC22A860405A8BF2F2C095E5B6529F12=3050C4E65C40A4458E0F6742C0D0A3BF 4 | eclipse.preferences.version=1 5 | -------------------------------------------------------------------------------- /Drivers/CMSIS/Device/ST/STM32F4xx/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This software component is provided to you as part of a software package and 2 | applicable license terms are in the Package_license file. If you received this 3 | software component outside of a package or without applicable license terms, 4 | the terms of the Apache-2.0 license shall apply. 5 | You may obtain a copy of the Apache-2.0 at: 6 | https://opensource.org/licenses/Apache-2.0 7 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This software component is provided to you as part of a software package and 2 | applicable license terms are in the Package_license file. If you received this 3 | software component outside of a package or without applicable license terms, 4 | the terms of the BSD-3-Clause license shall apply. 5 | You may obtain a copy of the BSD-3-Clause at: 6 | https://opensource.org/licenses/BSD-3-Clause 7 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # motorcontrol 2 | Written specifically for these motor controllers 3 | https://github.com/bgkatz/3phase_integrated 4 | but intended to be easy to port. 5 | 6 | Written/compiled with ST's Cube IDE: 7 | https://www.st.com/en/development-tools/stm32cubeide.html 8 | 9 | Most hardware configuration can be done through hw_config.h 10 | 11 | I think it's now fully compatible with the old mbed-based firmware, but let me know if you find bugs. 12 | -------------------------------------------------------------------------------- /Core/Inc/math_ops.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_OPS_H 2 | #define MATH_OPS_H 3 | 4 | #define SQRT3 1.73205080757f 5 | #define SQRT3_2 0.86602540378f 6 | #define SQRT1_3 0.57735026919f 7 | #ifdef STM32F446 8 | #define PI_F 3.14159274101f 9 | #define TWO_PI_F 6.28318548203f 10 | #define PI_OVER_2_F 1.57079632679f 11 | #else 12 | #define PI_F 3.1415926535897932f 13 | #define TWO_PI_F 6.2831853071795865f 14 | #define PI_OVER_2_F 1.5707963267948966f 15 | #endif 16 | #define LUT_MULT 81.4873308631f 17 | 18 | #include "math.h" 19 | 20 | float fast_fmaxf(float x, float y); 21 | float fast_fminf(float x, float y); 22 | float fmaxf3(float x, float y, float z); 23 | float fminf3(float x, float y, float z); 24 | //float roundf(float x); 25 | void limit_norm(float *x, float *y, float limit); 26 | void limit(float *x, float min, float max); 27 | int float_to_uint(float x, float x_min, float x_max, int bits); 28 | float uint_to_float(int x_int, float x_min, float x_max, int bits); 29 | float sin_lut(float theta); 30 | float cos_lut(float theta); 31 | 32 | 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /Core/Inc/structs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * structs.h 3 | * 4 | * Created on: Mar 5, 2020 5 | * Author: Ben 6 | */ 7 | 8 | #ifndef STRUCTS_H 9 | #define STRUCTS_H 10 | 11 | 12 | 13 | 14 | #include 15 | #include "spi.h" 16 | #include "gpio.h" 17 | #include "adc.h" 18 | #include "tim.h" 19 | #include "position_sensor.h" 20 | #include "preference_writer.h" 21 | #include "fsm.h" 22 | #include "drv8323.h" 23 | #include "foc.h" 24 | #include "calibration.h" 25 | #include "can.h" 26 | 27 | typedef struct{ 28 | } GPIOStruct; 29 | 30 | typedef struct{ 31 | }COMStruct; 32 | 33 | 34 | /* Global Structs */ 35 | extern ControllerStruct controller; 36 | extern ObserverStruct observer; 37 | extern COMStruct com; 38 | extern FSMStruct state; 39 | extern EncoderStruct comm_encoder; 40 | extern DRVStruct drv; 41 | extern PreferenceWriter prefs; 42 | extern CalStruct comm_encoder_cal; 43 | 44 | #ifdef STM32F446 45 | extern CANTxMessage can_tx; 46 | extern CANRxMessage can_rx; 47 | #else 48 | extern can_trasnmit_message_struct can_tx; 49 | extern can_receive_message_struct can_rx; 50 | #endif 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /Core/Inc/position_sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * position_sensor.h 3 | * 4 | * Created on: Jul 26, 2020 5 | * Author: Ben 6 | */ 7 | 8 | #ifndef INC_POSITION_SENSOR_H_ 9 | #define INC_POSITION_SENSOR_H_ 10 | 11 | 12 | #include "spi.h" 13 | #include 14 | 15 | #define N_POS_SAMPLES 20 // Number of position samples to store. should put this somewhere else... 16 | #define N_LUT 128 17 | 18 | typedef struct{ 19 | union{ 20 | uint8_t spi_tx_buff[2]; 21 | uint16_t spi_tx_word; 22 | }; 23 | union{ 24 | uint8_t spi_rx_buff[2]; 25 | uint16_t spi_rx_word; 26 | }; 27 | float angle_singleturn, old_angle, angle_multiturn[N_POS_SAMPLES], elec_angle, velocity, elec_velocity, ppairs, vel2; 28 | float output_angle_multiturn; 29 | int raw, count, old_count, turns; 30 | int count_buff[N_POS_SAMPLES]; 31 | int m_zero, e_zero; 32 | int offset_lut[N_LUT]; 33 | uint8_t first_sample; 34 | } EncoderStruct; 35 | 36 | 37 | void ps_warmup(EncoderStruct * encoder, int n); 38 | void ps_sample(EncoderStruct * encoder, float dt); 39 | void ps_print(EncoderStruct * encoder, int dt_ms); 40 | 41 | #endif /* INC_POSITION_SENSOR_H_ */ 42 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Ben Katz 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Core/Inc/fsm.h: -------------------------------------------------------------------------------- 1 | /* 2 | * fsm.h 3 | * 4 | * Created on: Mar 5, 2020 5 | * Author: Ben 6 | */ 7 | 8 | 9 | #ifndef INC_FSM_H_ 10 | #define INC_FSM_H_ 11 | #ifdef __cplusplus 12 | extern "C" { 13 | #endif 14 | 15 | #include 16 | 17 | 18 | 19 | #define MENU_MODE 0 20 | #define CALIBRATION_MODE 1 21 | #define MOTOR_MODE 2 22 | #define SETUP_MODE 4 23 | #define ENCODER_MODE 5 24 | #define INIT_TEMP_MODE 6 25 | 26 | #define ESC_CMD '\e' 27 | #define MOTOR_CMD 'm' 28 | #define CAL_CMD 'c' 29 | #define ENCODER_CMD 'e' 30 | #define SETUP_CMD 's' 31 | #define ZERO_CMD 'z' 32 | #define ENTER_CMD '\r' 33 | 34 | 35 | 36 | typedef struct{ 37 | uint8_t state; 38 | uint8_t next_state; 39 | uint8_t state_change; 40 | uint8_t ready; 41 | char cmd_buff[8]; 42 | char bytecount; 43 | char cmd_id; 44 | }FSMStruct; 45 | 46 | void run_fsm(FSMStruct* fsmstate); 47 | void update_fsm(FSMStruct * fsmstate, char fsm_input); 48 | void fsm_enter_state(FSMStruct * fsmstate); 49 | void fsm_exit_state(FSMStruct * fsmstate); 50 | void enter_menu_state(void); 51 | void enter_setup_state(void); 52 | void enter_motor_mode(void); 53 | void process_user_input(FSMStruct * fsmstate); 54 | 55 | #ifdef __cplusplus 56 | } 57 | #endif 58 | 59 | #endif /* INC_FSM_H_ */ 60 | -------------------------------------------------------------------------------- /Core/Inc/gpio.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file gpio.h 5 | * @brief This file contains all the function prototypes for 6 | * the gpio.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2022 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __GPIO_H__ 22 | #define __GPIO_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | #ifdef STM32F446 32 | /* USER CODE BEGIN Includes */ 33 | 34 | /* USER CODE END Includes */ 35 | 36 | /* USER CODE BEGIN Private defines */ 37 | 38 | /* USER CODE END Private defines */ 39 | 40 | void MX_GPIO_Init(void); 41 | 42 | /* USER CODE BEGIN Prototypes */ 43 | 44 | /* USER CODE END Prototypes */ 45 | 46 | #else 47 | void MX_GPIO_Init(void); 48 | #endif 49 | #ifdef __cplusplus 50 | } 51 | #endif 52 | #endif /*__ GPIO_H__ */ 53 | 54 | -------------------------------------------------------------------------------- /Core/Inc/preference_writer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * preference_writer.h 3 | * 4 | * Created on: Apr 13, 2020 5 | * Author: ben 6 | */ 7 | 8 | #ifndef INC_PREFERENCE_WRITER_H_ 9 | #define INC_PREFERENCE_WRITER_H_ 10 | 11 | 12 | #ifndef __PREFERENCE_WRITER_H 13 | #define __PREFERENCE_WRITER_H 14 | 15 | #include "flash_writer.h" 16 | #include "stdbool.h" 17 | 18 | /* 19 | class PreferenceWriter { 20 | public: 21 | PreferenceWriter(uint32_t sector); 22 | void open(); 23 | bool ready(); 24 | void write(int x, int index); 25 | void write(float x, int index); 26 | void flush(); 27 | void load(); 28 | void close(); 29 | private: 30 | FlashWriter *writer; 31 | uint32_t __sector; 32 | bool __ready; 33 | }; 34 | */ 35 | 36 | /* 37 | PreferenceWriter::PreferenceWriter(uint32_t sector) { 38 | writer = new FlashWriter(sector); 39 | __sector = sector; 40 | __ready = false; 41 | } 42 | */ 43 | typedef struct{ 44 | FlashWriter fw; 45 | #ifdef STM32F446 46 | uint32_t sector; 47 | #endif 48 | bool ready; 49 | }PreferenceWriter; 50 | 51 | void preference_writer_init(PreferenceWriter * pr, uint32_t sector); 52 | void preference_writer_open(PreferenceWriter * pr); 53 | bool preference_writer_ready(PreferenceWriter pr); 54 | void preference_writer_write_int(int x, int index); 55 | void preference_writer_write_float(float x, int index); 56 | void preference_writer_flush(PreferenceWriter * pr); 57 | void preference_writer_load(PreferenceWriter pr); 58 | void preference_writer_close(PreferenceWriter *pr); 59 | 60 | extern PreferenceWriter prefs; 61 | 62 | 63 | #endif 64 | 65 | 66 | #endif /* INC_PREFERENCE_WRITER_H_ */ 67 | -------------------------------------------------------------------------------- /Core/Inc/tim.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file tim.h 5 | * @brief This file contains all the function prototypes for 6 | * the tim.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2022 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __TIM_H__ 22 | #define __TIM_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | #ifdef STM32F446 32 | /* USER CODE BEGIN Includes */ 33 | 34 | /* USER CODE END Includes */ 35 | 36 | extern TIM_HandleTypeDef htim1; 37 | 38 | /* USER CODE BEGIN Private defines */ 39 | 40 | /* USER CODE END Private defines */ 41 | 42 | void MX_TIM1_Init(void); 43 | 44 | void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); 45 | 46 | /* USER CODE BEGIN Prototypes */ 47 | 48 | /* USER CODE END Prototypes */ 49 | #else 50 | void MX_TIM0_Init(void); 51 | #endif 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | 57 | #endif /* __TIM_H__ */ 58 | 59 | -------------------------------------------------------------------------------- /Core/Inc/usart.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file usart.h 5 | * @brief This file contains all the function prototypes for 6 | * the usart.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2022 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __USART_H__ 22 | #define __USART_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | #ifdef STM32F446 36 | extern UART_HandleTypeDef huart2; 37 | 38 | /* USER CODE BEGIN Private defines */ 39 | extern uint8_t Serial2RxBuffer[1]; 40 | 41 | /* USER CODE END Private defines */ 42 | 43 | void MX_USART2_UART_Init(void); 44 | 45 | /* USER CODE BEGIN Prototypes */ 46 | int __io_putchar(int ch); 47 | void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart); 48 | /* USER CODE END Prototypes */ 49 | #else 50 | void MX_USART1_Init(void); 51 | #endif 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | 57 | #endif /* __USART_H__ */ 58 | 59 | -------------------------------------------------------------------------------- /Core/Src/preference_writer.c: -------------------------------------------------------------------------------- 1 | /* 2 | * preference_writer.c 3 | * 4 | * Created on: Apr 13, 2020 5 | * Author: ben 6 | */ 7 | 8 | 9 | #include "preference_writer.h" 10 | #include "flash_writer.h" 11 | #include "user_config.h" 12 | /* 13 | PreferenceWriter::PreferenceWriter(uint32_t sector) { 14 | writer = new FlashWriter(sector); 15 | __sector = sector; 16 | __ready = false; 17 | } 18 | */ 19 | 20 | void preference_writer_init(PreferenceWriter * pr, uint32_t sector){ 21 | flash_writer_init(&pr->fw, sector); 22 | #ifdef STM32F446 23 | pr->sector = sector; 24 | #endif 25 | } 26 | 27 | 28 | void preference_writer_open(PreferenceWriter * pr) { 29 | flash_writer_open(&pr->fw); 30 | pr->ready = true; 31 | } 32 | 33 | bool preference_writer_ready(PreferenceWriter pr) { 34 | return pr.ready; 35 | } 36 | 37 | void preference_writer_write_int(int x, int index) { 38 | __int_reg[index] = x; 39 | } 40 | 41 | void preference_writer_write_float(float x, int index) { 42 | __float_reg[index] = x; 43 | } 44 | 45 | void preference_writer_flush(PreferenceWriter * pr) { 46 | int offs; 47 | for (offs = 0; offs < 256; offs++) { 48 | flash_writer_write_int(pr->fw, offs, __int_reg[offs]); 49 | } 50 | for (; offs < 320; offs++) { 51 | flash_writer_write_float(pr->fw, offs, __float_reg[offs - 256]); 52 | } 53 | pr->ready = false; 54 | } 55 | 56 | void preference_writer_load(PreferenceWriter pr) { 57 | int offs; 58 | for (offs = 0; offs < 256; offs++) { 59 | __int_reg[offs] = flash_read_int(pr.fw, offs); 60 | } 61 | for(; offs < 320; offs++) { 62 | __float_reg[offs - 256] = flash_read_float(pr.fw, offs); 63 | } 64 | } 65 | 66 | void preference_writer_close(PreferenceWriter *pr) { 67 | pr->ready = false; 68 | flash_writer_close(&pr->fw); 69 | } 70 | 71 | 72 | -------------------------------------------------------------------------------- /Drivers/CMSIS/Include/cmsis_version.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************//** 2 | * @file cmsis_version.h 3 | * @brief CMSIS Core(M) Version definitions 4 | * @version V5.0.2 5 | * @date 19. April 2017 6 | ******************************************************************************/ 7 | /* 8 | * Copyright (c) 2009-2017 ARM Limited. All rights reserved. 9 | * 10 | * SPDX-License-Identifier: Apache-2.0 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the License); you may 13 | * not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * 16 | * www.apache.org/licenses/LICENSE-2.0 17 | * 18 | * Unless required by applicable law or agreed to in writing, software 19 | * distributed under the License is distributed on an AS IS BASIS, WITHOUT 20 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | * See the License for the specific language governing permissions and 22 | * limitations under the License. 23 | */ 24 | 25 | #if defined ( __ICCARM__ ) 26 | #pragma system_include /* treat file as system include file for MISRA check */ 27 | #elif defined (__clang__) 28 | #pragma clang system_header /* treat file as system include file */ 29 | #endif 30 | 31 | #ifndef __CMSIS_VERSION_H 32 | #define __CMSIS_VERSION_H 33 | 34 | /* CMSIS Version definitions */ 35 | #define __CM_CMSIS_VERSION_MAIN ( 5U) /*!< [31:16] CMSIS Core(M) main version */ 36 | #define __CM_CMSIS_VERSION_SUB ( 1U) /*!< [15:0] CMSIS Core(M) sub version */ 37 | #define __CM_CMSIS_VERSION ((__CM_CMSIS_VERSION_MAIN << 16U) | \ 38 | __CM_CMSIS_VERSION_SUB ) /*!< CMSIS Core(M) version number */ 39 | #endif 40 | -------------------------------------------------------------------------------- /Core/Inc/calibration.h: -------------------------------------------------------------------------------- 1 | /* 2 | * calibration.h 3 | * 4 | * Created on: Aug 11, 2020 5 | * Author: ben 6 | */ 7 | 8 | #ifndef INC_CALIBRATION_H_ 9 | #define INC_CALIBRATION_H_ 10 | 11 | #include "position_sensor.h" 12 | #include "foc.h" 13 | 14 | #define V_CAL 1.0f // Calibration voltage 15 | //#define I_CAL 5.0f 16 | #define W_CAL TWO_PI_F // Calibration speed in rad/s 17 | #define T1 1.0f // Cal settling period 18 | #define PPAIRS_MAX 21 19 | #define SAMPLES_PER_PPAIR N_LUT 20 | #define N_CAL SAMPLES_PER_PPAIR*PPAIRS_MAX // Calibration lookup table maximum length, so I don't have to deal with dynamic allocation based on number of pole pairs 21 | 22 | typedef struct{ 23 | uint8_t ppairs; // number of pole pairs measured 24 | int offset; // electrical zero position in counts 25 | float theta_ref; // reference angle used for calibration 26 | int start_count; // loop count at cal start 27 | uint8_t started; // has cal started or not? 28 | float time; // cal time 29 | float theta_start; // cal start angle 30 | int ezero; 31 | uint8_t phase_order; // phase order correct (0) or swapped (1) 32 | uint8_t done_ordering, done_cal, done_rl; // flags for different cals finished 33 | uint16_t sample_count; // keep track of how many samples taken 34 | float next_sample_time; // time to take next sample 35 | int error_arr[N_CAL]; 36 | int lut_arr[N_LUT]; 37 | EncoderStruct cal_position; // Position reference used for calibration 38 | 39 | } CalStruct; 40 | 41 | void order_phases(EncoderStruct *encoder, ControllerStruct *controller, CalStruct *cal, int loop_count); 42 | void calibrate_encoder(EncoderStruct *encoder, ControllerStruct *controller, CalStruct *cal, 43 | int loop_count); 44 | void measure_lr(EncoderStruct *encoder, ControllerStruct *controller, CalStruct *cal, int loop_count); 45 | 46 | //extern int *error_array; 47 | //extern int *lut_array; 48 | 49 | #endif /* INC_CALIBRATION_H_ */ 50 | -------------------------------------------------------------------------------- /Core/Src/math_ops.c: -------------------------------------------------------------------------------- 1 | 2 | #include "math_ops.h" 3 | #include "lookup.h" 4 | 5 | 6 | float fast_fmaxf(float x, float y){ 7 | /// Returns maximum of x, y /// 8 | return (((x)>(y))?(x):(y)); 9 | } 10 | 11 | float fast_fminf(float x, float y){ 12 | /// Returns minimum of x, y /// 13 | return (((x)<(y))?(x):(y)); 14 | } 15 | 16 | float fmaxf3(float x, float y, float z){ 17 | /// Returns maximum of x, y, z /// 18 | return (x > y ? (x > z ? x : z) : (y > z ? y : z)); 19 | } 20 | 21 | float fminf3(float x, float y, float z){ 22 | /// Returns minimum of x, y, z /// 23 | return (x < y ? (x < z ? x : z) : (y < z ? y : z)); 24 | } 25 | /* 26 | float roundf(float x){ 27 | /// Returns nearest integer /// 28 | return x < 0.0f ? ceilf(x - 0.5f) : floorf(x + 0.5f); 29 | } 30 | */ 31 | void limit_norm(float *x, float *y, float limit){ 32 | /// Scales the lenght of vector (x, y) to be <= limit /// 33 | float norm = sqrtf(*x * *x + *y * *y); 34 | if(norm > limit){ 35 | *x = *x * limit/norm; 36 | *y = *y * limit/norm; 37 | } 38 | } 39 | 40 | void limit(float *x, float min, float max){ 41 | *x = fast_fmaxf(fast_fminf(*x, max), min); 42 | } 43 | 44 | int float_to_uint(float x, float x_min, float x_max, int bits){ 45 | /// Converts a float to an unsigned int, given range and number of bits /// 46 | float span = x_max - x_min; 47 | float offset = x_min; 48 | return (int) ((x-offset)*((float)((1< 42 | 43 | /* configure systick */ 44 | void systick_config(void); 45 | /* delay a time in milliseconds */ 46 | void delay_1ms(uint32_t count); 47 | /* delay decrement */ 48 | void delay_decrement(void); 49 | 50 | #endif /* SYS_TICK_H */ 51 | -------------------------------------------------------------------------------- /Core/Inc/stm32f4xx_it.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file stm32f4xx_it.h 5 | * @brief This file contains the headers of the interrupt handlers. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | *

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

11 | * 12 | * This software component is licensed by ST under BSD 3-Clause license, 13 | * the "License"; You may not use this file except in compliance with the 14 | * License. You may obtain a copy of the License at: 15 | * opensource.org/licenses/BSD-3-Clause 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | 21 | /* Define to prevent recursive inclusion -------------------------------------*/ 22 | #ifndef __STM32F4xx_IT_H 23 | #define __STM32F4xx_IT_H 24 | 25 | #ifdef __cplusplus 26 | extern "C" { 27 | #endif 28 | 29 | /* Private includes ----------------------------------------------------------*/ 30 | /* USER CODE BEGIN Includes */ 31 | /* USER CODE END Includes */ 32 | 33 | /* Exported types ------------------------------------------------------------*/ 34 | /* USER CODE BEGIN ET */ 35 | /* USER CODE END ET */ 36 | 37 | /* Exported constants --------------------------------------------------------*/ 38 | /* USER CODE BEGIN EC */ 39 | 40 | /* USER CODE END EC */ 41 | 42 | /* Exported macro ------------------------------------------------------------*/ 43 | /* USER CODE BEGIN EM */ 44 | 45 | /* USER CODE END EM */ 46 | 47 | /* Exported functions prototypes ---------------------------------------------*/ 48 | void NMI_Handler(void); 49 | void HardFault_Handler(void); 50 | void MemManage_Handler(void); 51 | void BusFault_Handler(void); 52 | void UsageFault_Handler(void); 53 | void SVC_Handler(void); 54 | void DebugMon_Handler(void); 55 | void PendSV_Handler(void); 56 | void SysTick_Handler(void); 57 | void CAN1_RX0_IRQHandler(void); 58 | void TIM1_UP_TIM10_IRQHandler(void); 59 | void USART2_IRQHandler(void); 60 | /* USER CODE BEGIN EFP */ 61 | 62 | /* USER CODE END EFP */ 63 | 64 | #ifdef __cplusplus 65 | } 66 | #endif 67 | 68 | #endif /* __STM32F4xx_IT_H */ 69 | -------------------------------------------------------------------------------- /Core/Inc/flash_writer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * flash_writer.h 3 | * 4 | * Created on: Apr 12, 2020 5 | * Author: Ben 6 | */ 7 | 8 | #ifndef INC_FLASH_WRITER_H_ 9 | #define INC_FLASH_WRITER_H_ 10 | #ifdef __cplusplus 11 | extern "C" { 12 | #endif 13 | 14 | 15 | #ifdef STM32F446 16 | #include "stm32f4xx_flash.h" 17 | #endif 18 | #include 19 | #include 20 | 21 | #ifdef STM32F446 22 | /* Base address of the Flash sectors */ 23 | #define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000) /* Base @ of Sector 0, 16 Kbytes */ 24 | #define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08004000) /* Base @ of Sector 1, 16 Kbytes */ 25 | #define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08008000) /* Base @ of Sector 2, 16 Kbytes */ 26 | #define ADDR_FLASH_SECTOR_3 ((uint32_t)0x0800C000) /* Base @ of Sector 3, 16 Kbytes */ 27 | #define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08010000) /* Base @ of Sector 4, 64 Kbytes */ 28 | #define ADDR_FLASH_SECTOR_5 ((uint32_t)0x08020000) /* Base @ of Sector 5, 128 Kbytes */ 29 | #define ADDR_FLASH_SECTOR_6 ((uint32_t)0x08040000) /* Base @ of Sector 6, 128 Kbytes */ 30 | #define ADDR_FLASH_SECTOR_7 ((uint32_t)0x08060000) /* Base @ of Sector 7, 128 Kbytes */ 31 | 32 | static uint32_t __SECTOR_ADDRS[] = {ADDR_FLASH_SECTOR_0, ADDR_FLASH_SECTOR_1, ADDR_FLASH_SECTOR_2, ADDR_FLASH_SECTOR_3, 33 | ADDR_FLASH_SECTOR_4, ADDR_FLASH_SECTOR_5, ADDR_FLASH_SECTOR_6, ADDR_FLASH_SECTOR_7}; 34 | static uint32_t __SECTORS[] = {FLASH_Sector_0, FLASH_Sector_1, FLASH_Sector_2, FLASH_Sector_3, 35 | FLASH_Sector_4, FLASH_Sector_6, FLASH_Sector_6, FLASH_Sector_7}; 36 | #endif 37 | 38 | typedef struct { 39 | bool ready; 40 | uint32_t base; 41 | uint32_t sector; 42 | }FlashWriter; 43 | 44 | void flash_writer_init(FlashWriter *fw, uint32_t sector); 45 | bool flash_writer_ready(FlashWriter fw); 46 | void flash_writer_open(FlashWriter *fw); 47 | void flash_writer_write_int(FlashWriter fw, uint32_t index, int x); 48 | void flash_writer_write_uint(FlashWriter fw, uint32_t index, unsigned int x); 49 | void flash_writer_write_float(FlashWriter fw, uint32_t index, float x); 50 | void flash_writer_close(FlashWriter *fw); 51 | int flash_read_int(FlashWriter fw, uint32_t index); 52 | uint32_t flash_read_uint(FlashWriter fw, uint32_t index); 53 | float flash_read_float(FlashWriter fw, uint32_t index); 54 | 55 | #ifdef __cplusplus 56 | } 57 | #endif 58 | 59 | #endif /* INC_FLASH_WRITER_H_ */ 60 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_hal_flash_ramfunc.h 4 | * @author MCD Application Team 5 | * @brief Header file of FLASH RAMFUNC driver. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2017 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file in 13 | * the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | ****************************************************************************** 16 | */ 17 | 18 | /* Define to prevent recursive inclusion -------------------------------------*/ 19 | #ifndef __STM32F4xx_FLASH_RAMFUNC_H 20 | #define __STM32F4xx_FLASH_RAMFUNC_H 21 | 22 | #ifdef __cplusplus 23 | extern "C" { 24 | #endif 25 | #if defined(STM32F410Tx) || defined(STM32F410Cx) || defined(STM32F410Rx) || defined(STM32F411xE) || defined(STM32F446xx) || defined(STM32F412Zx) ||\ 26 | defined(STM32F412Vx) || defined(STM32F412Rx) || defined(STM32F412Cx) 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "stm32f4xx_hal_def.h" 30 | 31 | /** @addtogroup STM32F4xx_HAL_Driver 32 | * @{ 33 | */ 34 | 35 | /** @addtogroup FLASH_RAMFUNC 36 | * @{ 37 | */ 38 | 39 | /* Exported types ------------------------------------------------------------*/ 40 | /* Exported macro ------------------------------------------------------------*/ 41 | /* Exported functions --------------------------------------------------------*/ 42 | /** @addtogroup FLASH_RAMFUNC_Exported_Functions 43 | * @{ 44 | */ 45 | 46 | /** @addtogroup FLASH_RAMFUNC_Exported_Functions_Group1 47 | * @{ 48 | */ 49 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_StopFlashInterfaceClk(void); 50 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_StartFlashInterfaceClk(void); 51 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_EnableFlashSleepMode(void); 52 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_DisableFlashSleepMode(void); 53 | /** 54 | * @} 55 | */ 56 | 57 | /** 58 | * @} 59 | */ 60 | 61 | /** 62 | * @} 63 | */ 64 | 65 | /** 66 | * @} 67 | */ 68 | 69 | #endif /* STM32F410xx || STM32F411xE || STM32F446xx || STM32F412Zx || STM32F412Vx || STM32F412Rx || STM32F412Cx */ 70 | #ifdef __cplusplus 71 | } 72 | #endif 73 | 74 | 75 | #endif /* __STM32F4xx_FLASH_RAMFUNC_H */ 76 | 77 | -------------------------------------------------------------------------------- /Firmware/CMSIS/GD/GD32F30x/Include/system_gd32f30x.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file system_gd32f30x.h 3 | \brief CMSIS Cortex-M4 Device Peripheral Access Layer Header File for 4 | GD32F30x Device Series 5 | */ 6 | 7 | /* Copyright (c) 2012 ARM LIMITED 8 | 9 | All rights reserved. 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | - Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | - Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | - Neither the name of ARM nor the names of its contributors may be used 18 | to endorse or promote products derived from this software without 19 | specific prior written permission. 20 | * 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | ---------------------------------------------------------------------------*/ 33 | /* This file refers the CMSIS standard, some adjustments are made according to GigaDevice chips */ 34 | 35 | #ifndef SYSTEM_GD32F30X_H 36 | #define SYSTEM_GD32F30X_H 37 | 38 | #ifdef __cplusplus 39 | extern "C" { 40 | #endif 41 | 42 | #include 43 | 44 | /* system clock frequency (core clock) */ 45 | extern uint32_t SystemCoreClock; 46 | 47 | /* function declarations */ 48 | /* initialize the system and update the SystemCoreClock variable */ 49 | extern void SystemInit (void); 50 | /* update the SystemCoreClock with current core clock retrieved from cpu registers */ 51 | extern void SystemCoreClockUpdate (void); 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | 57 | #endif /* SYSTEM_GD32F30X_H */ 58 | -------------------------------------------------------------------------------- /Core/Inc/can.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file can.h 5 | * @brief This file contains all the function prototypes for 6 | * the can.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2022 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __CAN_H__ 22 | #define __CAN_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | #ifdef STM32F446 36 | extern CAN_HandleTypeDef hcan1; 37 | 38 | /* USER CODE BEGIN Private defines */ 39 | #endif 40 | //#define P_MIN -12.5f 41 | //#define P_MAX 12.5f 42 | //#define V_MIN -65.0f 43 | //#define V_MAX 65.0f 44 | #define KP_MIN 0.0f 45 | //#define KP_MAX 500.0f 46 | #define KD_MIN 0.0f 47 | //#define KD_MAX 5.0f 48 | #define T_MIN -18.0f 49 | #define T_MAX 18.0f 50 | #ifdef STM32F446 51 | /* USER CODE END Private defines */ 52 | 53 | void MX_CAN1_Init(void); 54 | 55 | /* USER CODE BEGIN Prototypes */ 56 | typedef struct{ 57 | uint8_t id; 58 | uint8_t data[8]; 59 | CAN_RxHeaderTypeDef rx_header; 60 | CAN_FilterTypeDef filter; 61 | }CANRxMessage ; 62 | 63 | typedef struct{ 64 | uint8_t id; 65 | uint8_t data[6]; 66 | CAN_TxHeaderTypeDef tx_header; 67 | }CANTxMessage ; 68 | 69 | void can_rx_init(CANRxMessage *msg); 70 | void can_tx_init(CANTxMessage *msg); 71 | void pack_reply(CANTxMessage *msg, uint8_t id, float p, float v, float t); 72 | void unpack_cmd(CANRxMessage msg, float *commands); 73 | /* USER CODE END Prototypes */ 74 | 75 | #else 76 | void can_rx_init(can_receive_message_struct *msg); 77 | void can_tx_init(can_trasnmit_message_struct *msg); 78 | void pack_reply(can_trasnmit_message_struct *msg, uint8_t id, float p, float v, float t); 79 | void unpack_cmd(can_receive_message_struct msg, float *commands); 80 | 81 | void MX_CAN0_Init(void); 82 | #endif 83 | #ifdef __cplusplus 84 | } 85 | #endif 86 | 87 | #endif /* __CAN_H__ */ 88 | 89 | -------------------------------------------------------------------------------- /Core/Src/stm32f4xx_hal_msp.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * File Name : stm32f4xx_hal_msp.c 5 | * Description : This file provides code for the MSP Initialization 6 | * and de-Initialization codes. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | *

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

12 | * 13 | * This software component is licensed by ST under BSD 3-Clause license, 14 | * the "License"; You may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at: 16 | * opensource.org/licenses/BSD-3-Clause 17 | * 18 | ****************************************************************************** 19 | */ 20 | /* USER CODE END Header */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "main.h" 24 | /* USER CODE BEGIN Includes */ 25 | 26 | /* USER CODE END Includes */ 27 | 28 | /* Private typedef -----------------------------------------------------------*/ 29 | /* USER CODE BEGIN TD */ 30 | 31 | /* USER CODE END TD */ 32 | 33 | /* Private define ------------------------------------------------------------*/ 34 | /* USER CODE BEGIN Define */ 35 | 36 | /* USER CODE END Define */ 37 | 38 | /* Private macro -------------------------------------------------------------*/ 39 | /* USER CODE BEGIN Macro */ 40 | 41 | /* USER CODE END Macro */ 42 | 43 | /* Private variables ---------------------------------------------------------*/ 44 | /* USER CODE BEGIN PV */ 45 | 46 | /* USER CODE END PV */ 47 | 48 | /* Private function prototypes -----------------------------------------------*/ 49 | /* USER CODE BEGIN PFP */ 50 | 51 | /* USER CODE END PFP */ 52 | 53 | /* External functions --------------------------------------------------------*/ 54 | /* USER CODE BEGIN ExternalFunctions */ 55 | 56 | /* USER CODE END ExternalFunctions */ 57 | 58 | /* USER CODE BEGIN 0 */ 59 | 60 | /* USER CODE END 0 */ 61 | /** 62 | * Initializes the Global MSP. 63 | */ 64 | void HAL_MspInit(void) 65 | { 66 | /* USER CODE BEGIN MspInit 0 */ 67 | 68 | /* USER CODE END MspInit 0 */ 69 | 70 | __HAL_RCC_SYSCFG_CLK_ENABLE(); 71 | __HAL_RCC_PWR_CLK_ENABLE(); 72 | 73 | HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_0); 74 | 75 | /* System interrupt init*/ 76 | 77 | /* USER CODE BEGIN MspInit 1 */ 78 | 79 | /* USER CODE END MspInit 1 */ 80 | } 81 | 82 | /* USER CODE BEGIN 1 */ 83 | 84 | /* USER CODE END 1 */ 85 | -------------------------------------------------------------------------------- /Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f4xx.h 4 | * @author MCD Application Team 5 | * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2017 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | 19 | /** @addtogroup CMSIS 20 | * @{ 21 | */ 22 | 23 | /** @addtogroup stm32f4xx_system 24 | * @{ 25 | */ 26 | 27 | /** 28 | * @brief Define to prevent recursive inclusion 29 | */ 30 | #ifndef __SYSTEM_STM32F4XX_H 31 | #define __SYSTEM_STM32F4XX_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /** @addtogroup STM32F4xx_System_Includes 38 | * @{ 39 | */ 40 | 41 | /** 42 | * @} 43 | */ 44 | 45 | 46 | /** @addtogroup STM32F4xx_System_Exported_types 47 | * @{ 48 | */ 49 | /* This variable is updated in three ways: 50 | 1) by calling CMSIS function SystemCoreClockUpdate() 51 | 2) by calling HAL API function HAL_RCC_GetSysClockFreq() 52 | 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 53 | Note: If you use this function to configure the system clock; then there 54 | is no need to call the 2 first functions listed above, since SystemCoreClock 55 | variable is updated automatically. 56 | */ 57 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ 58 | 59 | extern const uint8_t AHBPrescTable[16]; /*!< AHB prescalers table values */ 60 | extern const uint8_t APBPrescTable[8]; /*!< APB prescalers table values */ 61 | 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @addtogroup STM32F4xx_System_Exported_Constants 67 | * @{ 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @addtogroup STM32F4xx_System_Exported_Macros 75 | * @{ 76 | */ 77 | 78 | /** 79 | * @} 80 | */ 81 | 82 | /** @addtogroup STM32F4xx_System_Exported_Functions 83 | * @{ 84 | */ 85 | 86 | extern void SystemInit(void); 87 | extern void SystemCoreClockUpdate(void); 88 | /** 89 | * @} 90 | */ 91 | 92 | #ifdef __cplusplus 93 | } 94 | #endif 95 | 96 | #endif /*__SYSTEM_STM32F4XX_H */ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | /** 103 | * @} 104 | */ 105 | -------------------------------------------------------------------------------- /Core/Inc/gd32f30x_libopt.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_libopt.h 3 | \brief library optional for gd32f30x 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #ifndef GD32F30X_LIBOPT_H 39 | #define GD32F30X_LIBOPT_H 40 | 41 | #include "gd32f30x_rcu.h" 42 | #include "gd32f30x_adc.h" 43 | #include "gd32f30x_can.h" 44 | #include "gd32f30x_crc.h" 45 | #include "gd32f30x_ctc.h" 46 | #include "gd32f30x_dac.h" 47 | #include "gd32f30x_dbg.h" 48 | #include "gd32f30x_dma.h" 49 | #include "gd32f30x_exti.h" 50 | #include "gd32f30x_fmc.h" 51 | #include "gd32f30x_fwdgt.h" 52 | #include "gd32f30x_gpio.h" 53 | #include "gd32f30x_i2c.h" 54 | #include "gd32f30x_pmu.h" 55 | #include "gd32f30x_bkp.h" 56 | #include "gd32f30x_rtc.h" 57 | #include "gd32f30x_sdio.h" 58 | #include "gd32f30x_spi.h" 59 | #include "gd32f30x_timer.h" 60 | #include "gd32f30x_usart.h" 61 | #include "gd32f30x_wwdgt.h" 62 | #include "gd32f30x_misc.h" 63 | #include "gd32f30x_enet.h" 64 | #include "gd32f30x_exmc.h" 65 | 66 | // #undef CAN_TIMEOUT 67 | 68 | #endif /* GD32F30X_LIBOPT_H */ 69 | -------------------------------------------------------------------------------- /Drivers/CMSIS/Include/tz_context.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * @file tz_context.h 3 | * @brief Context Management for Armv8-M TrustZone 4 | * @version V1.0.1 5 | * @date 10. January 2018 6 | ******************************************************************************/ 7 | /* 8 | * Copyright (c) 2017-2018 Arm Limited. All rights reserved. 9 | * 10 | * SPDX-License-Identifier: Apache-2.0 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the License); you may 13 | * not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * 16 | * www.apache.org/licenses/LICENSE-2.0 17 | * 18 | * Unless required by applicable law or agreed to in writing, software 19 | * distributed under the License is distributed on an AS IS BASIS, WITHOUT 20 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | * See the License for the specific language governing permissions and 22 | * limitations under the License. 23 | */ 24 | 25 | #if defined ( __ICCARM__ ) 26 | #pragma system_include /* treat file as system include file for MISRA check */ 27 | #elif defined (__clang__) 28 | #pragma clang system_header /* treat file as system include file */ 29 | #endif 30 | 31 | #ifndef TZ_CONTEXT_H 32 | #define TZ_CONTEXT_H 33 | 34 | #include 35 | 36 | #ifndef TZ_MODULEID_T 37 | #define TZ_MODULEID_T 38 | /// \details Data type that identifies secure software modules called by a process. 39 | typedef uint32_t TZ_ModuleId_t; 40 | #endif 41 | 42 | /// \details TZ Memory ID identifies an allocated memory slot. 43 | typedef uint32_t TZ_MemoryId_t; 44 | 45 | /// Initialize secure context memory system 46 | /// \return execution status (1: success, 0: error) 47 | uint32_t TZ_InitContextSystem_S (void); 48 | 49 | /// Allocate context memory for calling secure software modules in TrustZone 50 | /// \param[in] module identifies software modules called from non-secure mode 51 | /// \return value != 0 id TrustZone memory slot identifier 52 | /// \return value 0 no memory available or internal error 53 | TZ_MemoryId_t TZ_AllocModuleContext_S (TZ_ModuleId_t module); 54 | 55 | /// Free context memory that was previously allocated with \ref TZ_AllocModuleContext_S 56 | /// \param[in] id TrustZone memory slot identifier 57 | /// \return execution status (1: success, 0: error) 58 | uint32_t TZ_FreeModuleContext_S (TZ_MemoryId_t id); 59 | 60 | /// Load secure context (called on RTOS thread context switch) 61 | /// \param[in] id TrustZone memory slot identifier 62 | /// \return execution status (1: success, 0: error) 63 | uint32_t TZ_LoadContext_S (TZ_MemoryId_t id); 64 | 65 | /// Store secure context (called on RTOS thread context switch) 66 | /// \param[in] id TrustZone memory slot identifier 67 | /// \return execution status (1: success, 0: error) 68 | uint32_t TZ_StoreContext_S (TZ_MemoryId_t id); 69 | 70 | #endif // TZ_CONTEXT_H 71 | -------------------------------------------------------------------------------- /Core/Src/flash_writer.c: -------------------------------------------------------------------------------- 1 | #ifdef STM32F446 2 | #include "stm32f4xx_flash.h" 3 | #else 4 | #include "gd32f30x.h" 5 | #define FMC_PAGE_SIZE ((uint16_t)0x800) 6 | #define DATA_START_BASE ((uint32_t)0x08000000 + 120 * FMC_PAGE_SIZE) 7 | #endif 8 | #include "flash_writer.h" 9 | 10 | 11 | 12 | 13 | void flash_writer_init(FlashWriter *fw, uint32_t sector) { 14 | if(sector>7) sector = 7; 15 | fw->sector = sector; 16 | #ifdef STM32F446 17 | fw->base = __SECTOR_ADDRS[sector]; 18 | #else 19 | fw->base = DATA_START_BASE; // start 240k, 16k data 20 | #endif 21 | fw->ready = false; 22 | } 23 | bool flash_writer_ready(FlashWriter fw) { 24 | return fw.ready; 25 | } 26 | 27 | void flash_writer_open(FlashWriter * fw) { 28 | #ifdef STM32F446 29 | FLASH_Unlock(); 30 | FLASH_ClearFlag( FLASH_FLAG_EOP | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); 31 | FLASH_EraseSector(__SECTORS[fw->sector], VoltageRange_3); 32 | #else 33 | fmc_unlock(); 34 | fmc_flag_clear(FMC_FLAG_BANK0_PGERR | FMC_FLAG_BANK0_WPERR | FMC_FLAG_BANK0_END); 35 | fmc_page_erase(fw->base); 36 | #endif 37 | fw->ready = true; 38 | } 39 | 40 | void flash_writer_write_int(FlashWriter fw, uint32_t index, int x) { 41 | union UN {int a; uint32_t b;}; 42 | union UN un; 43 | un.a = x; 44 | #ifdef STM32F446 45 | FLASH_ProgramWord(fw.base + 4 * index, un.b); 46 | #else 47 | fmc_word_program(fw.base + 4 * index, un.b); 48 | #endif 49 | } 50 | 51 | void flash_writer_write_uint(FlashWriter fw, uint32_t index, unsigned int x) { 52 | #ifdef STM32F446 53 | FLASH_ProgramWord(fw.base + 4 * index, x); 54 | #else 55 | fmc_word_program(fw.base + 4 * index, x); 56 | #endif 57 | } 58 | 59 | void flash_writer_write_float(FlashWriter fw, uint32_t index, float x) { 60 | union UN {float a; uint32_t b;}; 61 | union UN un; 62 | un.a = x; 63 | #ifdef STM32F446 64 | FLASH_ProgramWord(fw.base + 4 * index, un.b); 65 | #else 66 | fmc_word_program(fw.base + 4 * index, un.b); 67 | #endif 68 | } 69 | 70 | void flash_writer_close(FlashWriter * fw) { 71 | #ifdef STM32F446 72 | FLASH_Lock(); 73 | #else 74 | fmc_lock(); 75 | #endif 76 | fw->ready = false; 77 | } 78 | 79 | int flash_read_int(FlashWriter fw, uint32_t index) { 80 | #ifdef STM32F446 81 | return *(int*) (__SECTOR_ADDRS[fw.sector] + 4 * index); 82 | #else 83 | return *(int*) (fw.base + 4 * index); 84 | #endif 85 | } 86 | 87 | uint32_t flash_read_uint(FlashWriter fw, uint32_t index) { 88 | #ifdef STM32F446 89 | return *(uint32_t*) (__SECTOR_ADDRS[fw.sector] + 4 * index); 90 | #else 91 | return *(uint32_t*) (fw.base + 4 * index);; 92 | #endif 93 | } 94 | 95 | float flash_read_float(FlashWriter fw, uint32_t index) { 96 | #ifdef STM32F446 97 | return *(float*) (__SECTOR_ADDRS[fw.sector] + 4 * index); 98 | #else 99 | return *(float*) (fw.base + 4 * index); 100 | #endif 101 | } 102 | 103 | 104 | -------------------------------------------------------------------------------- /Core/Inc/user_config.h: -------------------------------------------------------------------------------- 1 | /// Values stored in flash, which are modified by user actions /// 2 | 3 | #ifndef USER_CONFIG_H 4 | #define USER_CONFIG_H 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | 11 | #define I_BW __float_reg[2] // Current loop bandwidth 12 | #define I_MAX __float_reg[3] // Current limit 13 | #define THETA_MIN __float_reg[4] // Minimum position setpoint 14 | #define THETA_MAX __float_reg[5] // Maximum position setpoint 15 | #define I_FW_MAX __float_reg[6] // Maximum field weakening current 16 | #define R_NOMINAL __float_reg[7] // Nominal motor resistance, set during calibration 17 | #define TEMP_MAX __float_reg[8] // Temperature safety lmit 18 | #define I_MAX_CONT __float_reg[9] // Continuous max current 19 | #define PPAIRS __float_reg[10] // Number of motor pole-pairs 20 | //#define L_D __float_reg[11] // D-axis inductance 21 | //#define L_Q __float_reg[12] // Q-axis inductance 22 | #define R_PHASE __float_reg[13] // Single phase resistance 23 | #define KT __float_reg[14] // Torque Constant (N-m/A) 24 | #define R_TH __float_reg[15] // Thermal resistance (C/W) 25 | #define C_TH __float_reg[16] // Thermal mass (C/J) 26 | #define GR __float_reg[17] // Gear ratio 27 | #define I_CAL __float_reg[18] // Calibration Current 28 | #define P_MIN __float_reg[19] // Position setpoint lower limit (rad) 29 | #define P_MAX __float_reg[20] // Position setupoint upper bound (rad) 30 | #define V_MIN __float_reg[21] // Velocity setpoint lower bound (rad/s) 31 | #define V_MAX __float_reg[22] // Velocity setpoint upper bound (rad/s) 32 | #define KP_MAX __float_reg[23] // Max position gain (N-m/rad) 33 | #define KD_MAX __float_reg[24] // Max velocity gain (N-m/rad/s) 34 | 35 | 36 | #define PHASE_ORDER __int_reg[0] // Phase swapping during calibration 37 | #define CAN_ID __int_reg[1] // CAN bus ID 38 | #define CAN_MASTER __int_reg[2] // CAN bus "master" ID 39 | #define CAN_TIMEOUT __int_reg[3] // CAN bus timeout period 40 | #define M_ZERO __int_reg[4] 41 | #define E_ZERO __int_reg[5] 42 | #define ENCODER_LUT __int_reg[6] // Encoder offset LUT - 128 elements long 43 | 44 | 45 | 46 | 47 | extern float __float_reg[]; 48 | extern int __int_reg[]; 49 | 50 | #ifdef __cplusplus 51 | } 52 | #endif 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /Core/Src/systick.c: -------------------------------------------------------------------------------- 1 | /*! 2 | \file systick.c 3 | \brief the systick configuration file 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #include "gd32f30x.h" 39 | #include "systick.h" 40 | 41 | volatile static uint32_t delay; 42 | 43 | /*! 44 | \brief configure systick 45 | \param[in] none 46 | \param[out] none 47 | \retval none 48 | */ 49 | void systick_config(void) 50 | { 51 | /* setup systick timer for 1000Hz interrupts */ 52 | if (SysTick_Config(SystemCoreClock / 1000U)){ 53 | /* capture error */ 54 | while (1){ 55 | } 56 | } 57 | /* configure the systick handler priority */ 58 | NVIC_SetPriority(SysTick_IRQn, 0x01U); 59 | } 60 | 61 | /*! 62 | \brief delay a time in milliseconds 63 | \param[in] count: count in milliseconds 64 | \param[out] none 65 | \retval none 66 | */ 67 | void delay_1ms(uint32_t count) 68 | { 69 | delay = count; 70 | 71 | while(0U != delay){ 72 | } 73 | } 74 | 75 | /*! 76 | \brief delay decrement 77 | \param[in] none 78 | \param[out] none 79 | \retval none 80 | */ 81 | void delay_decrement(void) 82 | { 83 | if (0U != delay){ 84 | delay--; 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /.settings/language.settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /Core/Src/sysmem.c: -------------------------------------------------------------------------------- 1 | /** 2 | ***************************************************************************** 3 | ** 4 | ** File : sysmem.c 5 | ** 6 | ** Author : Auto-generated by STM32CubeIDE 7 | ** 8 | ** Abstract : STM32CubeIDE Minimal System Memory calls file 9 | ** 10 | ** For more information about which c-functions 11 | ** need which of these lowlevel functions 12 | ** please consult the Newlib libc-manual 13 | ** 14 | ** Environment : STM32CubeIDE MCU 15 | ** 16 | ** Distribution: The file is distributed as is, without any warranty 17 | ** of any kind. 18 | ** 19 | ***************************************************************************** 20 | ** 21 | **

© COPYRIGHT(c) 2018 STMicroelectronics

22 | ** 23 | ** Redistribution and use in source and binary forms, with or without modification, 24 | ** are permitted provided that the following conditions are met: 25 | ** 1. Redistributions of source code must retain the above copyright notice, 26 | ** this list of conditions and the following disclaimer. 27 | ** 2. Redistributions in binary form must reproduce the above copyright notice, 28 | ** this list of conditions and the following disclaimer in the documentation 29 | ** and/or other materials provided with the distribution. 30 | ** 3. Neither the name of STMicroelectronics nor the names of its contributors 31 | ** may be used to endorse or promote products derived from this software 32 | ** without specific prior written permission. 33 | ** 34 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 35 | ** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 36 | ** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 37 | ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 38 | ** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 39 | ** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 40 | ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 41 | ** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 42 | ** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 43 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 44 | ** 45 | ** 46 | ***************************************************************************** 47 | */ 48 | 49 | /* Includes */ 50 | #include 51 | #include 52 | 53 | /* Variables */ 54 | extern int errno; 55 | register char * stack_ptr asm("sp"); 56 | 57 | /* Functions */ 58 | 59 | /** 60 | _sbrk 61 | Increase program data space. Malloc and related functions depend on this 62 | **/ 63 | caddr_t _sbrk(int incr) 64 | { 65 | extern char end asm("end"); 66 | static char *heap_end; 67 | char *prev_heap_end; 68 | 69 | if (heap_end == 0) 70 | heap_end = &end; 71 | 72 | prev_heap_end = heap_end; 73 | if (heap_end + incr > stack_ptr) 74 | { 75 | errno = ENOMEM; 76 | return (caddr_t) -1; 77 | } 78 | 79 | heap_end += incr; 80 | 81 | return (caddr_t) prev_heap_end; 82 | } 83 | 84 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_hal_dma_ex.h 4 | * @author MCD Application Team 5 | * @brief Header file of DMA HAL extension module. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2017 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file in 13 | * the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | 19 | /* Define to prevent recursive inclusion -------------------------------------*/ 20 | #ifndef __STM32F4xx_HAL_DMA_EX_H 21 | #define __STM32F4xx_HAL_DMA_EX_H 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | /* Includes ------------------------------------------------------------------*/ 28 | #include "stm32f4xx_hal_def.h" 29 | 30 | /** @addtogroup STM32F4xx_HAL_Driver 31 | * @{ 32 | */ 33 | 34 | /** @addtogroup DMAEx 35 | * @{ 36 | */ 37 | 38 | /* Exported types ------------------------------------------------------------*/ 39 | /** @defgroup DMAEx_Exported_Types DMAEx Exported Types 40 | * @brief DMAEx Exported types 41 | * @{ 42 | */ 43 | 44 | /** 45 | * @brief HAL DMA Memory definition 46 | */ 47 | typedef enum 48 | { 49 | MEMORY0 = 0x00U, /*!< Memory 0 */ 50 | MEMORY1 = 0x01U /*!< Memory 1 */ 51 | }HAL_DMA_MemoryTypeDef; 52 | 53 | /** 54 | * @} 55 | */ 56 | 57 | /* Exported functions --------------------------------------------------------*/ 58 | /** @defgroup DMAEx_Exported_Functions DMAEx Exported Functions 59 | * @brief DMAEx Exported functions 60 | * @{ 61 | */ 62 | 63 | /** @defgroup DMAEx_Exported_Functions_Group1 Extended features functions 64 | * @brief Extended features functions 65 | * @{ 66 | */ 67 | 68 | /* IO operation functions *******************************************************/ 69 | HAL_StatusTypeDef HAL_DMAEx_MultiBufferStart(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, uint32_t DstAddress, uint32_t SecondMemAddress, uint32_t DataLength); 70 | HAL_StatusTypeDef HAL_DMAEx_MultiBufferStart_IT(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, uint32_t DstAddress, uint32_t SecondMemAddress, uint32_t DataLength); 71 | HAL_StatusTypeDef HAL_DMAEx_ChangeMemory(DMA_HandleTypeDef *hdma, uint32_t Address, HAL_DMA_MemoryTypeDef memory); 72 | 73 | /** 74 | * @} 75 | */ 76 | /** 77 | * @} 78 | */ 79 | 80 | /* Private functions ---------------------------------------------------------*/ 81 | /** @defgroup DMAEx_Private_Functions DMAEx Private Functions 82 | * @brief DMAEx Private functions 83 | * @{ 84 | */ 85 | /** 86 | * @} 87 | */ 88 | 89 | /** 90 | * @} 91 | */ 92 | 93 | /** 94 | * @} 95 | */ 96 | 97 | #ifdef __cplusplus 98 | } 99 | #endif 100 | 101 | #endif /*__STM32F4xx_HAL_DMA_EX_H*/ 102 | 103 | -------------------------------------------------------------------------------- /Core/Inc/main.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : main.h 5 | * @brief : Header for main.c file. 6 | * This file contains the common defines of the application. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | *

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

12 | * 13 | * This software component is licensed by ST under BSD 3-Clause license, 14 | * the "License"; You may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at: 16 | * opensource.org/licenses/BSD-3-Clause 17 | * 18 | ****************************************************************************** 19 | */ 20 | /* USER CODE END Header */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __MAIN_H 24 | #define __MAIN_H 25 | 26 | #ifdef __cplusplus 27 | extern "C" { 28 | #endif 29 | 30 | #ifdef STM32F446 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f4xx_hal.h" 33 | #else 34 | #include 35 | #include "gd32f30x.h" 36 | #include "systick.h" 37 | #endif 38 | 39 | /* Private includes ----------------------------------------------------------*/ 40 | /* USER CODE BEGIN Includes */ 41 | 42 | /* USER CODE END Includes */ 43 | 44 | /* Exported types ------------------------------------------------------------*/ 45 | /* USER CODE BEGIN ET */ 46 | 47 | /* USER CODE END ET */ 48 | 49 | /* Exported constants --------------------------------------------------------*/ 50 | /* USER CODE BEGIN EC */ 51 | 52 | /* USER CODE END EC */ 53 | 54 | /* Exported macro ------------------------------------------------------------*/ 55 | /* USER CODE BEGIN EM */ 56 | 57 | /* USER CODE END EM */ 58 | 59 | /* Exported functions prototypes ---------------------------------------------*/ 60 | void Error_Handler(void); 61 | 62 | /* USER CODE BEGIN EFP */ 63 | 64 | /* USER CODE END EFP */ 65 | 66 | /* Private defines -----------------------------------------------------------*/ 67 | #ifdef STM32F446 68 | #define B1_Pin GPIO_PIN_13 69 | #define B1_GPIO_Port GPIOC 70 | #define USART_TX_Pin GPIO_PIN_2 71 | #define USART_TX_GPIO_Port GPIOA 72 | #define USART_RX_Pin GPIO_PIN_3 73 | #define USART_RX_GPIO_Port GPIOA 74 | #define TMS_Pin GPIO_PIN_13 75 | #define TMS_GPIO_Port GPIOA 76 | #define TCK_Pin GPIO_PIN_14 77 | #define TCK_GPIO_Port GPIOA 78 | #define SWO_Pin GPIO_PIN_3 79 | #define SWO_GPIO_Port GPIOB 80 | #else 81 | #define SVPWM_PERIOD 2000 // 30kHz, TODO 82 | 83 | // "CAN_TIMEOUT" redefined in gd32f30x_can.h 84 | #undef CAN_TIMEOUT 85 | #endif 86 | /* USER CODE BEGIN Private defines */ 87 | 88 | #define debug printf 89 | #define info printf 90 | 91 | #define VERSION_MAJOR 2 92 | #define VERSION_MINOR 0 93 | #define VERSION_PATCH 1 94 | /* USER CODE END Private defines */ 95 | 96 | #ifdef __cplusplus 97 | } 98 | #endif 99 | 100 | #endif /* __MAIN_H */ 101 | -------------------------------------------------------------------------------- /Core/Inc/gd32f30x_it.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_it.h 3 | \brief the header file of the ISR 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #ifndef GD32F30X_IT_H 39 | #define GD32F30X_IT_H 40 | 41 | #include "gd32f30x.h" 42 | 43 | /* function declarations */ 44 | /* this function handles NMI exception */ 45 | void NMI_Handler(void); 46 | /* this function handles HardFault exception */ 47 | void HardFault_Handler(void); 48 | /* this function handles MemManage exception */ 49 | void MemManage_Handler(void); 50 | /* this function handles BusFault exception */ 51 | void BusFault_Handler(void); 52 | /* this function handles UsageFault exception */ 53 | void UsageFault_Handler(void); 54 | /* this function handles SVC exception */ 55 | void SVC_Handler(void); 56 | /* this function handles DebugMon exception */ 57 | void DebugMon_Handler(void); 58 | /* this function handles PendSV exception */ 59 | void PendSV_Handler(void); 60 | /* this function handles SysTick exception */ 61 | void SysTick_Handler(void); 62 | /* this function handles CAN0_RX0 exception */ 63 | void USBD_LP_CAN0_RX0_IRQHandler(void); 64 | /* this function handles TIMER0_UP exception */ 65 | void TIMER0_UP_IRQHandler(void); 66 | /* this function handles EXTI10_15 exception */ 67 | void EXTI10_15_IRQHandler(void); 68 | /* this function handles USART1 exception */ 69 | void USART1_IRQHandler(void); 70 | 71 | #endif /* GD32F30X_IT_H */ 72 | -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Include/gd32f30x_crc.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_crc.h 3 | \brief definitions for the CRC 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #ifndef GD32F30X_CRC_H 39 | #define GD32F30X_CRC_H 40 | 41 | #include "gd32f30x.h" 42 | 43 | /* CRC definitions */ 44 | #define CRC CRC_BASE 45 | 46 | /* registers definitions */ 47 | #define CRC_DATA REG32(CRC + 0x00U) /*!< CRC data register */ 48 | #define CRC_FDATA REG32(CRC + 0x04U) /*!< CRC free data register */ 49 | #define CRC_CTL REG32(CRC + 0x08U) /*!< CRC control register */ 50 | 51 | /* bits definitions */ 52 | /* CRC_DATA */ 53 | #define CRC_DATA_DATA BITS(0,31) /*!< CRC calculation result bits */ 54 | 55 | /* CRC_FDATA */ 56 | #define CRC_FDATA_FDATA BITS(0,7) /*!< CRC free data bits */ 57 | 58 | /* CRC_CTL */ 59 | #define CRC_CTL_RST BIT(0) /*!< CRC reset CRC_DATA register bit */ 60 | 61 | 62 | /* function declarations */ 63 | /* deinit CRC calculation unit */ 64 | void crc_deinit(void); 65 | 66 | /* reset data register(CRC_DATA) to the value of 0xFFFFFFFF */ 67 | void crc_data_register_reset(void); 68 | /* read the value of the data register */ 69 | uint32_t crc_data_register_read(void); 70 | 71 | /* read the value of the free data register */ 72 | uint8_t crc_free_data_register_read(void); 73 | /* write data to the free data register */ 74 | void crc_free_data_register_write(uint8_t free_data); 75 | 76 | /* calculate the CRC value of a 32-bit data */ 77 | uint32_t crc_single_data_calculate(uint32_t sdata); 78 | /* calculate the CRC value of an array of 32-bit values */ 79 | uint32_t crc_block_data_calculate(const uint32_t *array, uint32_t size); 80 | 81 | #endif /* GD32F30X_CRC_H */ 82 | -------------------------------------------------------------------------------- /Core/Inc/foc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * foc.h 3 | * 4 | * Created on: Aug 2, 2020 5 | * Author: ben 6 | */ 7 | 8 | #ifndef INC_FOC_H_ 9 | #define INC_FOC_H_ 10 | 11 | #include 12 | #include "position_sensor.h" 13 | 14 | 15 | typedef struct{ 16 | uint32_t tim_ch_w; // Terminal W timer channel 17 | int adc_a_raw, adc_b_raw, adc_c_raw, adc_vbus_raw; // Raw ADC Values 18 | float i_a, i_b, i_c; // Phase currents 19 | float v_bus, v_bus_filt; // DC link voltage 20 | float theta_mech, theta_elec; // Rotor mechanical and electrical angle 21 | float dtheta_mech, dtheta_elec, dtheta_elec_filt; // Rotor mechanical and electrical angular velocit 22 | float i_d, i_q, i_q_filt, i_d_filt; // D/Q currents 23 | float i_mag; // Current magnitude 24 | float v_d, v_q; // D/Q voltages 25 | float dtc_u, dtc_v, dtc_w; // Terminal duty cycles 26 | float v_u, v_v, v_w; // Terminal voltages 27 | float i_scale, k_d, k_q, ki_d, ki_q, ki_fw, alpha; // Current loop gains, current reference filter coefficient 28 | float d_int, q_int; // Current error integrals 29 | int adc_a_offset, adc_b_offset, adc_c_offset, adc_vbus_offset; // ADC offsets 30 | float i_d_des, i_q_des, i_d_des_filt, i_q_des_filt; // Current references 31 | int loop_count; // Degubbing counter 32 | int timeout; // Watchdog counter 33 | int mode; 34 | int ovp_flag; // Over-voltage flag 35 | int oc_flag; // Over-current flag 36 | int phase_order; 37 | union{ 38 | float commands[5]; // Making this easier to pass around without including foc.h everywhere 39 | struct{ 40 | float p_des, v_des, kp, kd, t_ff; // Desired position, velocity, gains, torque 41 | }; 42 | }; 43 | float v_max, v_ref, fw_int; // output voltage magnitude, field-weakening integral 44 | int otw_flag; // Over-temp warning 45 | float i_max; // Maximum current 46 | float inverter_tab[128]; // Inverter linearization table 47 | uint8_t invert_dtc; // Inverter duty cycle inverting/non-inverting 48 | } ControllerStruct; 49 | 50 | typedef struct{ 51 | double temperature; // Estimated temperature 52 | float temp_measured; // "Measured" temperature computed from resistance 53 | float qd_in, qd_out; // Thermal power in and out 54 | float resistance; // Motor resistance 55 | float k; // Temperature observer gain 56 | float trust; // Temperature observer "trust' (kind of like 1/covariance) 57 | float delta_t; // Temperature rise 58 | } ObserverStruct; 59 | 60 | void set_dtc(ControllerStruct *controller); 61 | void analog_sample(ControllerStruct *controller); 62 | void abc(float theta, float d, float q, float *a, float *b, float *c); 63 | void dq0(float theta, float a, float b, float c, float *d, float *q); 64 | void svm(float v_max, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w); 65 | void zero_current(ControllerStruct *controller); 66 | void reset_foc(ControllerStruct *controller); 67 | void reset_observer(ObserverStruct *observer); 68 | void init_controller_params(ControllerStruct *controller); 69 | void commutate(ControllerStruct *controller, EncoderStruct *encoder); 70 | void torque_control(ControllerStruct *controller); 71 | void limit_current_ref (ControllerStruct *controller); 72 | void update_observer(ControllerStruct *controller, ObserverStruct *observer); 73 | void field_weaken(ControllerStruct *controller); 74 | float linearize_dtc(ControllerStruct *controller, float dtc); 75 | void zero_commands(ControllerStruct * controller); 76 | 77 | 78 | #endif /* INC_FOC_H_ */ 79 | -------------------------------------------------------------------------------- /Core/Inc/lookup.h: -------------------------------------------------------------------------------- 1 | /* 2 | * lookup.h 3 | * 4 | * Created on: Jan 1, 2021 5 | * Author: ben 6 | */ 7 | 8 | #ifndef INC_LOOKUP_H_ 9 | #define INC_LOOKUP_H_ 10 | 11 | const float sin_tab[] = { 12 | 0,0.012296,0.024589,0.036879,0.049164,0.061441,0.073708,0.085965,0.098208,0.11044,0.12265,0.13484,0.14702,0.15917,0.17129,0.18339,0.19547,0.20751,0.21952,0.2315,0.24345,0.25535,0.26722,0.27905,0.29084,0.30258,0.31427,0.32592,0.33752,0.34907,0.36057,0.37201,0.38339,0.39472,0.40599,0.41719,0.42834,0.43941,0.45043,0.46137,0.47224,0.48305,0.49378,0.50443,0.51501,0.52551,0.53593,0.54627,0.55653,0.5667,0.57679,0.58679,0.5967,0.60652,0.61625,0.62589,0.63543,0.64488,0.65423,0.66348,0.67263,0.68167,0.69062,0.69946,0.70819,0.71682,0.72534,0.73375,0.74205,0.75023,0.75831,0.76626,0.77411,0.78183,0.78944,0.79693,0.80429,0.81154,0.81866,0.82566,0.83254,0.83928,0.84591,0.8524,0.85876,0.865,0.8711,0.87708,0.88292,0.88862,0.89419,0.89963,0.90493,0.9101,0.91512,0.92001,0.92476,0.92937,0.93384,0.93816,0.94235,0.94639,0.95029,0.95405,0.95766,0.96113,0.96445,0.96763,0.97066,0.97354,0.97628,0.97887,0.98131,0.9836,0.98574,0.98774,0.98958,0.99128,0.99282,0.99422,0.99546,0.99656,0.9975,0.99829,0.99894,0.99943,0.99977,0.99996,1,0.99988,0.99962,0.9992,0.99863,0.99792,0.99705,0.99603,0.99486,0.99354,0.99207,0.99045,0.98868,0.98676,0.98469,0.98247,0.9801,0.97759,0.97493,0.97212,0.96916,0.96606,0.96281,0.95941,0.95587,0.95219,0.94836,0.94439,0.94028,0.93602,0.93162,0.92708,0.9224,0.91758,0.91263,0.90753,0.9023,0.89693,0.89142,0.88579,0.88001,0.87411,0.86807,0.8619,0.8556,0.84917,0.84261,0.83593,0.82911,0.82218,0.81512,0.80793,0.80062,0.7932,0.78565,0.77798,0.7702,0.7623,0.75428,0.74615,0.73791,0.72956,0.72109,0.71252,0.70384,0.69505,0.68616,0.67716,0.66806,0.65886,0.64956,0.64017,0.63067,0.62108,0.6114,0.60162,0.59176,0.5818,0.57176,0.56163,0.55141,0.54111,0.53073,0.52027,0.50973,0.49911,0.48842,0.47765,0.46682,0.45591,0.44493,0.43388,0.42277,0.4116,0.40036,0.38906,0.37771,0.36629,0.35483,0.3433,0.33173,0.32011,0.30843,0.29671,0.28495,0.27314,0.26129,0.2494,0.23748,0.22552,0.21352,0.20149,0.18943,0.17735,0.16523,0.15309,0.14093,0.12875,0.11655,0.10432,0.092088,0.079838,0.067576,0.055303,0.043022,0.030735,0.018443,0.0061479,-0.0061479,-0.018443,-0.030735,-0.043022,-0.055303,-0.067576,-0.079838,-0.092088,-0.10432,-0.11655,-0.12875,-0.14093,-0.15309,-0.16523,-0.17735,-0.18943,-0.20149,-0.21352,-0.22552,-0.23748,-0.2494,-0.26129,-0.27314,-0.28495,-0.29671,-0.30843,-0.32011,-0.33173,-0.3433,-0.35483,-0.36629,-0.37771,-0.38906,-0.40036,-0.4116,-0.42277,-0.43388,-0.44493,-0.45591,-0.46682,-0.47765,-0.48842,-0.49911,-0.50973,-0.52027,-0.53073,-0.54111,-0.55141,-0.56163,-0.57176,-0.5818,-0.59176,-0.60162,-0.6114,-0.62108,-0.63067,-0.64017,-0.64956,-0.65886,-0.66806,-0.67716,-0.68616,-0.69505,-0.70384,-0.71252,-0.72109,-0.72956,-0.73791,-0.74615,-0.75428,-0.7623,-0.7702,-0.77798,-0.78565,-0.7932,-0.80062,-0.80793,-0.81512,-0.82218,-0.82911,-0.83593,-0.84261,-0.84917,-0.8556,-0.8619,-0.86807,-0.87411,-0.88001,-0.88579,-0.89142,-0.89693,-0.9023,-0.90753,-0.91263,-0.91758,-0.9224,-0.92708,-0.93162,-0.93602,-0.94028,-0.94439,-0.94836,-0.95219,-0.95587,-0.95941,-0.96281,-0.96606,-0.96916,-0.97212,-0.97493,-0.97759,-0.9801,-0.98247,-0.98469,-0.98676,-0.98868,-0.99045,-0.99207,-0.99354,-0.99486,-0.99603,-0.99705,-0.99792,-0.99863,-0.9992,-0.99962,-0.99988,-1,-0.99996,-0.99977,-0.99943,-0.99894,-0.99829,-0.9975,-0.99656,-0.99546,-0.99422,-0.99282,-0.99128,-0.98958,-0.98774,-0.98574,-0.9836,-0.98131,-0.97887,-0.97628,-0.97354,-0.97066,-0.96763,-0.96445,-0.96113,-0.95766,-0.95405,-0.95029,-0.94639,-0.94235,-0.93816,-0.93384,-0.92937,-0.92476,-0.92001,-0.91512,-0.9101,-0.90493,-0.89963,-0.89419,-0.88862,-0.88292,-0.87708,-0.8711,-0.865,-0.85876,-0.8524,-0.84591,-0.83928,-0.83254,-0.82566,-0.81866,-0.81154,-0.80429,-0.79693,-0.78944,-0.78183,-0.77411,-0.76626,-0.75831,-0.75023,-0.74205,-0.73375,-0.72534,-0.71682,-0.70819,-0.69946,-0.69062,-0.68167,-0.67263,-0.66348,-0.65423,-0.64488,-0.63543,-0.62589,-0.61625,-0.60652,-0.5967,-0.58679,-0.57679,-0.5667,-0.55653,-0.54627,-0.53593,-0.52551,-0.51501,-0.50443,-0.49378,-0.48305,-0.47224,-0.46137,-0.45043,-0.43941,-0.42834,-0.41719,-0.40599,-0.39472,-0.38339,-0.37201,-0.36057,-0.34907,-0.33752,-0.32592,-0.31427,-0.30258,-0.29084,-0.27905,-0.26722,-0.25535,-0.24345,-0.2315,-0.21952,-0.20751,-0.19547,-0.18339,-0.17129,-0.15917,-0.14702,-0.13484,-0.12265,-0.11044,-0.098208,-0.085965,-0.073708,-0.061441,-0.049164,-0.036879,-0.024589,-0.012296,0 13 | }; 14 | 15 | 16 | 17 | #endif /* INC_LOOKUP_H_ */ 18 | -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_crc.c: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_crc.c 3 | \brief CRC driver 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #include "gd32f30x_crc.h" 39 | 40 | #define CRC_DATA_RESET_VALUE ((uint32_t)0xFFFFFFFFU) 41 | #define CRC_FDATA_RESET_VALUE ((uint32_t)0x00000000U) 42 | 43 | /*! 44 | \brief deinit CRC calculation unit 45 | \param[in] none 46 | \param[out] none 47 | \retval none 48 | */ 49 | void crc_deinit(void) 50 | { 51 | CRC_DATA = CRC_DATA_RESET_VALUE; 52 | CRC_FDATA = CRC_FDATA_RESET_VALUE; 53 | CRC_CTL = (uint32_t)CRC_CTL_RST; 54 | } 55 | 56 | /*! 57 | \brief reset data register(CRC_DATA) to the value of 0xFFFFFFFF 58 | \param[in] none 59 | \param[out] none 60 | \retval none 61 | */ 62 | void crc_data_register_reset(void) 63 | { 64 | CRC_CTL |= (uint32_t)CRC_CTL_RST; 65 | } 66 | 67 | /*! 68 | \brief read the value of the data register 69 | \param[in] none 70 | \param[out] none 71 | \retval 32-bit value of the data register 72 | */ 73 | uint32_t crc_data_register_read(void) 74 | { 75 | uint32_t data; 76 | data = CRC_DATA; 77 | return (data); 78 | } 79 | 80 | /*! 81 | \brief read the value of the free data register 82 | \param[in] none 83 | \param[out] none 84 | \retval 8-bit value of the free data register 85 | */ 86 | uint8_t crc_free_data_register_read(void) 87 | { 88 | uint8_t fdata; 89 | fdata = (uint8_t)CRC_FDATA; 90 | return (fdata); 91 | } 92 | 93 | /*! 94 | \brief write data to the free data register 95 | \param[in] free_data: specified 8-bit data 96 | \param[out] none 97 | \retval none 98 | */ 99 | void crc_free_data_register_write(uint8_t free_data) 100 | { 101 | CRC_FDATA = (uint32_t)free_data; 102 | } 103 | 104 | /*! 105 | \brief calculate the CRC value of a 32-bit data 106 | \param[in] sdata: specified 32-bit data 107 | \param[out] none 108 | \retval 32-bit value calculated by CRC 109 | */ 110 | uint32_t crc_single_data_calculate(uint32_t sdata) 111 | { 112 | CRC_DATA = sdata; 113 | return (CRC_DATA); 114 | } 115 | 116 | /*! 117 | \brief calculate the CRC value of an array of 32-bit values 118 | \param[in] array: pointer to an array of 32-bit values 119 | \param[in] size: size of the array 120 | \param[out] none 121 | \retval 32-bit value calculated by CRC 122 | */ 123 | uint32_t crc_block_data_calculate(const uint32_t *array, uint32_t size) 124 | { 125 | uint32_t index; 126 | for(index = 0U; index < size; index++){ 127 | CRC_DATA = *(array+index); 128 | } 129 | return (CRC_DATA); 130 | } 131 | -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Include/gd32f30x_wwdgt.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_wwdgt.h 3 | \brief definitions for the WWDGT 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | 39 | #ifndef GD32F30X_WWDGT_H 40 | #define GD32F30X_WWDGT_H 41 | 42 | #include "gd32f30x.h" 43 | 44 | /* WWDGT definitions */ 45 | #define WWDGT WWDGT_BASE 46 | 47 | /* registers definitions */ 48 | #define WWDGT_CTL REG32((WWDGT) + 0x00U) /*!< WWDGT control register */ 49 | #define WWDGT_CFG REG32((WWDGT) + 0x04U) /*!< WWDGT configuration register */ 50 | #define WWDGT_STAT REG32((WWDGT) + 0x08U) /*!< WWDGT status register */ 51 | 52 | /* bits definitions */ 53 | /* WWDGT_CTL */ 54 | #define WWDGT_CTL_CNT BITS(0,6) /*!< WWDGT counter value */ 55 | #define WWDGT_CTL_WDGTEN BIT(7) /*!< WWDGT counter enable */ 56 | 57 | /* WWDGT_CFG */ 58 | #define WWDGT_CFG_WIN BITS(0,6) /*!< WWDGT counter window value */ 59 | #define WWDGT_CFG_PSC BITS(7,8) /*!< WWDGT prescaler divider value */ 60 | #define WWDGT_CFG_EWIE BIT(9) /*!< early wakeup interrupt enable */ 61 | 62 | /* WWDGT_STAT */ 63 | #define WWDGT_STAT_EWIF BIT(0) /*!< early wakeup interrupt flag */ 64 | 65 | /* constants definitions */ 66 | #define CFG_PSC(regval) (BITS(7,8) & ((uint32_t)(regval) << 7)) /*!< write value to WWDGT_CFG_PSC bit field */ 67 | #define WWDGT_CFG_PSC_DIV1 CFG_PSC(0) /*!< the time base of WWDGT = (PCLK1/4096)/1 */ 68 | #define WWDGT_CFG_PSC_DIV2 CFG_PSC(1) /*!< the time base of WWDGT = (PCLK1/4096)/2 */ 69 | #define WWDGT_CFG_PSC_DIV4 CFG_PSC(2) /*!< the time base of WWDGT = (PCLK1/4096)/4 */ 70 | #define WWDGT_CFG_PSC_DIV8 CFG_PSC(3) /*!< the time base of WWDGT = (PCLK1/4096)/8 */ 71 | 72 | /* function declarations */ 73 | /* reset the window watchdog timer configuration */ 74 | void wwdgt_deinit(void); 75 | /* start the window watchdog timer counter */ 76 | void wwdgt_enable(void); 77 | 78 | /* configure the window watchdog timer counter value */ 79 | void wwdgt_counter_update(uint16_t counter_value); 80 | /* configure counter value, window value, and prescaler divider value */ 81 | void wwdgt_config(uint16_t counter, uint16_t window, uint32_t prescaler); 82 | 83 | /* enable early wakeup interrupt of WWDGT */ 84 | void wwdgt_interrupt_enable(void); 85 | /* check early wakeup interrupt state of WWDGT */ 86 | FlagStatus wwdgt_flag_get(void); 87 | /* clear early wakeup interrupt state of WWDGT */ 88 | void wwdgt_flag_clear(void); 89 | 90 | #endif /* GD32F30X_WWDGT_H */ 91 | -------------------------------------------------------------------------------- /Core/Src/usart.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file usart.c 5 | * @brief This file provides code for the configuration 6 | * of the USART instances. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2022 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Includes ------------------------------------------------------------------*/ 21 | #include 22 | #include "usart.h" 23 | 24 | #ifdef STM32F446 25 | /* USER CODE BEGIN 0 */ 26 | 27 | /* USER CODE END 0 */ 28 | 29 | UART_HandleTypeDef huart2; 30 | 31 | /* USART2 init function */ 32 | 33 | void MX_USART2_UART_Init(void) 34 | { 35 | 36 | /* USER CODE BEGIN USART2_Init 0 */ 37 | 38 | /* USER CODE END USART2_Init 0 */ 39 | 40 | /* USER CODE BEGIN USART2_Init 1 */ 41 | 42 | /* USER CODE END USART2_Init 1 */ 43 | huart2.Instance = USART2; 44 | huart2.Init.BaudRate = 115200; 45 | huart2.Init.WordLength = UART_WORDLENGTH_8B; 46 | huart2.Init.StopBits = UART_STOPBITS_1; 47 | huart2.Init.Parity = UART_PARITY_NONE; 48 | huart2.Init.Mode = UART_MODE_TX_RX; 49 | huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; 50 | huart2.Init.OverSampling = UART_OVERSAMPLING_16; 51 | if (HAL_UART_Init(&huart2) != HAL_OK) 52 | { 53 | Error_Handler(); 54 | } 55 | /* USER CODE BEGIN USART2_Init 2 */ 56 | 57 | /* USER CODE END USART2_Init 2 */ 58 | 59 | } 60 | 61 | void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) 62 | { 63 | 64 | GPIO_InitTypeDef GPIO_InitStruct = {0}; 65 | if(uartHandle->Instance==USART2) 66 | { 67 | /* USER CODE BEGIN USART2_MspInit 0 */ 68 | 69 | /* USER CODE END USART2_MspInit 0 */ 70 | /* USART2 clock enable */ 71 | __HAL_RCC_USART2_CLK_ENABLE(); 72 | 73 | __HAL_RCC_GPIOA_CLK_ENABLE(); 74 | /**USART2 GPIO Configuration 75 | PA2 ------> USART2_TX 76 | PA3 ------> USART2_RX 77 | */ 78 | GPIO_InitStruct.Pin = USART_TX_Pin|USART_RX_Pin; 79 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; 80 | GPIO_InitStruct.Pull = GPIO_NOPULL; 81 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; 82 | GPIO_InitStruct.Alternate = GPIO_AF7_USART2; 83 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 84 | 85 | /* USART2 interrupt Init */ 86 | HAL_NVIC_SetPriority(USART2_IRQn, 0, 0); 87 | HAL_NVIC_EnableIRQ(USART2_IRQn); 88 | /* USER CODE BEGIN USART2_MspInit 1 */ 89 | 90 | /* USER CODE END USART2_MspInit 1 */ 91 | } 92 | } 93 | 94 | void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) 95 | { 96 | 97 | if(uartHandle->Instance==USART2) 98 | { 99 | /* USER CODE BEGIN USART2_MspDeInit 0 */ 100 | 101 | /* USER CODE END USART2_MspDeInit 0 */ 102 | /* Peripheral clock disable */ 103 | __HAL_RCC_USART2_CLK_DISABLE(); 104 | 105 | /**USART2 GPIO Configuration 106 | PA2 ------> USART2_TX 107 | PA3 ------> USART2_RX 108 | */ 109 | HAL_GPIO_DeInit(GPIOA, USART_TX_Pin|USART_RX_Pin); 110 | 111 | /* USART2 interrupt Deinit */ 112 | HAL_NVIC_DisableIRQ(USART2_IRQn); 113 | /* USER CODE BEGIN USART2_MspDeInit 1 */ 114 | 115 | /* USER CODE END USART2_MspDeInit 1 */ 116 | } 117 | } 118 | 119 | /* USER CODE BEGIN 1 */ 120 | int __io_putchar(int ch) { 121 | HAL_UART_Transmit(&huart2, (uint8_t*)&ch, 1, 0xffff); 122 | return 0; 123 | } 124 | 125 | void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) 126 | { 127 | /* Prevent unused argument(s) compilation warning */ 128 | HAL_UART_Receive_IT(&huart2, (uint8_t *)Serial2RxBuffer, 1); 129 | /* NOTE: This function should not be modified, when the callback is needed, 130 | the HAL_UART_RxCpltCallback could be implemented in the user file 131 | */ 132 | } 133 | /* USER CODE END 1 */ 134 | #else 135 | 136 | void MX_USART1_Init(void) 137 | { 138 | /* USART configure */ 139 | usart_deinit(USART1); 140 | usart_baudrate_set(USART1, 115200U); 141 | usart_receive_config(USART1, USART_RECEIVE_ENABLE); 142 | usart_transmit_config(USART1, USART_TRANSMIT_ENABLE); 143 | usart_enable(USART1); 144 | 145 | /* enable USART RBNE interrupt */ 146 | usart_interrupt_enable(USART1, USART_INT_RBNE); 147 | } 148 | 149 | /* retarget the C library printf function to the USART */ 150 | int __io_putchar(int ch) 151 | { 152 | usart_data_transmit(USART1, (uint8_t)ch); 153 | while(RESET == usart_flag_get(USART1, USART_FLAG_TBE)); 154 | return ch; 155 | } 156 | #endif -------------------------------------------------------------------------------- /Core/Src/syscalls.c: -------------------------------------------------------------------------------- 1 | /** 2 | ***************************************************************************** 3 | ** 4 | ** File : syscalls.c 5 | ** 6 | ** Author : Auto-generated by STM32CubeIDE 7 | ** 8 | ** Abstract : STM32CubeIDE Minimal System calls file 9 | ** 10 | ** For more information about which c-functions 11 | ** need which of these lowlevel functions 12 | ** please consult the Newlib libc-manual 13 | ** 14 | ** Environment : STM32CubeIDE MCU 15 | ** 16 | ** Distribution: The file is distributed as is, without any warranty 17 | ** of any kind. 18 | ** 19 | ***************************************************************************** 20 | ** 21 | **

© COPYRIGHT(c) 2018 STMicroelectronics

22 | ** 23 | ** Redistribution and use in source and binary forms, with or without modification, 24 | ** are permitted provided that the following conditions are met: 25 | ** 1. Redistributions of source code must retain the above copyright notice, 26 | ** this list of conditions and the following disclaimer. 27 | ** 2. Redistributions in binary form must reproduce the above copyright notice, 28 | ** this list of conditions and the following disclaimer in the documentation 29 | ** and/or other materials provided with the distribution. 30 | ** 3. Neither the name of STMicroelectronics nor the names of its contributors 31 | ** may be used to endorse or promote products derived from this software 32 | ** without specific prior written permission. 33 | ** 34 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 35 | ** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 36 | ** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 37 | ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 38 | ** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 39 | ** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 40 | ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 41 | ** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 42 | ** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 43 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 44 | ** 45 | ** 46 | ***************************************************************************** 47 | */ 48 | 49 | /* Includes */ 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | 60 | /* Variables */ 61 | //#undef errno 62 | extern int errno; 63 | extern int __io_putchar(int ch) __attribute__((weak)); 64 | extern int __io_getchar(void) __attribute__((weak)); 65 | 66 | register char * stack_ptr asm("sp"); 67 | 68 | char *__env[1] = { 0 }; 69 | char **environ = __env; 70 | 71 | 72 | /* Functions */ 73 | void initialise_monitor_handles() 74 | { 75 | } 76 | 77 | int _getpid(void) 78 | { 79 | return 1; 80 | } 81 | 82 | int _kill(int pid, int sig) 83 | { 84 | errno = EINVAL; 85 | return -1; 86 | } 87 | 88 | void _exit (int status) 89 | { 90 | _kill(status, -1); 91 | while (1) {} /* Make sure we hang here */ 92 | } 93 | 94 | __attribute__((weak)) int _read(int file, char *ptr, int len) 95 | { 96 | int DataIdx; 97 | 98 | for (DataIdx = 0; DataIdx < len; DataIdx++) 99 | { 100 | *ptr++ = __io_getchar(); 101 | } 102 | 103 | return len; 104 | } 105 | 106 | __attribute__((weak)) int _write(int file, char *ptr, int len) 107 | { 108 | int DataIdx; 109 | 110 | for (DataIdx = 0; DataIdx < len; DataIdx++) 111 | { 112 | __io_putchar(*ptr++); 113 | } 114 | return len; 115 | } 116 | 117 | int _close(int file) 118 | { 119 | return -1; 120 | } 121 | 122 | 123 | int _fstat(int file, struct stat *st) 124 | { 125 | st->st_mode = S_IFCHR; 126 | return 0; 127 | } 128 | 129 | int _isatty(int file) 130 | { 131 | return 1; 132 | } 133 | 134 | int _lseek(int file, int ptr, int dir) 135 | { 136 | return 0; 137 | } 138 | 139 | int _open(char *path, int flags, ...) 140 | { 141 | /* Pretend like we always fail */ 142 | return -1; 143 | } 144 | 145 | int _wait(int *status) 146 | { 147 | errno = ECHILD; 148 | return -1; 149 | } 150 | 151 | int _unlink(char *name) 152 | { 153 | errno = ENOENT; 154 | return -1; 155 | } 156 | 157 | int _times(struct tms *buf) 158 | { 159 | return -1; 160 | } 161 | 162 | int _stat(char *file, struct stat *st) 163 | { 164 | st->st_mode = S_IFCHR; 165 | return 0; 166 | } 167 | 168 | int _link(char *old, char *new) 169 | { 170 | errno = EMLINK; 171 | return -1; 172 | } 173 | 174 | int _fork(void) 175 | { 176 | errno = EAGAIN; 177 | return -1; 178 | } 179 | 180 | int _execve(char *name, char **argv, char **env) 181 | { 182 | errno = ENOMEM; 183 | return -1; 184 | } 185 | -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Include/gd32f30x_misc.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_misc.h 3 | \brief definitions for the MISC 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #ifndef GD32F30X_MISC_H 39 | #define GD32F30X_MISC_H 40 | 41 | #include "gd32f30x.h" 42 | 43 | /* constants definitions */ 44 | /* set the RAM and FLASH base address */ 45 | #define NVIC_VECTTAB_RAM ((uint32_t)0x20000000) /*!< RAM base address */ 46 | #define NVIC_VECTTAB_FLASH ((uint32_t)0x08000000) /*!< Flash base address */ 47 | 48 | /* set the NVIC vector table offset mask */ 49 | #define NVIC_VECTTAB_OFFSET_MASK ((uint32_t)0x1FFFFF80) 50 | 51 | /* the register key mask, if you want to do the write operation, you should write 0x5FA to VECTKEY bits */ 52 | #define NVIC_AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) 53 | 54 | /* priority group - define the pre-emption priority and the subpriority */ 55 | #define NVIC_PRIGROUP_PRE0_SUB4 ((uint32_t)0x700) /*!< 0 bits for pre-emption priority 4 bits for subpriority */ 56 | #define NVIC_PRIGROUP_PRE1_SUB3 ((uint32_t)0x600) /*!< 1 bits for pre-emption priority 3 bits for subpriority */ 57 | #define NVIC_PRIGROUP_PRE2_SUB2 ((uint32_t)0x500) /*!< 2 bits for pre-emption priority 2 bits for subpriority */ 58 | #define NVIC_PRIGROUP_PRE3_SUB1 ((uint32_t)0x400) /*!< 3 bits for pre-emption priority 1 bits for subpriority */ 59 | #define NVIC_PRIGROUP_PRE4_SUB0 ((uint32_t)0x300) /*!< 4 bits for pre-emption priority 0 bits for subpriority */ 60 | 61 | /* choose the method to enter or exit the lowpower mode */ 62 | #define SCB_SCR_SLEEPONEXIT ((uint8_t)0x02) /*!< choose the the system whether enter low power mode by exiting from ISR */ 63 | #define SCB_SCR_SLEEPDEEP ((uint8_t)0x04) /*!< choose the the system enter the DEEPSLEEP mode or SLEEP mode */ 64 | #define SCB_SCR_SEVONPEND ((uint8_t)0x10) /*!< choose the interrupt source that can wake up the lowpower mode */ 65 | 66 | #define SCB_LPM_SLEEP_EXIT_ISR SCB_SCR_SLEEPONEXIT 67 | #define SCB_LPM_DEEPSLEEP SCB_SCR_SLEEPDEEP 68 | #define SCB_LPM_WAKE_BY_ALL_INT SCB_SCR_SEVONPEND 69 | 70 | /* choose the systick clock source */ 71 | #define SYSTICK_CLKSOURCE_HCLK_DIV8 ((uint32_t)0xFFFFFFFBU) /*!< systick clock source is from HCLK/8 */ 72 | #define SYSTICK_CLKSOURCE_HCLK ((uint32_t)0x00000004U) /*!< systick clock source is from HCLK */ 73 | 74 | /* function declarations */ 75 | /* set the priority group */ 76 | void nvic_priority_group_set(uint32_t nvic_prigroup); 77 | 78 | /* enable NVIC request */ 79 | void nvic_irq_enable(uint8_t nvic_irq, uint8_t nvic_irq_pre_priority, uint8_t nvic_irq_sub_priority); 80 | /* disable NVIC request */ 81 | void nvic_irq_disable(uint8_t nvic_irq); 82 | 83 | /* set the NVIC vector table base address */ 84 | void nvic_vector_table_set(uint32_t nvic_vict_tab, uint32_t offset); 85 | 86 | /* set the state of the low power mode */ 87 | void system_lowpower_set(uint8_t lowpower_mode); 88 | /* reset the state of the low power mode */ 89 | void system_lowpower_reset(uint8_t lowpower_mode); 90 | 91 | /* set the systick clock source */ 92 | void systick_clksource_set(uint32_t systick_clksource); 93 | 94 | #endif /* GD32F30X_MISC_H */ 95 | -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_wwdgt.c: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_wwdgt.c 3 | \brief WWDGT driver 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #include "gd32f30x_wwdgt.h" 39 | 40 | /* write value to WWDGT_CTL_CNT bit field */ 41 | #define CTL_CNT(regval) (BITS(0,6) & ((uint32_t)(regval) << 0)) 42 | /* write value to WWDGT_CFG_WIN bit field */ 43 | #define CFG_WIN(regval) (BITS(0,6) & ((uint32_t)(regval) << 0)) 44 | 45 | /*! 46 | \brief reset the window watchdog timer configuration 47 | \param[in] none 48 | \param[out] none 49 | \retval none 50 | */ 51 | void wwdgt_deinit(void) 52 | { 53 | rcu_periph_reset_enable(RCU_WWDGTRST); 54 | rcu_periph_reset_disable(RCU_WWDGTRST); 55 | } 56 | 57 | /*! 58 | \brief start the window watchdog timer counter 59 | \param[in] none 60 | \param[out] none 61 | \retval none 62 | */ 63 | void wwdgt_enable(void) 64 | { 65 | WWDGT_CTL |= WWDGT_CTL_WDGTEN; 66 | } 67 | 68 | /*! 69 | \brief configure the window watchdog timer counter value 70 | \param[in] counter_value: 0x00 - 0x7F 71 | \param[out] none 72 | \retval none 73 | */ 74 | void wwdgt_counter_update(uint16_t counter_value) 75 | { 76 | WWDGT_CTL = (uint32_t)(CTL_CNT(counter_value)); 77 | } 78 | 79 | /*! 80 | \brief configure counter value, window value, and prescaler divider value 81 | \param[in] counter: 0x00 - 0x7F 82 | \param[in] window: 0x00 - 0x7F 83 | \param[in] prescaler: wwdgt prescaler value 84 | only one parameter can be selected which is shown as below: 85 | \arg WWDGT_CFG_PSC_DIV1: the time base of window watchdog counter = (PCLK1/4096)/1 86 | \arg WWDGT_CFG_PSC_DIV2: the time base of window watchdog counter = (PCLK1/4096)/2 87 | \arg WWDGT_CFG_PSC_DIV4: the time base of window watchdog counter = (PCLK1/4096)/4 88 | \arg WWDGT_CFG_PSC_DIV8: the time base of window watchdog counter = (PCLK1/4096)/8 89 | \param[out] none 90 | \retval none 91 | */ 92 | void wwdgt_config(uint16_t counter, uint16_t window, uint32_t prescaler) 93 | { 94 | WWDGT_CTL = (uint32_t)(CTL_CNT(counter)); 95 | WWDGT_CFG = (uint32_t)(CFG_WIN(window) | prescaler); 96 | } 97 | 98 | /*! 99 | \brief enable early wakeup interrupt of WWDGT 100 | \param[in] none 101 | \param[out] none 102 | \retval none 103 | */ 104 | void wwdgt_interrupt_enable(void) 105 | { 106 | WWDGT_CFG |= WWDGT_CFG_EWIE; 107 | } 108 | 109 | /*! 110 | \brief check early wakeup interrupt state of WWDGT 111 | \param[in] none 112 | \param[out] none 113 | \retval FlagStatus: SET or RESET 114 | */ 115 | FlagStatus wwdgt_flag_get(void) 116 | { 117 | if(RESET != (WWDGT_STAT & WWDGT_STAT_EWIF)){ 118 | return SET; 119 | } 120 | 121 | return RESET; 122 | } 123 | 124 | /*! 125 | \brief clear early wakeup interrupt state of WWDGT 126 | \param[in] none 127 | \param[out] none 128 | \retval none 129 | */ 130 | void wwdgt_flag_clear(void) 131 | { 132 | WWDGT_STAT = (uint32_t)(RESET); 133 | } 134 | -------------------------------------------------------------------------------- /.settings/org.eclipse.cdt.core.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1105024929/PATH/delimiter=; 3 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1105024929/PATH/operation=replace 4 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1105024929/PATH/value=C\:\\ST\\STM32CubeIDE_1.5.0\\STM32CubeIDE\\plugins\\com.st.stm32cube.ide.mcu.externaltools.gnu-tools-for-stm32.7-2018-q2-update.win32_1.5.0.202011040924\\tools\\bin;C\:\\ST\\STM32CubeIDE_1.5.0\\STM32CubeIDE\\plugins\\com.st.stm32cube.ide.mcu.externaltools.make.win32_1.5.0.202011040924\\tools\\bin;C\:/ST/STM32CubeIDE_1.5.0/STM32CubeIDE/jre/bin/server;C\:/ST/STM32CubeIDE_1.5.0/STM32CubeIDE/jre/bin;C\:/ST/STM32CubeIDE_1.5.0/STM32CubeIDE/jre/lib/amd64;C\:\\Windows\\system32;C\:\\Windows;C\:\\Windows\\System32\\Wbem;C\:\\Windows\\System32\\WindowsPowerShell\\v1.0\\;C\:\\Windows\\System32\\OpenSSH\\;C\:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;C\:\\Program Files\\NVIDIA Corporation\\NVIDIA NvDLISR;C\:\\Program Files (x86)\\Windows Kits\\8.1\\Windows Performance Toolkit\\;C\:\\Users\\ben\\AppData\\Local\\Programs\\Python\\Python39\\Scripts\\;C\:\\Users\\ben\\AppData\\Local\\Programs\\Python\\Python39\\;C\:\\Users\\ben\\AppData\\Local\\Microsoft\\WindowsApps;C\:\\Users\\ben\\AppData\\Local\\GitHubDesktop\\bin;C\:\\ST\\STM32CubeIDE_1.5.0\\STM32CubeIDE;C\:\\ST\\STM32CubeIDE_1.5.0\\STM32CubeIDE\\plugins\\com.st.stm32cube.ide.mcu.externaltools.make.win32_1.5.0.202011040924\\tools\\bin 5 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1105024929/append=true 6 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1105024929/appendContributed=true 7 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1333564725/PATH/delimiter=; 8 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1333564725/PATH/operation=replace 9 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1333564725/PATH/value=${ProgramFiles(x86)}C\:\\ST\\STM32CubeIDE_1.2.0\\STM32CubeIDE\\plugins\\com.st.stm32cube.ide.mcu.externaltools.gnu-tools-for-stm32.7-2018-q2-update.win32_1.0.0.201904181610\\tools\\bin;C\:\\ST\\STM32CubeIDE_1.2.0\\STM32CubeIDE\\plugins\\com.st.stm32cube.ide.mcu.externaltools.make.win32_1.1.0.201910081157\\tools\\bin;C\:/ST/STM32CubeIDE_1.2.0/STM32CubeIDE/jre/bin/server;C\:/ST/STM32CubeIDE_1.2.0/STM32CubeIDE/jre/bin;C\:/ST/STM32CubeIDE_1.2.0/STM32CubeIDE/jre/lib/amd64;C\:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;c\:\\Program Files (x86)\\Common Files\\Intel\\Shared Libraries\\redist\\intel64_win\\compiler;C\:\\Program Files (x86)\\Measurement Computing\\DAQ\\;C\:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C\:\\WINDOWS\\system32;C\:\\WINDOWS;C\:\\WINDOWS\\System32\\Wbem;C\:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;C\:\\Program Files (x86)\\Windows Kits\\8.1\\Windows Performance Toolkit\\;C\:\\Program Files\\MATLAB\\R2016b\\runtime\\win64;C\:\\Program Files\\MATLAB\\R2016b\\bin;C\:\\Program Files\\MATLAB\\R2016b\\polyspace\\bin;C\:\\WINDOWS\\System32\\OpenSSH\\;C\:\\Program Files\\MATLAB\\MATLAB Runtime\\v93\\runtime\\win64;C\:\\Program Files\\Git\\cmd;C\:\\Program Files (x86)\\IVI Foundation\\VISA\\WinNT\\Bin\\;C\:\\Program Files\\IVI Foundation\\VISA\\Win64\\Bin\\;C\:\\Program Files (x86)\\IVI Foundation\\VISA\\WinNT\\Bin;C\:\\Program Files (x86)\\PharosSystems\\Core;C\:\\Users\\ben\\AppData\\Local\\Programs\\Python\\Python37-32\\Scripts\\;C\:\\Users\\ben\\AppData\\Local\\Programs\\Python\\Python37-32\\;C\:\\Users\\ben\\AppData\\Local\\Microsoft\\WindowsApps;C\:\\Users\\ben\\AppData\\Local\\GitHubDesktop\\bin;C\:\\Users\\ben\\AppData\\Local\\Programs\\MiKTeX 2.9\\miktex\\bin\\x64\\;C\:\\Users\\ben\\AppData\\Local\\Microsoft\\WindowsApps;;C\:\\Users\\ben\\AppData\\Local\\Programs\\Microsoft VS Code\\bin;C\:\\ST\\STM32CubeIDE_1.2.0\\STM32CubeIDE;C\:\\ST\\STM32CubeIDE_1.5.0\\STM32CubeIDE\\plugins\\com.st.stm32cube.ide.mcu.externaltools.gnu-tools-for-stm32.7-2018-q2-update.win32_1.5.0.202011040924\\tools\\bin;C\:\\ST\\STM32CubeIDE_1.5.0\\STM32CubeIDE\\plugins\\com.st.stm32cube.ide.mcu.externaltools.make.win32_1.5.0.202011040924\\tools\\bin;C\:/ST/STM32CubeIDE_1.5.0/STM32CubeIDE/jre/bin/server;C\:/ST/STM32CubeIDE_1.5.0/STM32CubeIDE/jre/bin;C\:/ST/STM32CubeIDE_1.5.0/STM32CubeIDE/jre/lib/amd64;C\:\\Windows\\system32;C\:\\Windows;C\:\\Windows\\System32\\Wbem;C\:\\Windows\\System32\\WindowsPowerShell\\v1.0\\;C\:\\Windows\\System32\\OpenSSH\\;C\:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;C\:\\Program Files\\NVIDIA Corporation\\NVIDIA NvDLISR;C\:\\Program Files (x86)\\Windows Kits\\8.1\\Windows Performance Toolkit\\;C\:\\Users\\ben\\AppData\\Local\\Programs\\Python\\Python39\\Scripts\\;C\:\\Users\\ben\\AppData\\Local\\Programs\\Python\\Python39\\;C\:\\Users\\ben\\AppData\\Local\\Microsoft\\WindowsApps;C\:\\Users\\ben\\AppData\\Local\\GitHubDesktop\\bin;C\:\\ST\\STM32CubeIDE_1.5.0\\STM32CubeIDE;C\:\\ST\\STM32CubeIDE_1.5.0\\STM32CubeIDE\\plugins\\com.st.stm32cube.ide.mcu.externaltools.make.win32_1.5.0.202011040924\\tools\\bin 10 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1333564725/append=true 11 | environment/project/com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1333564725/appendContributed=true 12 | -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_fwdgt.c: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_fwdgt.c 3 | \brief FWDGT driver 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #include "gd32f30x_fwdgt.h" 39 | 40 | /* write value to FWDGT_CTL_CMD bit field */ 41 | #define CTL_CMD(regval) (BITS(0,15) & ((uint32_t)(regval) << 0)) 42 | /* write value to FWDGT_RLD_RLD bit field */ 43 | #define RLD_RLD(regval) (BITS(0,11) & ((uint32_t)(regval) << 0)) 44 | 45 | /*! 46 | \brief enable write access to FWDGT_PSC and FWDGT_RLD 47 | \param[in] none 48 | \param[out] none 49 | \retval none 50 | */ 51 | void fwdgt_write_enable(void) 52 | { 53 | FWDGT_CTL = FWDGT_WRITEACCESS_ENABLE; 54 | } 55 | 56 | /*! 57 | \brief disable write access to FWDGT_PSC and FWDGT_RLD 58 | \param[in] none 59 | \param[out] none 60 | \retval none 61 | */ 62 | void fwdgt_write_disable(void) 63 | { 64 | FWDGT_CTL = FWDGT_WRITEACCESS_DISABLE; 65 | } 66 | 67 | /*! 68 | \brief start the free watchdog timer counter 69 | \param[in] none 70 | \param[out] none 71 | \retval none 72 | */ 73 | void fwdgt_enable(void) 74 | { 75 | FWDGT_CTL = FWDGT_KEY_ENABLE; 76 | } 77 | 78 | /*! 79 | \brief reload the counter of FWDGT 80 | \param[in] none 81 | \param[out] none 82 | \retval none 83 | */ 84 | void fwdgt_counter_reload(void) 85 | { 86 | FWDGT_CTL = FWDGT_KEY_RELOAD; 87 | } 88 | 89 | /*! 90 | \brief configure counter reload value, and prescaler divider value 91 | \param[in] reload_value: specify reload value(0x0000 - 0x0FFF) 92 | \param[in] prescaler_div: FWDGT prescaler value 93 | only one parameter can be selected which is shown as below: 94 | \arg FWDGT_PSC_DIV4: FWDGT prescaler set to 4 95 | \arg FWDGT_PSC_DIV8: FWDGT prescaler set to 8 96 | \arg FWDGT_PSC_DIV16: FWDGT prescaler set to 16 97 | \arg FWDGT_PSC_DIV32: FWDGT prescaler set to 32 98 | \arg FWDGT_PSC_DIV64: FWDGT prescaler set to 64 99 | \arg FWDGT_PSC_DIV128: FWDGT prescaler set to 128 100 | \arg FWDGT_PSC_DIV256: FWDGT prescaler set to 256 101 | \param[out] none 102 | \retval ErrStatus: ERROR or SUCCESS 103 | */ 104 | ErrStatus fwdgt_config(uint16_t reload_value, uint8_t prescaler_div) 105 | { 106 | uint32_t timeout = FWDGT_PSC_TIMEOUT; 107 | uint32_t flag_status = RESET; 108 | 109 | /* enable write access to FWDGT_PSC,and FWDGT_RLD */ 110 | FWDGT_CTL = FWDGT_WRITEACCESS_ENABLE; 111 | 112 | /* wait until the PUD flag to be reset */ 113 | do{ 114 | flag_status = FWDGT_STAT & FWDGT_STAT_PUD; 115 | }while((--timeout > 0U) && ((uint32_t)RESET != flag_status)); 116 | 117 | if ((uint32_t)RESET != flag_status){ 118 | return ERROR; 119 | } 120 | 121 | /* configure FWDGT */ 122 | FWDGT_PSC = (uint32_t)prescaler_div; 123 | 124 | timeout = FWDGT_RLD_TIMEOUT; 125 | /* wait until the RUD flag to be reset */ 126 | do{ 127 | flag_status = FWDGT_STAT & FWDGT_STAT_RUD; 128 | }while((--timeout > 0U) && ((uint32_t)RESET != flag_status)); 129 | 130 | if ((uint32_t)RESET != flag_status){ 131 | return ERROR; 132 | } 133 | 134 | FWDGT_RLD = RLD_RLD(reload_value); 135 | 136 | /* reload the counter */ 137 | FWDGT_CTL = FWDGT_KEY_RELOAD; 138 | 139 | return SUCCESS; 140 | } 141 | 142 | /*! 143 | \brief get flag state of FWDGT 144 | \param[in] flag: flag to get 145 | only one parameter can be selected which is shown as below: 146 | \arg FWDGT_FLAG_PUD: a write operation to FWDGT_PSC register is on going 147 | \arg FWDGT_FLAG_RUD: a write operation to FWDGT_RLD register is on going 148 | \param[out] none 149 | \retval FlagStatus: SET or RESET 150 | */ 151 | FlagStatus fwdgt_flag_get(uint16_t flag) 152 | { 153 | if(RESET != (FWDGT_STAT & flag)){ 154 | return SET; 155 | } 156 | 157 | return RESET; 158 | } 159 | -------------------------------------------------------------------------------- /Core/Src/gpio.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file gpio.c 5 | * @brief This file provides code for the configuration 6 | * of all used GPIO pins. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2022 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | 21 | /* Includes ------------------------------------------------------------------*/ 22 | #include "gpio.h" 23 | 24 | /* USER CODE BEGIN 0 */ 25 | 26 | /* USER CODE END 0 */ 27 | 28 | /*----------------------------------------------------------------------------*/ 29 | /* Configure GPIO */ 30 | /*----------------------------------------------------------------------------*/ 31 | /* USER CODE BEGIN 1 */ 32 | 33 | /* USER CODE END 1 */ 34 | 35 | /** Configure pins as 36 | * Analog 37 | * Input 38 | * Output 39 | * EVENT_OUT 40 | * EXTI 41 | */ 42 | #ifdef STM32F446 43 | void MX_GPIO_Init(void) 44 | { 45 | 46 | GPIO_InitTypeDef GPIO_InitStruct = {0}; 47 | 48 | /* GPIO Ports Clock Enable */ 49 | __HAL_RCC_GPIOC_CLK_ENABLE(); 50 | __HAL_RCC_GPIOH_CLK_ENABLE(); 51 | __HAL_RCC_GPIOA_CLK_ENABLE(); 52 | __HAL_RCC_GPIOB_CLK_ENABLE(); 53 | 54 | /*Configure GPIO pin Output Level */ 55 | HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4|GPIO_PIN_11|GPIO_PIN_15, GPIO_PIN_RESET); 56 | 57 | /*Configure GPIO pin Output Level */ 58 | HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET); 59 | 60 | /*Configure GPIO pin : PtPin */ 61 | GPIO_InitStruct.Pin = B1_Pin; 62 | GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; 63 | GPIO_InitStruct.Pull = GPIO_NOPULL; 64 | HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct); 65 | 66 | /*Configure GPIO pins : PA4 PA11 PA15 */ 67 | GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_11|GPIO_PIN_15; 68 | GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; 69 | GPIO_InitStruct.Pull = GPIO_NOPULL; 70 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; 71 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 72 | 73 | /*Configure GPIO pin : PC5 */ 74 | GPIO_InitStruct.Pin = GPIO_PIN_5; 75 | GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; 76 | GPIO_InitStruct.Pull = GPIO_NOPULL; 77 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; 78 | HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); 79 | 80 | } 81 | #else 82 | void MX_GPIO_Init(void) 83 | { 84 | /* AS5047P */ 85 | /* SPI2_NSS PA15 */ 86 | /* SPI2_SCK PC10 */ 87 | /* SPI2_MISO PC11 */ 88 | /* SPI2_MOSI PC12 */ 89 | gpio_pin_remap_config(GPIO_SPI2_REMAP, ENABLE); 90 | gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE); // nCS/PA15 91 | gpio_init(GPIOC, GPIO_MODE_AF_PP, GPIO_OSPEED_10MHZ, GPIO_PIN_10 | GPIO_PIN_12); 92 | gpio_init(GPIOC, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_10MHZ, GPIO_PIN_11); 93 | gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_10MHZ, GPIO_PIN_15); 94 | gpio_bit_set(GPIOA, GPIO_PIN_15); 95 | 96 | /* DRV8323RS SPI */ 97 | /* SPI1_NSS PB12 */ 98 | /* SPI1_SCK PB13 */ 99 | /* SPI1_MISO PB14 */ 100 | /* SPI1_MOSI PB15 */ 101 | gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_10MHZ, GPIO_PIN_13 | GPIO_PIN_15); 102 | gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_10MHZ, GPIO_PIN_14); 103 | gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_10MHZ, GPIO_PIN_12); 104 | gpio_bit_set(GPIOB, GPIO_PIN_12); 105 | 106 | /* DRV8323RS Enable/nFault */ 107 | /* ENABLE PA11 */ 108 | /* nFAULT PA12 */ 109 | gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_2MHZ, GPIO_PIN_11); 110 | gpio_init(GPIOA, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_12); 111 | gpio_bit_reset(GPIOA, GPIO_PIN_11); 112 | 113 | /* DRV8323RS PWM */ 114 | /* TIMER0_CH0 PA8 */ 115 | /* TIMER0_CH1 PA9 */ 116 | /* TIMER0_CH2 PA10 */ 117 | gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_10); 118 | 119 | /* DRV8323RS Current Sense */ 120 | /* SOC/ADC012_IN10 PC0 */ 121 | /* SOB/ADC012_IN11 PC1 */ 122 | gpio_init(GPIOC, GPIO_MODE_AIN, GPIO_OSPEED_MAX, GPIO_PIN_0|GPIO_PIN_1); 123 | 124 | /* TEMP_PCB/ADC01_IN8 PB0 */ 125 | /* TEMP_MOTOR/ADC012_IN12 PC2 */ 126 | /* VOLT_VBUS/ADC012_IN13 PC3 */ 127 | gpio_init(GPIOB, GPIO_MODE_AIN, GPIO_OSPEED_MAX, GPIO_PIN_0); 128 | gpio_init(GPIOC, GPIO_MODE_AIN, GPIO_OSPEED_MAX, GPIO_PIN_2|GPIO_PIN_3); 129 | 130 | /* HALL0/ADC01_IN4 PA4 */ 131 | /* HALL1/ADC01_IN5 PA5 */ 132 | /* HALL2/ADC01_IN6 PA6 */ 133 | /* HALL3/ADC01_IN7 PA7 */ 134 | /* HALL4/ADC01_IN14 PC4 */ 135 | /* HALL5/ADC01_IN15 PC5 */ 136 | gpio_init(GPIOA, GPIO_MODE_AIN, GPIO_OSPEED_MAX, GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7); 137 | gpio_init(GPIOC, GPIO_MODE_AIN, GPIO_OSPEED_MAX, GPIO_PIN_4|GPIO_PIN_5); 138 | 139 | /* LED PC13 */ 140 | gpio_init(GPIOC, GPIO_MODE_OUT_PP, GPIO_OSPEED_2MHZ, GPIO_PIN_13); 141 | 142 | /* USART1_TX PA2 */ 143 | /* USART1_RX PA3 */ 144 | gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_10MHZ, GPIO_PIN_2); 145 | gpio_init(GPIOA, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_10MHZ, GPIO_PIN_3); 146 | 147 | /* CAN0_RX PB8 */ 148 | /* CAN0_TX PB9 */ 149 | gpio_pin_remap_config(GPIO_CAN_PARTIAL_REMAP, ENABLE); 150 | gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_8); 151 | gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_2MHZ, GPIO_PIN_9); 152 | } 153 | #endif 154 | /* USER CODE BEGIN 2 */ 155 | 156 | /* USER CODE END 2 */ 157 | -------------------------------------------------------------------------------- /Core/Inc/hw_config.h: -------------------------------------------------------------------------------- 1 | #ifndef HW_CONFIG_H 2 | #define HW_CONFIG_H 3 | 4 | 5 | #ifdef STM32F446 6 | /* Timer and PWM */ 7 | #define TIM_PWM htim1 // PWM/ISR timer handle 8 | #define TIM_CH_U TIM_CHANNEL_1 // Terminal U timer channel 9 | #define TIM_CH_V TIM_CHANNEL_2 // Terminal V timer channel 10 | #define TIM_CH_W TIM_CHANNEL_3 // Terminal W timer channel 11 | #define INVERT_DTC 1 // PWM inverting (1) or non-inverting (0) 12 | 13 | /* ISRs */ 14 | #define PWM_ISR TIM1_UP_TIM10_IRQn // PWM Timer ISR 15 | #define CAN_ISR CAN1_RX0_IRQn // CAN Receive ISR 16 | 17 | /* ADC */ 18 | 19 | #define ADC_CH_MAIN hadc1 // ADC channel handle which drives simultaneous mode 20 | #define ADC_CH_IA hadc1 // Phase A current sense ADC channel handle. 0 = unused 21 | #define ADC_CH_IB hadc2 // Phase B current sense ADC channel handle. 0 = unused 22 | #define ADC_CH_IC 0 // Phase C current sense ADC channel handle. 0 = unused 23 | #define ADC_CH_VBUS hadc3 // Bus voltage ADC channel handle. 0 = unused 24 | 25 | /* DRV Gate drive */ 26 | #define ENABLE_PIN GPIOA, GPIO_PIN_11 // Enable gate drive pin. 27 | #define DRV_SPI hspi1 // DRV SPI handle 28 | #define DRV_CS GPIOA, GPIO_PIN_4 // DRV CS pin 29 | 30 | /* SPI encoder */ 31 | #define ENC_SPI hspi3 // Encoder SPI handle 32 | #define ENC_CS GPIOA, GPIO_PIN_15 // Encoder SPI CS pin 33 | #define ENC_CPR 65536 // Encoder counts per revolution 34 | #define INV_CPR 1.0f/ENC_CPR 35 | #define ENC_READ_WORD 0x00 // Encoder read command 36 | 37 | /* Misc. GPIO */ 38 | #define LED GPIOC, GPIO_PIN_5 // LED Pin 39 | 40 | /* CAN */ 41 | #define CAN_H hcan1 // CAN handle 42 | 43 | /* Other hardware-related constants */ 44 | #define I_SCALE 0.0201416f // Amps per A/D Count at 40X amplifier gain 45 | #define V_SCALE 0.0128906f // Bus volts per A/D Count 46 | #define DTC_MAX 0.94f // Max duty cycle 47 | #define DTC_MIN 0.0f // Min duty cycle 48 | #define DTC_COMP 0.000f // deadtime compensation (100 ns / 25 us) 49 | #define DT .000025f // Loop period 50 | #define EN_ENC_LINEARIZATION 1 // Enable/disable encoder linearization 51 | 52 | 53 | /* Current controller */ 54 | #define L_D .000003f // D axis inductance 55 | #define L_Q .000003f // Q axis inductance 56 | #define K_D .05f // Loop gain, Volts/Amp 57 | #define K_Q .05f // Loop gain, Volts/Amp 58 | #define K_SCALE 0.0001f // K_loop/Loop BW (Hz) 0.0042 59 | #define KI_D 0.045f // PI zero, in radians per sample 60 | #define KI_Q 0.045f // PI zero, in radians per sample 61 | #define OVERMODULATION 1.15f // 1.0 = no overmodulation 62 | #define CURRENT_FILT_ALPHA .1f // 1st order d/q current filter (not used in control) 63 | #define VBUS_FILT_ALPHA .1f // 1st order bus voltage filter 64 | 65 | #define D_INT_LIM V_BUS/(K_D*KI_D) // Amps*samples 66 | #define Q_INT_LIM V_BUS/(K_Q*KI_Q) // Amps*samples 67 | 68 | #else 69 | 70 | /* Timer and PWM */ 71 | #define TIM_PWM TIMER0 // PWM/ISR timer handle 72 | #define TIM_CH_U TIMER_CH_0 // Terminal U timer channel 73 | #define TIM_CH_V TIMER_CH_1 // Terminal V timer channel 74 | #define TIM_CH_W TIMER_CH_2 // Terminal W timer channel 75 | #define INVERT_DTC 1 // PWM inverting (1) or non-inverting (0) 76 | 77 | /* ISRs */ 78 | #define PWM_ISR TIMER0_UP_IRQn // PWM Timer ISR 79 | #define CAN_ISR USBD_LP_CAN0_RX0_IRQn // CAN Receive ISR 80 | 81 | /* ADC */ 82 | 83 | #define ADC_CH_MAIN ADC0 // ADC channel handle which drives simultaneous mode 84 | #define ADC_CH_IA 0 // Phase A current sense ADC channel handle. 0 = unused 85 | #define ADC_CH_IB ADC0 // Phase B current sense ADC channel handle. 0 = unused 86 | #define ADC_CH_IC ADC1 // Phase C current sense ADC channel handle. 0 = unused 87 | #define ADC_CH_VBUS ADC2 // Bus voltage ADC channel handle. 0 = unused 88 | 89 | /* DRV Gate drive */ 90 | #define ENABLE_PIN GPIOA, GPIO_PIN_11 // Enable gate drive pin. 91 | #define DRV_SPI SPI1 // DRV SPI handle 92 | #define DRV_CS GPIOB, GPIO_PIN_12 // DRV CS pin 93 | 94 | /* SPI encoder */ 95 | #define ENC_SPI SPI2 // Encoder SPI handle 96 | #define ENC_CS GPIOA, GPIO_PIN_15 // Encoder SPI CS pin 97 | #define ENC_CPR 65536 // Encoder counts per revolution 98 | #define INV_CPR 1.0f/ENC_CPR 99 | #define ENC_READ_WORD 0x00 // Encoder read command 100 | 101 | /* Misc. GPIO */ 102 | #define LED GPIOC, GPIO_PIN_13 // LED Pin 103 | 104 | /* CAN */ 105 | #define CAN_H CAN0 // CAN handle 106 | 107 | /* Other hardware-related constants */ 108 | #define I_SCALE 0.0201416f // Amps per A/D Count at 40X amplifier gain 109 | #define V_SCALE 0.0128906f // Bus volts per A/D Count 110 | #define DTC_MAX 0.94f // Max duty cycle 111 | #define DTC_MIN 0.0f // Min duty cycle 112 | #define DTC_COMP 0.000f // deadtime compensation (100 ns / 25 us) 113 | #define DT 0.00003333f // Loop period 114 | #define EN_ENC_LINEARIZATION 1 // Enable/disable encoder linearization 115 | 116 | 117 | /* Current controller */ 118 | #define L_D .000003f // D axis inductance 119 | #define L_Q .000003f // Q axis inductance 120 | #define K_D .05f // Loop gain, Volts/Amp 121 | #define K_Q .05f // Loop gain, Volts/Amp 122 | #define K_SCALE 0.0001f // K_loop/Loop BW (Hz) 0.0042 123 | #define KI_D 0.045f // PI zero, in radians per sample 124 | #define KI_Q 0.045f // PI zero, in radians per sample 125 | #define OVERMODULATION 1.15f // 1.0 = no overmodulation 126 | #define CURRENT_FILT_ALPHA .1f // 1st order d/q current filter (not used in control) 127 | #define VBUS_FILT_ALPHA .1f // 1st order bus voltage filter 128 | 129 | #define D_INT_LIM V_BUS/(K_D*KI_D) // Amps*samples 130 | #define Q_INT_LIM V_BUS/(K_Q*KI_Q) // Amps*samples 131 | 132 | #endif 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Include/gd32f30x_fwdgt.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_fwdgt.h 3 | \brief definitions for the FWDGT 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #ifndef GD32F30X_FWDGT_H 39 | #define GD32F30X_FWDGT_H 40 | 41 | #include "gd32f30x.h" 42 | 43 | /* FWDGT definitions */ 44 | #define FWDGT FWDGT_BASE 45 | 46 | /* registers definitions */ 47 | #define FWDGT_CTL REG32((FWDGT) + 0x00U) /*!< FWDGT control register */ 48 | #define FWDGT_PSC REG32((FWDGT) + 0x04U) /*!< FWDGT prescaler register */ 49 | #define FWDGT_RLD REG32((FWDGT) + 0x08U) /*!< FWDGT reload register */ 50 | #define FWDGT_STAT REG32((FWDGT) + 0x0CU) /*!< FWDGT status register */ 51 | 52 | /* bits definitions */ 53 | /* FWDGT_CTL */ 54 | #define FWDGT_CTL_CMD BITS(0,15) /*!< FWDGT command value */ 55 | 56 | /* FWDGT_PSC */ 57 | #define FWDGT_PSC_PSC BITS(0,2) /*!< FWDGT prescaler divider value */ 58 | 59 | /* FWDGT_RLD */ 60 | #define FWDGT_RLD_RLD BITS(0,11) /*!< FWDGT counter reload value */ 61 | 62 | /* FWDGT_STAT */ 63 | #define FWDGT_STAT_PUD BIT(0) /*!< FWDGT prescaler divider value update */ 64 | #define FWDGT_STAT_RUD BIT(1) /*!< FWDGT counter reload value update */ 65 | 66 | /* constants definitions */ 67 | /* psc register value */ 68 | #define PSC_PSC(regval) (BITS(0,2) & ((uint32_t)(regval) << 0)) 69 | #define FWDGT_PSC_DIV4 ((uint8_t)PSC_PSC(0)) /*!< FWDGT prescaler set to 4 */ 70 | #define FWDGT_PSC_DIV8 ((uint8_t)PSC_PSC(1)) /*!< FWDGT prescaler set to 8 */ 71 | #define FWDGT_PSC_DIV16 ((uint8_t)PSC_PSC(2)) /*!< FWDGT prescaler set to 16 */ 72 | #define FWDGT_PSC_DIV32 ((uint8_t)PSC_PSC(3)) /*!< FWDGT prescaler set to 32 */ 73 | #define FWDGT_PSC_DIV64 ((uint8_t)PSC_PSC(4)) /*!< FWDGT prescaler set to 64 */ 74 | #define FWDGT_PSC_DIV128 ((uint8_t)PSC_PSC(5)) /*!< FWDGT prescaler set to 128 */ 75 | #define FWDGT_PSC_DIV256 ((uint8_t)PSC_PSC(6)) /*!< FWDGT prescaler set to 256 */ 76 | 77 | /* control value */ 78 | #define FWDGT_WRITEACCESS_ENABLE ((uint16_t)0x5555U) /*!< FWDGT_CTL bits write access enable value */ 79 | #define FWDGT_WRITEACCESS_DISABLE ((uint16_t)0x0000U) /*!< FWDGT_CTL bits write access disable value */ 80 | #define FWDGT_KEY_RELOAD ((uint16_t)0xAAAAU) /*!< FWDGT_CTL bits fwdgt counter reload value */ 81 | #define FWDGT_KEY_ENABLE ((uint16_t)0xCCCCU) /*!< FWDGT_CTL bits fwdgt counter enable value */ 82 | 83 | /* FWDGT timeout value */ 84 | #define FWDGT_PSC_TIMEOUT ((uint32_t)0x000FFFFFU) /*!< FWDGT_PSC register write operation state flag timeout */ 85 | #define FWDGT_RLD_TIMEOUT ((uint32_t)0x000FFFFFU) /*!< FWDGT_RLD register write operation state flag timeout */ 86 | 87 | /* FWDGT flag definitions */ 88 | #define FWDGT_FLAG_PUD FWDGT_STAT_PUD /*!< FWDGT prescaler divider value update flag */ 89 | #define FWDGT_FLAG_RUD FWDGT_STAT_RUD /*!< FWDGT counter reload value update flag */ 90 | 91 | /* function declarations */ 92 | /* enable write access to FWDGT_PSC and FWDGT_RLD */ 93 | void fwdgt_write_enable(void); 94 | /* disable write access to FWDGT_PSC and FWDGT_RLD */ 95 | void fwdgt_write_disable(void); 96 | /* start the free watchdog timer counter */ 97 | void fwdgt_enable(void); 98 | 99 | /* reload the counter of FWDGT */ 100 | void fwdgt_counter_reload(void); 101 | /* configure counter reload value, and prescaler divider value */ 102 | ErrStatus fwdgt_config(uint16_t reload_value, uint8_t prescaler_div); 103 | 104 | /* get flag state of FWDGT */ 105 | FlagStatus fwdgt_flag_get(uint16_t flag); 106 | 107 | #endif /* GD32F30X_FWDGT_H */ 108 | -------------------------------------------------------------------------------- /Core/Src/drv8323.c: -------------------------------------------------------------------------------- 1 | /* 2 | * drv8323.c 3 | * 4 | * Created on: Aug 1, 2020 5 | * Author: ben 6 | */ 7 | 8 | 9 | #include "drv8323.h" 10 | #include 11 | #include "structs.h" 12 | #include "hw_config.h" 13 | #include "user_config.h" 14 | 15 | uint16_t drv_spi_write(DRVStruct * drv, uint16_t val){ 16 | #ifdef STM32F446 17 | drv->spi_tx_word = val; 18 | HAL_GPIO_WritePin(DRV_CS, GPIO_PIN_RESET ); // CS low 19 | HAL_SPI_TransmitReceive(&DRV_SPI, (uint8_t*)drv->spi_tx_buff, (uint8_t *)drv->spi_rx_buff, 1, 100); 20 | while( DRV_SPI.State == HAL_SPI_STATE_BUSY ); // wait for transmission complete 21 | HAL_GPIO_WritePin(DRV_CS, GPIO_PIN_SET ); // CS high 22 | return drv->spi_rx_word; 23 | #else 24 | drv->spi_tx_word = val; 25 | 26 | SPI_SET_NSS_LOW(DRV_CS); 27 | 28 | spi_transmit_receive(DRV_SPI, &drv->spi_tx_word, &drv->spi_rx_word); 29 | 30 | SPI_SET_NSS_HIGH(DRV_CS); 31 | 32 | return drv->spi_rx_word; 33 | #endif 34 | } 35 | uint16_t drv_read_FSR1(DRVStruct drv){ 36 | return drv_spi_write(&drv, (1 << 15) | (FSR1 << 11)); 37 | } 38 | 39 | uint16_t drv_read_FSR2(DRVStruct drv){ 40 | return drv_spi_write(&drv, (1 << 15) | (FSR2 << 11)); 41 | } 42 | 43 | uint16_t drv_read_register(DRVStruct drv, int reg){ 44 | return drv_spi_write(&drv, (1<<15)|(reg<<11)); 45 | } 46 | void drv_write_register(DRVStruct drv, int reg, int val){ 47 | drv_spi_write(&drv, (reg<<11)|val); 48 | } 49 | void drv_write_DCR(DRVStruct drv, int DIS_CPUV, int DIS_GDF, int OTW_REP, int PWM_MODE, int PWM_COM, int PWM_DIR, int COAST, int BRAKE, int CLR_FLT){ 50 | uint16_t val = (DCR<<11) | (DIS_CPUV<<9) | (DIS_GDF<<8) | (OTW_REP<<7) | (PWM_MODE<<5) | (PWM_COM<<4) | (PWM_DIR<<3) | (COAST<<2) | (BRAKE<<1) | CLR_FLT; 51 | drv_spi_write(&drv, val); 52 | } 53 | void drv_write_HSR(DRVStruct drv, int LOCK, int IDRIVEP_HS, int IDRIVEN_HS){ 54 | uint16_t val = (HSR<<11) | (LOCK<<8) | (IDRIVEP_HS<<4) | IDRIVEN_HS; 55 | drv_spi_write(&drv, val); 56 | } 57 | void drv_write_LSR(DRVStruct drv, int CBC, int TDRIVE, int IDRIVEP_LS, int IDRIVEN_LS){ 58 | uint16_t val = (LSR<<11) | (CBC<<10) | (TDRIVE<<8) | (IDRIVEP_LS<<4) | IDRIVEN_LS; 59 | drv_spi_write(&drv, val); 60 | } 61 | void drv_write_OCPCR(DRVStruct drv, int TRETRY, int DEAD_TIME, int OCP_MODE, int OCP_DEG, int VDS_LVL){ 62 | uint16_t val = (OCPCR<<11) | (TRETRY<<10) | (DEAD_TIME<<8) | (OCP_MODE<<6) | (OCP_DEG<<4) | VDS_LVL; 63 | drv_spi_write(&drv, val); 64 | } 65 | void drv_write_CSACR(DRVStruct drv, int CSA_FET, int VREF_DIV, int LS_REF, int CSA_GAIN, int DIS_SEN, int CSA_CAL_A, int CSA_CAL_B, int CSA_CAL_C, int SEN_LVL){ 66 | uint16_t val = (CSACR<<11) | (CSA_FET<<10) | (VREF_DIV<<9) | (LS_REF<<8) | (CSA_GAIN<<6) | (DIS_SEN<<5) | (CSA_CAL_A<<4) | (CSA_CAL_B<<3) | (CSA_CAL_C<<2) | SEN_LVL; 67 | drv_spi_write(&drv, val); 68 | } 69 | void drv_enable_gd(DRVStruct drv){ 70 | uint16_t val = (drv_read_register(drv, DCR)) & (~(0x1<<2)); 71 | drv_write_register(drv, DCR, val); 72 | } 73 | void drv_disable_gd(DRVStruct drv){ 74 | uint16_t val = (drv_read_register(drv, DCR)) | (0x1<<2); 75 | drv_write_register(drv, DCR, val); 76 | } 77 | void drv_calibrate(DRVStruct drv){ 78 | uint16_t val = (0x1<<4) + (0x1<<3) + (0x1<<2); 79 | drv_write_register(drv, CSACR, val); 80 | } 81 | void drv_print_faults(DRVStruct drv, uint32_t loop_count){ 82 | uint16_t val1 = drv_read_FSR1(drv); 83 | uint16_t val2 = drv_read_FSR2(drv); 84 | 85 | if ((val1 | val2) > 0) { 86 | printf("loop_count: %lu\r\n", loop_count); 87 | } 88 | 89 | if(val1 & (1<<10)){printf("\n\rFAULT\n\r");} 90 | 91 | if(val1 & (1<<9)){printf("VDS_OCP\n\r");} 92 | if(val1 & (1<<8)){printf("GDF\n\r");} 93 | if(val1 & (1<<7)){printf("UVLO\n\r");} 94 | if(val1 & (1<<6)){printf("OTSD\n\r");} 95 | if(val1 & (1<<5)){printf("VDS_HA\n\r");} 96 | if(val1 & (1<<4)){printf("VDS_LA\n\r");} 97 | if(val1 & (1<<3)){printf("VDS_HB\n\r");} 98 | if(val1 & (1<<2)){printf("VDS_LB\n\r");} 99 | if(val1 & (1<<1)){printf("VDS_HC\n\r");} 100 | if(val1 & (1)){printf("VDS_LC\n\r");} 101 | 102 | if(val2 & (1<<10)){printf("SA_OC\n\r");} 103 | if(val2 & (1<<9)){printf("SB_OC\n\r");} 104 | if(val2 & (1<<8)){printf("SC_OC\n\r");} 105 | if(val2 & (1<<7)){printf("OTW\n\r");} 106 | if(val2 & (1<<6)){printf("CPUV\n\r");} 107 | if(val2 & (1<<5)){printf("VGS_HA\n\r");} 108 | if(val2 & (1<<4)){printf("VGS_LA\n\r");} 109 | if(val2 & (1<<3)){printf("VGS_HB\n\r");} 110 | if(val2 & (1<<2)){printf("VGS_LB\n\r");} 111 | if(val2 & (1<<1)){printf("VGS_HC\n\r");} 112 | if(val2 & (1)){printf("VGS_LC\n\r");} 113 | 114 | } 115 | 116 | void drv_init_config(DRVStruct drv) 117 | { 118 | /* DRV8323 setup */ 119 | 120 | // Up to 40A use 40X amplifier gain 121 | // From 40-60A use 20X amplifier gain. (Make this generic in the future) 122 | int CSA_GAIN = I_MAX <= 40.0f ? CSA_GAIN_40 : CSA_GAIN_20; 123 | 124 | // Enable DRV8323 125 | gpio_bit_set(ENABLE_PIN); 126 | delay_1ms(10); 127 | 128 | // Driver Control, Gate drive fault is disabled, 3x PWM mode, clear latched fault bits 129 | drv_write_DCR(drv, 0x0, DIS_GDF_EN, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, CLR_FLT_RST); 130 | 131 | // CSA Control, VREF_DIV=2, CSA_GAIN, CSA_CAL_A/B/C, SEN_LVL=0.25v 132 | drv_write_CSACR(drv, 0x0, VREF_DIV_2, 0x0, CSA_GAIN, 0x0, 0x1, 0x1, 0x1, SEN_LVL_0_25); 133 | 134 | zero_current(&controller); 135 | 136 | // CSA Control, VREF_DIV=2, CSA_GAIN, DIS_SEN, SEN_LVL=0.25v 137 | drv_write_CSACR(drv, 0x0, VREF_DIV_2, 0x0, CSA_GAIN, 0x1, 0x0, 0x0, 0x0, SEN_LVL_0_25); 138 | // drv_write_CSACR(drv, 0x0, 0x1, 0x0, CSA_GAIN, 0x0, 0x0, 0x0, 0x0, SEN_LVL_0_25); 139 | 140 | // OCP Contro, TRETRY=50us, DEAD_TIME=50us, OCP_MODE=retry, OCP_DEG=4us, VDS_LVL=0.45v 141 | drv_write_OCPCR(drv, TRETRY_50US, DEADTIME_50NS, OCP_RETRY, OCP_DEG_4US, VDS_LVL_0_45); 142 | 143 | // all MOSFETs in the Hi-Z state, disable output 144 | drv_disable_gd(drv); 145 | } 146 | 147 | void drv_clear_fault(DRVStruct drv) 148 | { 149 | uint16_t val = (drv_read_register(drv, DCR)) | CLR_FLT_RST; 150 | drv_write_register(drv, DCR, val); 151 | } 152 | 153 | void MX_EXTI_Init() 154 | { 155 | gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOA, GPIO_PIN_SOURCE_12); 156 | 157 | exti_init(EXTI_12, EXTI_INTERRUPT, EXTI_TRIG_BOTH); 158 | exti_interrupt_flag_clear(EXTI_12); 159 | } 160 | -------------------------------------------------------------------------------- /GD32F303RETx_FLASH.ld: -------------------------------------------------------------------------------- 1 | /* 2 | ****************************************************************************** 3 | ** 4 | 5 | ** File : LinkerScript.ld 6 | ** 7 | ** Author : STM32CubeMX 8 | ** 9 | ** Abstract : Linker script for STM32F446RETx series 10 | ** 512Kbytes FLASH and 128Kbytes RAM 11 | ** 12 | ** Set heap size, stack size and stack location according 13 | ** to application requirements. 14 | ** 15 | ** Set memory bank area and size if external memory is used. 16 | ** 17 | ** Target : STMicroelectronics STM32 18 | ** 19 | ** Distribution: The file is distributed “as is,” without any warranty 20 | ** of any kind. 21 | ** 22 | ***************************************************************************** 23 | ** @attention 24 | ** 25 | **

© COPYRIGHT(c) 2019 STMicroelectronics

26 | ** 27 | ** Redistribution and use in source and binary forms, with or without modification, 28 | ** are permitted provided that the following conditions are met: 29 | ** 1. Redistributions of source code must retain the above copyright notice, 30 | ** this list of conditions and the following disclaimer. 31 | ** 2. Redistributions in binary form must reproduce the above copyright notice, 32 | ** this list of conditions and the following disclaimer in the documentation 33 | ** and/or other materials provided with the distribution. 34 | ** 3. Neither the name of STMicroelectronics nor the names of its contributors 35 | ** may be used to endorse or promote products derived from this software 36 | ** without specific prior written permission. 37 | ** 38 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 39 | ** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 40 | ** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 41 | ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 42 | ** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 43 | ** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 44 | ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 45 | ** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 46 | ** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 47 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 48 | ** 49 | ***************************************************************************** 50 | */ 51 | 52 | /* Entry Point */ 53 | ENTRY(Reset_Handler) 54 | 55 | /* Highest address of the user mode stack */ 56 | _estack = ORIGIN(RAM) + LENGTH(RAM); /* end of RAM */ 57 | /* Generate a link error if heap and stack don't fit into RAM */ 58 | _Min_Heap_Size = 0x200; /* required amount of heap */ 59 | _Min_Stack_Size = 0x400; /* required amount of stack */ 60 | 61 | /* Specify the memory areas */ 62 | MEMORY 63 | { 64 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 64K 65 | FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 512K 66 | } 67 | 68 | /* Define output sections */ 69 | SECTIONS 70 | { 71 | /* The startup code goes first into FLASH */ 72 | .isr_vector : 73 | { 74 | . = ALIGN(4); 75 | KEEP(*(.isr_vector)) /* Startup code */ 76 | . = ALIGN(4); 77 | } >FLASH 78 | 79 | /* The program code and other data goes into FLASH */ 80 | .text : 81 | { 82 | . = ALIGN(4); 83 | *(.text) /* .text sections (code) */ 84 | *(.text*) /* .text* sections (code) */ 85 | *(.glue_7) /* glue arm to thumb code */ 86 | *(.glue_7t) /* glue thumb to arm code */ 87 | *(.eh_frame) 88 | 89 | KEEP (*(.init)) 90 | KEEP (*(.fini)) 91 | 92 | . = ALIGN(4); 93 | _etext = .; /* define a global symbols at end of code */ 94 | } >FLASH 95 | 96 | /* Constant data goes into FLASH */ 97 | .rodata : 98 | { 99 | . = ALIGN(4); 100 | *(.rodata) /* .rodata sections (constants, strings, etc.) */ 101 | *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ 102 | . = ALIGN(4); 103 | } >FLASH 104 | 105 | .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH 106 | .ARM : { 107 | __exidx_start = .; 108 | *(.ARM.exidx*) 109 | __exidx_end = .; 110 | } >FLASH 111 | 112 | .preinit_array : 113 | { 114 | PROVIDE_HIDDEN (__preinit_array_start = .); 115 | KEEP (*(.preinit_array*)) 116 | PROVIDE_HIDDEN (__preinit_array_end = .); 117 | } >FLASH 118 | .init_array : 119 | { 120 | PROVIDE_HIDDEN (__init_array_start = .); 121 | KEEP (*(SORT(.init_array.*))) 122 | KEEP (*(.init_array*)) 123 | PROVIDE_HIDDEN (__init_array_end = .); 124 | } >FLASH 125 | .fini_array : 126 | { 127 | PROVIDE_HIDDEN (__fini_array_start = .); 128 | KEEP (*(SORT(.fini_array.*))) 129 | KEEP (*(.fini_array*)) 130 | PROVIDE_HIDDEN (__fini_array_end = .); 131 | } >FLASH 132 | 133 | /* used by the startup to initialize data */ 134 | _sidata = LOADADDR(.data); 135 | 136 | /* Initialized data sections goes into RAM, load LMA copy after code */ 137 | .data : 138 | { 139 | . = ALIGN(4); 140 | _sdata = .; /* create a global symbol at data start */ 141 | *(.data) /* .data sections */ 142 | *(.data*) /* .data* sections */ 143 | 144 | . = ALIGN(4); 145 | _edata = .; /* define a global symbol at data end */ 146 | } >RAM AT> FLASH 147 | 148 | 149 | /* Uninitialized data section */ 150 | . = ALIGN(4); 151 | .bss : 152 | { 153 | /* This is used by the startup in order to initialize the .bss secion */ 154 | _sbss = .; /* define a global symbol at bss start */ 155 | __bss_start__ = _sbss; 156 | *(.bss) 157 | *(.bss*) 158 | *(COMMON) 159 | 160 | . = ALIGN(4); 161 | _ebss = .; /* define a global symbol at bss end */ 162 | __bss_end__ = _ebss; 163 | } >RAM 164 | 165 | /* User_heap_stack section, used to check that there is enough RAM left */ 166 | ._user_heap_stack : 167 | { 168 | . = ALIGN(8); 169 | PROVIDE ( end = . ); 170 | PROVIDE ( _end = . ); 171 | . = . + _Min_Heap_Size; 172 | . = . + _Min_Stack_Size; 173 | . = ALIGN(8); 174 | } >RAM 175 | 176 | 177 | 178 | /* Remove information from the standard libraries */ 179 | /DISCARD/ : 180 | { 181 | libc.a ( * ) 182 | libm.a ( * ) 183 | libgcc.a ( * ) 184 | } 185 | 186 | .ARM.attributes 0 : { *(.ARM.attributes) } 187 | } 188 | 189 | 190 | -------------------------------------------------------------------------------- /STM32F446RETx_FLASH.ld: -------------------------------------------------------------------------------- 1 | /* 2 | ****************************************************************************** 3 | ** 4 | 5 | ** File : LinkerScript.ld 6 | ** 7 | ** Author : STM32CubeMX 8 | ** 9 | ** Abstract : Linker script for STM32F446RETx series 10 | ** 512Kbytes FLASH and 128Kbytes RAM 11 | ** 12 | ** Set heap size, stack size and stack location according 13 | ** to application requirements. 14 | ** 15 | ** Set memory bank area and size if external memory is used. 16 | ** 17 | ** Target : STMicroelectronics STM32 18 | ** 19 | ** Distribution: The file is distributed “as is,” without any warranty 20 | ** of any kind. 21 | ** 22 | ***************************************************************************** 23 | ** @attention 24 | ** 25 | **

© COPYRIGHT(c) 2019 STMicroelectronics

26 | ** 27 | ** Redistribution and use in source and binary forms, with or without modification, 28 | ** are permitted provided that the following conditions are met: 29 | ** 1. Redistributions of source code must retain the above copyright notice, 30 | ** this list of conditions and the following disclaimer. 31 | ** 2. Redistributions in binary form must reproduce the above copyright notice, 32 | ** this list of conditions and the following disclaimer in the documentation 33 | ** and/or other materials provided with the distribution. 34 | ** 3. Neither the name of STMicroelectronics nor the names of its contributors 35 | ** may be used to endorse or promote products derived from this software 36 | ** without specific prior written permission. 37 | ** 38 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 39 | ** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 40 | ** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 41 | ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 42 | ** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 43 | ** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 44 | ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 45 | ** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 46 | ** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 47 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 48 | ** 49 | ***************************************************************************** 50 | */ 51 | 52 | /* Entry Point */ 53 | ENTRY(Reset_Handler) 54 | 55 | /* Highest address of the user mode stack */ 56 | _estack = ORIGIN(RAM) + LENGTH(RAM); /* end of RAM */ 57 | /* Generate a link error if heap and stack don't fit into RAM */ 58 | _Min_Heap_Size = 0x200; /* required amount of heap */ 59 | _Min_Stack_Size = 0x400; /* required amount of stack */ 60 | 61 | /* Specify the memory areas */ 62 | MEMORY 63 | { 64 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K 65 | FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 512K 66 | } 67 | 68 | /* Define output sections */ 69 | SECTIONS 70 | { 71 | /* The startup code goes first into FLASH */ 72 | .isr_vector : 73 | { 74 | . = ALIGN(4); 75 | KEEP(*(.isr_vector)) /* Startup code */ 76 | . = ALIGN(4); 77 | } >FLASH 78 | 79 | /* The program code and other data goes into FLASH */ 80 | .text : 81 | { 82 | . = ALIGN(4); 83 | *(.text) /* .text sections (code) */ 84 | *(.text*) /* .text* sections (code) */ 85 | *(.glue_7) /* glue arm to thumb code */ 86 | *(.glue_7t) /* glue thumb to arm code */ 87 | *(.eh_frame) 88 | 89 | KEEP (*(.init)) 90 | KEEP (*(.fini)) 91 | 92 | . = ALIGN(4); 93 | _etext = .; /* define a global symbols at end of code */ 94 | } >FLASH 95 | 96 | /* Constant data goes into FLASH */ 97 | .rodata : 98 | { 99 | . = ALIGN(4); 100 | *(.rodata) /* .rodata sections (constants, strings, etc.) */ 101 | *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ 102 | . = ALIGN(4); 103 | } >FLASH 104 | 105 | .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH 106 | .ARM : { 107 | __exidx_start = .; 108 | *(.ARM.exidx*) 109 | __exidx_end = .; 110 | } >FLASH 111 | 112 | .preinit_array : 113 | { 114 | PROVIDE_HIDDEN (__preinit_array_start = .); 115 | KEEP (*(.preinit_array*)) 116 | PROVIDE_HIDDEN (__preinit_array_end = .); 117 | } >FLASH 118 | .init_array : 119 | { 120 | PROVIDE_HIDDEN (__init_array_start = .); 121 | KEEP (*(SORT(.init_array.*))) 122 | KEEP (*(.init_array*)) 123 | PROVIDE_HIDDEN (__init_array_end = .); 124 | } >FLASH 125 | .fini_array : 126 | { 127 | PROVIDE_HIDDEN (__fini_array_start = .); 128 | KEEP (*(SORT(.fini_array.*))) 129 | KEEP (*(.fini_array*)) 130 | PROVIDE_HIDDEN (__fini_array_end = .); 131 | } >FLASH 132 | 133 | /* used by the startup to initialize data */ 134 | _sidata = LOADADDR(.data); 135 | 136 | /* Initialized data sections goes into RAM, load LMA copy after code */ 137 | .data : 138 | { 139 | . = ALIGN(4); 140 | _sdata = .; /* create a global symbol at data start */ 141 | *(.data) /* .data sections */ 142 | *(.data*) /* .data* sections */ 143 | 144 | . = ALIGN(4); 145 | _edata = .; /* define a global symbol at data end */ 146 | } >RAM AT> FLASH 147 | 148 | 149 | /* Uninitialized data section */ 150 | . = ALIGN(4); 151 | .bss : 152 | { 153 | /* This is used by the startup in order to initialize the .bss secion */ 154 | _sbss = .; /* define a global symbol at bss start */ 155 | __bss_start__ = _sbss; 156 | *(.bss) 157 | *(.bss*) 158 | *(COMMON) 159 | 160 | . = ALIGN(4); 161 | _ebss = .; /* define a global symbol at bss end */ 162 | __bss_end__ = _ebss; 163 | } >RAM 164 | 165 | /* User_heap_stack section, used to check that there is enough RAM left */ 166 | ._user_heap_stack : 167 | { 168 | . = ALIGN(8); 169 | PROVIDE ( end = . ); 170 | PROVIDE ( _end = . ); 171 | . = . + _Min_Heap_Size; 172 | . = . + _Min_Stack_Size; 173 | . = ALIGN(8); 174 | } >RAM 175 | 176 | 177 | 178 | /* Remove information from the standard libraries */ 179 | /DISCARD/ : 180 | { 181 | libc.a ( * ) 182 | libm.a ( * ) 183 | libgcc.a ( * ) 184 | } 185 | 186 | .ARM.attributes 0 : { *(.ARM.attributes) } 187 | } 188 | 189 | 190 | -------------------------------------------------------------------------------- /Core/Src/calibration.c: -------------------------------------------------------------------------------- 1 | /* 2 | * calibration.c 3 | * 4 | * Created on: Aug 11, 2020 5 | * Author: ben 6 | */ 7 | 8 | 9 | #include "calibration.h" 10 | #include "hw_config.h" 11 | #include "user_config.h" 12 | #include 13 | #include 14 | #include "usart.h" 15 | #include "math_ops.h" 16 | 17 | void order_phases(EncoderStruct *encoder, ControllerStruct *controller, CalStruct * cal, int loop_count){ 18 | /* Checks phase order, to ensure that positive Q current produces 19 | torque in the positive direction wrt the position sensor */ 20 | PHASE_ORDER = 0; 21 | 22 | if(!cal->started){ 23 | printf("Checking phase sign, pole pairs\r\n"); 24 | cal->started = 1; 25 | cal->start_count = loop_count; 26 | } 27 | cal->time = (float)(loop_count - cal->start_count)*DT; 28 | 29 | if(cal->time < T1){ 30 | // Set voltage angle to zero, wait for rotor position to settle 31 | cal->theta_ref = 0;//W_CAL*cal->time; 32 | cal->cal_position.elec_angle = cal->theta_ref; 33 | cal->cal_position.elec_velocity = 0; 34 | controller->i_d_des = I_CAL; 35 | controller->i_q_des = 0.0f; 36 | commutate(controller, &cal->cal_position); 37 | cal->theta_start = encoder->angle_multiturn[0]; 38 | return; 39 | } 40 | 41 | else if(cal->time < T1+2.0f*PI_F/W_CAL){ 42 | // rotate voltage vector through one electrical cycle 43 | cal->theta_ref = W_CAL*(cal->time-T1); 44 | cal->cal_position.elec_angle = cal->theta_ref; 45 | commutate(controller, &cal->cal_position); 46 | return; 47 | } 48 | 49 | reset_foc(controller); 50 | 51 | float theta_end = encoder->angle_multiturn[0]; 52 | cal->ppairs = round(2.0f*PI_F/fabsf(theta_end-cal->theta_start)); 53 | 54 | if(cal->theta_start < theta_end){ 55 | cal->phase_order = 0; 56 | printf("Phase order correct\r\n"); 57 | } 58 | else{ 59 | cal->phase_order = 1; 60 | printf("Swapping phase sign\r\n"); 61 | } 62 | printf("Pole Pairs: %d\r\n", cal->ppairs); 63 | printf("Start: %.3f End: %.3f\r\n", cal->theta_start, theta_end); 64 | PHASE_ORDER = cal->phase_order; 65 | PPAIRS = (float)cal->ppairs; 66 | cal->started = 0; 67 | cal->done_ordering = 1; // Finished checking phase order 68 | } 69 | 70 | void calibrate_encoder(EncoderStruct *encoder, ControllerStruct *controller, CalStruct * cal, int loop_count){ 71 | /* Calibrates e-zero and encoder nonliearity */ 72 | if (PPAIRS > PPAIRS_MAX) { 73 | printf("Pole Pairs > %d, remalloc error_arr[] size.\r\n", PPAIRS_MAX); 74 | return; 75 | } 76 | 77 | if(!cal->started){ 78 | printf("Starting offset cal and linearization\r\n"); 79 | cal->started = 1; 80 | cal->start_count = loop_count; 81 | cal->next_sample_time = T1; 82 | cal->sample_count = 0; 83 | } 84 | 85 | cal->time = (float)(loop_count - cal->start_count)*DT; 86 | 87 | if(cal->time < T1){ 88 | // Set voltage angle to zero, wait for rotor position to settle 89 | cal->theta_ref = 0;//W_CAL*cal->time; 90 | cal->cal_position.elec_angle = cal->theta_ref; 91 | controller->i_d_des = I_CAL; 92 | controller->i_q_des = 0.0f; 93 | commutate(controller, &cal->cal_position); 94 | 95 | cal->theta_start = encoder->angle_multiturn[0]; 96 | cal->next_sample_time = cal->time; 97 | return; 98 | } 99 | else if (cal->time < T1+2.0f*PI_F*PPAIRS/W_CAL){ 100 | // rotate voltage vector through one mechanical rotation in the positive direction 101 | cal->theta_ref += W_CAL*DT;//(cal->time-T1); 102 | cal->cal_position.elec_angle = cal->theta_ref; 103 | commutate(controller, &cal->cal_position); 104 | 105 | // sample SAMPLES_PER_PPAIR times per pole-pair 106 | if(cal->time > cal->next_sample_time){ 107 | int count_ref = cal->theta_ref * (float)ENC_CPR/(2.0f*PI_F*PPAIRS); 108 | int error = encoder->raw - count_ref;//- encoder->raw; 109 | cal->error_arr[cal->sample_count] = error + ENC_CPR*(error<0); 110 | printf("%d %d %d %.3f\r\n", cal->sample_count, count_ref, cal->error_arr[cal->sample_count], cal->theta_ref); 111 | cal->next_sample_time += 2.0f*PI_F/(W_CAL*SAMPLES_PER_PPAIR); 112 | if(cal->sample_count == PPAIRS*SAMPLES_PER_PPAIR-1){ 113 | return; 114 | } 115 | cal->sample_count++; 116 | 117 | } 118 | return; 119 | } 120 | else if (cal->time < T1+4.0f*PI_F*PPAIRS/W_CAL){ 121 | // rotate voltage vector through one mechanical rotation in the negative direction 122 | cal->theta_ref -= W_CAL*DT;//(cal->time-T1); 123 | controller->i_d_des = I_CAL; 124 | controller->i_q_des = 0.0f; 125 | cal->cal_position.elec_angle = cal->theta_ref; 126 | commutate(controller, &cal->cal_position); 127 | 128 | // sample SAMPLES_PER_PPAIR times per pole-pair 129 | if((cal->time > cal->next_sample_time)&&(cal->sample_count>0)){ 130 | int count_ref = cal->theta_ref * (float)ENC_CPR/(2.0f*PI_F*PPAIRS); 131 | int error = encoder->raw - count_ref;// - encoder->raw; 132 | error = error + ENC_CPR*(error<0); 133 | 134 | cal->error_arr[cal->sample_count] = (cal->error_arr[cal->sample_count] + error)/2; 135 | printf("%d %d %d %.3f\r\n", cal->sample_count, count_ref, cal->error_arr[cal->sample_count], cal->theta_ref); 136 | cal->sample_count--; 137 | cal->next_sample_time += 2.0f*PI_F/(W_CAL*SAMPLES_PER_PPAIR); 138 | } 139 | return; 140 | } 141 | 142 | reset_foc(controller); 143 | 144 | // Calculate average offset 145 | int ezero_mean = 0; 146 | for(int i = 0; i<((int)PPAIRS*SAMPLES_PER_PPAIR); i++){ 147 | ezero_mean += cal->error_arr[i]; 148 | } 149 | cal->ezero = ezero_mean/(SAMPLES_PER_PPAIR*PPAIRS); 150 | 151 | // Moving average to filter out cogging ripple 152 | 153 | int window = SAMPLES_PER_PPAIR; 154 | int lut_offset = (ENC_CPR-cal->error_arr[0])*N_LUT/ENC_CPR; 155 | for(int i = 0; i(SAMPLES_PER_PPAIR*PPAIRS-1)){index -= (SAMPLES_PER_PPAIR*PPAIRS);} 161 | moving_avg += cal->error_arr[index]; 162 | } 163 | moving_avg = moving_avg/window; 164 | int lut_index = lut_offset + i; 165 | if(lut_index>(N_LUT-1)){lut_index -= N_LUT;} 166 | cal->lut_arr[lut_index] = moving_avg - cal->ezero; 167 | printf("%d %d\r\n", lut_index, moving_avg - cal->ezero); 168 | 169 | } 170 | 171 | cal->started = 0; 172 | cal->done_cal = 1; 173 | } 174 | 175 | void measure_lr(EncoderStruct *encoder, ControllerStruct *controller, CalStruct * cal, int loop_count){ 176 | 177 | } 178 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_hal_flash_ramfunc.c 4 | * @author MCD Application Team 5 | * @brief FLASH RAMFUNC module driver. 6 | * This file provides a FLASH firmware functions which should be 7 | * executed from internal SRAM 8 | * + Stop/Start the flash interface while System Run 9 | * + Enable/Disable the flash sleep while System Run 10 | @verbatim 11 | ============================================================================== 12 | ##### APIs executed from Internal RAM ##### 13 | ============================================================================== 14 | [..] 15 | *** ARM Compiler *** 16 | -------------------- 17 | [..] RAM functions are defined using the toolchain options. 18 | Functions that are be executed in RAM should reside in a separate 19 | source module. Using the 'Options for File' dialog you can simply change 20 | the 'Code / Const' area of a module to a memory space in physical RAM. 21 | Available memory areas are declared in the 'Target' tab of the 22 | Options for Target' dialog. 23 | 24 | *** ICCARM Compiler *** 25 | ----------------------- 26 | [..] RAM functions are defined using a specific toolchain keyword "__ramfunc". 27 | 28 | *** GNU Compiler *** 29 | -------------------- 30 | [..] RAM functions are defined using a specific toolchain attribute 31 | "__attribute__((section(".RamFunc")))". 32 | 33 | @endverbatim 34 | ****************************************************************************** 35 | * @attention 36 | * 37 | * Copyright (c) 2017 STMicroelectronics. 38 | * All rights reserved. 39 | * 40 | * This software is licensed under terms that can be found in the LICENSE file in 41 | * the root directory of this software component. 42 | * If no LICENSE file comes with this software, it is provided AS-IS. 43 | ****************************************************************************** 44 | */ 45 | 46 | /* Includes ------------------------------------------------------------------*/ 47 | #include "stm32f4xx_hal.h" 48 | 49 | /** @addtogroup STM32F4xx_HAL_Driver 50 | * @{ 51 | */ 52 | 53 | /** @defgroup FLASH_RAMFUNC FLASH RAMFUNC 54 | * @brief FLASH functions executed from RAM 55 | * @{ 56 | */ 57 | #ifdef HAL_FLASH_MODULE_ENABLED 58 | #if defined(STM32F410Tx) || defined(STM32F410Cx) || defined(STM32F410Rx) || defined(STM32F411xE) || defined(STM32F446xx) || defined(STM32F412Zx) || defined(STM32F412Vx) || \ 59 | defined(STM32F412Rx) || defined(STM32F412Cx) 60 | 61 | /* Private typedef -----------------------------------------------------------*/ 62 | /* Private define ------------------------------------------------------------*/ 63 | /* Private macro -------------------------------------------------------------*/ 64 | /* Private variables ---------------------------------------------------------*/ 65 | /* Private function prototypes -----------------------------------------------*/ 66 | /* Exported functions --------------------------------------------------------*/ 67 | /** @defgroup FLASH_RAMFUNC_Exported_Functions FLASH RAMFUNC Exported Functions 68 | * @{ 69 | */ 70 | 71 | /** @defgroup FLASH_RAMFUNC_Exported_Functions_Group1 Peripheral features functions executed from internal RAM 72 | * @brief Peripheral Extended features functions 73 | * 74 | @verbatim 75 | 76 | =============================================================================== 77 | ##### ramfunc functions ##### 78 | =============================================================================== 79 | [..] 80 | This subsection provides a set of functions that should be executed from RAM 81 | transfers. 82 | 83 | @endverbatim 84 | * @{ 85 | */ 86 | 87 | /** 88 | * @brief Stop the flash interface while System Run 89 | * @note This mode is only available for STM32F41xxx/STM32F446xx devices. 90 | * @note This mode couldn't be set while executing with the flash itself. 91 | * It should be done with specific routine executed from RAM. 92 | * @retval HAL status 93 | */ 94 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_StopFlashInterfaceClk(void) 95 | { 96 | /* Enable Power ctrl clock */ 97 | __HAL_RCC_PWR_CLK_ENABLE(); 98 | /* Stop the flash interface while System Run */ 99 | SET_BIT(PWR->CR, PWR_CR_FISSR); 100 | 101 | return HAL_OK; 102 | } 103 | 104 | /** 105 | * @brief Start the flash interface while System Run 106 | * @note This mode is only available for STM32F411xx/STM32F446xx devices. 107 | * @note This mode couldn't be set while executing with the flash itself. 108 | * It should be done with specific routine executed from RAM. 109 | * @retval HAL status 110 | */ 111 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_StartFlashInterfaceClk(void) 112 | { 113 | /* Enable Power ctrl clock */ 114 | __HAL_RCC_PWR_CLK_ENABLE(); 115 | /* Start the flash interface while System Run */ 116 | CLEAR_BIT(PWR->CR, PWR_CR_FISSR); 117 | 118 | return HAL_OK; 119 | } 120 | 121 | /** 122 | * @brief Enable the flash sleep while System Run 123 | * @note This mode is only available for STM32F41xxx/STM32F446xx devices. 124 | * @note This mode could n't be set while executing with the flash itself. 125 | * It should be done with specific routine executed from RAM. 126 | * @retval HAL status 127 | */ 128 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_EnableFlashSleepMode(void) 129 | { 130 | /* Enable Power ctrl clock */ 131 | __HAL_RCC_PWR_CLK_ENABLE(); 132 | /* Enable the flash sleep while System Run */ 133 | SET_BIT(PWR->CR, PWR_CR_FMSSR); 134 | 135 | return HAL_OK; 136 | } 137 | 138 | /** 139 | * @brief Disable the flash sleep while System Run 140 | * @note This mode is only available for STM32F41xxx/STM32F446xx devices. 141 | * @note This mode couldn't be set while executing with the flash itself. 142 | * It should be done with specific routine executed from RAM. 143 | * @retval HAL status 144 | */ 145 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_DisableFlashSleepMode(void) 146 | { 147 | /* Enable Power ctrl clock */ 148 | __HAL_RCC_PWR_CLK_ENABLE(); 149 | /* Disable the flash sleep while System Run */ 150 | CLEAR_BIT(PWR->CR, PWR_CR_FMSSR); 151 | 152 | return HAL_OK; 153 | } 154 | 155 | /** 156 | * @} 157 | */ 158 | 159 | /** 160 | * @} 161 | */ 162 | 163 | #endif /* STM32F410xx || STM32F411xE || STM32F446xx || STM32F412Zx || STM32F412Vx || STM32F412Rx || STM32F412Cx */ 164 | #endif /* HAL_FLASH_MODULE_ENABLED */ 165 | /** 166 | * @} 167 | */ 168 | 169 | /** 170 | * @} 171 | */ 172 | 173 | -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_dbg.c: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_dbg.c 3 | \brief DBG driver 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | #include "gd32f30x_dbg.h" 39 | 40 | #define DBG_RESET_VAL 0x00000000U 41 | 42 | /*! 43 | \brief deinitialize the DBG 44 | \param[in] none 45 | \param[out] none 46 | \retval none 47 | */ 48 | void dbg_deinit(void) 49 | { 50 | DBG_CTL0 = DBG_RESET_VAL; 51 | } 52 | 53 | /*! 54 | \brief read DBG_ID code register 55 | \param[in] none 56 | \param[out] none 57 | \retval DBG_ID code 58 | */ 59 | uint32_t dbg_id_get(void) 60 | { 61 | return DBG_ID; 62 | } 63 | 64 | /*! 65 | \brief enable low power behavior when the mcu is in debug mode 66 | \param[in] dbg_low_power: 67 | this parameter can be any combination of the following values: 68 | \arg DBG_LOW_POWER_SLEEP: keep debugger connection during sleep mode 69 | \arg DBG_LOW_POWER_DEEPSLEEP: keep debugger connection during deepsleep mode 70 | \arg DBG_LOW_POWER_STANDBY: keep debugger connection during standby mode 71 | \param[out] none 72 | \retval none 73 | */ 74 | void dbg_low_power_enable(uint32_t dbg_low_power) 75 | { 76 | DBG_CTL0 |= dbg_low_power; 77 | } 78 | 79 | /*! 80 | \brief disable low power behavior when the mcu is in debug mode 81 | \param[in] dbg_low_power: 82 | this parameter can be any combination of the following values: 83 | \arg DBG_LOW_POWER_SLEEP: donot keep debugger connection during sleep mode 84 | \arg DBG_LOW_POWER_DEEPSLEEP: donot keep debugger connection during deepsleep mode 85 | \arg DBG_LOW_POWER_STANDBY: donot keep debugger connection during standby mode 86 | \param[out] none 87 | \retval none 88 | */ 89 | void dbg_low_power_disable(uint32_t dbg_low_power) 90 | { 91 | DBG_CTL0 &= ~dbg_low_power; 92 | } 93 | 94 | /*! 95 | \brief enable peripheral behavior when the mcu is in debug mode 96 | \param[in] dbg_periph: refer to dbg_periph_enum 97 | only one parameter can be selected which is shown as below: 98 | \arg DBG_FWDGT_HOLD : debug FWDGT kept when core is halted 99 | \arg DBG_WWDGT_HOLD : debug WWDGT kept when core is halted 100 | \arg DBG_CANx_HOLD (x=0,1,CAN1 is only available for CL series): hold CANx counter when core is halted 101 | \arg DBG_I2Cx_HOLD (x=0,1): hold I2Cx smbus when core is halted 102 | \arg DBG_TIMERx_HOLD (x=0,1,2,3,4,5,6,7,8,9,10,11,12,13,TIMER8..13 are not available for HD series): hold TIMERx counter when core is halted 103 | \param[out] none 104 | \retval none 105 | */ 106 | void dbg_periph_enable(dbg_periph_enum dbg_periph) 107 | { 108 | DBG_REG_VAL(dbg_periph) |= BIT(DBG_BIT_POS(dbg_periph)); 109 | } 110 | 111 | /*! 112 | \brief disable peripheral behavior when the mcu is in debug mode 113 | \param[in] dbg_periph: refer to dbg_periph_enum 114 | only one parameter can be selected which is shown as below: 115 | \arg DBG_FWDGT_HOLD : debug FWDGT kept when core is halted 116 | \arg DBG_WWDGT_HOLD : debug WWDGT kept when core is halted 117 | \arg DBG_CANx_HOLD (x=0,1,CAN1 is only available for CL series): hold CAN0 counter when core is halted 118 | \arg DBG_I2Cx_HOLD (x=0,1): hold I2Cx smbus when core is halted 119 | \arg DBG_TIMERx_HOLD (x=0,1,2,3,4,5,6,7,8,9,10,11,12,13,TIMER8..13 are not available for HD series): hold TIMERx counter when core is halted 120 | \param[out] none 121 | \retval none 122 | */ 123 | void dbg_periph_disable(dbg_periph_enum dbg_periph) 124 | { 125 | DBG_REG_VAL(dbg_periph) &= ~BIT(DBG_BIT_POS(dbg_periph)); 126 | } 127 | 128 | /*! 129 | \brief enable trace pin assignment 130 | \param[in] none 131 | \param[out] none 132 | \retval none 133 | */ 134 | void dbg_trace_pin_enable(void) 135 | { 136 | DBG_CTL0 |= DBG_CTL0_TRACE_IOEN; 137 | } 138 | 139 | /*! 140 | \brief disable trace pin assignment 141 | \param[in] none 142 | \param[out] none 143 | \retval none 144 | */ 145 | void dbg_trace_pin_disable(void) 146 | { 147 | DBG_CTL0 &= ~DBG_CTL0_TRACE_IOEN; 148 | } 149 | 150 | /*! 151 | \brief trace pin mode selection 152 | \param[in] trace_mode: 153 | \arg TRACE_MODE_ASYNC: trace pin used for async mode 154 | \arg TRACE_MODE_SYNC_DATASIZE_1: trace pin used for sync mode and data size is 1 155 | \arg TRACE_MODE_SYNC_DATASIZE_2: trace pin used for sync mode and data size is 2 156 | \arg TRACE_MODE_SYNC_DATASIZE_4: trace pin used for sync mode and data size is 4 157 | \param[out] none 158 | \retval none 159 | */ 160 | void dbg_trace_pin_mode_set(uint32_t trace_mode) 161 | { 162 | DBG_CTL0 &= ~DBG_CTL0_TRACE_MODE; 163 | DBG_CTL0 |= trace_mode; 164 | } 165 | -------------------------------------------------------------------------------- /STM32F446RETX_RAM.ld: -------------------------------------------------------------------------------- 1 | /* 2 | ****************************************************************************** 3 | ** 4 | ** File : LinkerScript.ld (debug in RAM dedicated) 5 | ** 6 | ** Author : Auto-generated by STM32CubeIDE 7 | ** 8 | ** Abstract : Linker script for NUCLEO-F446RE Board embedding STM32F446RETx Device from stm32f4 series 9 | ** 512Kbytes FLASH 10 | ** 128Kbytes RAM 11 | ** 12 | ** Set heap size, stack size and stack location according 13 | ** to application requirements. 14 | ** 15 | ** Set memory bank area and size if external memory is used. 16 | ** 17 | ** Target : STMicroelectronics STM32 18 | ** 19 | ** Distribution: The file is distributed as is without any warranty 20 | ** of any kind. 21 | ** 22 | ***************************************************************************** 23 | ** @attention 24 | ** 25 | **

© COPYRIGHT(c) 2020 STMicroelectronics

26 | ** 27 | ** Redistribution and use in source and binary forms, with or without modification, 28 | ** are permitted provided that the following conditions are met: 29 | ** 1. Redistributions of source code must retain the above copyright notice, 30 | ** this list of conditions and the following disclaimer. 31 | ** 2. Redistributions in binary form must reproduce the above copyright notice, 32 | ** this list of conditions and the following disclaimer in the documentation 33 | ** and/or other materials provided with the distribution. 34 | ** 3. Neither the name of STMicroelectronics nor the names of its contributors 35 | ** may be used to endorse or promote products derived from this software 36 | ** without specific prior written permission. 37 | ** 38 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 39 | ** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 40 | ** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 41 | ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 42 | ** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 43 | ** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 44 | ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 45 | ** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 46 | ** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 47 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 48 | ** 49 | ***************************************************************************** 50 | */ 51 | 52 | /* Entry Point */ 53 | ENTRY(Reset_Handler) 54 | 55 | /* Highest address of the user mode stack */ 56 | _estack = ORIGIN(RAM) + LENGTH(RAM); /* end of "RAM" Ram type memory */ 57 | 58 | _Min_Heap_Size = 0x200; /* required amount of heap */ 59 | _Min_Stack_Size = 0x400; /* required amount of stack */ 60 | 61 | /* Memories definition */ 62 | MEMORY 63 | { 64 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K 65 | FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 512K 66 | } 67 | 68 | /* Sections */ 69 | SECTIONS 70 | { 71 | /* The startup code into "RAM" Ram type memory */ 72 | .isr_vector : 73 | { 74 | . = ALIGN(4); 75 | KEEP(*(.isr_vector)) /* Startup code */ 76 | . = ALIGN(4); 77 | } >RAM 78 | 79 | /* The program code and other data into "RAM" Ram type memory */ 80 | .text : 81 | { 82 | . = ALIGN(4); 83 | *(.text) /* .text sections (code) */ 84 | *(.text*) /* .text* sections (code) */ 85 | *(.glue_7) /* glue arm to thumb code */ 86 | *(.glue_7t) /* glue thumb to arm code */ 87 | *(.eh_frame) 88 | 89 | KEEP (*(.init)) 90 | KEEP (*(.fini)) 91 | 92 | . = ALIGN(4); 93 | _etext = .; /* define a global symbols at end of code */ 94 | } >RAM 95 | 96 | /* Constant data into "RAM" Ram type memory */ 97 | .rodata : 98 | { 99 | . = ALIGN(4); 100 | *(.rodata) /* .rodata sections (constants, strings, etc.) */ 101 | *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ 102 | . = ALIGN(4); 103 | } >RAM 104 | 105 | .ARM.extab : { 106 | . = ALIGN(4); 107 | *(.ARM.extab* .gnu.linkonce.armextab.*) 108 | . = ALIGN(4); 109 | } >RAM 110 | 111 | .ARM : { 112 | . = ALIGN(4); 113 | __exidx_start = .; 114 | *(.ARM.exidx*) 115 | __exidx_end = .; 116 | . = ALIGN(4); 117 | } >RAM 118 | 119 | .preinit_array : 120 | { 121 | . = ALIGN(4); 122 | PROVIDE_HIDDEN (__preinit_array_start = .); 123 | KEEP (*(.preinit_array*)) 124 | PROVIDE_HIDDEN (__preinit_array_end = .); 125 | . = ALIGN(4); 126 | } >RAM 127 | 128 | .init_array : 129 | { 130 | . = ALIGN(4); 131 | PROVIDE_HIDDEN (__init_array_start = .); 132 | KEEP (*(SORT(.init_array.*))) 133 | KEEP (*(.init_array*)) 134 | PROVIDE_HIDDEN (__init_array_end = .); 135 | . = ALIGN(4); 136 | } >RAM 137 | 138 | .fini_array : 139 | { 140 | . = ALIGN(4); 141 | PROVIDE_HIDDEN (__fini_array_start = .); 142 | KEEP (*(SORT(.fini_array.*))) 143 | KEEP (*(.fini_array*)) 144 | PROVIDE_HIDDEN (__fini_array_end = .); 145 | . = ALIGN(4); 146 | } >RAM 147 | 148 | /* Used by the startup to initialize data */ 149 | _sidata = LOADADDR(.data); 150 | 151 | /* Initialized data sections into "RAM" Ram type memory */ 152 | .data : 153 | { 154 | . = ALIGN(4); 155 | _sdata = .; /* create a global symbol at data start */ 156 | *(.data) /* .data sections */ 157 | *(.data*) /* .data* sections */ 158 | 159 | . = ALIGN(4); 160 | _edata = .; /* define a global symbol at data end */ 161 | 162 | } >RAM 163 | 164 | /* Uninitialized data section into "RAM" Ram type memory */ 165 | . = ALIGN(4); 166 | .bss : 167 | { 168 | /* This is used by the startup in order to initialize the .bss section */ 169 | _sbss = .; /* define a global symbol at bss start */ 170 | __bss_start__ = _sbss; 171 | *(.bss) 172 | *(.bss*) 173 | *(COMMON) 174 | 175 | . = ALIGN(4); 176 | _ebss = .; /* define a global symbol at bss end */ 177 | __bss_end__ = _ebss; 178 | } >RAM 179 | 180 | /* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */ 181 | ._user_heap_stack : 182 | { 183 | . = ALIGN(8); 184 | PROVIDE ( end = . ); 185 | PROVIDE ( _end = . ); 186 | . = . + _Min_Heap_Size; 187 | . = . + _Min_Stack_Size; 188 | . = ALIGN(8); 189 | } >RAM 190 | 191 | /* Remove information from the compiler libraries */ 192 | /DISCARD/ : 193 | { 194 | libc.a ( * ) 195 | libm.a ( * ) 196 | libgcc.a ( * ) 197 | } 198 | 199 | .ARM.attributes 0 : { *(.ARM.attributes) } 200 | } 201 | -------------------------------------------------------------------------------- /STM32F446RETX_FLASH.ld: -------------------------------------------------------------------------------- 1 | /* 2 | ****************************************************************************** 3 | ** 4 | ** File : LinkerScript.ld 5 | ** 6 | ** Author : Auto-generated by STM32CubeIDE 7 | ** 8 | ** Abstract : Linker script for NUCLEO-F446RE Board embedding STM32F446RETx Device from stm32f4 series 9 | ** 512Kbytes FLASH 10 | ** 128Kbytes RAM 11 | ** 12 | ** Set heap size, stack size and stack location according 13 | ** to application requirements. 14 | ** 15 | ** Set memory bank area and size if external memory is used. 16 | ** 17 | ** Target : STMicroelectronics STM32 18 | ** 19 | ** Distribution: The file is distributed as is without any warranty 20 | ** of any kind. 21 | ** 22 | ***************************************************************************** 23 | ** @attention 24 | ** 25 | **

© COPYRIGHT(c) 2020 STMicroelectronics

26 | ** 27 | ** Redistribution and use in source and binary forms, with or without modification, 28 | ** are permitted provided that the following conditions are met: 29 | ** 1. Redistributions of source code must retain the above copyright notice, 30 | ** this list of conditions and the following disclaimer. 31 | ** 2. Redistributions in binary form must reproduce the above copyright notice, 32 | ** this list of conditions and the following disclaimer in the documentation 33 | ** and/or other materials provided with the distribution. 34 | ** 3. Neither the name of STMicroelectronics nor the names of its contributors 35 | ** may be used to endorse or promote products derived from this software 36 | ** without specific prior written permission. 37 | ** 38 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 39 | ** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 40 | ** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 41 | ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 42 | ** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 43 | ** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 44 | ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 45 | ** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 46 | ** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 47 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 48 | ** 49 | ***************************************************************************** 50 | */ 51 | 52 | /* Entry Point */ 53 | ENTRY(Reset_Handler) 54 | 55 | /* Highest address of the user mode stack */ 56 | _estack = ORIGIN(RAM) + LENGTH(RAM); /* end of "RAM" Ram type memory */ 57 | 58 | _Min_Heap_Size = 0x200 ; /* required amount of heap */ 59 | _Min_Stack_Size = 0x400 ; /* required amount of stack */ 60 | 61 | /* Memories definition */ 62 | MEMORY 63 | { 64 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K 65 | FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 512K 66 | } 67 | 68 | /* Sections */ 69 | SECTIONS 70 | { 71 | /* The startup code into "FLASH" Rom type memory */ 72 | .isr_vector : 73 | { 74 | . = ALIGN(4); 75 | KEEP(*(.isr_vector)) /* Startup code */ 76 | . = ALIGN(4); 77 | } >FLASH 78 | 79 | /* The program code and other data into "FLASH" Rom type memory */ 80 | .text : 81 | { 82 | . = ALIGN(4); 83 | *(.text) /* .text sections (code) */ 84 | *(.text*) /* .text* sections (code) */ 85 | *(.glue_7) /* glue arm to thumb code */ 86 | *(.glue_7t) /* glue thumb to arm code */ 87 | *(.eh_frame) 88 | 89 | KEEP (*(.init)) 90 | KEEP (*(.fini)) 91 | 92 | . = ALIGN(4); 93 | _etext = .; /* define a global symbols at end of code */ 94 | } >FLASH 95 | 96 | /* Constant data into "FLASH" Rom type memory */ 97 | .rodata : 98 | { 99 | . = ALIGN(4); 100 | *(.rodata) /* .rodata sections (constants, strings, etc.) */ 101 | *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ 102 | . = ALIGN(4); 103 | } >FLASH 104 | 105 | .ARM.extab : { 106 | . = ALIGN(4); 107 | *(.ARM.extab* .gnu.linkonce.armextab.*) 108 | . = ALIGN(4); 109 | } >FLASH 110 | 111 | .ARM : { 112 | . = ALIGN(4); 113 | __exidx_start = .; 114 | *(.ARM.exidx*) 115 | __exidx_end = .; 116 | . = ALIGN(4); 117 | } >FLASH 118 | 119 | .preinit_array : 120 | { 121 | . = ALIGN(4); 122 | PROVIDE_HIDDEN (__preinit_array_start = .); 123 | KEEP (*(.preinit_array*)) 124 | PROVIDE_HIDDEN (__preinit_array_end = .); 125 | . = ALIGN(4); 126 | } >FLASH 127 | 128 | .init_array : 129 | { 130 | . = ALIGN(4); 131 | PROVIDE_HIDDEN (__init_array_start = .); 132 | KEEP (*(SORT(.init_array.*))) 133 | KEEP (*(.init_array*)) 134 | PROVIDE_HIDDEN (__init_array_end = .); 135 | . = ALIGN(4); 136 | } >FLASH 137 | 138 | .fini_array : 139 | { 140 | . = ALIGN(4); 141 | PROVIDE_HIDDEN (__fini_array_start = .); 142 | KEEP (*(SORT(.fini_array.*))) 143 | KEEP (*(.fini_array*)) 144 | PROVIDE_HIDDEN (__fini_array_end = .); 145 | . = ALIGN(4); 146 | } >FLASH 147 | 148 | /* Used by the startup to initialize data */ 149 | _sidata = LOADADDR(.data); 150 | 151 | /* Initialized data sections into "RAM" Ram type memory */ 152 | .data : 153 | { 154 | . = ALIGN(4); 155 | _sdata = .; /* create a global symbol at data start */ 156 | *(.data) /* .data sections */ 157 | *(.data*) /* .data* sections */ 158 | 159 | . = ALIGN(4); 160 | _edata = .; /* define a global symbol at data end */ 161 | 162 | } >RAM AT> FLASH 163 | 164 | /* Uninitialized data section into "RAM" Ram type memory */ 165 | . = ALIGN(4); 166 | .bss : 167 | { 168 | /* This is used by the startup in order to initialize the .bss section */ 169 | _sbss = .; /* define a global symbol at bss start */ 170 | __bss_start__ = _sbss; 171 | *(.bss) 172 | *(.bss*) 173 | *(COMMON) 174 | 175 | . = ALIGN(4); 176 | _ebss = .; /* define a global symbol at bss end */ 177 | __bss_end__ = _ebss; 178 | } >RAM 179 | 180 | /* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */ 181 | ._user_heap_stack : 182 | { 183 | . = ALIGN(8); 184 | PROVIDE ( end = . ); 185 | PROVIDE ( _end = . ); 186 | . = . + _Min_Heap_Size; 187 | . = . + _Min_Stack_Size; 188 | . = ALIGN(8); 189 | } >RAM 190 | 191 | /* Remove information from the compiler libraries */ 192 | /DISCARD/ : 193 | { 194 | libc.a ( * ) 195 | libm.a ( * ) 196 | libgcc.a ( * ) 197 | } 198 | 199 | .ARM.attributes 0 : { *(.ARM.attributes) } 200 | } 201 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | ########################################################################################################################## 2 | # File automatically-generated by tool: [projectgenerator] version: [3.17.1] date: [Mon Jul 11 22:45:14 CST 2022] 3 | ########################################################################################################################## 4 | 5 | # ------------------------------------------------ 6 | # Generic Makefile (based on gcc) 7 | # 8 | # ChangeLog : 9 | # 2017-02-10 - Several enhancements + project update mode 10 | # 2015-07-22 - first version 11 | # ------------------------------------------------ 12 | 13 | ###################################### 14 | # target 15 | ###################################### 16 | TARGET = motorcontrol 17 | 18 | 19 | ###################################### 20 | # building variables 21 | ###################################### 22 | # debug build? 23 | # DEBUG = 1 24 | # optimization 25 | # OPT = -Og 26 | OPT = -O2 27 | OPT += -fsingle-precision-constant 28 | 29 | 30 | ####################################### 31 | # paths 32 | ####################################### 33 | # Build path 34 | BUILD_DIR = build 35 | 36 | ###################################### 37 | # source 38 | ###################################### 39 | # C sources 40 | C_SOURCES = \ 41 | Core/Src/main.c \ 42 | Core/Src/gpio.c \ 43 | Core/Src/adc.c \ 44 | Core/Src/can.c \ 45 | Core/Src/spi.c \ 46 | Core/Src/tim.c \ 47 | Core/Src/usart.c \ 48 | Core/Src/calibration.c \ 49 | Core/Src/drv8323.c \ 50 | Core/Src/flash_writer.c \ 51 | Core/Src/foc.c \ 52 | Core/Src/fsm.c \ 53 | Core/Src/math_ops.c \ 54 | Core/Src/position_sensor.c \ 55 | Core/Src/preference_writer.c \ 56 | Core/Src/syscalls.c \ 57 | Core/Src/sysmem.c \ 58 | Core/Src/systick.c \ 59 | Core/Src/gd32f30x_it.c \ 60 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_adc.c \ 61 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_bkp.c \ 62 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_can.c \ 63 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_crc.c \ 64 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_ctc.c \ 65 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_dac.c \ 66 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_dbg.c \ 67 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_dma.c \ 68 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_enet.c \ 69 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_exmc.c \ 70 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_exti.c \ 71 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_fmc.c \ 72 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_fwdgt.c \ 73 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_gpio.c \ 74 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_i2c.c \ 75 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_misc.c \ 76 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_pmu.c \ 77 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_rcu.c \ 78 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_rtc.c \ 79 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_sdio.c \ 80 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_spi.c \ 81 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_timer.c \ 82 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_usart.c \ 83 | Firmware/GD32F30x_standard_peripheral/Source/gd32f30x_wwdgt.c \ 84 | Firmware/CMSIS/GD/GD32F30x/Source/system_gd32f30x.c 85 | 86 | # ASM sources 87 | ASM_SOURCES = \ 88 | startup_gd32f30x_hd.s 89 | 90 | 91 | ####################################### 92 | # binaries 93 | ####################################### 94 | PREFIX = arm-none-eabi- 95 | # The gcc compiler bin path can be either defined in make command via GCC_PATH variable (> make GCC_PATH=xxx) 96 | # either it can be added to the PATH environment variable. 97 | ifdef GCC_PATH 98 | CC = $(GCC_PATH)/$(PREFIX)gcc 99 | AS = $(GCC_PATH)/$(PREFIX)gcc -x assembler-with-cpp 100 | CP = $(GCC_PATH)/$(PREFIX)objcopy 101 | SZ = $(GCC_PATH)/$(PREFIX)size 102 | else 103 | CC = $(PREFIX)gcc 104 | AS = $(PREFIX)gcc -x assembler-with-cpp 105 | CP = $(PREFIX)objcopy 106 | SZ = $(PREFIX)size 107 | endif 108 | HEX = $(CP) -O ihex 109 | BIN = $(CP) -O binary -S 110 | 111 | ####################################### 112 | # CFLAGS 113 | ####################################### 114 | # cpu 115 | CPU = -mcpu=cortex-m4 116 | 117 | # fpu 118 | FPU = -mfpu=fpv4-sp-d16 119 | 120 | # float-abi 121 | FLOAT-ABI = -mfloat-abi=hard 122 | 123 | # mcu 124 | MCU = $(CPU) -mthumb $(FPU) $(FLOAT-ABI) 125 | 126 | # macros for gcc 127 | # AS defines 128 | AS_DEFS = 129 | 130 | # C defines 131 | C_DEFS = \ 132 | -DGD32F30X_HD 133 | 134 | 135 | # AS includes 136 | AS_INCLUDES = 137 | 138 | # C includes 139 | C_INCLUDES = \ 140 | -ICore/Inc \ 141 | -IFirmware/GD32F30x_standard_peripheral/Include \ 142 | -IFirmware/CMSIS/GD/GD32F30x/Include \ 143 | -IFirmware/CMSIS 144 | 145 | 146 | # compile gcc flags 147 | ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections 148 | 149 | CFLAGS += $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections 150 | 151 | ifeq ($(DEBUG), 1) 152 | CFLAGS += -g -gdwarf-2 153 | endif 154 | 155 | 156 | # Generate dependency information 157 | CFLAGS += -MMD -MP -MF"$(@:%.o=%.d)" 158 | 159 | 160 | ####################################### 161 | # LDFLAGS 162 | ####################################### 163 | # link script 164 | LDSCRIPT = GD32F303RETx_FLASH.ld 165 | 166 | # libraries 167 | LIBS = -lc -lm -lnosys 168 | LIBDIR = 169 | LDFLAGS = $(MCU) -specs=nano.specs -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections 170 | 171 | # printf support float, 6Kbytes 172 | LDFLAGS += -u _printf_float 173 | 174 | # default action: build all 175 | all: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).hex $(BUILD_DIR)/$(TARGET).bin 176 | 177 | 178 | ####################################### 179 | # build the application 180 | ####################################### 181 | # list of objects 182 | OBJECTS = $(addprefix $(BUILD_DIR)/,$(notdir $(C_SOURCES:.c=.o))) 183 | vpath %.c $(sort $(dir $(C_SOURCES))) 184 | # list of ASM program objects 185 | OBJECTS += $(addprefix $(BUILD_DIR)/,$(notdir $(ASM_SOURCES:.s=.o))) 186 | vpath %.s $(sort $(dir $(ASM_SOURCES))) 187 | 188 | $(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR) 189 | $(CC) -c $(CFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.c=.lst)) $< -o $@ 190 | 191 | $(BUILD_DIR)/%.o: %.s Makefile | $(BUILD_DIR) 192 | $(AS) -c $(CFLAGS) $< -o $@ 193 | 194 | $(BUILD_DIR)/$(TARGET).elf: $(OBJECTS) Makefile 195 | $(CC) $(OBJECTS) $(LDFLAGS) -o $@ 196 | $(SZ) $@ 197 | 198 | $(BUILD_DIR)/%.hex: $(BUILD_DIR)/%.elf | $(BUILD_DIR) 199 | $(HEX) $< $@ 200 | 201 | $(BUILD_DIR)/%.bin: $(BUILD_DIR)/%.elf | $(BUILD_DIR) 202 | $(BIN) $< $@ 203 | 204 | $(BUILD_DIR): 205 | mkdir $@ 206 | 207 | ####################################### 208 | # clean up 209 | ####################################### 210 | clean: 211 | -rm -fR $(BUILD_DIR) 212 | 213 | ####################################### 214 | # dependencies 215 | ####################################### 216 | -include $(wildcard $(BUILD_DIR)/*.d) 217 | 218 | # *** EOF *** -------------------------------------------------------------------------------- /Firmware/GD32F30x_standard_peripheral/Include/gd32f30x_rtc.h: -------------------------------------------------------------------------------- 1 | /*! 2 | \file gd32f30x_rtc.h 3 | \brief definitions for the RTC 4 | 5 | \version 2017-02-10, V1.0.0, firmware for GD32F30x 6 | \version 2018-10-10, V1.1.0, firmware for GD32F30x 7 | \version 2018-12-25, V2.0.0, firmware for GD32F30x 8 | \version 2020-09-30, V2.1.0, firmware for GD32F30x 9 | */ 10 | 11 | /* 12 | Copyright (c) 2020, GigaDevice Semiconductor Inc. 13 | 14 | Redistribution and use in source and binary forms, with or without modification, 15 | are permitted provided that the following conditions are met: 16 | 17 | 1. Redistributions of source code must retain the above copyright notice, this 18 | list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder nor the names of its contributors 23 | may be used to endorse or promote products derived from this software without 24 | specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 29 | IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 30 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 31 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 32 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 35 | OF SUCH DAMAGE. 36 | */ 37 | 38 | 39 | #ifndef GD32F30X_RTC_H 40 | #define GD32F30X_RTC_H 41 | 42 | #include "gd32f30x.h" 43 | 44 | /* RTC definitions */ 45 | #define RTC RTC_BASE 46 | 47 | /* registers definitions */ 48 | #define RTC_INTEN REG32(RTC + 0x00U) /*!< interrupt enable register */ 49 | #define RTC_CTL REG32(RTC + 0x04U) /*!< control register */ 50 | #define RTC_PSCH REG32(RTC + 0x08U) /*!< prescaler high register */ 51 | #define RTC_PSCL REG32(RTC + 0x0CU) /*!< prescaler low register */ 52 | #define RTC_DIVH REG32(RTC + 0x10U) /*!< divider high register */ 53 | #define RTC_DIVL REG32(RTC + 0x14U) /*!< divider low register */ 54 | #define RTC_CNTH REG32(RTC + 0x18U) /*!< counter high register */ 55 | #define RTC_CNTL REG32(RTC + 0x1CU) /*!< counter low register */ 56 | #define RTC_ALRMH REG32(RTC + 0x20U) /*!< alarm high register */ 57 | #define RTC_ALRML REG32(RTC + 0x24U) /*!< alarm low register */ 58 | 59 | /* bits definitions */ 60 | /* RTC_INTEN */ 61 | #define RTC_INTEN_SCIE BIT(0) /*!< second interrupt enable */ 62 | #define RTC_INTEN_ALRMIE BIT(1) /*!< alarm interrupt enable */ 63 | #define RTC_INTEN_OVIE BIT(2) /*!< overflow interrupt enable */ 64 | 65 | /* RTC_CTL */ 66 | #define RTC_CTL_SCIF BIT(0) /*!< second interrupt flag */ 67 | #define RTC_CTL_ALRMIF BIT(1) /*!< alarm interrupt flag */ 68 | #define RTC_CTL_OVIF BIT(2) /*!< overflow interrupt flag */ 69 | #define RTC_CTL_RSYNF BIT(3) /*!< registers synchronized flag */ 70 | #define RTC_CTL_CMF BIT(4) /*!< configuration mode flag */ 71 | #define RTC_CTL_LWOFF BIT(5) /*!< last write operation finished flag */ 72 | 73 | /* RTC_PSC */ 74 | #define RTC_PSCH_PSC BITS(0, 3) /*!< prescaler high value */ 75 | #define RTC_PSCL_PSC BITS(0, 15) /*!< prescaler low value */ 76 | 77 | /* RTC_DIV */ 78 | #define RTC_DIVH_DIV BITS(0, 3) /*!< divider high value */ 79 | #define RTC_DIVL_DIV BITS(0, 15) /*!< divider low value */ 80 | 81 | /* RTC_CNT */ 82 | #define RTC_CNTH_CNT BITS(0, 15) /*!< counter high value */ 83 | #define RTC_CNTL_CNT BITS(0, 15) /*!< counter low value */ 84 | 85 | /* RTC_ALRM */ 86 | #define RTC_ALRMH_ALRM BITS(0, 15) /*!< alarm high value */ 87 | #define RTC_ALRML_ALRM BITS(0, 15) /*!< alarm low value */ 88 | 89 | /* constants definitions */ 90 | #define RTC_HIGH_VALUE 0x000F0000U /*!< RTC high value */ 91 | #define RTC_LOW_VALUE 0x0000FFFFU /*!< RTC low value */ 92 | 93 | /* RTC interrupt enable or disable definitions */ 94 | #define RTC_INT_SECOND RTC_INTEN_SCIE /*!< second interrupt enable */ 95 | #define RTC_INT_ALARM RTC_INTEN_ALRMIE /*!< alarm interrupt enable */ 96 | #define RTC_INT_OVERFLOW RTC_INTEN_OVIE /*!< overflow interrupt enable */ 97 | 98 | /* RTC flag definitions */ 99 | #define RTC_FLAG_SECOND RTC_CTL_SCIF /*!< second interrupt flag */ 100 | #define RTC_FLAG_ALARM RTC_CTL_ALRMIF /*!< alarm interrupt flag */ 101 | #define RTC_FLAG_OVERFLOW RTC_CTL_OVIF /*!< overflow interrupt flag */ 102 | #define RTC_FLAG_RSYN RTC_CTL_RSYNF /*!< registers synchronized flag */ 103 | #define RTC_FLAG_LWOF RTC_CTL_LWOFF /*!< last write operation finished flag */ 104 | 105 | /* function declarations */ 106 | /* enable RTC interrupt */ 107 | void rtc_interrupt_enable(uint32_t interrupt); 108 | /* disable RTC interrupt */ 109 | void rtc_interrupt_disable(uint32_t interrupt); 110 | 111 | /* enter RTC configuration mode */ 112 | void rtc_configuration_mode_enter(void); 113 | /* exit RTC configuration mode */ 114 | void rtc_configuration_mode_exit(void); 115 | 116 | /* wait RTC last write operation finished flag set */ 117 | void rtc_lwoff_wait(void); 118 | /* wait RTC registers synchronized flag set */ 119 | void rtc_register_sync_wait(void); 120 | 121 | /* get RTC counter value */ 122 | uint32_t rtc_counter_get(void); 123 | /* set RTC counter value */ 124 | void rtc_counter_set(uint32_t cnt); 125 | 126 | /* set RTC prescaler value */ 127 | void rtc_prescaler_set(uint32_t psc); 128 | /* set RTC alarm value */ 129 | void rtc_alarm_config(uint32_t alarm); 130 | /* get RTC divider value */ 131 | uint32_t rtc_divider_get(void); 132 | 133 | /* get RTC flag status */ 134 | FlagStatus rtc_flag_get(uint32_t flag); 135 | /* clear RTC flag status */ 136 | void rtc_flag_clear(uint32_t flag); 137 | 138 | #endif /* GD32F30X_RTC_H */ 139 | -------------------------------------------------------------------------------- /Core/Src/position_sensor.c: -------------------------------------------------------------------------------- 1 | /* 2 | * position_sensor.c 3 | * 4 | * Created on: Jul 26, 2020 5 | * Author: Ben 6 | */ 7 | #include 8 | #include 9 | #include "position_sensor.h" 10 | #include "math_ops.h" 11 | #include "hw_config.h" 12 | #include "user_config.h" 13 | 14 | #define PARD_BIT 0x8000 15 | #define READ_BIT 0x4000 16 | #define READ_NOP (0x0000 | READ_BIT | PARD_BIT) 17 | #define READ_DIAAGC (0x3FFC | READ_BIT | PARD_BIT) 18 | #define READ_MAG (0x3FFD | READ_BIT) 19 | #define READ_ANGLEUNC (0x3FFE | READ_BIT) 20 | #define READ_ANGLECOM (0x3FFF | READ_BIT | PARD_BIT) 21 | 22 | void ps_warmup(EncoderStruct * encoder, int n){ 23 | #ifdef STM32F446 24 | /* Hall position sensors noisy on startup. Take a bunch of samples to clear this data */ 25 | for(int i = 0; ispi_tx_word = 0x0000; 27 | HAL_GPIO_WritePin(ENC_CS, GPIO_PIN_RESET ); // CS low 28 | HAL_SPI_TransmitReceive(&ENC_SPI, (uint8_t*)encoder->spi_tx_buff, (uint8_t *)encoder->spi_rx_buff, 1, 100); 29 | while( ENC_SPI.State == HAL_SPI_STATE_BUSY ); // wait for transmission complete 30 | HAL_GPIO_WritePin(ENC_CS, GPIO_PIN_SET ); // CS high 31 | } 32 | #else 33 | 34 | for (int i = 0; i < n; i++) 35 | { 36 | encoder->spi_tx_word = READ_ANGLECOM; 37 | 38 | SPI_SET_NSS_LOW(ENC_CS); 39 | 40 | spi_transmit_receive(ENC_SPI, &encoder->spi_tx_word, &encoder->spi_rx_word); 41 | 42 | SPI_SET_NSS_HIGH(ENC_CS); 43 | 44 | #ifdef DEBUG_PS 45 | uint16_t raw = encoder->spi_rx_word & 0x3FFF; 46 | info("ANGLECOM: raw: %04x angle: %d\r\n", raw, (int)(360.0f * raw / 0x3FFF)); 47 | delay_1ms(200); 48 | #endif 49 | } 50 | 51 | #ifdef DEBUG_PS 52 | 53 | encoder->spi_tx_word = READ_DIAAGC; 54 | 55 | SPI_SET_NSS_LOW(ENC_CS); 56 | 57 | spi_transmit_receive(ENC_SPI, &encoder->spi_tx_word, &encoder->spi_rx_word); 58 | 59 | SPI_SET_NSS_HIGH(ENC_CS); 60 | 61 | info("DIAAGC: %04x\r\n", encoder->spi_rx_word); 62 | 63 | encoder->spi_tx_word = READ_MAG; 64 | 65 | SPI_SET_NSS_LOW(ENC_CS); 66 | 67 | spi_transmit_receive(ENC_SPI, &encoder->spi_tx_word, &encoder->spi_rx_word); 68 | 69 | SPI_SET_NSS_HIGH(ENC_CS); 70 | 71 | info("MAG: %04x\r\n", encoder->spi_rx_word); 72 | 73 | encoder->spi_tx_word = READ_ANGLEUNC; 74 | 75 | SPI_SET_NSS_LOW(ENC_CS); 76 | 77 | spi_transmit_receive(ENC_SPI, &encoder->spi_tx_word, &encoder->spi_rx_word); 78 | 79 | SPI_SET_NSS_HIGH(ENC_CS); 80 | 81 | info("ANGLEUNC: %04x\r\n", encoder->spi_rx_word); 82 | 83 | encoder->spi_tx_word = READ_ANGLECOM; 84 | 85 | SPI_SET_NSS_LOW(ENC_CS); 86 | 87 | spi_transmit_receive(ENC_SPI, &encoder->spi_tx_word, &encoder->spi_rx_word); 88 | 89 | SPI_SET_NSS_HIGH(ENC_CS); 90 | 91 | info("ANGLECOM: %04x\r\n", encoder->spi_rx_word); 92 | #endif 93 | 94 | #endif 95 | } 96 | 97 | void ps_sample(EncoderStruct * encoder, float dt){ 98 | /* updates EncoderStruct encoder with the latest sample 99 | * after elapsed time dt */ 100 | 101 | /* Shift around previous samples */ 102 | encoder->old_angle = encoder->angle_singleturn; 103 | for(int i = N_POS_SAMPLES-1; i>0; i--){encoder->angle_multiturn[i] = encoder->angle_multiturn[i-1];} 104 | //for(int i = N_POS_SAMPLES-1; i>0; i--){encoder->count_buff[i] = encoder->count_buff[i-1];} 105 | //memmove(&encoder->angle_multiturn[1], &encoder->angle_multiturn[0], (N_POS_SAMPLES-1)*sizeof(float)); // this is much slower for some reason 106 | 107 | /* SPI read/write */ 108 | #ifdef STM32F446 109 | encoder->spi_tx_word = ENC_READ_WORD; 110 | HAL_GPIO_WritePin(ENC_CS, GPIO_PIN_RESET ); // CS low 111 | HAL_SPI_TransmitReceive(&ENC_SPI, (uint8_t*)encoder->spi_tx_buff, (uint8_t *)encoder->spi_rx_buff, 1, 100); 112 | while( ENC_SPI.State == HAL_SPI_STATE_BUSY ); // wait for transmission complete 113 | HAL_GPIO_WritePin(ENC_CS, GPIO_PIN_SET ); // CS high 114 | encoder->raw = encoder ->spi_rx_word; 115 | #else 116 | encoder->spi_tx_word = READ_ANGLECOM; 117 | 118 | SPI_SET_NSS_LOW(ENC_CS); 119 | 120 | spi_transmit_receive(ENC_SPI, &encoder->spi_tx_word, &encoder->spi_rx_word); 121 | 122 | SPI_SET_NSS_HIGH(ENC_CS); 123 | 124 | encoder->raw = (encoder->spi_rx_word & 0x3FFF) << 2; 125 | #endif 126 | 127 | /* Linearization */ 128 | int off_1 = encoder->offset_lut[(encoder->raw)>>9]; // lookup table lower entry 129 | int off_2 = encoder->offset_lut[((encoder->raw>>9)+1)%128]; // lookup table higher entry 130 | int off_interp = off_1 + ((off_2 - off_1)*(encoder->raw - ((encoder->raw>>9)<<9))>>9); // Interpolate between lookup table entries 131 | encoder->count = encoder->raw + off_interp; 132 | 133 | 134 | /* Real angles in radians */ 135 | encoder->angle_singleturn = ((float)(encoder->count-M_ZERO))/((float)ENC_CPR); 136 | int int_angle = encoder->angle_singleturn; 137 | encoder->angle_singleturn = TWO_PI_F*(encoder->angle_singleturn - (float)int_angle); 138 | //encoder->angle_singleturn = TWO_PI_F*fmodf(((float)(encoder->count-M_ZERO))/((float)ENC_CPR), 1.0f); 139 | encoder->angle_singleturn = encoder->angle_singleturn<0 ? encoder->angle_singleturn + TWO_PI_F : encoder->angle_singleturn; 140 | 141 | encoder->elec_angle = (encoder->ppairs*(float)(encoder->count-E_ZERO))/((float)ENC_CPR); 142 | int_angle = (int)encoder->elec_angle; 143 | encoder->elec_angle = TWO_PI_F*(encoder->elec_angle - (float)int_angle); 144 | //encoder->elec_angle = TWO_PI_F*fmodf((encoder->ppairs*(float)(encoder->count-E_ZERO))/((float)ENC_CPR), 1.0f); 145 | encoder->elec_angle = encoder->elec_angle<0 ? encoder->elec_angle + TWO_PI_F : encoder->elec_angle; // Add 2*pi to negative numbers 146 | /* Rollover */ 147 | int rollover = 0; 148 | float angle_diff = encoder->angle_singleturn - encoder->old_angle; 149 | if(angle_diff > PI_F){rollover = -1;} 150 | else if(angle_diff < -PI_F){rollover = 1;} 151 | encoder->turns += rollover; 152 | if(!encoder->first_sample){ 153 | encoder->turns = 0; 154 | encoder->first_sample = 1; 155 | } 156 | 157 | 158 | 159 | /* Multi-turn position */ 160 | encoder->angle_multiturn[0] = encoder->angle_singleturn + TWO_PI_F*(float)encoder->turns; 161 | 162 | /* Velocity */ 163 | /* 164 | // Attempt at a moving least squares. Wasn't any better 165 | float m = (float)N_POS_SAMPLES; 166 | float w = 1.0f/m; 167 | float q = 12.0f/(m*m*m - m); 168 | float c1 = 0.0f; 169 | float ibar = (m - 1.0f)/2.0f; 170 | for(int i = 0; iangle_multiturn[i]*q*(i - ibar); 172 | } 173 | encoder->vel2 = -c1/dt; 174 | */ 175 | //encoder->velocity = vel2 176 | encoder->velocity = (encoder->angle_multiturn[0] - encoder->angle_multiturn[N_POS_SAMPLES-1])/(dt*(float)(N_POS_SAMPLES-1)); 177 | encoder->elec_velocity = encoder->ppairs*encoder->velocity; 178 | 179 | } 180 | 181 | void ps_print(EncoderStruct * encoder, int loop_count){ 182 | 183 | #define PS_PRINT_INTERVAL 10000 184 | static uint32_t ps_print_mark = 0; 185 | 186 | if (loop_count < ps_print_mark + PS_PRINT_INTERVAL) { 187 | return; 188 | } 189 | 190 | printf("Raw: %5d", encoder->raw); 191 | printf(" Linearized Count: %5d", encoder->count); 192 | printf(" Single Turn:% 4.3f", encoder->angle_singleturn); 193 | printf(" Multi Turn:% 5.3f", encoder->angle_multiturn[0]); 194 | printf(" Electrical:% 5.3f", encoder->elec_angle); 195 | printf(" Turns:% 2d\r\n", encoder->turns); 196 | 197 | ps_print_mark = loop_count; 198 | } 199 | --------------------------------------------------------------------------------