diff --git a/.cproject b/.cproject index ba1fd85f..82873701 100644 --- a/.cproject +++ b/.cproject @@ -1,11 +1,8 @@ - - - - - + + @@ -17,25 +14,30 @@ - - - - - - + + @@ -134,94 +135,102 @@ - - - - @@ -229,12 +238,12 @@ - + - + @@ -245,4 +254,4 @@ - + \ No newline at end of file diff --git a/.cproject_org b/.cproject_org new file mode 100644 index 00000000..ba1fd85f --- /dev/null +++ b/.cproject_org @@ -0,0 +1,248 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/.gitignore b/.gitignore index 4df66e40..610ce252 100644 --- a/.gitignore +++ b/.gitignore @@ -19,3 +19,4 @@ Debug /JavaConfigurator/dist/ /Release/ /build/ +experimental settings/ diff --git a/.project b/.project index c5202eac..55e85593 100644 --- a/.project +++ b/.project @@ -1,7 +1,7 @@ LishuiFOC_01 - + @@ -20,11 +20,8 @@ org.eclipse.cdt.core.cnature - org.eclipse.cdt.managedbuilder.core.managedBuildNature org.eclipse.cdt.managedbuilder.core.ScannerConfigNature - fr.ac6.mcu.ide.core.MCUProjectNature + com.st.stm32cube.ide.mcu.MCUSW4STM32ConvertedProjectNature + org.eclipse.cdt.managedbuilder.core.managedBuildNature - - - diff --git a/.project_org b/.project_org new file mode 100644 index 00000000..6e9dd54f --- /dev/null +++ b/.project_org @@ -0,0 +1,30 @@ + + + LishuiFOC_01 + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + fr.ac6.mcu.ide.core.MCUProjectNature + + + + + diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index d22f6583..13c76f97 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -1,25 +1,25 @@ - + - + - + - + - + \ No newline at end of file diff --git a/Inc/config.h b/Inc/config.h index 5915b922..36b73059 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -8,109 +8,72 @@ #ifndef CONFIG_H_ #define CONFIG_H_ #include "stdint.h" - -// System constants, don't touch! -#define DISPLAY_TYPE_DEBUG (1<<0) // For ASCII-Output in Debug mode); -#define DISPLAY_TYPE_KUNTENG (1<<1) // For Kunteng display -#define DISPLAY_TYPE_BAFANG_LCD (1<<2) // For 'Blaupunkt' Display of Prophete Entdecker -#define DISPLAY_TYPE_BAFANG_850_860 (1<<3) // Bafang 850/860. Can do 9k6 baud, also compatible with 1200 baud detection at startup -#define DISPLAY_TYPE_KINGMETER_618U (1<<4) // King-Meter 618U protocol ( J-LCD) -#define DISPLAY_TYPE_KINGMETER_901U (1<<5) // King-Meter 901U protocol (KM5s) -#define DISPLAY_TYPE_EBiCS (1<<6) // Protocol using the ANT+ LEV logic -#define DISPLAY_TYPE_NO2 (1<<7) // For China Protokoll "No_2" S866 display for example -#define DISPLAY_TYPE_BAFANG (DISPLAY_TYPE_BAFANG_LCD|DISPLAY_TYPE_BAFANG_850_860) -#define DISPLAY_TYPE_KINGMETER (DISPLAY_TYPE_KINGMETER_618U|DISPLAY_TYPE_KINGMETER_901U) +#define DISPLAY_TYPE_DEBUG (1 << 0) // For ASCII-Output in Debug mode); +#define DISPLAY_TYPE_KUNTENG (1 << 1) // For Kunteng display +#define DISPLAY_TYPE_BAFANG_LCD (1 << 2) // For 'Blaupunkt' Display of Prophete Entdecker +#define DISPLAY_TYPE_BAFANG_850_860 (1 << 3) // Bafang 850/860. Can do 9k6 baud, also compatible with 1200 baud detection at startup +#define DISPLAY_TYPE_KINGMETER_618U (1 << 4) // King-Meter 618U protocol ( J-LCD) +#define DISPLAY_TYPE_KINGMETER_901U (1 << 5) // King-Meter 901U protocol (KM5s) +#define DISPLAY_TYPE_EBiCS (1 << 6) // Protocol using the ANT+ LEV logic +#define DISPLAY_TYPE_NO2 (1 << 7) // For China Protokoll "No_2" S866 display for example +#define DISPLAY_TYPE_BAFANG (DISPLAY_TYPE_BAFANG_LCD | DISPLAY_TYPE_BAFANG_850_860) +#define DISPLAY_TYPE_KINGMETER (DISPLAY_TYPE_KINGMETER_618U | DISPLAY_TYPE_KINGMETER_901U) #define EXTERNAL 1 #define INTERNAL 0 -//---------------------------------------------------------------------- -// advanced setting, don't touch, if you don't know what you are doing! +// #define LEGALFLAG + #define TRIGGER_OFFSET_ADC 50 #define TRIGGER_DEFAULT 2020 #define _T 2028 #define CAL_BAT_V 256 #define CAL_V 25 -#define CAL_I 38LL<<8 +#define CAL_I 48LL << 8 #define INDUCTANCE 6LL #define RESISTANCE 40LL #define FLUX_LINKAGE 1200LL #define GAMMA 9LL -//#define FAST_LOOP_LOG -//#define DISABLE_DYNAMIC_ADC -//#define INDIVIDUAL_MODES -//#define SPEEDTHROTTLE -#define SIXSTEPTHRESHOLD 20000 -#define SPEED_PLL 0 //1 for using PLL, 0 for angle extrapolation -#define P_FACTOR_PLL 10 -#define I_FACTOR_PLL 10 - -//---------------------------------------------------------------------- -//Battery bar settings for Kunteng and Bafang Display -#define BATTERY_LEVEL_1 323000 -#define BATTERY_LEVEL_2 329000 -#define BATTERY_LEVEL_3 344000 -#define BATTERY_LEVEL_4 368000 -#define BATTERY_LEVEL_5 380000 - -//---------------------------------------------------------------------- -//PI-control factor settings -#define P_FACTOR_I_Q 50 -#define I_FACTOR_I_Q 2 -#define P_FACTOR_I_D 50 -#define I_FACTOR_I_D 2 -#define P_FACTOR_SPEED 1 +#define BATTERY_LEVEL_1 430000LL +#define BATTERY_LEVEL_2 450000LL +#define BATTERY_LEVEL_3 470000LL +#define BATTERY_LEVEL_4 490000LL +#define BATTERY_LEVEL_5 510000LL +#define P_FACTOR_I_Q 500 +#define I_FACTOR_I_Q 20 +#define P_FACTOR_I_D 500 +#define I_FACTOR_I_D 20 +#define P_FACTOR_PLL 5 +#define I_FACTOR_PLL 11 +#define P_FACTOR_SPEED 100 #define I_FACTOR_SPEED 10 - -//---------------------------------------------------------------------- -//PAS mode settings -//#define DIRDET -#define FRAC_HIGH 30 -#define FRAC_LOW 15 +#define SPDSHFT 0 +#define SPEEDFILTER 1 +#define SIXSTEPTHRESHOLD 20000 +#define TS_COEF 35000 #define PAS_TIMEOUT 3000 #define RAMP_END 1200 #define PAS_IMP_PER_TURN 32 - -//--------------------------------------------------------------------- -//Throttle settings -#define THROTTLE_OFFSET 1250 //only default value, throttle offset is set at startup automatically +#define FRAC_HIGH 30 +#define FRAC_LOW 15 +#define THROTTLE_OFFSET 1250 #define THROTTLE_MAX 2850 -#define THROTTLE_OVERRIDE - -//-------------------------------------------------------------------- -//Speed settings +#define PUSHASSIST_CURRENT 8000 #define WHEEL_CIRCUMFERENCE 2200 -#define GEAR_RATIO 11 //11 for BionX IGH3 -#define SPEEDLIMIT 25 -#define PULSES_PER_REVOLUTION 1 -#define SPEEDSOURCE INTERNAL -#define SPEEDFILTER 1 -#define SPDSHFT 0 - -//--------------------------------------------------------------------- -//power settings -#define PH_CURRENT_MAX 1200 -#define BATTERYCURRENT_MAX 14000 -#define REVERSE 1 //1 for normal direction, -1 for reverse -#define PUSHASSIST_CURRENT 300 -#define VOLTAGE_MIN 1320 //33V - -//--------------------------------------------------------------------- -//torquesensor settings -#define TS_COEF 2400 +#define GEAR_RATIO 88 +#define SPEEDLIMIT 99 +#define PULSES_PER_REVOLUTION 6 +#define PH_CURRENT_MAX 500 +#define BATTERYCURRENT_MAX 17000 +#define REGEN_CURRENT 20 +#define REGEN_CURRENT_MAX 5000 +#define VOLTAGE_MIN 1400 +#define VOLTAGE_MAX 2200 +#define SPEC_ANGLE -715827882 #define TS_MODE -//#define TQONAD1 - -//--------------------------------------------------------------------- -//Display settings -#define DISPLAY_TYPE DISPLAY_TYPE_KINGMETER_901U - -//--------------------------------------------------------------------- -//Regen settings - -#define REGEN_CURRENT 800 -#define REGEN_CURRENT_MAX 10000 -//#define ADC_BRAKE - -//--------------------------------------------------------------------- +#define TQONAD1 +#define DISPLAY_TYPE DISPLAY_TYPE_KUNTENG +#define SPEED_PLL 0 +#define SPEEDSOURCE EXTERNAL #define AUTODETECT 0 +#define REVERSE 1 #endif /* CONFIG_H_ */ diff --git a/Inc/display_kunteng.h b/Inc/display_kunteng.h index 66649686..d0b9b811 100644 --- a/Inc/display_kunteng.h +++ b/Inc/display_kunteng.h @@ -11,8 +11,8 @@ #include "config.h" void kunteng_init(); -void display_update(MotorState_t* MS_U); -void check_message(MotorState_t* MS_D, MotorParams_t* MP_D); +void display_update(MotorState_t *MS_U); +void check_message(MotorState_t *MS_D, MotorParams_t *MP_D); typedef struct _lcd_configuration_variables { @@ -37,7 +37,7 @@ typedef struct _lcd_configuration_variables uint8_t ui8_c14; } struc_lcd_configuration_variables; -#define COMMUNICATIONS_BATTERY_VOLTAGE (BATTERY_LI_ION_CELLS_NUMBER * 3.45) // example: 7S battery, should be = 24 +#define COMMUNICATIONS_BATTERY_VOLTAGE (BATTERY_LI_ION_CELLS_NUMBER * 3.45) // example: 7S battery, should be = 24 // Considering the follow voltage values for each li-ion battery cell // State of charge | voltage @@ -47,17 +47,17 @@ typedef struct _lcd_configuration_variables #define LI_ION_CELL_VOLTS_60 3.68 // 3.87 #define LI_ION_CELL_VOLTS_40 3.46 // 3.80 #define LI_ION_CELL_VOLTS_20 3.28 // 3.73 -#define LI_ION_CELL_VOLTS_0 3.10 // 3.27 +#define LI_ION_CELL_VOLTS_0 3.10 // 3.27 #define LI_ION_CELL_VOLTS_MIN 3.10 -#define BATTERY_PACK_VOLTS_100 (LI_ION_CELL_VOLTS_100 * BATTERY_LI_ION_CELLS_NUMBER) * 256 -#define BATTERY_PACK_VOLTS_80 (LI_ION_CELL_VOLTS_80 * BATTERY_LI_ION_CELLS_NUMBER) * 256 -#define BATTERY_PACK_VOLTS_60 (LI_ION_CELL_VOLTS_60 * BATTERY_LI_ION_CELLS_NUMBER) * 256 -#define BATTERY_PACK_VOLTS_40 (LI_ION_CELL_VOLTS_40 * BATTERY_LI_ION_CELLS_NUMBER) * 256 -#define BATTERY_PACK_VOLTS_20 (LI_ION_CELL_VOLTS_20 * BATTERY_LI_ION_CELLS_NUMBER) * 256 -#define BATTERY_PACK_VOLTS_0 (LI_ION_CELL_VOLTS_0 * BATTERY_LI_ION_CELLS_NUMBER) * 256 +#define BATTERY_PACK_VOLTS_100 (LI_ION_CELL_VOLTS_100 * BATTERY_LI_ION_CELLS_NUMBER) * 256 +#define BATTERY_PACK_VOLTS_80 (LI_ION_CELL_VOLTS_80 * BATTERY_LI_ION_CELLS_NUMBER) * 256 +#define BATTERY_PACK_VOLTS_60 (LI_ION_CELL_VOLTS_60 * BATTERY_LI_ION_CELLS_NUMBER) * 256 +#define BATTERY_PACK_VOLTS_40 (LI_ION_CELL_VOLTS_40 * BATTERY_LI_ION_CELLS_NUMBER) * 256 +#define BATTERY_PACK_VOLTS_20 (LI_ION_CELL_VOLTS_20 * BATTERY_LI_ION_CELLS_NUMBER) * 256 +#define BATTERY_PACK_VOLTS_0 (LI_ION_CELL_VOLTS_0 * BATTERY_LI_ION_CELLS_NUMBER) * 256 #define ADC_BATTERY_VOLTAGE_K 73 // 0.272 << 8 -#define BATTERY_LI_ION_CELLS_NUMBER 10 +#define BATTERY_LI_ION_CELLS_NUMBER 13 #endif /* DISPLAY_KUNTENG_H_ */ diff --git a/Inc/main.h b/Inc/main.h index 46dc0a5b..08fed625 100644 --- a/Inc/main.h +++ b/Inc/main.h @@ -1,41 +1,41 @@ /** - ****************************************************************************** - * @file : main.h - * @brief : Header for main.c file. - * This file contains the common defines of the application. - ****************************************************************************** - ** This notice applies to any and all portions of this file - * that are not between comment pairs USER CODE BEGIN and - * USER CODE END. Other portions of this file, whether - * inserted by the user or by software development tools - * are owned by their respective copyright owners. - * - * COPYRIGHT(c) 2019 STMicroelectronics - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file : main.h + * @brief : Header for main.c file. + * This file contains the common defines of the application. + ****************************************************************************** + ** This notice applies to any and all portions of this file + * that are not between comment pairs USER CODE BEGIN and + * USER CODE END. Other portions of this file, whether + * inserted by the user or by software development tools + * are owned by their respective copyright owners. + * + * COPYRIGHT(c) 2019 STMicroelectronics + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __MAIN_H__ @@ -46,7 +46,6 @@ /* USER CODE BEGIN Includes */ #include - /* USER CODE END Includes */ /* Private define ------------------------------------------------------------*/ @@ -78,7 +77,7 @@ #define BRAKE_LIGHT_GPIO_Port GPIOB #define PAS_Pin GPIO_PIN_8 #define PAS_GPIO_Port GPIOB -#define Brake_Pin GPIO_PIN_11 // put a 15 here for new generation controllers! +#define Brake_Pin GPIO_PIN_15 // put a 15 here for new generation controllers! #define Brake_GPIO_Port GPIOA #define Speed_EXTI5_Pin GPIO_PIN_5 #define Speed_EXTI5_GPIO_Port GPIOB @@ -87,13 +86,13 @@ #define PAS_EXTI8_GPIO_Port GPIOB #define PAS_EXTI8_EXTI_IRQn EXTI9_5_IRQn -//#define NCTE -//#define LEGALFLAG +// #define NCTE +// #define LEGALFLAG #define BATTERYVOLTAGE_MAX 53000 #define R_TEMP_PULLUP 0 #define INT_TEMP_25 0 #define USE_FIX_POSITIONS 0 -//Put values from the startup message after autodetect here, if you want to use fix positions. 32bit values for the hall angles! +// Put values from the startup message after autodetect here, if you want to use fix positions. 32bit values for the hall angles! #define KV 80 #define HALL_ORDER 1 #define HALL_45 2636578816 @@ -104,23 +103,20 @@ #define HALL_64 3209232384 #define CONTROLLER_TEMPERATURE_THRESHOLD 70 -#define CONTROLLER_TEMPERATURE_MAX 80 +#define CONTROLLER_TEMPERATURE_MAX 80 #define MOTOR_TEMPERATURE_THRESHOLD 100 -#define MOTOR_TEMPERATURE_MAX 130 +#define MOTOR_TEMPERATURE_MAX 130 /* ########################## Assert Selection ############################## */ /** - * @brief Uncomment the line below to expanse the "assert_param" macro in the - * HAL drivers code - */ + * @brief Uncomment the line below to expanse the "assert_param" macro in the + * HAL drivers code + */ /* #define USE_FULL_ASSERT 1U */ /* USER CODE BEGIN Private defines */ - - - -int32_t map (int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max); +int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max); void autodetect(); void runPIcontrol(); void kingmeter_update(void); @@ -136,77 +132,77 @@ extern void get_internal_temp_offset(void); typedef struct { - int16_t gain_p; - int16_t gain_i; - int16_t limit_i; - int16_t limit_output; - int16_t recent_value; - int32_t setpoint; - int32_t integral_part; - int16_t max_step; - int32_t out; - int8_t shift; - -}PI_control_t; + int16_t gain_p; + int16_t gain_i; + int16_t limit_i; + int16_t limit_output; + int16_t recent_value; + int32_t setpoint; + int32_t integral_part; + int16_t max_step; + int32_t out; + int8_t shift; + +} PI_control_t; typedef struct { - q31_t Voltage; - uint32_t Speed; - q31_t i_d; - q31_t i_q; - q31_t i_q_setpoint; - q31_t i_d_setpoint; - q31_t i_setpoint_abs; - int32_t i_q_setpoint_temp; - int32_t i_d_setpoint_temp; - q31_t u_d; - q31_t u_q; - q31_t u_abs; - q31_t Battery_Current; - uint8_t hall_angle_detect_flag; - uint8_t char_dyn_adc_state; - uint8_t assist_level; - uint8_t regen_level; - int16_t Temperature; - int16_t int_Temperature; - int8_t system_state; - int8_t gear_state; - int8_t error_state; - int8_t angle_est; - int16_t KV_detect_flag; - -}MotorState_t; + q31_t Voltage; + uint32_t Speed; + q31_t i_d; + q31_t i_q; + q31_t i_q_setpoint; + q31_t i_d_setpoint; + q31_t i_setpoint_abs; + int32_t i_q_setpoint_temp; + int32_t i_d_setpoint_temp; + q31_t u_d; + q31_t u_q; + q31_t u_abs; + q31_t Battery_Current; + uint8_t hall_angle_detect_flag; + uint8_t char_dyn_adc_state; + uint8_t assist_level; + uint8_t regen_level; + int16_t Temperature; + int16_t int_Temperature; + int8_t system_state; + int8_t gear_state; + int8_t error_state; + int8_t angle_est; + int16_t KV_detect_flag; + +} MotorState_t; typedef struct { - uint16_t wheel_cirumference; - uint16_t p_Iq; - uint16_t i_Iq; - uint16_t p_Id; - uint16_t i_Id; - uint16_t TS_coeff; - uint16_t PAS_timeout; - uint16_t ramp_end; - uint16_t throttle_offset; - uint16_t throttle_max; - uint16_t gear_ratio; - uint8_t speedLimit; - uint8_t pulses_per_revolution; - uint16_t phase_current_max; - int32_t battery_current_max; - - -}MotorParams_t; + uint16_t wheel_cirumference; + uint16_t p_Iq; + uint16_t i_Iq; + uint16_t p_Id; + uint16_t i_Id; + uint16_t TS_coeff; + uint16_t PAS_timeout; + uint16_t ramp_end; + uint16_t throttle_offset; + uint16_t throttle_max; + uint16_t gear_ratio; + uint8_t speedLimit; + uint8_t pulses_per_revolution; + uint16_t phase_current_max; + int32_t battery_current_max; + +} MotorParams_t; /* USER CODE END Private defines */ #ifdef __cplusplus - extern "C" { +extern "C" +{ #endif -void _Error_Handler(char *, int); + void _Error_Handler(char *, int); #define Error_Handler() _Error_Handler(__FILE__, __LINE__) #ifdef __cplusplus diff --git a/LishuiFOC_01 Debug.launch b/LishuiFOC_01 Debug.launch index 153440fe..391b8026 100644 --- a/LishuiFOC_01 Debug.launch +++ b/LishuiFOC_01 Debug.launch @@ -1,60 +1,96 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/LishuiFOC_01_converter.log b/LishuiFOC_01_converter.log new file mode 100644 index 00000000..b6cebc60 --- /dev/null +++ b/LishuiFOC_01_converter.log @@ -0,0 +1,21 @@ +Project: LishuiFOC_01 +Converter: ST System Workbench for STM32 project converter +Date: 20240715 + +Unknown value type "" for id gnu.c.link.option.other on tool fr.ac6.managedbuild.tool.gnu.cross.c.linker +Unknown value type "" for id gnu.c.link.option.userobjs on tool fr.ac6.managedbuild.tool.gnu.cross.c.linker +Unknown value type "" for id gnu.cpp.link.option.libs on tool fr.ac6.managedbuild.tool.gnu.cross.cpp.linker +Unknown value type "" for id gnu.cpp.link.option.paths on tool fr.ac6.managedbuild.tool.gnu.cross.cpp.linker +Option fr.ac6.managedbuild.tool.gnu.cross.cpp.linker.noexceptions is no longer supported +Option fr.ac6.managedbuild.tool.gnu.cross.cpp.linker.nortti is no longer supported +Value type err, id: gnu.cpp.link.option.flags +Unknown value type "" for id gnu.both.asm.option.include.paths on tool fr.ac6.managedbuild.tool.gnu.cross.assembler +No map found for id: gnu.cpp.debugging.level.max +No map found for id: gnu.cpp.compiler.option.preprocessor.def.symbols +Unknown value type "" for id gnu.c.link.option.libs on tool fr.ac6.managedbuild.tool.gnu.cross.c.linker +Unknown value type "" for id gnu.c.link.option.paths on tool fr.ac6.managedbuild.tool.gnu.cross.c.linker +Unknown value type "" for id gnu.cpp.link.option.libs on tool fr.ac6.managedbuild.tool.gnu.cross.cpp.linker +Unknown value type "" for id gnu.cpp.link.option.paths on tool fr.ac6.managedbuild.tool.gnu.cross.cpp.linker +Option fr.ac6.managedbuild.tool.gnu.cross.cpp.linker.noexceptions is no longer supported +Option fr.ac6.managedbuild.tool.gnu.cross.cpp.linker.nortti is no longer supported +Unknown value type "" for id gnu.both.asm.option.include.paths on tool fr.ac6.managedbuild.tool.gnu.cross.assembler.exe.release diff --git a/Src/FOC.c b/Src/FOC.c index 04788016..4d1c63e6 100644 --- a/Src/FOC.c +++ b/Src/FOC.c @@ -39,6 +39,8 @@ char PI_flag=0; //const q31_t _T = 2048; +extern TIM_HandleTypeDef htim1; + void FOC_calculation(int16_t int16_i_as, int16_t int16_i_bs, q31_t q31_teta, int16_t int16_i_q_target, MotorState_t* MS_FOC); void svpwm(q31_t q31_u_alpha, q31_t q31_u_beta); diff --git a/Src/display_kunteng.c b/Src/display_kunteng.c index 6665072b..ad78508c 100644 --- a/Src/display_kunteng.c +++ b/Src/display_kunteng.c @@ -14,8 +14,8 @@ static uint8_t ui8_tx_buffer[12]; uint8_t ui8_j; uint8_t ui8_crc; uint8_t ui8_last_XOR; -uint16_t ui16_wheel_period_ms =4500; -uint32_t ui32_battery_volts= 36; +uint16_t ui16_wheel_period_ms = 4500; +uint32_t ui32_battery_volts = 490000; uint8_t ui8_battery_soc = 12; uint8_t ui16_error; static uint8_t ui8_rx_buffer[13]; @@ -24,278 +24,309 @@ uint8_t ui8_rx_buffer_counter = 0; uint8_t ui8_byte_received; uint8_t ui8_moving_indication = 0; uint8_t ui8_UARTCounter = 0; -uint8_t ui8_msg_received=0; -int16_t i16_eeprom_temp=0; +uint8_t ui8_msg_received = 0; +int16_t i16_eeprom_temp = 0; uint8_t ui8_gear_ratio = GEAR_RATIO; volatile struc_lcd_configuration_variables lcd_configuration_variables; extern UART_HandleTypeDef huart1; + void check_recent(void); void kunteng_init() { - if (HAL_UART_Receive_DMA(&huart1, (uint8_t *)ui8_rx_buffer, 13) != HAL_OK) - { - Error_Handler(); - } - - EE_ReadVariable(EEPROM_KT_B0_B3, &i16_eeprom_temp); - ui8_rx_buffer[0] = i16_eeprom_temp>>8; - ui8_rx_initial_buffer[0] = i16_eeprom_temp>>8; - ui8_rx_buffer[3] = i16_eeprom_temp &0xFF; - ui8_rx_initial_buffer[3] = i16_eeprom_temp &0xFF; - - EE_ReadVariable(EEPROM_KT_B2_B4, &i16_eeprom_temp); - ui8_rx_buffer[2] = i16_eeprom_temp>>8; - ui8_rx_initial_buffer[2] = i16_eeprom_temp>>8; - ui8_rx_buffer[4] = i16_eeprom_temp &0xFF; - ui8_rx_initial_buffer[4] = i16_eeprom_temp &0xFF; - - EE_ReadVariable(EEPROM_KT_B6_B7, &i16_eeprom_temp); - ui8_rx_buffer[6] = i16_eeprom_temp>>8; - ui8_rx_initial_buffer[6] = i16_eeprom_temp>>8; - ui8_rx_buffer[7] = i16_eeprom_temp &0xFF; - ui8_rx_initial_buffer[7] = i16_eeprom_temp &0xFF; - - EE_ReadVariable(EEPROM_KT_B8_B9, &i16_eeprom_temp); - ui8_rx_buffer[8] = i16_eeprom_temp>>8; - ui8_rx_initial_buffer[8] = i16_eeprom_temp>>8; - ui8_rx_buffer[9] = i16_eeprom_temp &0xFF; - ui8_rx_initial_buffer[9] = i16_eeprom_temp &0xFF; - - EE_ReadVariable(EEPROM_KT_B1_B10, &i16_eeprom_temp); - ui8_rx_buffer[1] = i16_eeprom_temp>>8; - ui8_rx_initial_buffer[1] = i16_eeprom_temp>>8; - ui8_rx_buffer[10] = i16_eeprom_temp &0xFF; - ui8_rx_initial_buffer[10] = i16_eeprom_temp &0xFF; - - ui8_crc = 0; - - for (ui8_j = 0; ui8_j <= 12; ui8_j++) - { - if (ui8_j == 5) continue; // don't xor B5 (B7 in our case) - ui8_crc ^= ui8_rx_buffer[ui8_j]; - } - ui8_crc ^=10; //right XOR must be pasted here!!! - ui8_rx_buffer [5]=ui8_crc; - ui8_rx_buffer[11]=0x37; - ui8_rx_buffer[12]=0x0E; + if (HAL_UART_Receive_DMA(&huart1, (uint8_t *)ui8_rx_buffer, 13) != HAL_OK) + { + Error_Handler(); + } + EE_ReadVariable(EEPROM_KT_B0_B3, &i16_eeprom_temp); + ui8_rx_buffer[0] = i16_eeprom_temp >> 8; + ui8_rx_initial_buffer[0] = i16_eeprom_temp >> 8; + ui8_rx_buffer[3] = i16_eeprom_temp & 0xFF; + ui8_rx_initial_buffer[3] = i16_eeprom_temp & 0xFF; + + EE_ReadVariable(EEPROM_KT_B2_B4, &i16_eeprom_temp); + ui8_rx_buffer[2] = i16_eeprom_temp >> 8; + ui8_rx_initial_buffer[2] = i16_eeprom_temp >> 8; + ui8_rx_buffer[4] = i16_eeprom_temp & 0xFF; + ui8_rx_initial_buffer[4] = i16_eeprom_temp & 0xFF; + + EE_ReadVariable(EEPROM_KT_B6_B7, &i16_eeprom_temp); + ui8_rx_buffer[6] = i16_eeprom_temp >> 8; + ui8_rx_initial_buffer[6] = i16_eeprom_temp >> 8; + ui8_rx_buffer[7] = i16_eeprom_temp & 0xFF; + ui8_rx_initial_buffer[7] = i16_eeprom_temp & 0xFF; + + EE_ReadVariable(EEPROM_KT_B8_B9, &i16_eeprom_temp); + ui8_rx_buffer[8] = i16_eeprom_temp >> 8; + ui8_rx_initial_buffer[8] = i16_eeprom_temp >> 8; + ui8_rx_buffer[9] = i16_eeprom_temp & 0xFF; + ui8_rx_initial_buffer[9] = i16_eeprom_temp & 0xFF; + + EE_ReadVariable(EEPROM_KT_B1_B10, &i16_eeprom_temp); + ui8_rx_buffer[1] = i16_eeprom_temp >> 8; + ui8_rx_initial_buffer[1] = i16_eeprom_temp >> 8; + ui8_rx_buffer[10] = i16_eeprom_temp & 0xFF; + ui8_rx_initial_buffer[10] = i16_eeprom_temp & 0xFF; + + ui8_crc = 0; + + for (ui8_j = 0; ui8_j <= 12; ui8_j++) + { + if (ui8_j == 5) + continue; // don't xor B5 (B7 in our case) + ui8_crc ^= ui8_rx_buffer[ui8_j]; + } + ui8_crc ^= 10; // right XOR must be pasted here!!! + ui8_rx_buffer[5] = ui8_crc; + ui8_rx_buffer[11] = 0x37; + ui8_rx_buffer[12] = 0x0E; } -void display_update(MotorState_t* MS_U) +void display_update(MotorState_t *MS_U) { + // prepare moving indication info + ui8_moving_indication = 0; + if (!HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)) + { + ui8_moving_indication |= (1 << 5); + } + // if (ebike_app_cruise_control_is_set ()) { ui8_moving_indication |= (1 << 3); } + if (throttle_is_set()) + { + ui8_moving_indication |= (1 << 1); + } + // if (pas_is_set ()) { ui8_moving_indication |= (1 << 4); } + + // calc battery pack state of charge (SOC) + // filter for not so noisy battery indicaton. + ui32_battery_volts = ((MS_U->Voltage * CAL_BAT_V) + (ui32_battery_volts * 31)) / 32; + if (ui32_battery_volts > (BATTERY_LEVEL_5)) + { + ui8_battery_soc = 16; + } // 4 bars | full + else if (ui32_battery_volts > (BATTERY_LEVEL_4)) + { + ui8_battery_soc = 12; + } // 3 bars + else if (ui32_battery_volts > (BATTERY_LEVEL_3)) + { + ui8_battery_soc = 8; + } // 2 bars + else if (ui32_battery_volts > (BATTERY_LEVEL_2)) + { + ui8_battery_soc = 4; + } // 1 bar + else + { + ui8_battery_soc = 3; + } // empty + ui16_wheel_period_ms = (MS_U->Speed * PULSES_PER_REVOLUTION) >> 3; // for External speedsensor + // ui16_wheel_period_ms= ((MS_U->Speed)*6*(ui8_gear_ratio/2)/500); + // ui16_wheel_period_ms= ((MS_U->Speed)*6*GEAR_RATIO/500); //*6 because 6 hall interrupts per revolution, /500 because of 500 kHz timer setting + ui8_tx_buffer[0] = 65; + // B1: battery level + ui8_tx_buffer[1] = ui8_battery_soc; + // B2: 24V controller + ui8_tx_buffer[2] = (uint8_t)COMMUNICATIONS_BATTERY_VOLTAGE; + // B3: speed, wheel rotation period, ms; period(ms)=B3*256+B4; + ui8_tx_buffer[3] = (ui16_wheel_period_ms >> 8) & 0xff; + ui8_tx_buffer[4] = (ui16_wheel_period_ms) & 0xff; + + // B5: error info display + ui8_tx_buffer[5] = ui16_error; + // B6: CRC: xor B1,B2,B3,B4,B5,B7,B8,B9,B10,B11 + // 0 value so no effect on xor operation for now + ui8_tx_buffer[6] = 0; + // B7: moving mode indication, bit + // throttle: 2 + ui8_tx_buffer[7] = ui8_moving_indication; + // B8: 4x controller current + // Vbat = 30V: + // - B8 = 255, LCD shows 1912 watts + // - B8 = 250, LCD shows 1875 watts + // - B8 = 100, LCD shows 750 watts + // each unit of B8 = 0.25A + + // ui8_tx_buffer [8] = (uint8_t)(((ui16_BatteryCurrent-ui16_current_cal_b+1)<<2)/current_cal_a); + ui8_tx_buffer[8] = (uint8_t)(MS_U->Battery_Current / 250); + // ui8_tx_buffer [8] = MS_U->debug; + // B9: motor temperature + ui8_tx_buffer[9] = MS_U->Temperature - 15; // according to documentation at endless sphere + // B10 and B11: 0 + ui8_tx_buffer[10] = 0; + ui8_tx_buffer[11] = 0; + + // calculate CRC xor + ui8_crc = 0; + for (ui8_j = 1; ui8_j <= 11; ui8_j++) + { + ui8_crc ^= ui8_tx_buffer[ui8_j]; + } + ui8_tx_buffer[6] = ui8_crc; - // prepare moving indication info - ui8_moving_indication = 0; -if (!HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)) { ui8_moving_indication |= (1 << 5); } - //if (ebike_app_cruise_control_is_set ()) { ui8_moving_indication |= (1 << 3); } - if (throttle_is_set ()) { ui8_moving_indication |= (1 << 1); } - //if (pas_is_set ()) { ui8_moving_indication |= (1 << 4); } - - - // calc battery pack state of charge (SOC) - ui32_battery_volts = (MS_U->Voltage*CAL_BAT_V*256)/10000; //hier noch die richtige Kalibrierung einbauen (*256 für bessere Auflösung) - if (ui32_battery_volts > ((uint16_t) BATTERY_PACK_VOLTS_80)) { ui8_battery_soc = 16; } // 4 bars | full - else if (ui32_battery_volts > ((uint16_t) BATTERY_PACK_VOLTS_60)) { ui8_battery_soc = 12; } // 3 bars - else if (ui32_battery_volts > ((uint16_t) BATTERY_PACK_VOLTS_40)) { ui8_battery_soc = 8; } // 2 bars - else if (ui32_battery_volts > ((uint16_t) BATTERY_PACK_VOLTS_20)) { ui8_battery_soc = 4; } // 1 bar - else { ui8_battery_soc = 3; } // empty - //ui16_wheel_period_ms = (MS_U->Speed*PULSES_PER_REVOLUTION)>>3; //for External speedsensor - ui16_wheel_period_ms= ((MS_U->Speed)*6*(ui8_gear_ratio/2)/500); - //ui16_wheel_period_ms= ((MS_U->Speed)*6*GEAR_RATIO/500); //*6 because 6 hall interrupts per revolution, /500 because of 500 kHz timer setting - ui8_tx_buffer [0] = 65; - // B1: battery level - ui8_tx_buffer [1] = ui8_battery_soc; - // B2: 24V controller - ui8_tx_buffer [2] = (uint8_t) COMMUNICATIONS_BATTERY_VOLTAGE; - // B3: speed, wheel rotation period, ms; period(ms)=B3*256+B4; - ui8_tx_buffer [3] = (ui16_wheel_period_ms >> 8) & 0xff; - ui8_tx_buffer [4] = (ui16_wheel_period_ms) & 0xff; - - - - // B5: error info display - ui8_tx_buffer [5] = ui16_error; - // B6: CRC: xor B1,B2,B3,B4,B5,B7,B8,B9,B10,B11 - // 0 value so no effect on xor operation for now - ui8_tx_buffer [6] = 0; - // B7: moving mode indication, bit - // throttle: 2 - ui8_tx_buffer [7] = ui8_moving_indication; - // B8: 4x controller current - // Vbat = 30V: - // - B8 = 255, LCD shows 1912 watts - // - B8 = 250, LCD shows 1875 watts - // - B8 = 100, LCD shows 750 watts - // each unit of B8 = 0.25A - - - //ui8_tx_buffer [8] = (uint8_t)(((ui16_BatteryCurrent-ui16_current_cal_b+1)<<2)/current_cal_a); - ui8_tx_buffer [8] = (uint8_t)(MS_U->Battery_Current*MS_U->Voltage*CAL_BAT_V/82010000); //Kalibrierung nach Binatone, empririsch ermittelt. Strom und Spannung in Milli, 13W pro digit - // B9: motor temperature - ui8_tx_buffer [9] = MS_U->Temperature-15; //according to documentation at endless sphere - // B10 and B11: 0 - ui8_tx_buffer [10] = 0; - ui8_tx_buffer [11] = 0; - - // calculate CRC xor - ui8_crc = 0; - for (ui8_j = 1; ui8_j <= 11; ui8_j++) - { - ui8_crc ^= ui8_tx_buffer[ui8_j]; - } - ui8_tx_buffer [6] = ui8_crc; - - // send the package over UART - HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&ui8_tx_buffer, 12); + // send the package over UART + HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&ui8_tx_buffer, 12); } /********************************************************************************************/ - // Process received package from the LCD - // - - // see if we have a received package to be processed -void check_message(MotorState_t* MS_D, MotorParams_t* MP_D) - { - //printf("Byte recieved \r\n"); - // validation of the package data - ui8_crc = 0; - - for (ui8_j = 0; ui8_j <= 12; ui8_j++) - { - if (ui8_j == 5) continue; // don't xor B5 (B7 in our case) - ui8_crc ^= ui8_rx_buffer[ui8_j]; - } - - // check if end of message is OK - if((ui8_rx_buffer[11]==0x32||ui8_rx_buffer[11]==0x37) && ui8_rx_buffer[12]==0x0E ){ - // check if CRC is ok - if (((ui8_crc ^ 10) == ui8_rx_buffer [5]) || // some versions of CRC LCD5 (??) - ((ui8_crc ^ ui8_last_XOR) == ui8_rx_buffer [5]) - ) - { //printf("message valid \r\n"); - lcd_configuration_variables.ui8_assist_level = ui8_rx_buffer [1] & 7; - lcd_configuration_variables.ui8_light = ui8_rx_buffer [1]>>7 & 1; - lcd_configuration_variables.ui8_motor_characteristic = ui8_rx_buffer [3]; - lcd_configuration_variables.ui8_wheel_size = ((ui8_rx_buffer [4] & 192) >> 6) | ((ui8_rx_buffer [2] & 7) << 2); - lcd_configuration_variables.ui8_max_speed = (10 + ((ui8_rx_buffer [2] & 248) >> 3)) | (ui8_rx_buffer [4] & 32); - lcd_configuration_variables.ui8_power_assist_control_mode = ui8_rx_buffer [4] & 8; - lcd_configuration_variables.ui8_controller_max_current = (ui8_rx_buffer [7] & 15); - MS_D->assist_level = lcd_configuration_variables.ui8_assist_level; - MP_D->speedLimit = lcd_configuration_variables.ui8_max_speed; - - lcd_configuration_variables.ui8_p1 = ui8_rx_buffer[3]; - lcd_configuration_variables.ui8_p2 = ui8_rx_buffer[4] & 0x07; - lcd_configuration_variables.ui8_p3 = ui8_rx_buffer[4] & 0x08; - lcd_configuration_variables.ui8_p4 = ui8_rx_buffer[4] & 0x10; - lcd_configuration_variables.ui8_p5 = ui8_rx_buffer[0]; - - lcd_configuration_variables.ui8_c1 = (ui8_rx_buffer[6] & 0x38) >> 3; - lcd_configuration_variables.ui8_c2 = (ui8_rx_buffer[6] & 0x37); - lcd_configuration_variables.ui8_c4 = (ui8_rx_buffer[8] & 0xE0) >> 5; - lcd_configuration_variables.ui8_c5 = (ui8_rx_buffer[7] & 0x0F); - lcd_configuration_variables.ui8_c12 = (ui8_rx_buffer[9] & 0x0F); - lcd_configuration_variables.ui8_c13 = (ui8_rx_buffer[10] & 0x1C) >> 2; - lcd_configuration_variables.ui8_c14 = (ui8_rx_buffer[7] & 0x60) >> 5; - if(lcd_configuration_variables.ui8_p1 != ui8_gear_ratio){ - ui8_gear_ratio=lcd_configuration_variables.ui8_p1; - } - - if(lcd_configuration_variables.ui8_light){ - HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_SET); - HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); - } - else{ - HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_RESET); - HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); - } - - display_update(MS_D); - check_recent(); //byte 1 contains the PAS level, that may be changed quite often. Better run only at system shutdown, due to limited possible write cycles to flash - }//end CRC OK - else{ //search for right last XOR - - ui8_crc = 0; - - for (ui8_j = 0; ui8_j <= 12; ui8_j++) - { - if (ui8_j == 5) continue; // don't xor B5 (B7 in our case) - ui8_crc ^= ui8_rx_buffer[ui8_j]; - } - for (ui8_j = 0; ui8_j <= 50; ui8_j++) - { - if((ui8_crc ^ ui8_j) == ui8_rx_buffer [5]) ui8_last_XOR = ui8_j; - } - } - }// end EOT OK - else{ - //resyncronize the communication - CLEAR_BIT(DMA1_Channel5->CCR, DMA_CCR_EN); - DMA1_Channel5->CNDTR=2; - SET_BIT(DMA1_Channel5->CCR, DMA_CCR_EN); - - if((ui8_rx_buffer[0]==0x32||ui8_rx_buffer[0]==0x37) && ui8_rx_buffer[1]==0x0E ){ - CLEAR_BIT(DMA1_Channel5->CCR, DMA_CCR_EN); - DMA1_Channel5->CNDTR=13; - SET_BIT(DMA1_Channel5->CCR, DMA_CCR_EN); - } - - } - } - -//check if differences between initial values and recent values, store to emulated EEPROM if necessary +// Process received package from the LCD +// + +// see if we have a received package to be processed +void check_message(MotorState_t *MS_D, MotorParams_t *MP_D) +{ + // printf("Byte recieved \r\n"); + // validation of the package data + ui8_crc = 0; + + for (ui8_j = 0; ui8_j <= 12; ui8_j++) + { + if (ui8_j == 5) + continue; // don't xor B5 (B7 in our case) + ui8_crc ^= ui8_rx_buffer[ui8_j]; + } + + // check if end of message is OK + if ((ui8_rx_buffer[11] == 0x32 || ui8_rx_buffer[11] == 0x37) && ui8_rx_buffer[12] == 0x0E) + { + // check if CRC is ok + if (((ui8_crc ^ 10) == ui8_rx_buffer[5]) || // some versions of CRC LCD5 (??) + ((ui8_crc ^ ui8_last_XOR) == ui8_rx_buffer[5])) + { // printf("message valid \r\n"); + lcd_configuration_variables.ui8_assist_level = ui8_rx_buffer[1] & 7; + lcd_configuration_variables.ui8_light = ui8_rx_buffer[1] >> 7 & 1; + lcd_configuration_variables.ui8_motor_characteristic = ui8_rx_buffer[3]; + lcd_configuration_variables.ui8_wheel_size = ((ui8_rx_buffer[4] & 192) >> 6) | ((ui8_rx_buffer[2] & 7) << 2); + lcd_configuration_variables.ui8_max_speed = (10 + ((ui8_rx_buffer[2] & 248) >> 3)) | (ui8_rx_buffer[4] & 32); + lcd_configuration_variables.ui8_power_assist_control_mode = ui8_rx_buffer[4] & 8; + lcd_configuration_variables.ui8_controller_max_current = (ui8_rx_buffer[7] & 15); + MS_D->assist_level = lcd_configuration_variables.ui8_assist_level; + MP_D->speedLimit = lcd_configuration_variables.ui8_max_speed; + + lcd_configuration_variables.ui8_p1 = ui8_rx_buffer[3]; + lcd_configuration_variables.ui8_p2 = ui8_rx_buffer[4] & 0x07; + lcd_configuration_variables.ui8_p3 = ui8_rx_buffer[4] & 0x08; + lcd_configuration_variables.ui8_p4 = ui8_rx_buffer[4] & 0x10; + lcd_configuration_variables.ui8_p5 = ui8_rx_buffer[0]; + + lcd_configuration_variables.ui8_c1 = (ui8_rx_buffer[6] & 0x38) >> 3; + lcd_configuration_variables.ui8_c2 = (ui8_rx_buffer[6] & 0x37); + lcd_configuration_variables.ui8_c4 = (ui8_rx_buffer[8] & 0xE0) >> 5; + lcd_configuration_variables.ui8_c5 = (ui8_rx_buffer[7] & 0x0F); + lcd_configuration_variables.ui8_c12 = (ui8_rx_buffer[9] & 0x0F); + lcd_configuration_variables.ui8_c13 = (ui8_rx_buffer[10] & 0x1C) >> 2; + lcd_configuration_variables.ui8_c14 = (ui8_rx_buffer[7] & 0x60) >> 5; + if (lcd_configuration_variables.ui8_p1 != ui8_gear_ratio) + { + ui8_gear_ratio = lcd_configuration_variables.ui8_p1; + } + + if (lcd_configuration_variables.ui8_light) + { + HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); + } + else + { + HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); + } + + display_update(MS_D); + check_recent(); // byte 1 contains the PAS level, that may be changed quite often. Better run only at system shutdown, due to limited possible write cycles to flash + } // end CRC OK + else + { // search for right last XOR + + ui8_crc = 0; + + for (ui8_j = 0; ui8_j <= 12; ui8_j++) + { + if (ui8_j == 5) + continue; // don't xor B5 (B7 in our case) + ui8_crc ^= ui8_rx_buffer[ui8_j]; + } + for (ui8_j = 0; ui8_j <= 50; ui8_j++) + { + if ((ui8_crc ^ ui8_j) == ui8_rx_buffer[5]) + ui8_last_XOR = ui8_j; + } + } + } // end EOT OK + else + { + // resyncronize the communication + CLEAR_BIT(DMA1_Channel5->CCR, DMA_CCR_EN); + DMA1_Channel5->CNDTR = 2; + SET_BIT(DMA1_Channel5->CCR, DMA_CCR_EN); + + if ((ui8_rx_buffer[0] == 0x32 || ui8_rx_buffer[0] == 0x37) && ui8_rx_buffer[1] == 0x0E) + { + CLEAR_BIT(DMA1_Channel5->CCR, DMA_CCR_EN); + DMA1_Channel5->CNDTR = 13; + SET_BIT(DMA1_Channel5->CCR, DMA_CCR_EN); + } + } +} + +// check if differences between initial values and recent values, store to emulated EEPROM if necessary /* EEPROM_KT_B0_B3 EEPROM_KT_B2_B4 EEPROM_KT_B6_B7 EEPROM_KT_B8_B9 EEPROM_KT_B1_B10 -*/ -//byte 1 contains the PAS level, that may be changed quite often. Better run only at system shutdown. -void check_recent(void){ - - if(ui8_rx_buffer[0]!=ui8_rx_initial_buffer[0] || ui8_rx_buffer[3]!=ui8_rx_initial_buffer[3] ){ - HAL_FLASH_Unlock(); - EE_WriteVariable(EEPROM_KT_B0_B3,ui8_rx_buffer[0]<<8 | ui8_rx_buffer[3] ); - HAL_FLASH_Lock(); - ui8_rx_initial_buffer[0]=ui8_rx_buffer[0]; - ui8_rx_initial_buffer[3]=ui8_rx_buffer[3]; + */ +// byte 1 contains the PAS level, that may be changed quite often. Better run only at system shutdown. +void check_recent(void) +{ + + if (ui8_rx_buffer[0] != ui8_rx_initial_buffer[0] || ui8_rx_buffer[3] != ui8_rx_initial_buffer[3]) + { + HAL_FLASH_Unlock(); + EE_WriteVariable(EEPROM_KT_B0_B3, ui8_rx_buffer[0] << 8 | ui8_rx_buffer[3]); + HAL_FLASH_Lock(); + ui8_rx_initial_buffer[0] = ui8_rx_buffer[0]; + ui8_rx_initial_buffer[3] = ui8_rx_buffer[3]; } - if(ui8_rx_buffer[2]!=ui8_rx_initial_buffer[2] || ui8_rx_buffer[4]!=ui8_rx_initial_buffer[4] ){ - HAL_FLASH_Unlock(); - EE_WriteVariable(EEPROM_KT_B2_B4,ui8_rx_buffer[2]<<8 | ui8_rx_buffer[4] ); - HAL_FLASH_Lock(); - ui8_rx_initial_buffer[2]=ui8_rx_buffer[2]; - ui8_rx_initial_buffer[4]=ui8_rx_buffer[4]; + if (ui8_rx_buffer[2] != ui8_rx_initial_buffer[2] || ui8_rx_buffer[4] != ui8_rx_initial_buffer[4]) + { + HAL_FLASH_Unlock(); + EE_WriteVariable(EEPROM_KT_B2_B4, ui8_rx_buffer[2] << 8 | ui8_rx_buffer[4]); + HAL_FLASH_Lock(); + ui8_rx_initial_buffer[2] = ui8_rx_buffer[2]; + ui8_rx_initial_buffer[4] = ui8_rx_buffer[4]; } - if(ui8_rx_buffer[6]!=ui8_rx_initial_buffer[6] || ui8_rx_buffer[7]!=ui8_rx_initial_buffer[7] ){ - HAL_FLASH_Unlock(); - EE_WriteVariable(EEPROM_KT_B6_B7,ui8_rx_buffer[6]<<8 | ui8_rx_buffer[7] ); - HAL_FLASH_Lock(); - ui8_rx_initial_buffer[6]=ui8_rx_buffer[6]; - ui8_rx_initial_buffer[7]=ui8_rx_buffer[7]; + if (ui8_rx_buffer[6] != ui8_rx_initial_buffer[6] || ui8_rx_buffer[7] != ui8_rx_initial_buffer[7]) + { + HAL_FLASH_Unlock(); + EE_WriteVariable(EEPROM_KT_B6_B7, ui8_rx_buffer[6] << 8 | ui8_rx_buffer[7]); + HAL_FLASH_Lock(); + ui8_rx_initial_buffer[6] = ui8_rx_buffer[6]; + ui8_rx_initial_buffer[7] = ui8_rx_buffer[7]; } - if(ui8_rx_buffer[8]!=ui8_rx_initial_buffer[8] || ui8_rx_buffer[9]!=ui8_rx_initial_buffer[9] ){ - HAL_FLASH_Unlock(); - EE_WriteVariable(EEPROM_KT_B8_B9,ui8_rx_buffer[8]<<8 | ui8_rx_buffer[9] ); - HAL_FLASH_Lock(); - ui8_rx_initial_buffer[8]=ui8_rx_buffer[8]; - ui8_rx_initial_buffer[9]=ui8_rx_buffer[9]; + if (ui8_rx_buffer[8] != ui8_rx_initial_buffer[8] || ui8_rx_buffer[9] != ui8_rx_initial_buffer[9]) + { + HAL_FLASH_Unlock(); + EE_WriteVariable(EEPROM_KT_B8_B9, ui8_rx_buffer[8] << 8 | ui8_rx_buffer[9]); + HAL_FLASH_Lock(); + ui8_rx_initial_buffer[8] = ui8_rx_buffer[8]; + ui8_rx_initial_buffer[9] = ui8_rx_buffer[9]; } -//only check Byte 10 - if( ui8_rx_buffer[10]!=ui8_rx_initial_buffer[10] ){ - HAL_FLASH_Unlock(); - EE_WriteVariable(EEPROM_KT_B1_B10,ui8_rx_buffer[1]<<8 | ui8_rx_buffer[10] ); - HAL_FLASH_Lock(); - ui8_rx_initial_buffer[1]=ui8_rx_buffer[1]; - ui8_rx_initial_buffer[10]=ui8_rx_buffer[10]; + // only check Byte 10 + if (ui8_rx_buffer[10] != ui8_rx_initial_buffer[10]) + { + HAL_FLASH_Unlock(); + EE_WriteVariable(EEPROM_KT_B1_B10, ui8_rx_buffer[1] << 8 | ui8_rx_buffer[10]); + HAL_FLASH_Lock(); + ui8_rx_initial_buffer[1] = ui8_rx_buffer[1]; + ui8_rx_initial_buffer[10] = ui8_rx_buffer[10]; } - - } diff --git a/Src/main.c b/Src/main.c index 6f382ded..4d6a34ba 100644 --- a/Src/main.c +++ b/Src/main.c @@ -40,9 +40,6 @@ #include "main.h" #include "stm32f1xx_hal.h" - - - /* USER CODE BEGIN Includes */ // Lishui BLCD FOC Open Source Firmware @@ -56,8 +53,7 @@ #include "config.h" #include "eeprom.h" - -#if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER|| DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) +#if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER || DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) #include "display_kingmeter.h" #endif @@ -98,103 +94,101 @@ IWDG_HandleTypeDef hiwdg; /* USER CODE BEGIN PV */ /* Private variables ---------------------------------------------------------*/ - -uint32_t ui32_tim1_counter=0; -uint32_t ui32_tim3_counter=0; -uint8_t ui8_hall_state=0; -uint8_t ui8_hall_state_old=0; -uint8_t ui8_hall_case =0; -uint16_t ui16_tim2_recent=0; -uint16_t ui16_timertics=5000; //timertics between two hall events for 60° interpolation +uint32_t ui32_tim1_counter = 0; +uint32_t ui32_tim3_counter = 0; +uint8_t ui8_hall_state = 0; +uint8_t ui8_hall_state_old = 0; +uint8_t ui8_hall_case = 0; +uint16_t ui16_tim2_recent = 0; +uint16_t ui16_timertics = 5000; // timertics between two hall events for 60° interpolation uint16_t ui16_throttle; uint16_t ui16_throttle_offset = THROTTLE_OFFSET; uint16_t ui16_brake_adc; uint32_t ui32_throttle_cumulated; uint32_t ui32_brake_adc_cumulated; -uint32_t ui32_int_Temp_cumulated = INT_TEMP_25<<5; +uint32_t ui32_int_Temp_cumulated = INT_TEMP_25 << 5; int16_t i16_int_Temp_V25; -uint16_t ui16_ph1_offset=0; -uint16_t ui16_ph2_offset=0; -uint16_t ui16_ph3_offset=0; -int16_t i16_ph1_current=0; - -int16_t i16_ph2_current=0; -int16_t i16_ph2_current_filter=0; -int16_t i16_ph3_current=0; -uint16_t i=0; -uint16_t j=0; -uint16_t k=0; -uint16_t y=0; -uint8_t brake_flag=0; -volatile uint8_t ui8_overflow_flag=0; -uint8_t ui8_slowloop_counter=0; -volatile uint8_t ui8_adc_inj_flag=0; -volatile uint8_t ui8_adc_regular_flag=0; -uint8_t ui8_speedcase=0; -uint8_t ui8_speedfactor=0; -int8_t i8_direction= REVERSE; //for permanent reverse direction -int8_t i8_reverse_flag = 1; //for temporaribly reverse direction -uint8_t ui8_KV_detect_flag = 0; //for getting the KV of the motor after auto angle detect -uint16_t ui16_KV_detect_counter = 0; //for getting timing of the KV detect +uint16_t ui16_ph1_offset = 0; +uint16_t ui16_ph2_offset = 0; +uint16_t ui16_ph3_offset = 0; +int16_t i16_ph1_current = 0; + +int16_t i16_ph2_current = 0; +int16_t i16_ph2_current_filter = 0; +int16_t i16_ph3_current = 0; +uint16_t i = 0; +uint16_t j = 0; +uint16_t k = 0; +uint16_t y = 0; +uint8_t brake_flag = 0; +volatile uint8_t ui8_overflow_flag = 0; +uint8_t ui8_slowloop_counter = 0; +volatile uint8_t ui8_adc_inj_flag = 0; +volatile uint8_t ui8_adc_regular_flag = 0; +uint8_t ui8_speedcase = 0; +uint8_t ui8_speedfactor = 0; +int8_t i8_direction = REVERSE; // for permanent reverse direction +int8_t i8_reverse_flag = 1; // for temporaribly reverse direction +uint8_t ui8_KV_detect_flag = 0; // for getting the KV of the motor after auto angle detect +uint16_t ui16_KV_detect_counter = 0; // for getting timing of the KV detect int16_t ui32_KV = 0; - -volatile uint8_t ui8_adc_offset_done_flag=0; -volatile uint8_t ui8_print_flag=0; -volatile uint8_t ui8_UART_flag=0; -volatile uint8_t ui8_Push_Assist_flag=0; -volatile uint8_t ui8_UART_TxCplt_flag=1; -volatile uint8_t ui8_PAS_flag=0; -volatile uint8_t ui8_SPEED_flag=0; -volatile uint8_t ui8_SPEED_control_flag=0; -volatile uint8_t ui8_BC_limit_flag=0; //flag for Battery current limitation -volatile uint8_t ui8_6step_flag=0; -uint32_t uint32_PAS_counter= PAS_TIMEOUT+1; -uint32_t uint32_PAS_HIGH_counter= 0; -uint32_t uint32_PAS_HIGH_accumulated= 32000; -uint32_t uint32_PAS_fraction= 100; -uint32_t uint32_SPEED_counter=32000; -uint32_t uint32_SPEEDx100_cumulated=0; -uint32_t uint32_PAS=32000; -uint32_t PAS_IMP_PER_TURN_RECIP_MULTIPLIER = ((1 << 8) / PAS_IMP_PER_TURN); // recproce multiplier to avoid division in the main loop +volatile uint8_t ui8_adc_offset_done_flag = 0; +volatile uint8_t ui8_print_flag = 0; +volatile uint8_t ui8_UART_flag = 0; +volatile uint8_t ui8_Push_Assist_flag = 0; +volatile uint8_t ui8_UART_TxCplt_flag = 1; +volatile uint8_t ui8_PAS_flag = 0; +volatile uint8_t ui8_SPEED_flag = 0; +volatile uint8_t ui8_SPEED_control_flag = 0; +volatile uint8_t ui8_BC_limit_flag = 0; // flag for Battery current limitation +volatile uint8_t ui8_6step_flag = 0; +uint32_t uint32_PAS_counter = PAS_TIMEOUT + 1; +uint32_t uint32_PAS_HIGH_counter = 0; +uint32_t uint32_PAS_HIGH_accumulated = 32000; +uint32_t uint32_PAS_fraction = 100; +uint32_t uint32_SPEED_counter = 32000; +uint32_t uint32_SPEEDx100_cumulated = 0; +uint32_t uint32_PAS = 32000; +uint32_t PAS_IMP_PER_TURN_RECIP_MULTIPLIER = ((1 << 8) / PAS_IMP_PER_TURN); // recproce multiplier to avoid division in the main loop q31_t q31_rotorposition_PLL = 0; q31_t q31_angle_per_tic = 0; -uint8_t ui8_UART_Counter=0; -int8_t i8_recent_rotor_direction=1; -int16_t i16_hall_order=1; -uint16_t ui16_erps=0; - -uint32_t uint32_torque_cumulated=0; -uint32_t uint32_PAS_cumulated=32000; -uint16_t uint16_mapped_throttle=0; -uint16_t uint16_mapped_PAS=0; -uint16_t uint16_mapped_BRAKE=0; -uint16_t uint16_half_rotation_counter=0; -uint16_t uint16_full_rotation_counter=0; -int32_t int32_temp_current_target=0; -q31_t q31_PLL_error=0; -q31_t q31_t_Battery_Current_accumulated=0; +uint8_t ui8_UART_Counter = 0; +int8_t i8_recent_rotor_direction = 1; +int16_t i16_hall_order = 1; +uint16_t ui16_erps = 0; + +uint32_t uint32_torque_cumulated = 0; +uint32_t uint32_PAS_cumulated = 32000; +uint16_t uint16_mapped_throttle = 0; +uint16_t uint16_mapped_PAS = 0; +uint16_t uint16_mapped_BRAKE = 0; +uint16_t uint16_half_rotation_counter = 0; +uint16_t uint16_full_rotation_counter = 0; +int32_t int32_temp_current_target = 0; +q31_t q31_PLL_error = 0; +q31_t q31_t_Battery_Current_accumulated = 0; q31_t q31_rotorposition_absolute; q31_t q31_rotorposition_hall; -//q31_t q31_rotorposition_motor_specific = SPEC_ANGLE; -q31_t q31_u_d_temp=0; -q31_t q31_u_q_temp=0; -int16_t i16_sinus=0; -int16_t i16_cosinus=0; +// q31_t q31_rotorposition_motor_specific = SPEC_ANGLE; +q31_t q31_u_d_temp = 0; +q31_t q31_u_q_temp = 0; +int16_t i16_sinus = 0; +int16_t i16_cosinus = 0; char buffer[100]; -char char_dyn_adc_state_old=1; -const uint8_t assist_factor[10]={0, 51, 102, 153, 204, 255, 255, 255, 255, 255}; -const uint8_t assist_profile[2][6]= { {0,10,20,30,45,48}, - {64,64,128,200,255,0}}; +char char_dyn_adc_state_old = 1; +const uint8_t assist_factor[10] = {0, 51, 102, 153, 204, 255, 255, 255, 255, 255}; +const uint8_t assist_profile[2][6] = {{0, 10, 20, 30, 45, 48}, + {64, 64, 128, 200, 255, 0}}; uint16_t switchtime[3]; -volatile uint16_t adcData[9]; //Buffer for ADC1 Input +volatile uint16_t adcData[9]; // Buffer for ADC1 Input q31_t tic_array[6]; -//Rotor angle scaled from degree to q31 for arm_math. -180°-->-2^31, 0°-->0, +180°-->+2^31 +// Rotor angle scaled from degree to q31 for arm_math. -180°-->-2^31, 0°-->0, +180°-->+2^31 const q31_t deg_30 = 357913941; q31_t Hall_13 = 0; @@ -204,44 +198,52 @@ q31_t Hall_64 = 0; q31_t Hall_51 = 0; q31_t Hall_45 = 0; -const q31_t tics_lower_limit = WHEEL_CIRCUMFERENCE*5*3600/(6*GEAR_RATIO*SPEEDLIMIT*10); //tics=wheelcirc*timerfrequency/(no. of hallevents per rev*gear-ratio*speedlimit)*3600/1000000 -const q31_t tics_higher_limit = WHEEL_CIRCUMFERENCE*5*3600/(6*GEAR_RATIO*(SPEEDLIMIT+2)*10); -uint32_t uint32_tics_filtered=1000000; - -uint16_t VirtAddVarTab[NB_OF_VAR] = { EEPROM_POS_HALL_ORDER, - EEPROM_POS_HALL_45, - EEPROM_POS_HALL_51, - EEPROM_POS_HALL_13, - EEPROM_POS_HALL_32, - EEPROM_POS_HALL_26, - EEPROM_POS_HALL_64, - EEPROM_INT_TEMP_V25 -}; +const q31_t tics_lower_limit = WHEEL_CIRCUMFERENCE * 5 * 3600 / (6 * GEAR_RATIO * SPEEDLIMIT * 10); // tics=wheelcirc*timerfrequency/(no. of hallevents per rev*gear-ratio*speedlimit)*3600/1000000 +const q31_t tics_higher_limit = WHEEL_CIRCUMFERENCE * 5 * 3600 / (6 * GEAR_RATIO * (SPEEDLIMIT + 2) * 10); +uint32_t uint32_tics_filtered = 1000000; -enum state {Stop, SixStep, Regen, Running, BatteryCurrentLimit, Interpolation, PLL, IdleRun}; -enum state SystemState; +uint16_t VirtAddVarTab[NB_OF_VAR] = {EEPROM_POS_HALL_ORDER, + EEPROM_POS_HALL_45, + EEPROM_POS_HALL_51, + EEPROM_POS_HALL_13, + EEPROM_POS_HALL_32, + EEPROM_POS_HALL_26, + EEPROM_POS_HALL_64, + EEPROM_INT_TEMP_V25}; -#define iabs(x) (((x) >= 0)?(x):-(x)) -#define sign(x) (((x) >= 0)?(1):(-1)) +enum state +{ + Stop, + SixStep, + Regen, + Running, + BatteryCurrentLimit, + Interpolation, + PLL, + IdleRun +}; +enum state SystemState; +#define iabs(x) (((x) >= 0) ? (x) : -(x)) +#define sign(x) (((x) >= 0) ? (1) : (-1)) -//variables for display communication -#if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER|| DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) +// variables for display communication +#if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER || DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) KINGMETER_t KM; #endif -//variables for display communication +// variables for display communication #if (DISPLAY_TYPE & DISPLAY_TYPE_BAFANG) BAFANG_t BF; #endif #if (DISPLAY_TYPE & DISPLAY_TYPE_EBiCS) -uint8_t ui8_main_LEV_Page_counter=0; -uint8_t ui8_additional_LEV_Page_counter=0; -uint8_t ui8_LEV_Page_to_send=1; +uint8_t ui8_main_LEV_Page_counter = 0; +uint8_t ui8_additional_LEV_Page_counter = 0; +uint8_t ui8_LEV_Page_to_send = 1; #endif -//variables for display communication +// variables for display communication #if (DISPLAY_TYPE == DISPLAY_TYPE_NO2) No2_t No2; #endif @@ -249,17 +251,16 @@ No2_t No2; MotorState_t MS; MotorParams_t MP; -//structs for PI_control +// structs for PI_control PI_control_t PI_iq; PI_control_t PI_id; PI_control_t PI_speed; +int32_t battery_percent_fromcapacity = 50; // Calculation of used watthours not implemented yet +int16_t wheel_time = 1000; // duration of one wheel rotation for speed calculation +int16_t current_display; // pepared battery current for display -int32_t battery_percent_fromcapacity = 50; //Calculation of used watthours not implemented yet -int16_t wheel_time = 1000; //duration of one wheel rotation for speed calculation -int16_t current_display; //pepared battery current for display - -int16_t power; //recent power output +int16_t power; // recent power output /* USER CODE END PV */ @@ -279,10 +280,9 @@ void MX_IWDG_Init(void); void get_internal_temp_offset(void); void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); - /* USER CODE BEGIN PFP */ /* Private function prototypes -----------------------------------------------*/ -#if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER|| DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) +#if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER || DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) void kingmeter_update(void); #endif @@ -293,16 +293,12 @@ void bafang_update(void); static void dyn_adc_state(q31_t angle); static void set_inj_channel(char state); void get_standstill_position(); -q31_t speed_PLL (q31_t ist, q31_t soll, uint8_t speedadapt); -int32_t map (int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max); -int32_t speed_to_tics (uint8_t speed); -int8_t tics_to_speed (uint32_t tics); -int16_t internal_tics_to_speedx100 (uint32_t tics); -int16_t external_tics_to_speedx100 (uint32_t tics); - - - - +q31_t speed_PLL(q31_t ist, q31_t soll, uint8_t speedadapt); +int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max); +int32_t speed_to_tics(uint8_t speed); +int8_t tics_to_speed(uint32_t tics); +int16_t internal_tics_to_speedx100(uint32_t tics); +int16_t external_tics_to_speedx100(uint32_t tics); /* USER CODE END PFP */ @@ -319,8 +315,6 @@ int main(void) { /* USER CODE BEGIN 1 */ - - /* USER CODE END 1 */ /* MCU Configuration----------------------------------------------------------*/ @@ -344,53 +338,50 @@ int main(void) MX_DMA_Init(); MX_USART1_UART_Init(); - - //initialize MS struct. - MS.hall_angle_detect_flag=1; - MS.Speed=128000; - MS.assist_level=127; - MS.regen_level=7; + // initialize MS struct. + MS.hall_angle_detect_flag = 1; + MS.Speed = 128000; + MS.assist_level = 127; + MS.regen_level = 7; MS.i_q_setpoint = 0; MS.i_d_setpoint = 0; - MS.angle_est=SPEED_PLL; - + MS.angle_est = SPEED_PLL; MP.pulses_per_revolution = PULSES_PER_REVOLUTION; MP.wheel_cirumference = WHEEL_CIRCUMFERENCE; - MP.speedLimit=SPEEDLIMIT; + MP.speedLimit = SPEEDLIMIT; MP.battery_current_max = BATTERYCURRENT_MAX; - - //init PI structs - PI_id.gain_i=I_FACTOR_I_D; - PI_id.gain_p=P_FACTOR_I_D; + // init PI structs + PI_id.gain_i = I_FACTOR_I_D; + PI_id.gain_p = P_FACTOR_I_D; PI_id.setpoint = 0; PI_id.limit_output = _U_MAX; - PI_id.max_step=5000; - PI_id.shift=10; - PI_id.limit_i=1800; + PI_id.max_step = 5000; + PI_id.shift = 10; + PI_id.limit_i = 1800; - PI_iq.gain_i=I_FACTOR_I_Q; - PI_iq.gain_p=P_FACTOR_I_Q; + PI_iq.gain_i = I_FACTOR_I_Q; + PI_iq.gain_p = P_FACTOR_I_Q; PI_iq.setpoint = 0; PI_iq.limit_output = _U_MAX; - PI_iq.max_step=5000; - PI_iq.shift=10; - PI_iq.limit_i=_U_MAX; + PI_iq.max_step = 5000; + PI_iq.shift = 10; + PI_iq.limit_i = _U_MAX; #ifdef SPEEDTHROTTLE - PI_speed.gain_i=I_FACTOR_SPEED; - PI_speed.gain_p=P_FACTOR_SPEED; + PI_speed.gain_i = I_FACTOR_SPEED; + PI_speed.gain_p = P_FACTOR_SPEED; PI_speed.setpoint = 0; PI_speed.limit_output = PH_CURRENT_MAX; - PI_speed.max_step=50; - PI_speed.shift=5; - PI_speed.limit_i=PH_CURRENT_MAX; + PI_speed.max_step = 50; + PI_speed.shift = 5; + PI_speed.limit_i = PH_CURRENT_MAX; #endif - //Virtual EEPROM init + // Virtual EEPROM init HAL_FLASH_Unlock(); EE_Init(); HAL_FLASH_Lock(); @@ -411,21 +402,20 @@ int main(void) } /* USER CODE BEGIN 2 */ - SET_BIT(ADC1->CR2, ADC_CR2_JEXTTRIG);//external trigger enable - __HAL_ADC_ENABLE_IT(&hadc1,ADC_IT_JEOC); - SET_BIT(ADC2->CR2, ADC_CR2_JEXTTRIG);//external trigger enable - __HAL_ADC_ENABLE_IT(&hadc2,ADC_IT_JEOC); - + SET_BIT(ADC1->CR2, ADC_CR2_JEXTTRIG); // external trigger enable + __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC); + SET_BIT(ADC2->CR2, ADC_CR2_JEXTTRIG); // external trigger enable + __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); - //HAL_ADC_Start_IT(&hadc1); - HAL_ADCEx_MultiModeStart_DMA(&hadc1, (uint32_t*)adcData, 8); + // HAL_ADC_Start_IT(&hadc1); + HAL_ADCEx_MultiModeStart_DMA(&hadc1, (uint32_t *)adcData, 8); HAL_ADC_Start_IT(&hadc2); - MX_TIM1_Init(); //Hier die Reihenfolge getauscht! + MX_TIM1_Init(); // Hier die Reihenfolge getauscht! MX_TIM2_Init(); MX_TIM3_Init(); // Start Timer 1 - if(HAL_TIM_Base_Start_IT(&htim1) != HAL_OK) + if (HAL_TIM_Base_Start_IT(&htim1) != HAL_OK) { /* Counter Enable Error */ Error_Handler(); @@ -440,34 +430,30 @@ int main(void) HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4); + TIM1->CCR4 = TRIGGER_DEFAULT; // ADC sampling just before timer overflow (just before middle of PWM-Cycle) + // PWM Mode 1: Interrupt at counting down. - - - TIM1->CCR4 = TRIGGER_DEFAULT; //ADC sampling just before timer overflow (just before middle of PWM-Cycle) - //PWM Mode 1: Interrupt at counting down. - - //TIM1->BDTR |= 1L<<15; - // TIM1->BDTR &= ~(1L<<15); //reset MOE (Main Output Enable) bit to disable PWM output - // Start Timer 2 + // TIM1->BDTR |= 1L<<15; + // TIM1->BDTR &= ~(1L<<15); //reset MOE (Main Output Enable) bit to disable PWM output + // Start Timer 2 HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_1); - //HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_2); - //HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_3); + // HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_2); + // HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_3); // Start Timer 3 - if(HAL_TIM_Base_Start_IT(&htim3) != HAL_OK) + if (HAL_TIM_Base_Start_IT(&htim3) != HAL_OK) { /* Counter Enable Error */ Error_Handler(); } - #if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER || DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) - KingMeter_Init (&KM); + KingMeter_Init(&KM); #endif #if (DISPLAY_TYPE & DISPLAY_TYPE_BAFANG) - Bafang_Init (&BF); + Bafang_Init(&BF); #endif #if (DISPLAY_TYPE == DISPLAY_TYPE_KUNTENG) @@ -482,136 +468,147 @@ int main(void) #if (DISPLAY_TYPE == DISPLAY_TYPE_NO2) No2_Init(&No2); #endif - TIM1->CCR1 = 1023; //set initial PWM values + TIM1->CCR1 = 1023; // set initial PWM values TIM1->CCR2 = 1023; TIM1->CCR3 = 1023; + CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE); // Disable PWM - CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE);//Disable PWM + HAL_Delay(500); // wait for stable conditions - HAL_Delay(500); //wait for stable conditions - - for(i=0;i<32;i++){ - while(!ui8_adc_regular_flag){} - temp1+=adcData[2]; - temp2+=adcData[3]; - temp3+=adcData[4]; - ui8_adc_regular_flag=0; + for (i = 0; i < 32; i++) + { + while (!ui8_adc_regular_flag) + { + } + temp1 += adcData[2]; + temp2 += adcData[3]; + temp3 += adcData[4]; + ui8_adc_regular_flag = 0; #ifdef TQONAD1 - temp4+=adcData[6]; + temp4 += adcData[6]; #else - temp4+=adcData[1]; + temp4 += adcData[1]; #endif } - ui16_ph1_offset=temp1>>5; - ui16_ph2_offset=temp2>>5; - ui16_ph3_offset=temp3>>5; - ui16_throttle_offset=(temp4>>5)+5; + ui16_ph1_offset = temp1 >> 5; + ui16_ph2_offset = temp2 >> 5; + ui16_ph3_offset = temp3 >> 5; + ui16_throttle_offset = (temp4 >> 5) + 5; -#ifdef DISABLE_DYNAMIC_ADC // set injected channel with offsets - ADC1->JSQR=0b00100000000000000000; //ADC1 injected reads phase A JL = 0b00, JSQ4 = 0b00100 (decimal 4 = channel 4) +#ifdef DISABLE_DYNAMIC_ADC // set injected channel with offsets + ADC1->JSQR = 0b00100000000000000000; // ADC1 injected reads phase A JL = 0b00, JSQ4 = 0b00100 (decimal 4 = channel 4) ADC1->JOFR1 = ui16_ph1_offset; - ADC2->JSQR=0b00101000000000000000; //ADC2 injected reads phase B, JSQ4 = 0b00101, decimal 5 + ADC2->JSQR = 0b00101000000000000000; // ADC2 injected reads phase B, JSQ4 = 0b00101, decimal 5 ADC2->JOFR1 = ui16_ph2_offset; #endif - ui8_adc_offset_done_flag=1; + ui8_adc_offset_done_flag = 1; init_watchdog(); -#if defined (ADC_BRAKE) +#if defined(ADC_BRAKE) - while ((adcData[5]>ui16_throttle_offset)&&(adcData[1]>(THROTTLE_MAX-ui16_throttle_offset))){HAL_Delay(200); - HAL_IWDG_Refresh(&hiwdg); - y++; - if(y==35) autodetect(); + while ((adcData[5] > ui16_throttle_offset) && (adcData[1] > (THROTTLE_MAX - ui16_throttle_offset))) + { + HAL_Delay(200); + HAL_IWDG_Refresh(&hiwdg); + y++; + if (y == 35) + autodetect(); } #endif - //run autodect, whenn brake is pulled an throttle is pulled for 10 at startup + // run autodect, whenn brake is pulled an throttle is pulled for 10 at startup #ifndef NCTE - while ((!HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin))&&(adcData[1]>(ui16_throttle_offset+20))){ + while ((!HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin))) + { HAL_IWDG_Refresh(&hiwdg); HAL_Delay(200); y++; - if(y==35) autodetect(); + if (y == 35) + autodetect(); } #else - ui32_throttle_cumulated=ui16_throttle_offset<<4; + ui32_throttle_cumulated = ui16_throttle_offset << 4; #endif #if (DISPLAY_TYPE == DISPLAY_TYPE_DEBUG) printf_("phase current offsets: %d, %d, %d \n ", ui16_ph1_offset, ui16_ph2_offset, ui16_ph3_offset); printf_("internal temperature raw reading: %d, \n ", adcData[7]); #if (AUTODETECT == 1) - if(adcData[0]>VOLTAGE_MIN) autodetect(); - else printf_("Battery voltage too low!: %d,\n ",adcData[0]); + if (adcData[0] > VOLTAGE_MIN) + autodetect(); + else + printf_("Battery voltage too low!: %d,\n ", adcData[0]); #endif #endif #ifdef NCTE - while(adcData[1]ui16_throttle_offset) + // while(adcData[1]>ui16_throttle_offset) #endif { - HAL_IWDG_Refresh(&hiwdg);//do nothing (For Safety at switching on) + HAL_IWDG_Refresh(&hiwdg); // do nothing (For Safety at switching on) } - //read internal temp calibration from emulated EEPROM + // read internal temp calibration from emulated EEPROM EE_ReadVariable(EEPROM_INT_TEMP_V25, &i16_int_Temp_V25); - if(!i16_int_Temp_V25)i16_int_Temp_V25=INT_TEMP_25; //use value from main.h, if not in EEPROM yet. + if (!i16_int_Temp_V25) + i16_int_Temp_V25 = INT_TEMP_25; // use value from main.h, if not in EEPROM yet. #if (!USE_FIX_POSITIONS) EE_ReadVariable(EEPROM_POS_HALL_ORDER, &i16_hall_order); - printf_("Hall_Order: %d \n",i16_hall_order); + printf_("Hall_Order: %d \n", i16_hall_order); // set varaiables to value from emulated EEPROM only if valid - if(i16_hall_order!=0xFFFF) { + if (i16_hall_order != 0xFFFF) + { int16_t temp; EE_ReadVariable(EEPROM_POS_HALL_45, &temp); - Hall_45 = temp<<16; - printf_("Hall_45: %d \n", (int16_t) (((Hall_45 >> 23) * 180) >> 8)); - printf_("Hall_45: %u \n", Hall_45); + Hall_45 = temp << 16; + printf_("Hall_45: %d \n", (int16_t)(((Hall_45 >> 23) * 180) >> 8)); + printf_("Hall_45: %u \n", Hall_45); HAL_IWDG_Refresh(&hiwdg); EE_ReadVariable(EEPROM_POS_HALL_51, &temp); - Hall_51 = temp<<16; - printf_("Hall_51: %d \n", (int16_t) (((Hall_51 >> 23) * 180) >> 8)); - printf_("Hall_51: %u \n", Hall_51); + Hall_51 = temp << 16; + printf_("Hall_51: %d \n", (int16_t)(((Hall_51 >> 23) * 180) >> 8)); + printf_("Hall_51: %u \n", Hall_51); HAL_IWDG_Refresh(&hiwdg); EE_ReadVariable(EEPROM_POS_HALL_13, &temp); - Hall_13 = temp<<16; - printf_("Hall_13: %d \n", (int16_t) (((Hall_13 >> 23) * 180) >> 8)); - printf_("Hall_13: %u \n", Hall_13); + Hall_13 = temp << 16; + printf_("Hall_13: %d \n", (int16_t)(((Hall_13 >> 23) * 180) >> 8)); + printf_("Hall_13: %u \n", Hall_13); HAL_IWDG_Refresh(&hiwdg); EE_ReadVariable(EEPROM_POS_HALL_32, &temp); - Hall_32 = temp<<16; - printf_("Hall_32: %d \n", (int16_t) (((Hall_32 >> 23) * 180) >> 8)); - printf_("Hall_32: %u \n", Hall_32); + Hall_32 = temp << 16; + printf_("Hall_32: %d \n", (int16_t)(((Hall_32 >> 23) * 180) >> 8)); + printf_("Hall_32: %u \n", Hall_32); HAL_IWDG_Refresh(&hiwdg); EE_ReadVariable(EEPROM_POS_HALL_26, &temp); - Hall_26 = temp<<16; - printf_("Hall_26: %d \n", (int16_t) (((Hall_26 >> 23) * 180) >> 8)); - printf_("Hall_26: %u \n", Hall_26); + Hall_26 = temp << 16; + printf_("Hall_26: %d \n", (int16_t)(((Hall_26 >> 23) * 180) >> 8)); + printf_("Hall_26: %u \n", Hall_26); HAL_IWDG_Refresh(&hiwdg); EE_ReadVariable(EEPROM_POS_HALL_64, &temp); - Hall_64 = temp<<16; - printf_("Hall_64: %d \n", (int16_t) (((Hall_64 >> 23) * 180) >> 8)); - printf_("Hall_64: %u \n", Hall_64); + Hall_64 = temp << 16; + printf_("Hall_64: %d \n", (int16_t)(((Hall_64 >> 23) * 180) >> 8)); + printf_("Hall_64: %u \n", Hall_64); HAL_IWDG_Refresh(&hiwdg); EE_ReadVariable(EEPROM_POS_KV, &ui32_KV); - if(!ui32_KV)ui32_KV=111; - printf_("KV: %d \n",ui32_KV ); + if (!ui32_KV) + ui32_KV = 111; + printf_("KV: %d \n", ui32_KV); HAL_IWDG_Refresh(&hiwdg); - } #else @@ -625,7 +622,6 @@ int main(void) Hall_64 = HALL_64; #endif - // set absolute position to corresponding hall pattern. #if (DISPLAY_TYPE == DISPLAY_TYPE_DEBUG) @@ -633,15 +629,12 @@ int main(void) #endif - - CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE);//Disable PWM + CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE); // Disable PWM get_standstill_position(); - // init_watchdog(); - /* USER CODE END 2 */ /* Infinite loop */ @@ -650,14 +643,14 @@ int main(void) { HAL_IWDG_Refresh(&hiwdg); - /* if(PI_flag){ runPIcontrol(); PI_flag=0; }*/ - //display message processing - if(ui8_UART_flag){ -#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U||DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) + // display message processing + if (ui8_UART_flag) + { +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U || DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) KingMeter_Service(&KM); #endif @@ -672,8 +665,10 @@ int main(void) #if (DISPLAY_TYPE == DISPLAY_TYPE_KUNTENG) check_message(&MS, &MP); - if(MS.assist_level==6)ui8_Push_Assist_flag=1; - else ui8_Push_Assist_flag=0; + if (MS.assist_level == 6) + ui8_Push_Assist_flag = 1; + else + ui8_Push_Assist_flag = 0; #endif #if (DISPLAY_TYPE & DISPLAY_TYPE_EBiCS) @@ -683,1996 +678,2036 @@ int main(void) #if (DISPLAY_TYPE == DISPLAY_TYPE_NO2) No2_Service(&No2); #endif - ui8_UART_flag=0; + ui8_UART_flag = 0; } - - //process regualr ADC - if(ui8_adc_regular_flag){ - ui32_throttle_cumulated -= ui32_throttle_cumulated>>4; + // process regualr ADC + if (ui8_adc_regular_flag) + { + ui32_throttle_cumulated -= ui32_throttle_cumulated >> 4; #ifdef TQONAD1 - ui32_throttle_cumulated += adcData[6]; //get value from AD1 PB1 + ui32_throttle_cumulated += adcData[6]; // get value from AD1 PB1 #else - ui32_throttle_cumulated += adcData[1]; //get value from SP + ui32_throttle_cumulated += adcData[1]; // get value from SP #endif - ui32_brake_adc_cumulated -= ui32_brake_adc_cumulated>>4; - ui32_brake_adc_cumulated+=adcData[5];//get value for analog brake from AD2 = PB0 - ui16_brake_adc=ui32_brake_adc_cumulated>>4; - ui16_throttle = ui32_throttle_cumulated>>4; + ui32_brake_adc_cumulated -= ui32_brake_adc_cumulated >> 4; + ui32_brake_adc_cumulated += adcData[5]; // get value for analog brake from AD2 = PB0 + ui16_brake_adc = ui32_brake_adc_cumulated >> 4; + ui16_throttle = ui32_throttle_cumulated >> 4; - ui8_adc_regular_flag=0; + ui8_adc_regular_flag = 0; } - //PAS signal processing - if(ui8_PAS_flag){ + // PAS signal processing + if (ui8_PAS_flag) + { - if(uint32_PAS_counter>100){ //debounce - uint32_PAS_cumulated -= uint32_PAS_cumulated>>2; + if (uint32_PAS_counter > 100) + { // debounce + uint32_PAS_cumulated -= uint32_PAS_cumulated >> 2; uint32_PAS_cumulated += uint32_PAS_counter; - uint32_PAS = uint32_PAS_cumulated>>2; + uint32_PAS = uint32_PAS_cumulated >> 2; - uint32_PAS_HIGH_accumulated-=uint32_PAS_HIGH_accumulated>>2; - uint32_PAS_HIGH_accumulated+=uint32_PAS_HIGH_counter; + uint32_PAS_HIGH_accumulated -= uint32_PAS_HIGH_accumulated >> 2; + uint32_PAS_HIGH_accumulated += uint32_PAS_HIGH_counter; - uint32_PAS_fraction=(uint32_PAS_HIGH_accumulated>>2)*100/uint32_PAS; - uint32_PAS_HIGH_counter=0; - uint32_PAS_counter =0; - ui8_PAS_flag=0; - //read in and sum up torque-signal within one crank revolution (for sempu sensor 32 PAS pulses/revolution, 2^5=32) - uint32_torque_cumulated -= (uint32_torque_cumulated*PAS_IMP_PER_TURN_RECIP_MULTIPLIER) >> 8 ; + uint32_PAS_fraction = (uint32_PAS_HIGH_accumulated >> 2) * 100 / uint32_PAS; + uint32_PAS_HIGH_counter = 0; + uint32_PAS_counter = 0; + ui8_PAS_flag = 0; + // read in and sum up torque-signal within one crank revolution (for sempu sensor 32 PAS pulses/revolution, 2^5=32) + uint32_torque_cumulated -= (uint32_torque_cumulated * PAS_IMP_PER_TURN_RECIP_MULTIPLIER) >> 8; #ifdef NCTE - if(ui16_throttleui16_throttle_offset)uint32_torque_cumulated += (ui16_throttle-ui16_throttle_offset); + if (ui16_throttle > ui16_throttle_offset) + uint32_torque_cumulated += (ui16_throttle - ui16_throttle_offset); #endif - - } } - //SPEED signal processing + // SPEED signal processing #if (SPEEDSOURCE == INTERNAL) - MS.Speed = uint32_tics_filtered>>3; + MS.Speed = uint32_tics_filtered >> 3; #else - if(ui8_SPEED_flag){ + if (ui8_SPEED_flag) + { // HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); - if(uint32_SPEED_counter>200){ //debounce + if (uint32_SPEED_counter > 200) + { // debounce MS.Speed = uint32_SPEED_counter; - //HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); - uint32_SPEED_counter =0; - ui8_SPEED_flag=0; - uint32_SPEEDx100_cumulated -=uint32_SPEEDx100_cumulated>>SPEEDFILTER; - uint32_SPEEDx100_cumulated +=external_tics_to_speedx100(MS.Speed); + // HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); + uint32_SPEED_counter = 0; + ui8_SPEED_flag = 0; + uint32_SPEEDx100_cumulated -= uint32_SPEEDx100_cumulated >> SPEEDFILTER; + uint32_SPEEDx100_cumulated += external_tics_to_speedx100(MS.Speed); } } #endif - if(ui8_SPEED_control_flag){ + if (ui8_SPEED_control_flag) + { #if (SPEEDSOURCE == INTERNAL) - uint32_SPEEDx100_cumulated -=uint32_SPEEDx100_cumulated>>SPEEDFILTER; - uint32_SPEEDx100_cumulated +=internal_tics_to_speedx100(uint32_tics_filtered>>3); + uint32_SPEEDx100_cumulated -= uint32_SPEEDx100_cumulated >> SPEEDFILTER; + uint32_SPEEDx100_cumulated += internal_tics_to_speedx100(uint32_tics_filtered >> 3); #endif - ui16_erps=500000/((uint32_tics_filtered>>3)*6); - ui8_SPEED_control_flag=0; + ui16_erps = 500000 / ((uint32_tics_filtered >> 3) * 6); + ui8_SPEED_control_flag = 0; } #if (DISPLAY_TYPE == DISPLAY_TYPE_DEBUG && defined(FAST_LOOP_LOG)) - if(ui8_debug_state==3 && ui8_UART_TxCplt_flag){ - sprintf_(buffer, "%d, %d, %d, %d, %d, %d\r\n", e_log[k][0], e_log[k][1], e_log[k][2],e_log[k][3],e_log[k][4],e_log[k][5]); //>>24 - i=0; + if (ui8_debug_state == 3 && ui8_UART_TxCplt_flag) + { + sprintf_(buffer, "%d, %d, %d, %d, %d, %d\r\n", e_log[k][0], e_log[k][1], e_log[k][2], e_log[k][3], e_log[k][4], e_log[k][5]); //>>24 + i = 0; while (buffer[i] != '\0') - {i++;} - ui8_UART_TxCplt_flag=0; + { + i++; + } + ui8_UART_TxCplt_flag = 0; HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&buffer, i); k++; - if (k>299){ - k=0; - ui8_debug_state=0; - //Obs_flag=0; + if (k > 299) + { + k = 0; + ui8_debug_state = 0; + // Obs_flag=0; } } #endif //-------------------------------------------------------------------------------------------------------------------------------------------------- - //current target calculation - //highest priority: regen by brake lever - + // current target calculation + // highest priority: regen by brake lever #ifdef ADC_BRAKE - uint16_mapped_BRAKE = map(ui16_brake_adc, ui16_throttle_offset , THROTTLE_MAX, 0, REGEN_CURRENT); - + uint16_mapped_BRAKE = map(ui16_brake_adc, ui16_throttle_offset, THROTTLE_MAX, 0, REGEN_CURRENT); - if(uint16_mapped_BRAKE>0) brake_flag=1; - else brake_flag=0; - - - if(brake_flag){ + if (uint16_mapped_BRAKE > 0) + brake_flag = 1; + else + brake_flag = 0; - //if(!HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)){ - //if(tics_to_speed(uint32_tics_filtered>>3)>6)int32_current_target=-REGEN_CURRENT; //only apply regen, if motor is turning fast enough - if(tics_to_speed(uint32_tics_filtered>>3)>6)int32_temp_current_target=uint16_mapped_BRAKE; - else int32_temp_current_target=0; + if (brake_flag) + { + // if(!HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)){ + // if(tics_to_speed(uint32_tics_filtered>>3)>6)int32_current_target=-REGEN_CURRENT; //only apply regen, if motor is turning fast enough + if (tics_to_speed(uint32_tics_filtered >> 3) > 6) + int32_temp_current_target = uint16_mapped_BRAKE; + else + int32_temp_current_target = 0; #else - if(HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)) brake_flag=0; - else brake_flag=1; - if(brake_flag){ + if (HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)) + brake_flag = 0; + else + brake_flag = 1; + if (brake_flag) + { - if(tics_to_speed(uint32_tics_filtered>>3)>6){ - int32_temp_current_target=REGEN_CURRENT; //only apply regen, if motor is turning fast enough - } - else int32_temp_current_target=0; + if (tics_to_speed(uint32_tics_filtered >> 3) > 6) + { + int32_temp_current_target = REGEN_CURRENT; // only apply regen, if motor is turning fast enough + } + else + int32_temp_current_target = 0; #endif - int32_temp_current_target= -map(MS.Voltage*CAL_V,BATTERYVOLTAGE_MAX-1000,BATTERYVOLTAGE_MAX,int32_temp_current_target,0); - } + int32_temp_current_target = -map(MS.Voltage * CAL_V, BATTERYVOLTAGE_MAX - 1000, BATTERYVOLTAGE_MAX, int32_temp_current_target, 0); + } - //next priority: undervoltage protection - else if(MS.Voltage>8; //does not work for BAFANG and Kunteng protocol actually - // last priority normal ride conditiones - else { - -#ifdef TS_MODE //torque-sensor mode - //calculate current target form torque, cadence and assist level - int32_temp_current_target = (TS_COEF*(int32_t)(MS.assist_level)* ((uint32_torque_cumulated*PAS_IMP_PER_TURN_RECIP_MULTIPLIER)>>8)/uint32_PAS)>>8; // >>8 aus KM5S-Protokoll Assistlevel 0..255 - - //limit currest target to max value - if(int32_temp_current_target>PH_CURRENT_MAX) int32_temp_current_target = PH_CURRENT_MAX; - //set target to zero, if pedals are not turning - if(uint32_PAS_counter > PAS_TIMEOUT){ - int32_temp_current_target = 0; - if(uint32_torque_cumulated>0)uint32_torque_cumulated--; //ramp down cumulated torque value - } + // next priority: undervoltage protection + else if (MS.Voltage < VOLTAGE_MIN) + int32_temp_current_target = 0; + // next priority: push assist + else if (ui8_Push_Assist_flag) + int32_temp_current_target = (MS.assist_level * PUSHASSIST_CURRENT) >> 8; // does not work for BAFANG and Kunteng protocol actually + // last priority normal ride conditiones + else + { +#ifdef TS_MODE // torque-sensor mode + // calculate current target form torque, cadence and assist level + int32_temp_current_target = (TS_COEF * (int32_t)(MS.assist_level) * ((uint32_torque_cumulated * PAS_IMP_PER_TURN_RECIP_MULTIPLIER) >> 8) / uint32_PAS) >> 8; // >>8 aus KM5S-Protokoll Assistlevel 0..255 + // limit currest target to max value + if (int32_temp_current_target > PH_CURRENT_MAX) + int32_temp_current_target = PH_CURRENT_MAX; + // set target to zero, if pedals are not turning + if (uint32_PAS_counter > PAS_TIMEOUT) + { + int32_temp_current_target = 0; + if (uint32_torque_cumulated > 0) + uint32_torque_cumulated--; // ramp down cumulated torque value + } -#else // torque-simulation mode with throttle override +#else // torque-simulation mode with throttle override #if (DISPLAY_TYPE & DISPLAY_TYPE_BAFANG) - uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, (PH_CURRENT_MAX*(int32_t)(assist_factor[MS.assist_level]))>>8, 0); // level in range 0...5 + uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, (PH_CURRENT_MAX * (int32_t)(assist_factor[MS.assist_level])) >> 8, 0); // level in range 0...5 #endif -#if (DISPLAY_TYPE == DISPLAY_TYPE_KUNTENG) - uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, (PH_CURRENT_MAX*(int32_t)(MS.assist_level))/5, 0); // level in range 0...5 +#if (DISPLAY_TYPE & DISPLAY_TYPE_KUNTENG) + uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, (PH_CURRENT_MAX * (int32_t)(MS.assist_level)) / 5, 0); // level in range 0...5 #endif -#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_618U) - uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, (PH_CURRENT_MAX*(int32_t)(MS.assist_level-1))>>2, 0); // level in range 1...5 +#if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER_618U) + uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, (PH_CURRENT_MAX * (int32_t)(MS.assist_level - 1)) >> 2, 0); // level in range 1...5 #endif -#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U||DISPLAY_TYPE == DISPLAY_TYPE_NO2) - uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, ((PH_CURRENT_MAX*(int32_t)(MS.assist_level)))>>8, 0); // level in range 0...255 +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U || DISPLAY_TYPE == DISPLAY_TYPE_NO2) + uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, ((PH_CURRENT_MAX * (int32_t)(MS.assist_level))) >> 8, 0); // level in range 0...255 #endif #if (DISPLAY_TYPE == DISPLAY_TYPE_DEBUG) - uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, PH_CURRENT_MAX, 0); // Full amps in debug mode + uint16_mapped_PAS = map(uint32_PAS, RAMP_END, PAS_TIMEOUT, PH_CURRENT_MAX, 0); // Full amps in debug mode #endif - - #ifdef DIRDET - if (uint32_PAS_counter< PAS_TIMEOUT){ - if ((uint32_PAS_fraction < FRAC_LOW ||uint32_PAS_fraction > FRAC_HIGH)){ - uint16_mapped_PAS= 0;//pedals are turning backwards, stop motor - } + if (uint32_PAS_counter < PAS_TIMEOUT) + { + if ((uint32_PAS_fraction < FRAC_LOW || uint32_PAS_fraction > FRAC_HIGH)) + { + uint16_mapped_PAS = 0; // pedals are turning backwards, stop motor } - else uint32_PAS_HIGH_accumulated=uint32_PAS_cumulated; -#endif //end direction detection - + } + else + uint32_PAS_HIGH_accumulated = uint32_PAS_cumulated; +#endif // end direction detection -#endif //end if else TQ sensor mode +#endif // end if else TQ sensor mode #ifdef INDIVIDUAL_MODES - uint16_mapped_PAS = (uint16_mapped_PAS * ui8_speedfactor)>>8; + uint16_mapped_PAS = (uint16_mapped_PAS * ui8_speedfactor) >> 8; #endif #ifdef THROTTLE_OVERRIDE - #ifdef NCTE - // read in throttle for throttle override - uint16_mapped_throttle = map(ui16_throttle, THROTTLE_MAX, ui16_throttle_offset,PH_CURRENT_MAX,0); + // read in throttle for throttle override + uint16_mapped_throttle = map(ui16_throttle, THROTTLE_MAX, ui16_throttle_offset, PH_CURRENT_MAX, 0); +#else // else NTCE + // read in throttle for throttle override + uint16_mapped_throttle = map(ui16_throttle, ui16_throttle_offset, THROTTLE_MAX, 0, PH_CURRENT_MAX); -#else //else NTCE - // read in throttle for throttle override - uint16_mapped_throttle = map(ui16_throttle, ui16_throttle_offset, THROTTLE_MAX, 0,PH_CURRENT_MAX); +#endif // end NTCE -#endif //end NTCE +#ifndef TS_MODE // normal PAS Mode -#ifndef TS_MODE //normal PAS Mode - - if (uint32_PAS_counter < PAS_TIMEOUT) int32_temp_current_target = uint16_mapped_PAS; //set current target in torque-simulation-mode, if pedals are turning - else { - int32_temp_current_target= 0;//pedals are not turning, stop motor - uint32_PAS_cumulated=32000; - uint32_PAS=32000; - } + if (uint32_PAS_counter < PAS_TIMEOUT) + int32_temp_current_target = uint16_mapped_PAS; // set current target in torque-simulation-mode, if pedals are turning + else + { + int32_temp_current_target = 0; // pedals are not turning, stop motor + uint32_PAS_cumulated = 32000; + uint32_PAS = 32000; + } -#endif // end #ifndef TS_MODE - //check for throttle override - if(uint16_mapped_throttle>int32_temp_current_target){ +#endif // end #ifndef TS_MODE + // check for throttle override + if (uint16_mapped_throttle > int32_temp_current_target) + { #ifdef SPEEDTHROTTLE - - uint16_mapped_throttle = uint16_mapped_throttle*SPEEDLIMIT/PH_CURRENT_MAX;//throttle override: calulate speed target from thottle - - - - - PI_speed.setpoint = uint16_mapped_throttle*100; - PI_speed.recent_value = internal_tics_to_speedx100(uint32_tics_filtered>>3); - if( PI_speed.setpoint)SET_BIT(TIM1->BDTR, TIM_BDTR_MOE); - if (internal_tics_to_speedx100(uint32_tics_filtered>>3)<300){//control current slower than 3 km/h - PI_speed.limit_i=100; - PI_speed.limit_output=100; - int32_temp_current_target = PI_control(&PI_speed); - - if(int32_temp_current_target>100)int32_temp_current_target=100; - if(int32_temp_current_target*i8_direction*i8_reverse_flag<0){ - int32_temp_current_target=0; - } - + uint16_mapped_throttle = uint16_mapped_throttle * SPEEDLIMIT / PH_CURRENT_MAX; // throttle override: calulate speed target from thottle + + PI_speed.setpoint = uint16_mapped_throttle * 100; + PI_speed.recent_value = internal_tics_to_speedx100(uint32_tics_filtered >> 3); + if (PI_speed.setpoint) + SET_BIT(TIM1->BDTR, TIM_BDTR_MOE); + if (internal_tics_to_speedx100(uint32_tics_filtered >> 3) < 300) + { // control current slower than 3 km/h + PI_speed.limit_i = 100; + PI_speed.limit_output = 100; + int32_temp_current_target = PI_control(&PI_speed); + + if (int32_temp_current_target > 100) + int32_temp_current_target = 100; + if (int32_temp_current_target * i8_direction * i8_reverse_flag < 0) + { + int32_temp_current_target = 0; } - else{ - - - if(ui8_SPEED_control_flag){//update current target only, if new hall event was detected - PI_speed.limit_i=PH_CURRENT_MAX; - PI_speed.limit_output=PH_CURRENT_MAX; - int32_temp_current_target = PI_control(&PI_speed); - ui8_SPEED_control_flag=0; - } - if(int32_temp_current_target*i8_direction*i8_reverse_flag<0)int32_temp_current_target=0; + } + else + { + if (ui8_SPEED_control_flag) + { // update current target only, if new hall event was detected + PI_speed.limit_i = PH_CURRENT_MAX; + PI_speed.limit_output = PH_CURRENT_MAX; + int32_temp_current_target = PI_control(&PI_speed); + ui8_SPEED_control_flag = 0; } + if (int32_temp_current_target * i8_direction * i8_reverse_flag < 0) + int32_temp_current_target = 0; + } +#else // else speedthrottle + int32_temp_current_target = uint16_mapped_throttle; +#endif // end speedthrottle + } // end else of throttle override -#else // else speedthrottle - int32_temp_current_target=uint16_mapped_throttle; -#endif //end speedthrottle - - } //end else of throttle override - -#endif //end throttle override +#endif // end throttle override - } //end else for normal riding - //ramp down setpoint at speed limit + } // end else for normal riding + // ramp down setpoint at speed limit #ifdef LEGALFLAG - if(!brake_flag){ //only ramp down if no regen active - if(uint32_PAS_counter>SPEEDFILTER, MP.speedLimit*100,(MP.speedLimit+2)*100,int32_temp_current_target,0); - } - else{ //limit to 6km/h if pedals are not turning - int32_temp_current_target=map(uint32_SPEEDx100_cumulated>>SPEEDFILTER, 500,700,int32_temp_current_target,0); - } + if (!brake_flag) + { // only ramp down if no regen active + if (uint32_PAS_counter < PAS_TIMEOUT) + { + int32_temp_current_target = map(uint32_SPEEDx100_cumulated >> SPEEDFILTER, MP.speedLimit * 100, (MP.speedLimit + 2) * 100, int32_temp_current_target, 0); } - // else int32_temp_current_target=int32_temp_current_target; + else + { // limit to 6km/h if pedals are not turning + int32_temp_current_target = map(uint32_SPEEDx100_cumulated >> SPEEDFILTER, 500, 700, int32_temp_current_target, 0); + } + } + // else int32_temp_current_target=int32_temp_current_target; -#endif //legalflag +#endif // legalflag #if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER || DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) - if(KM.DirectSetpoint!=-1)int32_temp_current_target=(KM.DirectSetpoint*PH_CURRENT_MAX)>>7; -#endif - MS.i_q_setpoint=map(MS.Temperature, MOTOR_TEMPERATURE_THRESHOLD,MOTOR_TEMPERATURE_MAX,int32_temp_current_target,0); //ramp down power with temperature to avoid overheating the motor -#if(INT_TEMP_25) - MS.i_q_setpoint=map(MS.int_Temperature, CONTROLLER_TEMPERATURE_THRESHOLD,CONTROLLER_TEMPERATURE_MAX,MS.i_q_setpoint,0); //ramp down power with processor temperatur to avoid overheating the controller -#endif - - //auto KV detect - if(ui8_KV_detect_flag){ - MS.i_q_setpoint=ui8_KV_detect_flag; - if(ui16_KV_detect_counter>32){ - ui8_KV_detect_flag++; - ui16_KV_detect_counter=0; - if(MS.u_abs>1900){ - ui8_KV_detect_flag=0; - HAL_FLASH_Unlock(); - EE_WriteVariable(EEPROM_POS_KV, (int16_t) ui32_KV); - HAL_FLASH_Lock(); - } - } - ui32_KV -=ui32_KV>>4; - ui32_KV += ((uint32_SPEEDx100_cumulated*_T))/(MS.Voltage*MS.u_q); - - - }//end KV detect - - - - //------------------------------------------------------------------------------------------------------------ - //enable PWM if power is wanted - if (MS.i_q_setpoint&&!READ_BIT(TIM1->BDTR, TIM_BDTR_MOE)){ + if (KM.DirectSetpoint != -1) + int32_temp_current_target = (KM.DirectSetpoint * PH_CURRENT_MAX) >> 7; +#endif + MS.i_q_setpoint = map(MS.Temperature, MOTOR_TEMPERATURE_THRESHOLD, MOTOR_TEMPERATURE_MAX, int32_temp_current_target, 0); // ramp down power with temperature to avoid overheating the motor +#if (INT_TEMP_25) + MS.i_q_setpoint = map(MS.int_Temperature, CONTROLLER_TEMPERATURE_THRESHOLD, CONTROLLER_TEMPERATURE_MAX, MS.i_q_setpoint, 0); // ramp down power with processor temperatur to avoid overheating the controller +#endif - uint16_half_rotation_counter=0; - uint16_full_rotation_counter=0; - TIM1->CCR1 = 1023; //set initial PWM values - TIM1->CCR2 = 1023; - TIM1->CCR3 = 1023; - SET_BIT(TIM1->BDTR, TIM_BDTR_MOE); - if(SystemState == Stop)speed_PLL(0,0,0);//reset integral part - else { - PI_iq.integral_part = ((((uint32_SPEEDx100_cumulated*_T))/(MS.Voltage*ui32_KV))<<4)< 32) + { + ui8_KV_detect_flag++; + ui16_KV_detect_counter = 0; + if (MS.u_abs > 1900) + { + ui8_KV_detect_flag = 0; + HAL_FLASH_Unlock(); + EE_WriteVariable(EEPROM_POS_KV, (int16_t)ui32_KV); + HAL_FLASH_Lock(); } - __HAL_TIM_SET_COUNTER(&htim2,0); //reset tim2 counter - ui16_timertics=20000; //set interval between two hallevents to a large value - i8_recent_rotor_direction=i8_direction*i8_reverse_flag*sign(MS.i_q_setpoint); - get_standstill_position(); } + ui32_KV -= ui32_KV >> 4; + ui32_KV += ((uint32_SPEEDx100_cumulated * _T)) / (MS.Voltage * MS.u_q); + } // end KV detect - //---------------------------------------------------------------------------------------------------------------------------------------------------------- - //slow loop procedere @16Hz, for LEV standard every 4th loop run, send page, - if(ui32_tim3_counter>500){ - if(__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST)) HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); - else HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); + //------------------------------------------------------------------------------------------------------------ + // enable PWM if power is wanted + if (MS.i_q_setpoint && !READ_BIT(TIM1->BDTR, TIM_BDTR_MOE)) + { + uint16_half_rotation_counter = 0; + uint16_full_rotation_counter = 0; + TIM1->CCR1 = 1023; // set initial PWM values + TIM1->CCR2 = 1023; + TIM1->CCR3 = 1023; + SET_BIT(TIM1->BDTR, TIM_BDTR_MOE); + if (SystemState == Stop) + speed_PLL(0, 0, 0); // reset integral part + else + { + PI_iq.integral_part = ((((uint32_SPEEDx100_cumulated * _T)) / (MS.Voltage * ui32_KV)) << 4) << PI_iq.shift; + PI_iq.out = PI_iq.integral_part; + } + __HAL_TIM_SET_COUNTER(&htim2, 0); // reset tim2 counter + ui16_timertics = 20000; // set interval between two hallevents to a large value + i8_recent_rotor_direction = i8_direction * i8_reverse_flag * sign(MS.i_q_setpoint); + get_standstill_position(); + } + //---------------------------------------------------------------------------------------------------------------------------------------------------------- + // slow loop procedere @16Hz, for LEV standard every 4th loop run, send page, + if (ui32_tim3_counter > 500) + { + if (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST)) + HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); + else + HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); - if(ui8_KV_detect_flag){ui16_KV_detect_counter++;} + if (ui8_KV_detect_flag) + { + ui16_KV_detect_counter++; + } #if (R_TEMP_PULLUP) - MS.Temperature = T_NTC(adcData[6]); //Thank you Hendrik ;-) + MS.Temperature = T_NTC(adcData[6]); // Thank you Hendrik ;-) #else - MS.Temperature=25; + MS.Temperature = 25; #endif - //filter internal temperature reading - ui32_int_Temp_cumulated-=ui32_int_Temp_cumulated>>5; - ui32_int_Temp_cumulated+=adcData[7]; - MS.int_Temperature=(((i16_int_Temp_V25-(ui32_int_Temp_cumulated>>5))*24)>>7)+25; + // filter internal temperature reading + ui32_int_Temp_cumulated -= ui32_int_Temp_cumulated >> 5; + ui32_int_Temp_cumulated += adcData[7]; + MS.int_Temperature = (((i16_int_Temp_V25 - (ui32_int_Temp_cumulated >> 5)) * 24) >> 7) + 25; - MS.Voltage=adcData[0]; - if(uint32_SPEED_counter>32000){ - MS.Speed = 32000; + MS.Voltage = adcData[0]; + if (uint32_SPEED_counter > 32000) + { + MS.Speed = 32000; #if (SPEEDSOURCE == EXTERNAL) - uint32_SPEEDx100_cumulated=0; + uint32_SPEEDx100_cumulated = 0; #endif - } + } #ifdef INDIVIDUAL_MODES - // GET recent speedcase for assist profile - if (uint32_tics_filtered>>3 > speed_to_tics(assist_profile[0][1]))ui8_speedcase=0; - else if (uint32_tics_filtered>>3 < speed_to_tics(assist_profile[0][1]) && uint32_tics_filtered>>3 > speed_to_tics(assist_profile[0][2]))ui8_speedcase=1; - else if (uint32_tics_filtered>>3 < speed_to_tics(assist_profile[0][2]) && uint32_tics_filtered>>3 > speed_to_tics(assist_profile[0][3]))ui8_speedcase=2; - else if (uint32_tics_filtered>>3 < speed_to_tics(assist_profile[0][3]) && uint32_tics_filtered>>3 > speed_to_tics(assist_profile[0][4]))ui8_speedcase=3; - else if (uint32_tics_filtered>>3 < speed_to_tics(assist_profile[0][4]))ui8_speedcase=4; - - ui8_speedfactor = map(uint32_tics_filtered>>3,speed_to_tics(assist_profile[0][ui8_speedcase+1]),speed_to_tics(assist_profile[0][ui8_speedcase]),assist_profile[1][ui8_speedcase+1],assist_profile[1][ui8_speedcase]); + // GET recent speedcase for assist profile + if (uint32_tics_filtered >> 3 > speed_to_tics(assist_profile[0][1])) + ui8_speedcase = 0; + else if (uint32_tics_filtered >> 3 < speed_to_tics(assist_profile[0][1]) && uint32_tics_filtered >> 3 > speed_to_tics(assist_profile[0][2])) + ui8_speedcase = 1; + else if (uint32_tics_filtered >> 3 < speed_to_tics(assist_profile[0][2]) && uint32_tics_filtered >> 3 > speed_to_tics(assist_profile[0][3])) + ui8_speedcase = 2; + else if (uint32_tics_filtered >> 3 < speed_to_tics(assist_profile[0][3]) && uint32_tics_filtered >> 3 > speed_to_tics(assist_profile[0][4])) + ui8_speedcase = 3; + else if (uint32_tics_filtered >> 3 < speed_to_tics(assist_profile[0][4])) + ui8_speedcase = 4; + ui8_speedfactor = map(uint32_tics_filtered >> 3, speed_to_tics(assist_profile[0][ui8_speedcase + 1]), speed_to_tics(assist_profile[0][ui8_speedcase]), assist_profile[1][ui8_speedcase + 1], assist_profile[1][ui8_speedcase]); #endif - //check if rotor is turning - - if((uint16_full_rotation_counter>7999||uint16_half_rotation_counter>7999)){ - SystemState = Stop; - //HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); - if(READ_BIT(TIM1->BDTR, TIM_BDTR_MOE)){ - CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE); //Disable PWM if motor is not turning - uint32_tics_filtered=1000000; - get_standstill_position(); - } + // check if rotor is turning + if ((uint16_full_rotation_counter > 7999 || uint16_half_rotation_counter > 7999)) + { + SystemState = Stop; + // HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); + if (READ_BIT(TIM1->BDTR, TIM_BDTR_MOE)) + { + CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE); // Disable PWM if motor is not turning + uint32_tics_filtered = 1000000; + get_standstill_position(); } - else if(ui8_6step_flag) SystemState = SixStep; - else SystemState = Running; + } + else if (ui8_6step_flag) + SystemState = SixStep; + else + SystemState = Running; #if (DISPLAY_TYPE == DISPLAY_TYPE_DEBUG && !defined(FAST_LOOP_LOG)) - //print values for debugging - - sprintf_(buffer, "%d, %d, %d, %d, %d, %d, %d, %d, %d\r\n", - adcData[1], - ui16_throttle_offset, - ui16_timertics, - uint32_PAS, - MS.Battery_Current, - int32_temp_current_target , - MS.i_q, - MS.u_abs, - SystemState); - // sprintf_(buffer, "%d, %d, %d, %d, %d, %d, %d\r\n",(uint16_t)adcData[0],(uint16_t)adcData[1],(uint16_t)adcData[2],(uint16_t)adcData[3],(uint16_t)(adcData[4]),(uint16_t)(adcData[5]),(uint16_t)(adcData[6])) ; - // sprintf_(buffer, "%d, %d, %d, %d, %d, %d\r\n",tic_array[0],tic_array[1],tic_array[2],tic_array[3],tic_array[4],tic_array[5]) ; - i=0; - while (buffer[i] != '\0') - {i++;} - HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&buffer, i); - + // print values for debugging + + sprintf_(buffer, "%d, %d, %d, %d, %d, %d, %d, %d, %d\r\n", + adcData[1], + ui16_throttle_offset, + ui16_timertics, + uint32_PAS, + MS.Battery_Current, + int32_temp_current_target, + MS.i_q, + MS.u_abs, + SystemState); + // sprintf_(buffer, "%d, %d, %d, %d, %d, %d, %d\r\n",(uint16_t)adcData[0],(uint16_t)adcData[1],(uint16_t)adcData[2],(uint16_t)adcData[3],(uint16_t)(adcData[4]),(uint16_t)(adcData[5]),(uint16_t)(adcData[6])) ; + // sprintf_(buffer, "%d, %d, %d, %d, %d, %d\r\n",tic_array[0],tic_array[1],tic_array[2],tic_array[3],tic_array[4],tic_array[5]) ; + i = 0; + while (buffer[i] != '\0') + { + i++; + } + HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&buffer, i); - ui8_print_flag=0; + ui8_print_flag = 0; #endif #if (DISPLAY_TYPE == DISPLAY_TYPE_EBiCS) - ui8_slowloop_counter++; - if(ui8_slowloop_counter>3){ - ui8_slowloop_counter = 0; + ui8_slowloop_counter++; + if (ui8_slowloop_counter > 3) + { + ui8_slowloop_counter = 0; - switch (ui8_main_LEV_Page_counter){ - case 1: { - ui8_LEV_Page_to_send = 1; - } - break; - case 2: { - ui8_LEV_Page_to_send = 2; - } - break; - case 3: { - ui8_LEV_Page_to_send = 3; - } - break; - case 4: { - //to do, define other pages - ui8_LEV_Page_to_send = 4; - } - break; - }//end switch + switch (ui8_main_LEV_Page_counter) + { + case 1: + { + ui8_LEV_Page_to_send = 1; + } + break; + case 2: + { + ui8_LEV_Page_to_send = 2; + } + break; + case 3: + { + ui8_LEV_Page_to_send = 3; + } + break; + case 4: + { + // to do, define other pages + ui8_LEV_Page_to_send = 4; + } + break; + } // end switch - // send_ant_page(ui8_LEV_Page_to_send, &MS, &MP); + // send_ant_page(ui8_LEV_Page_to_send, &MS, &MP); - ui8_main_LEV_Page_counter++; - if(ui8_main_LEV_Page_counter>4)ui8_main_LEV_Page_counter=1; - } + ui8_main_LEV_Page_counter++; + if (ui8_main_LEV_Page_counter > 4) + ui8_main_LEV_Page_counter = 1; + } #endif - ui32_tim3_counter=0; - }// end of slow loop + ui32_tim3_counter = 0; + } // end of slow loop - /* USER CODE END WHILE */ + /* USER CODE END WHILE */ - /* USER CODE BEGIN 3 */ + /* USER CODE BEGIN 3 */ + } + /* USER CODE END 3 */ +} - } - /* USER CODE END 3 */ +/** + * @brief System Clock Configuration + * @retval None + */ +void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct; + RCC_ClkInitTypeDef RCC_ClkInitStruct; + RCC_PeriphCLKInitTypeDef PeriphClkInit; + + /**Initializes the CPU, AHB and APB busses clocks + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.HSICalibrationValue = 16; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2; + RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); } - /** - * @brief System Clock Configuration - * @retval None + /**Initializes the CPU, AHB and APB busses clocks */ - void SystemClock_Config(void) - { - - RCC_OscInitTypeDef RCC_OscInitStruct; - RCC_ClkInitTypeDef RCC_ClkInitStruct; - RCC_PeriphCLKInitTypeDef PeriphClkInit; - - /**Initializes the CPU, AHB and APB busses clocks - */ - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; - RCC_OscInitStruct.HSIState = RCC_HSI_ON; - RCC_OscInitStruct.HSICalibrationValue = 16; - RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; - RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2; - RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16; - if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; - /**Initializes the CPU, AHB and APB busses clocks - */ - RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK - |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; - RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; - RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; - RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC; + PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV6; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC; - PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV6; - if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + /**Configure the Systick interrupt time + */ + HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq() / 1000); - /**Configure the Systick interrupt time - */ - HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000); + /**Configure the Systick + */ + HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK); - /**Configure the Systick - */ - HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK); + /* SysTick_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); +} - /* SysTick_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); - } +/* ADC1 init function */ +static void MX_ADC1_Init(void) +{ + ADC_MultiModeTypeDef multimode; + ADC_InjectionConfTypeDef sConfigInjected; + ADC_ChannelConfTypeDef sConfig; - /* ADC1 init function */ - static void MX_ADC1_Init(void) + /**Common config + */ + hadc1.Instance = ADC1; + hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE; // Scan muß für getriggerte Wandlung gesetzt sein + hadc1.Init.ContinuousConvMode = DISABLE; + hadc1.Init.DiscontinuousConvMode = DISABLE; + hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T3_TRGO; // Trigger regular ADC with timer 3 ADC_EXTERNALTRIGCONV_T1_CC1;// // ADC_SOFTWARE_START; // + hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc1.Init.NbrOfConversion = 8; + hadc1.Init.NbrOfDiscConversion = 0; + + if (HAL_ADC_Init(&hadc1) != HAL_OK) { + _Error_Handler(__FILE__, __LINE__); + } - ADC_MultiModeTypeDef multimode; - ADC_InjectionConfTypeDef sConfigInjected; - ADC_ChannelConfTypeDef sConfig; + /**Configure the ADC multi-mode + */ + multimode.Mode = ADC_DUALMODE_REGSIMULT_INJECSIMULT; + if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /**Common config - */ - hadc1.Instance = ADC1; - hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE; //Scan muß für getriggerte Wandlung gesetzt sein - hadc1.Init.ContinuousConvMode = DISABLE; - hadc1.Init.DiscontinuousConvMode = DISABLE; - hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T3_TRGO;// Trigger regular ADC with timer 3 ADC_EXTERNALTRIGCONV_T1_CC1;// // ADC_SOFTWARE_START; // - hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc1.Init.NbrOfConversion = 8; - hadc1.Init.NbrOfDiscConversion = 0; + /**Configure Injected Channel + */ + sConfigInjected.InjectedChannel = ADC_CHANNEL_4; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; + sConfigInjected.InjectedNbrOfConversion = 1; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5; + sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_CC4; // Hier bin ich nicht sicher ob Trigger out oder direkt CC4 + sConfigInjected.AutoInjectedConv = DISABLE; // muß aus sein + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = ui16_ph1_offset; // 1900; + HAL_ADC_Stop(&hadc1); // ADC muß gestoppt sein, damit Triggerquelle gesetzt werden kann. + if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + /**Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_7; // battery voltage + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; // ADC_SAMPLETIME_239CYCLES_5; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - if (HAL_ADC_Init(&hadc1) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + /**Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_3; // Connector SP: throttle input + sConfig.Rank = ADC_REGULAR_RANK_2; + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; // ADC_SAMPLETIME_239CYCLES_5; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + /**Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_4; // Phase current 1 + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; // ADC_SAMPLETIME_239CYCLES_5; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + /**Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_5; // Phase current 2 + sConfig.Rank = ADC_REGULAR_RANK_4; + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; // ADC_SAMPLETIME_239CYCLES_5; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + /**Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_6; // Phase current 3 + sConfig.Rank = ADC_REGULAR_RANK_5; + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; // ADC_SAMPLETIME_239CYCLES_5; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /**Configure the ADC multi-mode - */ - multimode.Mode = ADC_DUALMODE_REGSIMULT_INJECSIMULT; - if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + /**Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_8; + sConfig.Rank = ADC_REGULAR_RANK_6; // connector AD2 + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; // ADC_SAMPLETIME_239CYCLES_5; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /**Configure Injected Channel - */ - sConfigInjected.InjectedChannel = ADC_CHANNEL_4; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedNbrOfConversion = 1; - sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5; - sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_CC4; // Hier bin ich nicht sicher ob Trigger out oder direkt CC4 - sConfigInjected.AutoInjectedConv = DISABLE; //muß aus sein - sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; - sConfigInjected.InjectedOffset = ui16_ph1_offset;//1900; - HAL_ADC_Stop(&hadc1); //ADC muß gestoppt sein, damit Triggerquelle gesetzt werden kann. - if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + /**Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_9; // connector AD1, temperature or torque input for Controller from PhoebeLiu @ aliexpress + sConfig.Rank = ADC_REGULAR_RANK_7; + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; // ADC_SAMPLETIME_239CYCLES_5; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /**Configure Regular Channel - */ - sConfig.Channel = ADC_CHANNEL_7; //battery voltage - sConfig.Rank = ADC_REGULAR_RANK_1; - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;//ADC_SAMPLETIME_239CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + /**Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_TEMPSENSOR; // internal STM32 temperature sensor + sConfig.Rank = ADC_REGULAR_RANK_8; + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; // ADC_SAMPLETIME_239CYCLES_5; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } +} +/* ADC2 init function */ +static void MX_ADC2_Init(void) +{ - /**Configure Regular Channel - */ - sConfig.Channel = ADC_CHANNEL_3; //Connector SP: throttle input - sConfig.Rank = ADC_REGULAR_RANK_2; - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;//ADC_SAMPLETIME_239CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - /**Configure Regular Channel - */ - sConfig.Channel = ADC_CHANNEL_4; //Phase current 1 - sConfig.Rank = ADC_REGULAR_RANK_3; - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;//ADC_SAMPLETIME_239CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - /**Configure Regular Channel - */ - sConfig.Channel = ADC_CHANNEL_5; //Phase current 2 - sConfig.Rank = ADC_REGULAR_RANK_4; - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;//ADC_SAMPLETIME_239CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - /**Configure Regular Channel - */ - sConfig.Channel = ADC_CHANNEL_6; //Phase current 3 - sConfig.Rank = ADC_REGULAR_RANK_5; - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;//ADC_SAMPLETIME_239CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + ADC_InjectionConfTypeDef sConfigInjected; - /**Configure Regular Channel - */ - sConfig.Channel = ADC_CHANNEL_8; - sConfig.Rank = ADC_REGULAR_RANK_6; // connector AD2 - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;//ADC_SAMPLETIME_239CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + /**Common config + */ + hadc2.Instance = ADC2; + hadc2.Init.ScanConvMode = ADC_SCAN_ENABLE; // hier auch Scan enable?! + hadc2.Init.ContinuousConvMode = DISABLE; + hadc2.Init.DiscontinuousConvMode = DISABLE; + hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc2.Init.NbrOfConversion = 1; + if (HAL_ADC_Init(&hadc2) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /**Configure Regular Channel - */ - sConfig.Channel = ADC_CHANNEL_9; // connector AD1, temperature or torque input for Controller from PhoebeLiu @ aliexpress - sConfig.Rank = ADC_REGULAR_RANK_7; - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;//ADC_SAMPLETIME_239CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + /**Configure Injected Channel + */ + sConfigInjected.InjectedChannel = ADC_CHANNEL_5; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; + sConfigInjected.InjectedNbrOfConversion = 1; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5; + sConfigInjected.ExternalTrigInjecConv = ADC_INJECTED_SOFTWARE_START; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = ui16_ph2_offset; // 1860; + if (HAL_ADCEx_InjectedConfigChannel(&hadc2, &sConfigInjected) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } +} +/* TIM1 init function */ +static void MX_TIM1_Init(void) +{ - /**Configure Regular Channel - */ - sConfig.Channel = ADC_CHANNEL_TEMPSENSOR; // internal STM32 temperature sensor - sConfig.Rank = ADC_REGULAR_RANK_8; - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;//ADC_SAMPLETIME_239CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } + TIM_ClockConfigTypeDef sClockSourceConfig; + TIM_MasterConfigTypeDef sMasterConfig; + TIM_OC_InitTypeDef sConfigOC; + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig; + + htim1.Instance = TIM1; + htim1.Init.Prescaler = 0; + htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; + htim1.Init.Period = _T; + htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim1.Init.RepetitionCounter = 0; + htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim1) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); } - /* ADC2 init function */ - static void MX_ADC2_Init(void) + if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) { + _Error_Handler(__FILE__, __LINE__); + } - ADC_InjectionConfTypeDef sConfigInjected; + if (HAL_TIM_OC_Init(&htim1) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /**Common config - */ - hadc2.Instance = ADC2; - hadc2.Init.ScanConvMode = ADC_SCAN_ENABLE; //hier auch Scan enable?! - hadc2.Init.ContinuousConvMode = DISABLE; - hadc2.Init.DiscontinuousConvMode = DISABLE; - hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START; - hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc2.Init.NbrOfConversion = 1; - if (HAL_ADC_Init(&hadc2) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - /**Configure Injected Channel - */ - sConfigInjected.InjectedChannel = ADC_CHANNEL_5; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedNbrOfConversion = 1; - sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5; - sConfigInjected.ExternalTrigInjecConv = ADC_INJECTED_SOFTWARE_START; - sConfigInjected.AutoInjectedConv = DISABLE; - sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; - sConfigInjected.InjectedOffset = ui16_ph2_offset;// 1860; - if (HAL_ADCEx_InjectedConfigChannel(&hadc2, &sConfigInjected) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - } - /* TIM1 init function */ - static void MX_TIM1_Init(void) + sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC4REF; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) { - - TIM_ClockConfigTypeDef sClockSourceConfig; - TIM_MasterConfigTypeDef sMasterConfig; - TIM_OC_InitTypeDef sConfigOC; - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig; - - htim1.Instance = TIM1; - htim1.Init.Prescaler = 0; - htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; - htim1.Init.Period = _T; - htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter = 0; - htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - if (HAL_TIM_Base_Init(&htim1) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; - if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - if (HAL_TIM_OC_Init(&htim1) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC4REF; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = 1; - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_LOW; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - //sConfigOC.OCMode = TIM_OCMODE_ACTIVE; // war hier ein Bock?! - sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; - if (HAL_TIM_OC_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfig.DeadTime = 32; - sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; - sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - HAL_TIM_MspPostInit(&htim1); - + _Error_Handler(__FILE__, __LINE__); } - /* TIM2 init function */ - static void MX_TIM2_Init(void) + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = 1; + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_LOW; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) { - TIM_ClockConfigTypeDef sClockSourceConfig; - TIM_SlaveConfigTypeDef sSlaveConfig; - TIM_MasterConfigTypeDef sMasterConfig; - TIM_IC_InitTypeDef sConfigIC; - - htim2.Instance = TIM2; - htim2.Init.Prescaler = 128; - htim2.Init.CounterMode = TIM_COUNTERMODE_UP; - htim2.Init.Period = 65535; - htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - if (HAL_TIM_Base_Init(&htim2) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; - if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - if (HAL_TIM_IC_Init(&htim2) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sSlaveConfig.SlaveMode = TIM_SLAVEMODE_RESET; - sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED; - sSlaveConfig.TriggerFilter = 8; - if (HAL_TIM_SlaveConfigSynchronization(&htim2, &sSlaveConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING; - sConfigIC.ICSelection = TIM_ICSELECTION_TRC; - sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; - sConfigIC.ICFilter = 15; - if (HAL_TIM_IC_ConfigChannel(&htim2, &sConfigIC, TIM_CHANNEL_1) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; - if (HAL_TIM_IC_ConfigChannel(&htim2, &sConfigIC, TIM_CHANNEL_2) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - if (HAL_TIM_IC_ConfigChannel(&htim2, &sConfigIC, TIM_CHANNEL_3) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - if (HAL_TIM_ConfigTI1Input(&htim2, TIM_TI1SELECTION_XORCOMBINATION) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - + _Error_Handler(__FILE__, __LINE__); } - /* TIM3 init function 8kHz interrupt frequency for regular adc triggering */ - static void MX_TIM3_Init(void) + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) { - - TIM_ClockConfigTypeDef sClockSourceConfig; - TIM_MasterConfigTypeDef sMasterConfig; - - htim3.Instance = TIM3; - htim3.Init.Prescaler = 0; - htim3.Init.CounterMode = TIM_COUNTERMODE_UP; - htim3.Init.Period = 7813; - htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - if (HAL_TIM_Base_Init(&htim3) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; - if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC1; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - - HAL_TIM_MspPostInit(&htim3); - + _Error_Handler(__FILE__, __LINE__); } - /* USART1 init function */ - static void MX_USART1_UART_Init(void) + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) { + _Error_Handler(__FILE__, __LINE__); + } - huart1.Instance = USART1; - -#if ((DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER) ||DISPLAY_TYPE==DISPLAY_TYPE_KUNTENG||DISPLAY_TYPE==DISPLAY_TYPE_EBiCS||DISPLAY_TYPE==DISPLAY_TYPE_NO2 || DISPLAY_TYPE==DISPLAY_TYPE_BAFANG_850_860) - huart1.Init.BaudRate = 9600; -#elif (DISPLAY_TYPE == DISPLAY_TYPE_BAFANG_LCD) - huart1.Init.BaudRate = 1200; -#else - huart1.Init.BaudRate = 56000; -#endif - - - huart1.Init.WordLength = UART_WORDLENGTH_8B; - huart1.Init.StopBits = UART_STOPBITS_1; - huart1.Init.Parity = UART_PARITY_NONE; - huart1.Init.Mode = UART_MODE_TX_RX; - huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; - huart1.Init.OverSampling = UART_OVERSAMPLING_16; - if (HAL_UART_Init(&huart1) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - __HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE); + // sConfigOC.OCMode = TIM_OCMODE_ACTIVE; // war hier ein Bock?! + sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; + if (HAL_TIM_OC_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); } - /** - * Enable DMA controller clock - */ - static void MX_DMA_Init(void) + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.DeadTime = 32; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) { - /* DMA controller clock enable */ - __HAL_RCC_DMA1_CLK_ENABLE(); + _Error_Handler(__FILE__, __LINE__); + } + HAL_TIM_MspPostInit(&htim1); +} +/* TIM2 init function */ +static void MX_TIM2_Init(void) +{ + TIM_ClockConfigTypeDef sClockSourceConfig; + TIM_SlaveConfigTypeDef sSlaveConfig; + TIM_MasterConfigTypeDef sMasterConfig; + TIM_IC_InitTypeDef sConfigIC; + + htim2.Instance = TIM2; + htim2.Init.Prescaler = 128; + htim2.Init.CounterMode = TIM_COUNTERMODE_UP; + htim2.Init.Period = 65535; + htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim2) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /* DMA interrupt init */ - /* DMA1_Channel1_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 1); - HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); - // /* DMA1_Channel4_IRQn interrupt configuration */ - // HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 3, 0); - // HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn); - // /* DMA1_Channel5_IRQn interrupt configuration */ - // //HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 0, 0); - // //HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn); + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + if (HAL_TIM_IC_Init(&htim2) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); } - /** Configure pins as - * Analog - * Input - * Output - * EVENT_OUT - * EXTI - */ - static void MX_GPIO_Init(void) + sSlaveConfig.SlaveMode = TIM_SLAVEMODE_RESET; + sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED; + sSlaveConfig.TriggerFilter = 8; + if (HAL_TIM_SlaveConfigSynchronization(&htim2, &sSlaveConfig) != HAL_OK) { + _Error_Handler(__FILE__, __LINE__); + } - GPIO_InitTypeDef GPIO_InitStruct; + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /* GPIO Ports Clock Enable */ - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); + sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING; + sConfigIC.ICSelection = TIM_ICSELECTION_TRC; + sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; + sConfigIC.ICFilter = 15; + if (HAL_TIM_IC_ConfigChannel(&htim2, &sConfigIC, TIM_CHANNEL_1) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); + sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; + if (HAL_TIM_IC_ConfigChannel(&htim2, &sConfigIC, TIM_CHANNEL_2) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /*Configure GPIO pins : Hall_1_Pin Hall_2_Pin Hall_3_Pin */ - GPIO_InitStruct.Pin = Hall_1_Pin|Hall_2_Pin|Hall_3_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_PULLUP; - HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + if (HAL_TIM_IC_ConfigChannel(&htim2, &sConfigIC, TIM_CHANNEL_3) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /*Configure GPIO pin : LED_Pin */ - GPIO_InitStruct.Pin = LED_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - HAL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct); + if (HAL_TIM_ConfigTI1Input(&htim2, TIM_TI1SELECTION_XORCOMBINATION) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } +} - /*Configure GPIO pin : LIGHT_Pin */ - GPIO_InitStruct.Pin = LIGHT_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(LIGHT_GPIO_Port, &GPIO_InitStruct); +/* TIM3 init function 8kHz interrupt frequency for regular adc triggering */ +static void MX_TIM3_Init(void) +{ - /*Configure GPIO pin : BRAKE_LIGHT_Pin */ - GPIO_InitStruct.Pin = BRAKE_LIGHT_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(BRAKE_LIGHT_GPIO_Port, &GPIO_InitStruct); + TIM_ClockConfigTypeDef sClockSourceConfig; + TIM_MasterConfigTypeDef sMasterConfig; - /*Configure GPIO pin : Brake_Pin */ - GPIO_InitStruct.Pin = Brake_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_PULLUP; - HAL_GPIO_Init(Brake_GPIO_Port, &GPIO_InitStruct); + htim3.Instance = TIM3; + htim3.Init.Prescaler = 0; + htim3.Init.CounterMode = TIM_COUNTERMODE_UP; + htim3.Init.Period = 7813; + htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim3) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } - /*Configure GPIO pins : Speed_EXTI5_Pin PAS_EXTI8_Pin */ - GPIO_InitStruct.Pin = Speed_EXTI5_Pin|PAS_EXTI8_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; - GPIO_InitStruct.Pull = GPIO_PULLUP; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC1; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + HAL_TIM_MspPostInit(&htim3); +} - /* EXTI interrupt init*/ +/* USART1 init function */ +static void MX_USART1_UART_Init(void) +{ + huart1.Instance = USART1; - HAL_NVIC_SetPriority(EXTI9_5_IRQn, 2, 0); - HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); +#if ((DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER) || DISPLAY_TYPE == DISPLAY_TYPE_KUNTENG || DISPLAY_TYPE == DISPLAY_TYPE_EBiCS || DISPLAY_TYPE == DISPLAY_TYPE_NO2 || DISPLAY_TYPE == DISPLAY_TYPE_BAFANG_850_860) + huart1.Init.BaudRate = 9600; +#elif (DISPLAY_TYPE == DISPLAY_TYPE_BAFANG_LCD) + huart1.Init.BaudRate = 1200; +#else + huart1.Init.BaudRate = 56000; +#endif + huart1.Init.WordLength = UART_WORDLENGTH_8B; + huart1.Init.StopBits = UART_STOPBITS_1; + huart1.Init.Parity = UART_PARITY_NONE; + huart1.Init.Mode = UART_MODE_TX_RX; + huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart1.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart1) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); } + __HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE); +} - /* USER CODE BEGIN 4 */ +/** + * Enable DMA controller clock + */ +static void MX_DMA_Init(void) +{ + /* DMA controller clock enable */ + __HAL_RCC_DMA1_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA1_Channel1_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 1); + HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); + // /* DMA1_Channel4_IRQn interrupt configuration */ + // HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 3, 0); + // HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn); + // /* DMA1_Channel5_IRQn interrupt configuration */ + // //HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 0, 0); + // //HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn); +} + +/** Configure pins as + * Analog + * Input + * Output + * EVENT_OUT + * EXTI + */ +static void MX_GPIO_Init(void) +{ - void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) + GPIO_InitTypeDef GPIO_InitStruct; + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pins : Hall_1_Pin Hall_2_Pin Hall_3_Pin */ + GPIO_InitStruct.Pin = Hall_1_Pin | Hall_2_Pin | Hall_3_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_PULLUP; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /*Configure GPIO pin : LED_Pin */ + GPIO_InitStruct.Pin = LED_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pin : LIGHT_Pin */ + GPIO_InitStruct.Pin = LIGHT_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(LIGHT_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pin : BRAKE_LIGHT_Pin */ + GPIO_InitStruct.Pin = BRAKE_LIGHT_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(BRAKE_LIGHT_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pin : Brake_Pin */ + GPIO_InitStruct.Pin = Brake_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_PULLUP; + HAL_GPIO_Init(Brake_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pins : Speed_EXTI5_Pin PAS_EXTI8_Pin */ + GPIO_InitStruct.Pin = Speed_EXTI5_Pin | PAS_EXTI8_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; + GPIO_InitStruct.Pull = GPIO_PULLUP; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* EXTI interrupt init*/ + + HAL_NVIC_SetPriority(EXTI9_5_IRQn, 2, 0); + HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); +} + +/* USER CODE BEGIN 4 */ + +void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) +{ + // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); + if (htim == &htim3) { - //HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); - if (htim == &htim3) { #if SPEED_PLL - if(!READ_BIT(TIM1->BDTR, TIM_BDTR_MOE))q31_rotorposition_PLL += (q31_angle_per_tic<<1); + if (!READ_BIT(TIM1->BDTR, TIM_BDTR_MOE)) + q31_rotorposition_PLL += (q31_angle_per_tic << 1); #endif - if(ui32_tim3_counter<32000)ui32_tim3_counter++; - if (uint32_PAS_counter < PAS_TIMEOUT+1){ - uint32_PAS_counter++; - if(HAL_GPIO_ReadPin(PAS_GPIO_Port, PAS_Pin))uint32_PAS_HIGH_counter++; - } - if (uint32_SPEED_counter<128000)uint32_SPEED_counter++; //counter for external Speedsensor - if(uint16_full_rotation_counter<8000)uint16_full_rotation_counter++; //full rotation counter for motor standstill detection - if(uint16_half_rotation_counter<8000)uint16_half_rotation_counter++; //half rotation counter for motor standstill detection - + if (ui32_tim3_counter < 32000) + ui32_tim3_counter++; + if (uint32_PAS_counter < PAS_TIMEOUT + 1) + { + uint32_PAS_counter++; + if (HAL_GPIO_ReadPin(PAS_GPIO_Port, PAS_Pin)) + uint32_PAS_HIGH_counter++; } - //HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); + if (uint32_SPEED_counter < 128000) + uint32_SPEED_counter++; // counter for external Speedsensor + if (uint16_full_rotation_counter < 8000) + uint16_full_rotation_counter++; // full rotation counter for motor standstill detection + if (uint16_half_rotation_counter < 8000) + uint16_half_rotation_counter++; // half rotation counter for motor standstill detection } + // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); +} +// regular ADC callback +void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc) +{ + ui8_adc_regular_flag = 1; +} +// injected ADC - // regular ADC callback - void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) - { - ui8_adc_regular_flag=1; - - } +void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc) +{ + // for oszi-check of used time in FOC procedere + // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); + ui32_tim1_counter++; - //injected ADC + /* else { + HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); + uint32_SPEED_counter=0; + }*/ - void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) + if (!ui8_adc_offset_done_flag) { - //for oszi-check of used time in FOC procedere - //HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); - ui32_tim1_counter++; + i16_ph1_current = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); + i16_ph2_current = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); - /* else { - HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin); - uint32_SPEED_counter=0; - }*/ - - if(!ui8_adc_offset_done_flag) - { - i16_ph1_current = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); - i16_ph2_current = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); - - ui8_adc_inj_flag=1; - } - else{ + ui8_adc_inj_flag = 1; + } + else + { #ifdef DISABLE_DYNAMIC_ADC - i16_ph1_current = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); - i16_ph2_current = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); - + i16_ph1_current = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); + i16_ph2_current = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); #else - switch (MS.char_dyn_adc_state) //read in according to state - { - case 1: //Phase C at high dutycycles, read from A+B directly - { - temp1=(q31_t)HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); - i16_ph1_current = temp1 ; - - temp2=(q31_t)HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); - i16_ph2_current = temp2; - } - break; - case 2: //Phase A at high dutycycles, read from B+C (A = -B -C) - { - - temp2=(q31_t)HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); - i16_ph2_current = temp2; - - temp1=(q31_t)HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); - i16_ph1_current = -i16_ph2_current-temp1; - - } - break; - case 3: //Phase B at high dutycycles, read from A+C (B=-A-C) - { - temp1=(q31_t)HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); - i16_ph1_current = temp1 ; - temp2=(q31_t)HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); - i16_ph2_current = -i16_ph1_current-temp2; - } - break; + switch (MS.char_dyn_adc_state) // read in according to state + { + case 1: // Phase C at high dutycycles, read from A+B directly + { + temp1 = (q31_t)HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); + i16_ph1_current = temp1; - case 0: //timeslot too small for ADC - { - //do nothing - } - break; + temp2 = (q31_t)HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); + i16_ph2_current = temp2; + } + break; + case 2: // Phase A at high dutycycles, read from B+C (A = -B -C) + { + temp2 = (q31_t)HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); + i16_ph2_current = temp2; + temp1 = (q31_t)HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); + i16_ph1_current = -i16_ph2_current - temp1; + } + break; + case 3: // Phase B at high dutycycles, read from A+C (B=-A-C) + { + temp1 = (q31_t)HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); + i16_ph1_current = temp1; + temp2 = (q31_t)HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1); + i16_ph2_current = -i16_ph1_current - temp2; + } + break; + case 0: // timeslot too small for ADC + { + // do nothing + } + break; - } // end case + } // end case #endif + __disable_irq(); // ENTER CRITICAL SECTION!!!!!!!!!!!!! - __disable_irq(); //ENTER CRITICAL SECTION!!!!!!!!!!!!! - - //extrapolate recent rotor position - ui16_tim2_recent = __HAL_TIM_GET_COUNTER(&htim2); // read in timertics since last event - if (MS.hall_angle_detect_flag) { - if(ui16_timertics(SIXSTEPTHRESHOLD*6)>>2)ui8_6step_flag=1; - + // extrapolate recent rotor position + ui16_tim2_recent = __HAL_TIM_GET_COUNTER(&htim2); // read in timertics since last event + if (MS.hall_angle_detect_flag) + { + if (ui16_timertics < SIXSTEPTHRESHOLD && ui16_tim2_recent < 200) + ui8_6step_flag = 0; + if (ui16_timertics > (SIXSTEPTHRESHOLD * 6) >> 2) + ui8_6step_flag = 1; - if(MS.angle_est){ - q31_rotorposition_PLL += q31_angle_per_tic; + if (MS.angle_est) + { + q31_rotorposition_PLL += q31_angle_per_tic; + } + if (ui16_tim2_recent < ui16_timertics + (ui16_timertics >> 2) && !ui8_overflow_flag && !ui8_6step_flag) + { // prevent angle running away at standstill + if (MS.angle_est && iabs(q31_PLL_error) < deg_30) + { + q31_rotorposition_absolute = q31_rotorposition_PLL; + MS.system_state = PLL; } - if (ui16_tim2_recent < ui16_timertics+(ui16_timertics>>2) && !ui8_overflow_flag && !ui8_6step_flag) { //prevent angle running away at standstill - if(MS.angle_est&&iabs(q31_PLL_error)> 23) * 180) >> 8); - __enable_irq(); //EXIT CRITICAL SECTION!!!!!!!!!!!!!! - -#ifndef DISABLE_DYNAMIC_ADC - - //get the Phase with highest duty cycle for dynamic phase current reading - dyn_adc_state(q31_rotorposition_absolute); - //set the according injected channels to read current at Low-Side active time - - if (MS.char_dyn_adc_state!=char_dyn_adc_state_old){ - set_inj_channel(MS.char_dyn_adc_state); - char_dyn_adc_state_old = MS.char_dyn_adc_state; } -#endif - - //int16_current_target=0; - // call FOC procedure if PWM is enabled - - if (READ_BIT(TIM1->BDTR, TIM_BDTR_MOE)){ - FOC_calculation(i16_ph1_current, i16_ph2_current, q31_rotorposition_absolute, (((int16_t)i8_direction*i8_reverse_flag)*MS.i_q_setpoint), &MS); + else + { + ui8_overflow_flag = 1; + if (MS.KV_detect_flag) + q31_rotorposition_absolute = q31_rotorposition_hall; + else + q31_rotorposition_absolute = q31_rotorposition_hall + i8_direction * sign(MS.i_q_setpoint) * deg_30; // offset of 30 degree to get the middle of the sector + MS.system_state = SixStep; + // } } - //temp5=__HAL_TIM_GET_COUNTER(&htim1); - //set PWM - - TIM1->CCR1 = (uint16_t) switchtime[0]; - TIM1->CCR2 = (uint16_t) switchtime[1]; - TIM1->CCR3 = (uint16_t) switchtime[2]; - //__enable_irq(); + } // end if hall angle detect + // temp2=(((q31_rotorposition_absolute >> 23) * 180) >> 8); + __enable_irq(); // EXIT CRITICAL SECTION!!!!!!!!!!!!!! +#ifndef DISABLE_DYNAMIC_ADC - //HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); + // get the Phase with highest duty cycle for dynamic phase current reading + dyn_adc_state(q31_rotorposition_absolute); + // set the according injected channels to read current at Low-Side active time - } // end else + if (MS.char_dyn_adc_state != char_dyn_adc_state_old) + { + set_inj_channel(MS.char_dyn_adc_state); + char_dyn_adc_state_old = MS.char_dyn_adc_state; + } +#endif - } + // int16_current_target=0; + // call FOC procedure if PWM is enabled - void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef* htim) - { - //__HAL_TIM_SET_COUNTER(&htim2,0); //reset tim2 counter - // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); + if (READ_BIT(TIM1->BDTR, TIM_BDTR_MOE)) + { + FOC_calculation(i16_ph1_current, i16_ph2_current, q31_rotorposition_absolute, (((int16_t)i8_direction * i8_reverse_flag) * MS.i_q_setpoint), &MS); + } + // temp5=__HAL_TIM_GET_COUNTER(&htim1); + // set PWM - temp5=TIM3->CNT; + TIM1->CCR1 = (uint16_t)switchtime[0]; + TIM1->CCR2 = (uint16_t)switchtime[1]; + TIM1->CCR3 = (uint16_t)switchtime[2]; + //__enable_irq(); - if(TIM2->CCR1>20)ui16_timertics = TIM2->CCR1; //debounce hall signals + // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); + } // end else +} - //Hall sensor event processing +void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) +{ + //__HAL_TIM_SET_COUNTER(&htim2,0); //reset tim2 counter + // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); - ui8_hall_state = GPIOA->IDR & 0b111; //Mask input register with Hall 1 - 3 bits + temp5 = TIM3->CNT; + if (TIM2->CCR1 > 20) + ui16_timertics = TIM2->CCR1; // debounce hall signals - ui8_hall_case=ui8_hall_state_old*10+ui8_hall_state; - if(MS.hall_angle_detect_flag){ //only process, if autodetect procedere is fininshed - ui8_hall_state_old=ui8_hall_state; - } + // Hall sensor event processing - uint32_tics_filtered-=uint32_tics_filtered>>3; - uint32_tics_filtered+=ui16_timertics; + ui8_hall_state = GPIOA->IDR & 0b111; // Mask input register with Hall 1 - 3 bits - ui8_overflow_flag=0; - ui8_SPEED_control_flag=1; + ui8_hall_case = ui8_hall_state_old * 10 + ui8_hall_state; + if (MS.hall_angle_detect_flag) + { // only process, if autodetect procedere is fininshed + ui8_hall_state_old = ui8_hall_state; + } + uint32_tics_filtered -= uint32_tics_filtered >> 3; + uint32_tics_filtered += ui16_timertics; + ui8_overflow_flag = 0; + ui8_SPEED_control_flag = 1; - switch (ui8_hall_case) //12 cases for each transition from one stage to the next. 6x forward, 6x reverse - { - //6 cases for forward direction - //6 cases for forward direction - case 64: - q31_rotorposition_hall = Hall_64; + switch (ui8_hall_case) // 12 cases for each transition from one stage to the next. 6x forward, 6x reverse + { + // 6 cases for forward direction + // 6 cases for forward direction + case 64: + q31_rotorposition_hall = Hall_64; - i8_recent_rotor_direction = -i16_hall_order; - uint16_full_rotation_counter = 0; - break; - case 45: - q31_rotorposition_hall = Hall_45; + i8_recent_rotor_direction = -i16_hall_order; + uint16_full_rotation_counter = 0; + break; + case 45: + q31_rotorposition_hall = Hall_45; - i8_recent_rotor_direction = -i16_hall_order; - break; - case 51: - q31_rotorposition_hall = Hall_51; + i8_recent_rotor_direction = -i16_hall_order; + break; + case 51: + q31_rotorposition_hall = Hall_51; - i8_recent_rotor_direction = -i16_hall_order; - break; - case 13: - q31_rotorposition_hall = Hall_13; + i8_recent_rotor_direction = -i16_hall_order; + break; + case 13: + q31_rotorposition_hall = Hall_13; - i8_recent_rotor_direction = -i16_hall_order; - uint16_half_rotation_counter = 0; - break; - case 32: - q31_rotorposition_hall = Hall_32; + i8_recent_rotor_direction = -i16_hall_order; + uint16_half_rotation_counter = 0; + break; + case 32: + q31_rotorposition_hall = Hall_32; - i8_recent_rotor_direction = -i16_hall_order; - break; - case 26: - q31_rotorposition_hall = Hall_26; + i8_recent_rotor_direction = -i16_hall_order; + break; + case 26: + q31_rotorposition_hall = Hall_26; - i8_recent_rotor_direction = -i16_hall_order; - break; + i8_recent_rotor_direction = -i16_hall_order; + break; - //6 cases for reverse direction - case 46: - q31_rotorposition_hall = Hall_64; + // 6 cases for reverse direction + case 46: + q31_rotorposition_hall = Hall_64; - i8_recent_rotor_direction = i16_hall_order; - break; - case 62: - q31_rotorposition_hall = Hall_26; + i8_recent_rotor_direction = i16_hall_order; + break; + case 62: + q31_rotorposition_hall = Hall_26; - i8_recent_rotor_direction = i16_hall_order; - break; - case 23: - q31_rotorposition_hall = Hall_32; + i8_recent_rotor_direction = i16_hall_order; + break; + case 23: + q31_rotorposition_hall = Hall_32; - i8_recent_rotor_direction = i16_hall_order; - uint16_half_rotation_counter = 0; - break; - case 31: - q31_rotorposition_hall = Hall_13; + i8_recent_rotor_direction = i16_hall_order; + uint16_half_rotation_counter = 0; + break; + case 31: + q31_rotorposition_hall = Hall_13; - i8_recent_rotor_direction = i16_hall_order; - break; - case 15: - q31_rotorposition_hall = Hall_51; + i8_recent_rotor_direction = i16_hall_order; + break; + case 15: + q31_rotorposition_hall = Hall_51; - i8_recent_rotor_direction = i16_hall_order; - break; - case 54: - q31_rotorposition_hall = Hall_45; + i8_recent_rotor_direction = i16_hall_order; + break; + case 54: + q31_rotorposition_hall = Hall_45; - i8_recent_rotor_direction = i16_hall_order; - uint16_full_rotation_counter = 0; - break; + i8_recent_rotor_direction = i16_hall_order; + uint16_full_rotation_counter = 0; + break; - } // end case + } // end case - if(MS.angle_est){ - q31_PLL_error=q31_rotorposition_PLL-q31_rotorposition_hall; - q31_angle_per_tic = speed_PLL(q31_rotorposition_PLL,q31_rotorposition_hall,0); - } + if (MS.angle_est) + { + q31_PLL_error = q31_rotorposition_PLL - q31_rotorposition_hall; + q31_angle_per_tic = speed_PLL(q31_rotorposition_PLL, q31_rotorposition_hall, 0); + } #if SPEED_PLL - if(ui16_erps>30){ //360 interpolation at higher erps - if(ui8_hall_case==32||ui8_hall_case==23){ - q31_angle_per_tic = speed_PLL(q31_rotorposition_PLL,q31_rotorposition_hall, SPDSHFT*tics_higher_limit/(uint32_tics_filtered>>3)); - - } + if (ui16_erps > 30) + { // 360 interpolation at higher erps + if (ui8_hall_case == 32 || ui8_hall_case == 23) + { + q31_angle_per_tic = speed_PLL(q31_rotorposition_PLL, q31_rotorposition_hall, SPDSHFT * tics_higher_limit / (uint32_tics_filtered >> 3)); } - else{ + } + else + { - q31_angle_per_tic = speed_PLL(q31_rotorposition_PLL,q31_rotorposition_hall, SPDSHFT*tics_higher_limit/(uint32_tics_filtered>>3)); - } + q31_angle_per_tic = speed_PLL(q31_rotorposition_PLL, q31_rotorposition_hall, SPDSHFT * tics_higher_limit / (uint32_tics_filtered >> 3)); + } #endif - // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); - + // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); +} - } - - void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) +{ + // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); + // PAS processing + if (GPIO_Pin == PAS_EXTI8_Pin) { - //HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); - //PAS processing - if(GPIO_Pin == PAS_EXTI8_Pin) - { - ui8_PAS_flag = 1; - } - - //Speed processing - if(GPIO_Pin == Speed_EXTI5_Pin) - { - - ui8_SPEED_flag = 1; //with debounce - - } - //HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); + ui8_PAS_flag = 1; } - void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) + // Speed processing + if (GPIO_Pin == Speed_EXTI5_Pin) { - //ui8_UART_flag=1; + ui8_SPEED_flag = 1; // with debounce } + // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); +} - void UART_IdleItCallback(void) - { - ui8_UART_flag=1; +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) +{ + // ui8_UART_flag=1; +} - } +void UART_IdleItCallback(void) +{ + ui8_UART_flag = 1; +} - void HAL_UART_TxCpltCallback(UART_HandleTypeDef *UartHandle) - { - ui8_UART_TxCplt_flag=1; - } +void HAL_UART_TxCpltCallback(UART_HandleTypeDef *UartHandle) +{ + ui8_UART_TxCplt_flag = 1; +} - void HAL_UART_ErrorCallback(UART_HandleTypeDef *UartHandle) { +void HAL_UART_ErrorCallback(UART_HandleTypeDef *UartHandle) +{ #if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER) - KingMeter_Init (&KM); + KingMeter_Init(&KM); #endif #if (DISPLAY_TYPE & DISPLAY_TYPE_BAFANG) - Bafang_Init (&BF); + Bafang_Init(&BF); #endif #if (DISPLAY_TYPE == DISPLAY_TYPE_KUNTENG) - kunteng_init(); + kunteng_init(); #endif #if (DISPLAY_TYPE == DISPLAY_TYPE_EBiCS) - // ebics_init(); + // ebics_init(); #endif #if (DISPLAY_TYPE == DISPLAY_TYPE_NO2) No2_Init(&No2); #endif - } +} - void get_internal_temp_offset(void){ - int32_t temp=0; - for(i=0;i<32;i++){ - while(!ui8_adc_regular_flag){} - temp+=adcData[7]; - ui8_adc_regular_flag=0; +void get_internal_temp_offset(void) +{ + int32_t temp = 0; + for (i = 0; i < 32; i++) + { + while (!ui8_adc_regular_flag) + { } - HAL_FLASH_Unlock(); - EE_WriteVariable(EEPROM_INT_TEMP_V25,temp>>5); - HAL_FLASH_Lock(); + temp += adcData[7]; + ui8_adc_regular_flag = 0; } -#if (DISPLAY_TYPE == DISPLAY_TYPE_NO2) - void No2_update(void) - { - /* Prepare Tx parameters */ + HAL_FLASH_Unlock(); + EE_WriteVariable(EEPROM_INT_TEMP_V25, temp >> 5); + HAL_FLASH_Lock(); +} +#if (DISPLAY_TYPE == DISPLAY_TYPE_NO2) +void No2_update(void) +{ + /* Prepare Tx parameters */ -#if (SPEEDSOURCE == EXTERNAL) - No2.Tx.Wheeltime_ms = ((MS.Speed>>3)*PULSES_PER_REVOLUTION); //>>3 because of 8 kHz counter frequency, so 8 tics per ms +#if (SPEEDSOURCE == EXTERNAL) + No2.Tx.Wheeltime_ms = ((MS.Speed >> 3) * PULSES_PER_REVOLUTION); //>>3 because of 8 kHz counter frequency, so 8 tics per ms #else - if(__HAL_TIM_GET_COUNTER(&htim2) < 12000) - { - No2.Tx.Wheeltime_ms = (MS.Speed*GEAR_RATIO*6)>>9; //>>9 because of 500kHZ timer2 frequency, 512 tics per ms should be OK *6 because of 6 hall interrupts per electric revolution. - - } - else - { - No2.Tx.Wheeltime_ms = 64000; - } + if (__HAL_TIM_GET_COUNTER(&htim2) < 12000) + { + No2.Tx.Wheeltime_ms = (MS.Speed * GEAR_RATIO * 6) >> 9; //>>9 because of 500kHZ timer2 frequency, 512 tics per ms should be OK *6 because of 6 hall interrupts per electric revolution. + } + else + { + No2.Tx.Wheeltime_ms = 64000; + } #endif - if(MS.Temperature>MOTOR_TEMPERATURE_MAX) No2.Tx.Error = 7; //motor failure - else if(MS.int_Temperature>CONTROLLER_TEMPERATURE_MAX)No2.Tx.Error = 9; //controller failure - else No2.Tx.Error = 0; //no failure - + if (MS.Temperature > MOTOR_TEMPERATURE_MAX) + No2.Tx.Error = 7; // motor failure + else if (MS.int_Temperature > CONTROLLER_TEMPERATURE_MAX) + No2.Tx.Error = 9; // controller failure + else + No2.Tx.Error = 0; // no failure - No2.Tx.Current_x10 = (uint16_t) (MS.Battery_Current/100); //MS.Battery_Current is in mA - No2.Tx.BrakeActive=brake_flag; + No2.Tx.Current_x10 = (uint16_t)(MS.Battery_Current / 100); // MS.Battery_Current is in mA + No2.Tx.BrakeActive = brake_flag; - /* Apply Rx parameters */ + /* Apply Rx parameters */ - MS.assist_level = No2.Rx.AssistLevel; + MS.assist_level = No2.Rx.AssistLevel; - if(!No2.Rx.Headlight) - { - HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_RESET); - - } - else // KM_HEADLIGHT_ON, KM_HEADLIGHT_LOW, KM_HEADLIGHT_HIGH - { - HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_SET); - - } - - - if(No2.Rx.PushAssist) - { - ui8_Push_Assist_flag=1; - } - else - { - ui8_Push_Assist_flag=0; - } + if (!No2.Rx.Headlight) + { + HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_RESET); + } + else // KM_HEADLIGHT_ON, KM_HEADLIGHT_LOW, KM_HEADLIGHT_HIGH + { + HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_SET); + } + if (No2.Rx.PushAssist) + { + ui8_Push_Assist_flag = 1; } + else + { + ui8_Push_Assist_flag = 0; + } +} #endif #if (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER || DISPLAY_TYPE & DISPLAY_TYPE_DEBUG) - void kingmeter_update(void) - { - /* Prepare Tx parameters */ - - if(battery_percent_fromcapacity > 10) - { - KM.Tx.Battery = KM_BATTERY_NORMAL; - } - else - { - KM.Tx.Battery = KM_BATTERY_LOW; - } +void kingmeter_update(void) +{ + /* Prepare Tx parameters */ + if (battery_percent_fromcapacity > 10) + { + KM.Tx.Battery = KM_BATTERY_NORMAL; + } + else + { + KM.Tx.Battery = KM_BATTERY_LOW; + } -#if (SPEEDSOURCE == EXTERNAL) - KM.Tx.Wheeltime_ms = ((MS.Speed>>3)*PULSES_PER_REVOLUTION); //>>3 because of 8 kHz counter frequency, so 8 tics per ms +#if (SPEEDSOURCE == EXTERNAL) + KM.Tx.Wheeltime_ms = ((MS.Speed >> 3) * PULSES_PER_REVOLUTION); //>>3 because of 8 kHz counter frequency, so 8 tics per ms #else - if(__HAL_TIM_GET_COUNTER(&htim2) < 12000) - { - KM.Tx.Wheeltime_ms = (MS.Speed*GEAR_RATIO*6)>>9; //>>9 because of 500kHZ timer2 frequency, 512 tics per ms should be OK *6 because of 6 hall interrupts per electric revolution. - - } - else - { - KM.Tx.Wheeltime_ms = 64000; - } + if (__HAL_TIM_GET_COUNTER(&htim2) < 12000) + { + KM.Tx.Wheeltime_ms = (MS.Speed * GEAR_RATIO * 6) >> 9; //>>9 because of 500kHZ timer2 frequency, 512 tics per ms should be OK *6 because of 6 hall interrupts per electric revolution. + } + else + { + KM.Tx.Wheeltime_ms = 64000; + } #endif - if(MS.Temperature>130) KM.Tx.Error = KM_ERROR_OVHT; - else if(MS.int_Temperature>80)KM.Tx.Error = KM_ERROR_IOVHT; - else KM.Tx.Error = KM_ERROR_NONE; - - - KM.Tx.Current_x10 = (uint16_t) (MS.Battery_Current/100); //MS.Battery_Current is in mA + if (MS.Temperature > 130) + KM.Tx.Error = KM_ERROR_OVHT; + else if (MS.int_Temperature > 80) + KM.Tx.Error = KM_ERROR_IOVHT; + else + KM.Tx.Error = KM_ERROR_NONE; + KM.Tx.Current_x10 = (uint16_t)(MS.Battery_Current / 100); // MS.Battery_Current is in mA - /* Receive Rx parameters/settings and send Tx parameters */ + /* Receive Rx parameters/settings and send Tx parameters */ #if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_618U) - KingMeter_Service(&KM); + KingMeter_Service(&KM); #endif + /* Apply Rx parameters */ - /* Apply Rx parameters */ - - MS.assist_level = KM.Rx.AssistLevel; - - if(KM.Rx.Headlight == KM_HEADLIGHT_OFF) - { - HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_RESET); - - } - else // KM_HEADLIGHT_ON, KM_HEADLIGHT_LOW, KM_HEADLIGHT_HIGH - { - HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_SET); - - } - - - if(KM.Rx.PushAssist == KM_PUSHASSIST_ON) - { - ui8_Push_Assist_flag=1; - } - else - { - ui8_Push_Assist_flag=0; - } -// if( KM.Settings.Reverse)i8_direction = -1; -// else i8_direction = 1; - // MP.speedLimit=KM.Rx.SPEEDMAX_Limit; - // MP.battery_current_max = KM.Rx.CUR_Limit_mA; - + MS.assist_level = KM.Rx.AssistLevel; + if (KM.Rx.Headlight == KM_HEADLIGHT_OFF) + { + HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_RESET); + } + else // KM_HEADLIGHT_ON, KM_HEADLIGHT_LOW, KM_HEADLIGHT_HIGH + { + HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_SET); + } + if (KM.Rx.PushAssist == KM_PUSHASSIST_ON) + { + ui8_Push_Assist_flag = 1; } + else + { + ui8_Push_Assist_flag = 0; + } + // if( KM.Settings.Reverse)i8_direction = -1; + // else i8_direction = 1; + // MP.speedLimit=KM.Rx.SPEEDMAX_Limit; + // MP.battery_current_max = KM.Rx.CUR_Limit_mA; +} #endif #if (DISPLAY_TYPE & DISPLAY_TYPE_BAFANG) - void bafang_update(void) - { - /* Prepare Tx parameters */ - - /* if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_5)battery_percent_fromcapacity=95; */ - /* else if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_4)battery_percent_fromcapacity=80; */ - /* else if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_3)battery_percent_fromcapacity=50; */ - /* else if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_2)battery_percent_fromcapacity=30; */ - /* else if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_1)battery_percent_fromcapacity=20; */ - /* else battery_percent_fromcapacity=5; */ - - battery_percent_fromcapacity = ((((MS.Voltage * CAL_BAT_V) - (BATTERY_LEVEL_1)) * 100) / ((BATTERY_LEVEL_5 - BATTERY_LEVEL_1))); +void bafang_update(void) +{ + /* Prepare Tx parameters */ - BF.Tx.Battery = battery_percent_fromcapacity; + /* if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_5)battery_percent_fromcapacity=95; */ + /* else if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_4)battery_percent_fromcapacity=80; */ + /* else if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_3)battery_percent_fromcapacity=50; */ + /* else if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_2)battery_percent_fromcapacity=30; */ + /* else if(MS.Voltage*CAL_BAT_V>BATTERY_LEVEL_1)battery_percent_fromcapacity=20; */ + /* else battery_percent_fromcapacity=5; */ + battery_percent_fromcapacity = ((((MS.Voltage * CAL_BAT_V) - (BATTERY_LEVEL_1)) * 100) / ((BATTERY_LEVEL_5 - BATTERY_LEVEL_1))); + BF.Tx.Battery = battery_percent_fromcapacity; -#if (SPEEDSOURCE == EXTERNAL) // Adapt wheeltime to match displayed speedo value according config.h setting +#if (SPEEDSOURCE == EXTERNAL) // Adapt wheeltime to match displayed speedo value according config.h setting - if(uint32_SPEED_counter<16000) BF.Tx.Speed = (external_tics_to_speedx100(MS.Speed)*20)>>8;// Geschwindigkeit ist Weg pro Zeit Radumfang durch Dauer einer Radumdrehung --> Umfang * 8000*3600/(n*1000000) * Skalierung Bafang Display 200/26,6 - else BF.Tx.Speed = 0; + if (uint32_SPEED_counter < 16000) + BF.Tx.Speed = (external_tics_to_speedx100(MS.Speed) * 20) >> 8; // Geschwindigkeit ist Weg pro Zeit Radumfang durch Dauer einer Radumdrehung --> Umfang * 8000*3600/(n*1000000) * Skalierung Bafang Display 200/26,6 + else + BF.Tx.Speed = 0; #else - if(__HAL_TIM_GET_COUNTER(&htim2) < 12000 && MS.system_state != Stop) - { - BF.Tx.Speed =(internal_tics_to_speedx100(MS.Speed)*20)>>8; //factor is *20/256, found empiric - - } - else - { - BF.Tx.Speed = 0; - } + if (__HAL_TIM_GET_COUNTER(&htim2) < 12000 && MS.system_state != Stop) + { + BF.Tx.Speed = (internal_tics_to_speedx100(MS.Speed) * 20) >> 8; // factor is *20/256, found empiric + } + else + { + BF.Tx.Speed = 0; + } #endif - if (MS.Battery_Current < 0) BF.Tx.Power = MS.Battery_Current*-1 > 0 && MS.Battery_Current*-1 < 500 ? 1 : ((MS.Battery_Current*-1)/500); - else BF.Tx.Power = MS.Battery_Current > 0 && MS.Battery_Current < 500 ? 1 : (MS.Battery_Current/500); // Unit: 1 digit --> 0.5 A, MS.Battery_Current is in mA + if (MS.Battery_Current < 0) + BF.Tx.Power = MS.Battery_Current * -1 > 0 && MS.Battery_Current * -1 < 500 ? 1 : ((MS.Battery_Current * -1) / 500); + else + BF.Tx.Power = MS.Battery_Current > 0 && MS.Battery_Current < 500 ? 1 : (MS.Battery_Current / 500); // Unit: 1 digit --> 0.5 A, MS.Battery_Current is in mA + /* Receive Rx parameters/settings and send Tx parameters */ + Bafang_Service(&BF, 1, &MS); - /* Receive Rx parameters/settings and send Tx parameters */ - Bafang_Service(&BF,1, &MS); + /* Apply Rx parameters */ + // No headlight supported on my controller hardware. + if (BF.Rx.Headlight) + { + HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_SET); + } + else + { + HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_RESET); + } + if (BF.Rx.PushAssist) + ui8_Push_Assist_flag = 1; + else + ui8_Push_Assist_flag = 0; - /* Apply Rx parameters */ + MS.assist_level = BF.Rx.AssistLevel; +} - //No headlight supported on my controller hardware. - if(BF.Rx.Headlight) - { - HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_SET); +#endif - } +int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max) +{ + // if input is smaller/bigger than expected return the min/max out ranges value + if (x < in_min) + return out_min; + else if (x > in_max) + return out_max; + + // map the input to the output range. + // round up if mapping bigger ranges to smaller ranges + else if ((in_max - in_min) > (out_max - out_min)) + return (x - in_min) * (out_max - out_min + 1) / (in_max - in_min + 1) + out_min; + // round down if mapping smaller ranges to bigger ranges + else + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +// assuming, a proper AD conversion takes 350 timer tics, to be confirmed. DT+TR+TS deadtime + noise subsiding + sample time +void dyn_adc_state(q31_t angle) +{ + if (switchtime[2] > switchtime[0] && switchtime[2] > switchtime[1]) + { + MS.char_dyn_adc_state = 1; // -90° .. +30°: Phase C at high dutycycles + if (switchtime[2] > 1500) + TIM1->CCR4 = switchtime[2] - TRIGGER_OFFSET_ADC; else - { - HAL_GPIO_WritePin(LIGHT_GPIO_Port, LIGHT_Pin, GPIO_PIN_RESET); - - } - - - if(BF.Rx.PushAssist) ui8_Push_Assist_flag=1; - else ui8_Push_Assist_flag=0; - - MS.assist_level=BF.Rx.AssistLevel; + TIM1->CCR4 = TRIGGER_DEFAULT; } -#endif - - int32_t map (int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max) + if (switchtime[0] > switchtime[1] && switchtime[0] > switchtime[2]) { - // if input is smaller/bigger than expected return the min/max out ranges value - if (x < in_min) - return out_min; - else if (x > in_max) - return out_max; - - // map the input to the output range. - // round up if mapping bigger ranges to smaller ranges - else if ((in_max - in_min) > (out_max - out_min)) - return (x - in_min) * (out_max - out_min + 1) / (in_max - in_min + 1) + out_min; - // round down if mapping smaller ranges to bigger ranges + MS.char_dyn_adc_state = 2; // +30° .. 150° Phase A at high dutycycles + if (switchtime[0] > 1500) + TIM1->CCR4 = switchtime[0] - TRIGGER_OFFSET_ADC; else - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + TIM1->CCR4 = TRIGGER_DEFAULT; } - //assuming, a proper AD conversion takes 350 timer tics, to be confirmed. DT+TR+TS deadtime + noise subsiding + sample time - void dyn_adc_state(q31_t angle){ - if (switchtime[2]>switchtime[0] && switchtime[2]>switchtime[1]){ - MS.char_dyn_adc_state = 1; // -90° .. +30°: Phase C at high dutycycles - if(switchtime[2]>1500)TIM1->CCR4 = switchtime[2]-TRIGGER_OFFSET_ADC; - else TIM1->CCR4 = TRIGGER_DEFAULT; - } - - if (switchtime[0]>switchtime[1] && switchtime[0]>switchtime[2]) { - MS.char_dyn_adc_state = 2; // +30° .. 150° Phase A at high dutycycles - if(switchtime[0]>1500)TIM1->CCR4 = switchtime[0]-TRIGGER_OFFSET_ADC; - else TIM1->CCR4 = TRIGGER_DEFAULT; - } + if (switchtime[1] > switchtime[0] && switchtime[1] > switchtime[2]) + { + MS.char_dyn_adc_state = 3; // +150 .. -90° Phase B at high dutycycles + if (switchtime[1] > 1500) + TIM1->CCR4 = switchtime[1] - TRIGGER_OFFSET_ADC; + else + TIM1->CCR4 = TRIGGER_DEFAULT; + } +} - if (switchtime[1]>switchtime[0] && switchtime[1]>switchtime[2]){ - MS.char_dyn_adc_state = 3; // +150 .. -90° Phase B at high dutycycles - if(switchtime[1]>1500)TIM1->CCR4 = switchtime[1]-TRIGGER_OFFSET_ADC; - else TIM1->CCR4 = TRIGGER_DEFAULT; - } +static void set_inj_channel(char state) +{ + switch (state) + { + case 1: // Phase C at high dutycycles, read current from phase A + B + { + ADC1->JSQR = 0b00100000000000000000; // ADC1 injected reads phase A JL = 0b00, JSQ4 = 0b00100 (decimal 4 = channel 4) + ADC1->JOFR1 = ui16_ph1_offset; + ADC2->JSQR = 0b00101000000000000000; // ADC2 injected reads phase B, JSQ4 = 0b00101, decimal 5 + ADC2->JOFR1 = ui16_ph2_offset; + } + break; + case 2: // Phase A at high dutycycles, read current from phase C + B + { + ADC1->JSQR = 0b00110000000000000000; // ADC1 injected reads phase C, JSQ4 = 0b00110, decimal 6 + ADC1->JOFR1 = ui16_ph3_offset; + ADC2->JSQR = 0b00101000000000000000; // ADC2 injected reads phase B, JSQ4 = 0b00101, decimal 5 + ADC2->JOFR1 = ui16_ph2_offset; } + break; - static void set_inj_channel(char state){ - switch (state) - { - case 1: //Phase C at high dutycycles, read current from phase A + B - { - ADC1->JSQR=0b00100000000000000000; //ADC1 injected reads phase A JL = 0b00, JSQ4 = 0b00100 (decimal 4 = channel 4) - ADC1->JOFR1 = ui16_ph1_offset; - ADC2->JSQR=0b00101000000000000000; //ADC2 injected reads phase B, JSQ4 = 0b00101, decimal 5 - ADC2->JOFR1 = ui16_ph2_offset; + case 3: // Phase B at high dutycycles, read current from phase A + C + { + ADC1->JSQR = 0b00100000000000000000; // ADC1 injected reads phase A JL = 0b00, JSQ4 = 0b00100 (decimal 4 = channel 4) + ADC1->JOFR1 = ui16_ph1_offset; + ADC2->JSQR = 0b00110000000000000000; // ADC2 injected reads phase C, JSQ4 = 0b00110, decimal 6 + ADC2->JOFR1 = ui16_ph3_offset; + } + break; + } +} +uint8_t brake_is_set(void) +{ + return !HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin) || (uint16_mapped_BRAKE > 0); +} +uint8_t throttle_is_set(void) +{ + if (uint16_mapped_throttle > 0) + { + return 1; + } + else + return 0; +} +void autodetect() +{ + SET_BIT(TIM1->BDTR, TIM_BDTR_MOE); + MS.hall_angle_detect_flag = 0; // set uq to contstant value in FOC.c for open loop control + q31_rotorposition_absolute = 1 << 31; + i16_hall_order = 1; // reset hall order + MS.i_d_setpoint = 300; // set MS.id to appr. 2000mA + MS.i_q_setpoint = 0; + // uint8_t zerocrossing = 0; + // q31_t diffangle = 0; + HAL_Delay(5); + for (i = 0; i < 1080; i++) + { + HAL_IWDG_Refresh(&hiwdg); + q31_rotorposition_absolute += 11930465; // drive motor in open loop with steps of 1 deg + HAL_Delay(5); + // printf_("%d, %d, %d, %d\n", temp3>>16,temp4>>16,temp5,temp6); - } - break; - case 2: //Phase A at high dutycycles, read current from phase C + B + if (ui8_hall_state_old != ui8_hall_state) { - ADC1->JSQR=0b00110000000000000000; //ADC1 injected reads phase C, JSQ4 = 0b00110, decimal 6 - ADC1->JOFR1 = ui16_ph3_offset; - ADC2->JSQR=0b00101000000000000000; //ADC2 injected reads phase B, JSQ4 = 0b00101, decimal 5 - ADC2->JOFR1 = ui16_ph2_offset; + printf_("angle: %d, hallstate: %d, hallcase %d \n", + (int16_t)(((q31_rotorposition_absolute >> 23) * 180) >> 8), + ui8_hall_state, ui8_hall_case); + switch (ui8_hall_case) // 12 cases for each transition from one stage to the next. 6x forward, 6x reverse + { + // 6 cases for forward direction + case 64: + Hall_64 = q31_rotorposition_absolute; + break; + case 45: + Hall_45 = q31_rotorposition_absolute; + break; + case 51: + Hall_51 = q31_rotorposition_absolute; + break; + case 13: + Hall_13 = q31_rotorposition_absolute; + break; + case 32: + Hall_32 = q31_rotorposition_absolute; + break; + case 26: + Hall_26 = q31_rotorposition_absolute; + break; + + // 6 cases for reverse direction + case 46: + Hall_64 = q31_rotorposition_absolute; + break; + case 62: + Hall_26 = q31_rotorposition_absolute; + break; + case 23: + Hall_32 = q31_rotorposition_absolute; + break; + case 31: + Hall_13 = q31_rotorposition_absolute; + break; + case 15: + Hall_51 = q31_rotorposition_absolute; + break; + case 54: + Hall_45 = q31_rotorposition_absolute; + break; - } - break; - - case 3: //Phase B at high dutycycles, read current from phase A + C - { - ADC1->JSQR=0b00100000000000000000; //ADC1 injected reads phase A JL = 0b00, JSQ4 = 0b00100 (decimal 4 = channel 4) - ADC1->JOFR1 = ui16_ph1_offset; - ADC2->JSQR=0b00110000000000000000; //ADC2 injected reads phase C, JSQ4 = 0b00110, decimal 6 - ADC2->JOFR1 = ui16_ph3_offset; - + } // end case + ui8_hall_state_old = ui8_hall_state; } - break; - + } - } + CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE); // Disable PWM if motor is not turning + TIM1->CCR1 = 1023; // set initial PWM values + TIM1->CCR2 = 1023; + TIM1->CCR3 = 1023; + MS.hall_angle_detect_flag = 1; + MS.i_d = 0; + MS.i_q = 0; + MS.u_d = 0; + MS.u_q = 0; + MS.i_d_setpoint = 0; + uint32_tics_filtered = 1000000; + HAL_FLASH_Unlock(); + if (i8_recent_rotor_direction == 1) + { + EE_WriteVariable(EEPROM_POS_HALL_ORDER, 1); + i16_hall_order = 1; } + else + { + EE_WriteVariable(EEPROM_POS_HALL_ORDER, -1); + i16_hall_order = -1; + } + EE_WriteVariable(EEPROM_POS_HALL_45, Hall_45 >> 16); + EE_WriteVariable(EEPROM_POS_HALL_51, Hall_51 >> 16); + EE_WriteVariable(EEPROM_POS_HALL_13, Hall_13 >> 16); + EE_WriteVariable(EEPROM_POS_HALL_32, Hall_32 >> 16); + EE_WriteVariable(EEPROM_POS_HALL_26, Hall_26 >> 16); + EE_WriteVariable(EEPROM_POS_HALL_64, Hall_64 >> 16); - uint8_t brake_is_set(void) { - return !HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin) || (uint16_mapped_BRAKE > 0); - } - uint8_t throttle_is_set(void){ - if(uint16_mapped_throttle > 0) - { - return 1; - } - else return 0; - } - void autodetect() { - SET_BIT(TIM1->BDTR, TIM_BDTR_MOE); - MS.hall_angle_detect_flag = 0; //set uq to contstant value in FOC.c for open loop control - q31_rotorposition_absolute = 1 << 31; - i16_hall_order = 1;//reset hall order - MS.i_d_setpoint= 300; //set MS.id to appr. 2000mA - MS.i_q_setpoint= 0; - // uint8_t zerocrossing = 0; - // q31_t diffangle = 0; - HAL_Delay(5); - for (i = 0; i < 1080; i++) { - HAL_IWDG_Refresh(&hiwdg); - q31_rotorposition_absolute += 11930465; //drive motor in open loop with steps of 1 deg - HAL_Delay(5); - //printf_("%d, %d, %d, %d\n", temp3>>16,temp4>>16,temp5,temp6); - - if (ui8_hall_state_old != ui8_hall_state) { - printf_("angle: %d, hallstate: %d, hallcase %d \n", - (int16_t) (((q31_rotorposition_absolute >> 23) * 180) >> 8), - ui8_hall_state, ui8_hall_case); - - switch (ui8_hall_case) //12 cases for each transition from one stage to the next. 6x forward, 6x reverse - { - //6 cases for forward direction - case 64: - Hall_64=q31_rotorposition_absolute; - break; - case 45: - Hall_45=q31_rotorposition_absolute; - break; - case 51: - Hall_51=q31_rotorposition_absolute; - break; - case 13: - Hall_13=q31_rotorposition_absolute; - break; - case 32: - Hall_32=q31_rotorposition_absolute; - break; - case 26: - Hall_26=q31_rotorposition_absolute; - break; - - //6 cases for reverse direction - case 46: - Hall_64=q31_rotorposition_absolute; - break; - case 62: - Hall_26=q31_rotorposition_absolute; - break; - case 23: - Hall_32=q31_rotorposition_absolute; - break; - case 31: - Hall_13=q31_rotorposition_absolute; - break; - case 15: - Hall_51=q31_rotorposition_absolute; - break; - case 54: - Hall_45=q31_rotorposition_absolute; - break; - - } // end case - - - ui8_hall_state_old = ui8_hall_state; - } - } - - CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE); //Disable PWM if motor is not turning - TIM1->CCR1 = 1023; //set initial PWM values - TIM1->CCR2 = 1023; - TIM1->CCR3 = 1023; - MS.hall_angle_detect_flag=1; - MS.i_d = 0; - MS.i_q = 0; - MS.u_d=0; - MS.u_q=0; - MS.i_d_setpoint= 0; - uint32_tics_filtered=1000000; - - HAL_FLASH_Unlock(); - - if (i8_recent_rotor_direction == 1) { - EE_WriteVariable(EEPROM_POS_HALL_ORDER, 1); - i16_hall_order = 1; - } else { - EE_WriteVariable(EEPROM_POS_HALL_ORDER, -1); - i16_hall_order = -1; - } - EE_WriteVariable(EEPROM_POS_HALL_45, Hall_45 >> 16); - EE_WriteVariable(EEPROM_POS_HALL_51, Hall_51 >> 16); - EE_WriteVariable(EEPROM_POS_HALL_13, Hall_13 >> 16); - EE_WriteVariable(EEPROM_POS_HALL_32, Hall_32 >> 16); - EE_WriteVariable(EEPROM_POS_HALL_26, Hall_26 >> 16); - EE_WriteVariable(EEPROM_POS_HALL_64, Hall_64 >> 16); + HAL_FLASH_Lock(); - HAL_FLASH_Lock(); + MS.hall_angle_detect_flag = 1; - MS.hall_angle_detect_flag = 1; + HAL_Delay(5); + ui8_KV_detect_flag = 30; +} - HAL_Delay(5); - ui8_KV_detect_flag = 30; +void get_standstill_position() +{ + HAL_Delay(100); + HAL_TIM_IC_CaptureCallback(&htim2); // read in initial rotor position + switch (ui8_hall_state) + { + // 6 cases for forward direction + case 2: + q31_rotorposition_hall = Hall_32; + break; + case 6: + q31_rotorposition_hall = Hall_26; + break; + case 4: + q31_rotorposition_hall = Hall_64; + break; + case 5: + q31_rotorposition_hall = Hall_45; + break; + case 1: + q31_rotorposition_hall = Hall_51; + break; + case 3: + q31_rotorposition_hall = Hall_13; + break; } - void get_standstill_position(){ - HAL_Delay(100); - HAL_TIM_IC_CaptureCallback(&htim2); //read in initial rotor position - - switch (ui8_hall_state) { - //6 cases for forward direction - case 2: - q31_rotorposition_hall = Hall_32; - break; - case 6: - q31_rotorposition_hall = Hall_26; - break; - case 4: - q31_rotorposition_hall = Hall_64; - break; - case 5: - q31_rotorposition_hall = Hall_45; - break; - case 1: - q31_rotorposition_hall = Hall_51; + q31_rotorposition_absolute = q31_rotorposition_hall; +} - break; - case 3: - q31_rotorposition_hall = Hall_13; - break; +int32_t speed_to_tics(uint8_t speed) +{ + return WHEEL_CIRCUMFERENCE * 5 * 3600 / (6 * GEAR_RATIO * speed * 10); +} - } +int8_t tics_to_speed(uint32_t tics) +{ + return WHEEL_CIRCUMFERENCE * 5 * 3600 / (6 * GEAR_RATIO * tics * 10); +} - q31_rotorposition_absolute = q31_rotorposition_hall; - } +int16_t internal_tics_to_speedx100(uint32_t tics) +{ + return WHEEL_CIRCUMFERENCE * 50 * 3600 / (6 * GEAR_RATIO * tics); +} - int32_t speed_to_tics (uint8_t speed){ - return WHEEL_CIRCUMFERENCE*5*3600/(6*GEAR_RATIO*speed*10); - } +int16_t external_tics_to_speedx100(uint32_t tics) +{ + return WHEEL_CIRCUMFERENCE * 8 * 360 / (PULSES_PER_REVOLUTION * tics); +} - int8_t tics_to_speed (uint32_t tics){ - return WHEEL_CIRCUMFERENCE*5*3600/(6*GEAR_RATIO*tics*10); - } +void runPIcontrol() +{ - int16_t internal_tics_to_speedx100 (uint32_t tics){ - return WHEEL_CIRCUMFERENCE*50*3600/(6*GEAR_RATIO*tics); + q31_t_Battery_Current_accumulated -= q31_t_Battery_Current_accumulated >> 8; + q31_t_Battery_Current_accumulated += ((MS.i_q * MS.u_abs) >> 11) * (uint16_t)(CAL_I >> 8); + + MS.Battery_Current = (q31_t_Battery_Current_accumulated >> 8) * i8_direction * i8_reverse_flag; // Battery current in mA + // Check battery current limit + if (MS.Battery_Current > MP.battery_current_max) + ui8_BC_limit_flag = 1; + if (MS.Battery_Current < -REGEN_CURRENT_MAX) + ui8_BC_limit_flag = 1; + // reset battery current flag with small hysteresis + if (brake_flag == 0) + { + // if(HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)){ + if (((MS.i_q_setpoint * MS.u_abs) >> 11) * (uint16_t)(CAL_I >> 8) < (MP.battery_current_max * 7) >> 3) + ui8_BC_limit_flag = 0; } - - int16_t external_tics_to_speedx100 (uint32_t tics){ - return WHEEL_CIRCUMFERENCE*8*360/(PULSES_PER_REVOLUTION*tics); + else + { + if (((MS.i_q_setpoint * MS.u_abs) >> 11) * (uint16_t)(CAL_I >> 8) > (-REGEN_CURRENT_MAX * 7) >> 3) + ui8_BC_limit_flag = 0; } - void runPIcontrol(){ - - - q31_t_Battery_Current_accumulated -= q31_t_Battery_Current_accumulated>>8; - q31_t_Battery_Current_accumulated += ((MS.i_q*MS.u_abs)>>11)*(uint16_t)(CAL_I>>8); + // control iq - MS.Battery_Current = (q31_t_Battery_Current_accumulated>>8)*i8_direction*i8_reverse_flag; //Battery current in mA - //Check battery current limit - if(MS.Battery_Current>MP.battery_current_max) ui8_BC_limit_flag=1; - if(MS.Battery_Current<-REGEN_CURRENT_MAX) ui8_BC_limit_flag=1; - //reset battery current flag with small hysteresis - if(brake_flag==0){ - //if(HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)){ - if(((MS.i_q_setpoint*MS.u_abs)>>11)*(uint16_t)(CAL_I>>8)<(MP.battery_current_max*7)>>3)ui8_BC_limit_flag=0; - } - else{ - if(((MS.i_q_setpoint*MS.u_abs)>>11)*(uint16_t)(CAL_I>>8)>(-REGEN_CURRENT_MAX*7)>>3)ui8_BC_limit_flag=0; - } - - //control iq - - //if - if (!ui8_BC_limit_flag){ - PI_iq.recent_value = MS.i_q; - PI_iq.setpoint = i8_direction*i8_reverse_flag*MS.i_q_setpoint; + // if + if (!ui8_BC_limit_flag) + { + PI_iq.recent_value = MS.i_q; + PI_iq.setpoint = i8_direction * i8_reverse_flag * MS.i_q_setpoint; + } + else + { + if (brake_flag == 0) + { + // if(HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)){ + PI_iq.recent_value = (MS.Battery_Current >> 6) * i8_direction * i8_reverse_flag; + PI_iq.setpoint = (MP.battery_current_max >> 6) * i8_direction * i8_reverse_flag; } - else{ - if(brake_flag==0){ - // if(HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)){ - PI_iq.recent_value= (MS.Battery_Current>>6)*i8_direction*i8_reverse_flag; - PI_iq.setpoint = (MP.battery_current_max>>6)*i8_direction*i8_reverse_flag; - } - else{ - PI_iq.recent_value= (MS.Battery_Current>>6)*i8_direction*i8_reverse_flag; - PI_iq.setpoint = (-REGEN_CURRENT_MAX>>6)*i8_direction*i8_reverse_flag; - } + else + { + PI_iq.recent_value = (MS.Battery_Current >> 6) * i8_direction * i8_reverse_flag; + PI_iq.setpoint = (-REGEN_CURRENT_MAX >> 6) * i8_direction * i8_reverse_flag; } + } - q31_u_q_temp = PI_control(&PI_iq); - - //Control id - PI_id.recent_value = MS.i_d; - PI_id.setpoint = MS.i_d_setpoint; - q31_u_d_temp = -PI_control(&PI_id); //control direct current to zero - + q31_u_q_temp = PI_control(&PI_iq); - //limit voltage in rotating frame, refer chapter 4.10.1 of UM1052 - //MS.u_abs = (q31_t)hypot((double)q31_u_d_temp, (double)q31_u_q_temp); //absolute value of U in static frame - arm_sqrt_q31((q31_u_d_temp*q31_u_d_temp+q31_u_q_temp*q31_u_q_temp)<<1,&MS.u_abs); - MS.u_abs = (MS.u_abs>>16)+1; + // Control id + PI_id.recent_value = MS.i_d; + PI_id.setpoint = MS.i_d_setpoint; + q31_u_d_temp = -PI_control(&PI_id); // control direct current to zero + // limit voltage in rotating frame, refer chapter 4.10.1 of UM1052 + // MS.u_abs = (q31_t)hypot((double)q31_u_d_temp, (double)q31_u_q_temp); //absolute value of U in static frame + arm_sqrt_q31((q31_u_d_temp * q31_u_d_temp + q31_u_q_temp * q31_u_q_temp) << 1, &MS.u_abs); + MS.u_abs = (MS.u_abs >> 16) + 1; - if (MS.u_abs > _U_MAX){ - MS.u_q = (q31_u_q_temp*_U_MAX)/MS.u_abs; //division! - MS.u_d = (q31_u_d_temp*_U_MAX)/MS.u_abs; //division! - MS.u_abs = _U_MAX; - } - else{ - MS.u_q=q31_u_q_temp; - MS.u_d=q31_u_d_temp; - } - - PI_flag=0; + if (MS.u_abs > _U_MAX) + { + MS.u_q = (q31_u_q_temp * _U_MAX) / MS.u_abs; // division! + MS.u_d = (q31_u_d_temp * _U_MAX) / MS.u_abs; // division! + MS.u_abs = _U_MAX; } - - q31_t speed_PLL (q31_t ist, q31_t soll, uint8_t speedadapt) + else { - q31_t q31_p; - static q31_t q31_d_i = 0; - static q31_t q31_d_dc = 0; - // temp6 = soll-ist; - // temp5 = speedadapt; - q31_p=(soll - ist)>>(P_FACTOR_PLL-speedadapt); //7 for Shengyi middrive, 10 for BionX IGH3 - q31_d_i+=(soll - ist)>>(I_FACTOR_PLL-speedadapt); //11 for Shengyi middrive, 10 for BionX IGH3 - - //clamp i part to twice the theoretical value from hall interrupts - if (q31_d_i>((deg_30>>18)*500/ui16_timertics)<<16) q31_d_i = ((deg_30>>18)*500/ui16_timertics)<<16; - if (q31_d_i<-((deg_30>>18)*500/ui16_timertics)<<16) q31_d_i =- ((deg_30>>18)*500/ui16_timertics)<<16; - + MS.u_q = q31_u_q_temp; + MS.u_d = q31_u_d_temp; + } - if (!ist&&!soll)q31_d_i=0; + PI_flag = 0; +} - q31_d_dc=q31_p+q31_d_i; - return (q31_d_dc); - } +q31_t speed_PLL(q31_t ist, q31_t soll, uint8_t speedadapt) +{ + q31_t q31_p; + static q31_t q31_d_i = 0; + static q31_t q31_d_dc = 0; + // temp6 = soll-ist; + // temp5 = speedadapt; + q31_p = (soll - ist) >> (P_FACTOR_PLL - speedadapt); // 7 for Shengyi middrive, 10 for BionX IGH3 + q31_d_i += (soll - ist) >> (I_FACTOR_PLL - speedadapt); // 11 for Shengyi middrive, 10 for BionX IGH3 + + // clamp i part to twice the theoretical value from hall interrupts + if (q31_d_i > ((deg_30 >> 18) * 500 / ui16_timertics) << 16) + q31_d_i = ((deg_30 >> 18) * 500 / ui16_timertics) << 16; + if (q31_d_i < -((deg_30 >> 18) * 500 / ui16_timertics) << 16) + q31_d_i = -((deg_30 >> 18) * 500 / ui16_timertics) << 16; + + if (!ist && !soll) + q31_d_i = 0; + + q31_d_dc = q31_p + q31_d_i; + return (q31_d_dc); +} #if (R_TEMP_PULLUP) - int16_t T_NTC(uint16_t ADC) // ADC 12 Bit, 10k Pullup, Rückgabewert in °C +int16_t T_NTC(uint16_t ADC) // ADC 12 Bit, 10k Pullup, Rückgabewert in °C - { - uint16_t Ux1000 = 3300; - uint16_t U2x1000 = ADC*Ux1000/4095; - uint16_t R1 = R_TEMP_PULLUP; - uint32_t R = U2x1000*R1/(Ux1000-U2x1000); - // printf("R= %d\r\n",R); - // printf("u2= %d\r\n",U2x1000); - if(R >> 19) return -44; - uint16_t n = 0; - while(R >> n > 1) n++; - R <<= 13; - for(n <<= 6; R >> (n >> 6) >> 13; n++) R -= (R >> 10)*11; // Annäherung 1-11/1024 für 2^(-1/64) - int16_t T6 = 2160580/(n+357)-1639; // Berechnung für 10 kOhm-NTC (bei 25 °C) mit beta=3900 K - return (T6 > 0 ? T6+3 : T6-2)/6; // Rundung +{ + uint16_t Ux1000 = 3300; + uint16_t U2x1000 = ADC * Ux1000 / 4095; + uint16_t R1 = R_TEMP_PULLUP; + uint32_t R = U2x1000 * R1 / (Ux1000 - U2x1000); + // printf("R= %d\r\n",R); + // printf("u2= %d\r\n",U2x1000); + if (R >> 19) + return -44; + uint16_t n = 0; + while (R >> n > 1) + n++; + R <<= 13; + for (n <<= 6; R >> (n >> 6) >> 13; n++) + R -= (R >> 10) * 11; // Annäherung 1-11/1024 für 2^(-1/64) + int16_t T6 = 2160580 / (n + 357) - 1639; // Berechnung für 10 kOhm-NTC (bei 25 °C) mit beta=3900 K + return (T6 > 0 ? T6 + 3 : T6 - 2) / 6; // Rundung +} +#endif +void init_watchdog(void) +{ - } -#endif - void init_watchdog(void) + if (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST)) { - if(__HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST)) - { - - printf_("watchdog reset!\n"); - HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); - // do not continue here if reset from watchdog - //while(1){} - //__HAL_RCC_CLEAR_RESET_FLAGS(); - } - else - { - HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); - printf_("regular reset\n\n"); - - } - // start independent watchdog - MX_IWDG_Init(); - - + printf_("watchdog reset!\n"); + HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET); + // do not continue here if reset from watchdog + // while(1){} + //__HAL_RCC_CLEAR_RESET_FLAGS(); } - - /* IWDG init function */ - void MX_IWDG_Init(void) + else { - // RM0008 - Table 96 IWDG timout period in seconds: - // (IWDG_PRESCALER) * (Period + 1) / f_LSI - // datasheet STM32F103x4 -> f_LSI = 40'000 Hz ?! - // - // 32 * 1000 / 32000 = 1s - hiwdg.Instance = IWDG; - hiwdg.Init.Prescaler = IWDG_PRESCALER_32; - hiwdg.Init.Reload = 1000; - // start the watchdog timer - if (HAL_IWDG_Init(&hiwdg) != HAL_OK) - { - _Error_Handler(__FILE__, __LINE__); - } - + HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET); + printf_("regular reset\n\n"); } - /* USER CODE END 4 */ + // start independent watchdog + MX_IWDG_Init(); +} - /** - * @brief This function is executed in case of error occurrence. - * @param file: The file name as string. - * @param line: The line in file as a number. - * @retval None - */ - void _Error_Handler(char *file, int line) +/* IWDG init function */ +void MX_IWDG_Init(void) +{ + // RM0008 - Table 96 IWDG timout period in seconds: + // (IWDG_PRESCALER) * (Period + 1) / f_LSI + // datasheet STM32F103x4 -> f_LSI = 40'000 Hz ?! + // + // 32 * 1000 / 32000 = 1s + hiwdg.Instance = IWDG; + hiwdg.Init.Prescaler = IWDG_PRESCALER_32; + hiwdg.Init.Reload = 1000; + // start the watchdog timer + if (HAL_IWDG_Init(&hiwdg) != HAL_OK) { - /* USER CODE BEGIN Error_Handler_Debug */ - /* User can add his own implementation to report the HAL error return state */ - while(1) - { - CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE); // Disable PWM in case of an error. - } - /* USER CODE END Error_Handler_Debug */ + _Error_Handler(__FILE__, __LINE__); } +} +/* USER CODE END 4 */ -#ifdef USE_FULL_ASSERT - /** - * @brief Reports the name of the source file and the source line number - * where the assert_param error has occurred. - * @param file: pointer to the source file name - * @param line: assert_param error line source number - * @retval None - */ - void assert_failed(uint8_t* file, uint32_t line) +/** + * @brief This function is executed in case of error occurrence. + * @param file: The file name as string. + * @param line: The line in file as a number. + * @retval None + */ +void _Error_Handler(char *file, int line) +{ + /* USER CODE BEGIN Error_Handler_Debug */ + /* User can add his own implementation to report the HAL error return state */ + while (1) { - /* USER CODE BEGIN 6 */ - /* User can add his own implementation to report the file name and line number, - tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ - /* USER CODE END 6 */ + CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE); // Disable PWM in case of an error. } + /* USER CODE END Error_Handler_Debug */ +} + +#ifdef USE_FULL_ASSERT +/** + * @brief Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * @param file: pointer to the source file name + * @param line: assert_param error line source number + * @retval None + */ +void assert_failed(uint8_t *file, uint32_t line) +{ + /* USER CODE BEGIN 6 */ + /* User can add his own implementation to report the file name and line number, + tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ + /* USER CODE END 6 */ +} #endif /* USE_FULL_ASSERT */ - /** - * @} - */ +/** + * @} + */ - /** - * @} - */ +/** + * @} + */ - /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/proven settings/20250517-123426MESZ.ini b/proven settings/20250517-123426MESZ.ini new file mode 100644 index 00000000..772a083f --- /dev/null +++ b/proven settings/20250517-123426MESZ.ini @@ -0,0 +1,70 @@ +50 +2020 +2028 +256 +25 +48LL<<8 +6 +40 +1200 +9 +43000 +45000 +47000 +49000 +51000 +500 +20 +500 +20 +5 +11 +100 +10 +0 +1 +3500 +35000 +4000 +1200 +18 +30 +15 +750 +2600 +8000 +2200 +88 +25 +6 +25000 +20000 +0 +0 +35000 +55000 +-715827882 +false +true +true +false +false +false +true +false +false +false +false +false +false +true +false +false +true +false +false +true +false +false +"C:\Ac6\SystemWorkbench" +"C:\Program Files (x86)\STMicroelectronics" diff --git a/proven settings/20250525-213131MESZ.ini b/proven settings/20250525-213131MESZ.ini new file mode 100644 index 00000000..ec65af06 --- /dev/null +++ b/proven settings/20250525-213131MESZ.ini @@ -0,0 +1,70 @@ +50 +2020 +2028 +256 +25 +48LL<<8 +6 +40 +1200 +9 +43000 +45000 +47000 +49000 +51000 +500 +20 +500 +20 +5 +11 +100 +10 +0 +1 +3500 +35000 +8000 +1600 +18 +30 +15 +750 +2600 +8000 +2200 +88 +35 +6 +25000 +17000 +0 +0 +35000 +55000 +-715827882 +false +true +true +false +false +false +true +false +false +false +false +false +false +true +false +false +true +false +false +true +false +false +"C:\Ac6\SystemWorkbench" +"C:\Program Files (x86)\STMicroelectronics" diff --git a/proven settings/20250526-214658MESZ.ini b/proven settings/20250526-214658MESZ.ini new file mode 100644 index 00000000..b28258e6 --- /dev/null +++ b/proven settings/20250526-214658MESZ.ini @@ -0,0 +1,70 @@ +50 +2020 +2028 +256 +25 +48LL<<8 +6 +40 +1200 +9 +43000 +45000 +47000 +49000 +51000 +500 +20 +500 +20 +5 +11 +100 +10 +0 +1 +3500 +35000 +8000 +1600 +18 +30 +15 +750 +2600 +8000 +2200 +88 +99 +6 +25000 +17000 +1000 +5000 +35000 +55000 +-715827882 +false +true +true +false +false +false +true +false +false +false +false +false +false +true +false +false +true +false +false +true +false +false +"C:\Ac6\SystemWorkbench" +"C:\Program Files (x86)\STMicroelectronics"