diff --git a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.c b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.c index 16a9b26..1ddb411 100644 --- a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.c +++ b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'filtLowPass'. * - * Model version : 1.1160 + * Model version : 1.1167 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Fri Sep 27 08:03:25 2019 + * C/C++ source code generated on : Sun Oct 6 22:11:53 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -117,7 +117,8 @@ extern int16_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass /* Output and update for atomic system: '/filtLowPass' */ int16_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW) { - int16_T rtb_Sum1; + int32_T tmp; + int16_T rty_y_0; /* Outputs for Atomic SubSystem: '/Low_Pass_Filter1' */ /* Sum: '/Sum1' incorporates: @@ -127,16 +128,25 @@ int16_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW) * Sum: '/Sum5' * UnitDelay: '/UnitDelay3' */ - rtb_Sum1 = (int16_T)((((int16_T)(rtu_u << 4) * rtu_coef) >> 16) + (((int32_T) - (65535U - rtu_coef) * localDW->UnitDelay3_DSTATE) >> 16)); + tmp = (((int16_T)(rtu_u << 4) * rtu_coef) >> 16) + (((int32_T)(65535U - + rtu_coef) * localDW->UnitDelay3_DSTATE) >> 16); + if (tmp > 32767) { + tmp = 32767; + } else { + if (tmp < -32768) { + tmp = -32768; + } + } - /* Update for UnitDelay: '/UnitDelay3' */ - localDW->UnitDelay3_DSTATE = rtb_Sum1; + rty_y_0 = (int16_T)tmp; + + /* Update for UnitDelay: '/UnitDelay3' incorporates: + * Sum: '/Sum1' + */ + localDW->UnitDelay3_DSTATE = (int16_T)tmp; /* End of Outputs for SubSystem: '/Low_Pass_Filter1' */ - - /* DataTypeConversion: '/Data Type Conversion1' */ - return (int16_T)(rtb_Sum1 >> 4); + return rty_y_0; } /* Model step function */ diff --git a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.h b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.h index cccf838..b137f33 100644 --- a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.h +++ b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'filtLowPass'. * - * Model version : 1.1160 + * Model version : 1.1167 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Fri Sep 27 08:03:25 2019 + * C/C++ source code generated on : Sun Oct 6 22:11:53 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -73,14 +73,14 @@ extern void filtLowPass_step(RT_MODEL *const rtM); * MATLAB hilite_system command to trace the generated code back * to the parent model. For example, * - * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass') - opens subsystem BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass - * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass/Kp') - opens and selects block Kp + * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass') - opens subsystem BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass + * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Kp') - opens and selects block Kp * * Here is the system hierarchy for this model * - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass/Low_Pass_Filter1' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Low_Pass_Filter1' */ #endif /* RTW_HEADER_filtLowPass_h_ */ diff --git a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c index 8660486..76cc256 100644 --- a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c +++ b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'filtLowPass'. * - * Model version : 1.1160 + * Model version : 1.1165 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Fri Sep 27 08:08:04 2019 + * C/C++ source code generated on : Sun Oct 6 22:00:52 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -81,7 +81,7 @@ preprocessor word size checks. #endif #endif -extern int16_T filtLowPass_l(int32_T rtu_u, uint16_T rtu_coef, DW_filtLowPass +extern int32_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW); /*===========* @@ -115,9 +115,11 @@ extern int16_T filtLowPass_l(int32_T rtu_u, uint16_T rtu_coef, DW_filtLowPass #endif /* Output and update for atomic system: '/filtLowPass' */ -int16_T filtLowPass_l(int32_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW) +int32_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW) { - int32_T rtb_Sum1; + int32_T q0; + int32_T q1; + int32_T rty_y_0; /* Outputs for Atomic SubSystem: '/Low_Pass_Filter1' */ /* Sum: '/Sum1' incorporates: @@ -127,16 +129,24 @@ int16_T filtLowPass_l(int32_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW) * Sum: '/Sum5' * UnitDelay: '/UnitDelay3' */ - rtb_Sum1 = (int32_T)(((int64_T)(rtu_u << 16) * rtu_coef) >> 16) + (int32_T) - (((int64_T)(65535U - rtu_coef) * localDW->UnitDelay3_DSTATE) >> 16); + q0 = (int32_T)(((int64_T)(rtu_u << 16) * rtu_coef) >> 16); + q1 = (int32_T)(((int64_T)(65535U - rtu_coef) * localDW->UnitDelay3_DSTATE) >> + 16); + if ((q0 < 0) && (q1 < MIN_int32_T - q0)) { + rty_y_0 = MIN_int32_T; + } else if ((q0 > 0) && (q1 > MAX_int32_T - q0)) { + rty_y_0 = MAX_int32_T; + } else { + rty_y_0 = q0 + q1; + } - /* Update for UnitDelay: '/UnitDelay3' */ - localDW->UnitDelay3_DSTATE = rtb_Sum1; + /* Update for UnitDelay: '/UnitDelay3' incorporates: + * Sum: '/Sum1' + */ + localDW->UnitDelay3_DSTATE = rty_y_0; /* End of Outputs for SubSystem: '/Low_Pass_Filter1' */ - - /* DataTypeConversion: '/Data Type Conversion1' */ - return (int16_T)(rtb_Sum1 >> 16); + return rty_y_0; } /* Model step function */ @@ -152,7 +162,7 @@ void filtLowPass_step(RT_MODEL *const rtM) * Inport: '/coef' * Inport: '/u' */ - rtY->y = (int16_T) filtLowPass_l(rtU->u, rtU->coef, &rtDW->filtLowPass_l2); + rtY->y = filtLowPass_l(rtU->u, rtU->coef, &rtDW->filtLowPass_l2); /* End of Outputs for SubSystem: '/filtLowPass' */ } diff --git a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h index 12d828b..f773172 100644 --- a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h +++ b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'filtLowPass'. * - * Model version : 1.1160 + * Model version : 1.1165 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Fri Sep 27 08:08:04 2019 + * C/C++ source code generated on : Sun Oct 6 22:00:52 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -41,13 +41,13 @@ typedef struct { /* External inputs (root inport signals with auto storage) */ typedef struct { - int32_T u; /* '/u' */ + int16_T u; /* '/u' */ uint16_T coef; /* '/coef' */ } ExtU; /* External outputs (root outports fed by signals with auto storage) */ typedef struct { - int16_T y; /* '/y' */ + int32_T y; /* '/y' */ } ExtY; /* Real-time Model Data Structure */ @@ -73,14 +73,14 @@ extern void filtLowPass_step(RT_MODEL *const rtM); * MATLAB hilite_system command to trace the generated code back * to the parent model. For example, * - * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass') - opens subsystem BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass - * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass/Kp') - opens and selects block Kp + * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass') - opens subsystem BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass + * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Kp') - opens and selects block Kp * * Here is the system hierarchy for this model * - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass/Low_Pass_Filter1' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Low_Pass_Filter1' */ #endif /* RTW_HEADER_filtLowPass_h_ */ diff --git a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx index ac455cc..12279d4 100644 Binary files a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx and b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx differ diff --git a/Inc/defines.h b/Inc/defines.h index 30e9911..d99da31 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -157,4 +157,8 @@ typedef struct { uint16_t l_tx2; uint16_t temp; uint16_t l_rx2; -} adc_buf_t; \ No newline at end of file +} adc_buf_t; + +// Define low-pass filter functions. Implementation is in main.c +int16_t filtLowPass16(int16_t u, uint16_t coef, int16_t yPrev); +int32_t filtLowPass32(int32_t u, uint16_t coef, int32_t yPrev); \ No newline at end of file diff --git a/README.md b/README.md index 6b9a6f8..f62cfbd 100644 --- a/README.md +++ b/README.md @@ -127,11 +127,21 @@ Nunchuck or PPM working bad: The i2c bus and PPM signal are very sensitive to em Most robust way for input is to use the ADC and potis. It works well even on 1m unshielded cable. Solder ~100k Ohm resistors between ADC-inputs and gnd directly on the mainboard. Use potis as pullups to 3.3V. --- - - ## Examples Have a look at the config.h in the Inc directory. That's where you configure to firmware to match your project. Currently supported: Wii Nunchuck, analog potentiometer and PPM-Sum signal from a RC remote. A good example of control via UART, eg. from an Arduino or raspberryPi, can be found here: https://github.com/p-h-a-i-l/hoverboard-firmware-hack + +--- +## Acknowledgements + +Last but not least, I would like to acknowledge and thank the following people: + +github: @TomTinkering, @ced2c, @btsimonh, @lalalandrus, @p-h-a-i-l , @AntumArk, @juodumas + +ST Employee: [cedric H](https://community.st.com/s/question/0D50X0000B28qTDSQY/custom-foc-control-current-measurement-dma-timer-interrupt-needs-review) + +for the very useful discussions, code snippets, and good suggestions to make this work possbile. + diff --git a/Src/main.c b/Src/main.c index ad8d365..d517977 100644 --- a/Src/main.c +++ b/Src/main.c @@ -429,3 +429,70 @@ void SystemClock_Config(void) { /* SysTick_IRQn interrupt configuration */ HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); } + + +// =========================================================== + /* Low pass filter fixed-point 16 bits: fixdt(1,16,4) + * Max: 2047.9375 + * Min: -2048 + * Res: 0.0625 + * coef: [0,65535U] = fixdt(0,16,16) + * + * Call function example: + * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point) + * y = filtLowPass16(u, 52429, y); + */ +int16_t filtLowPass16(int16_t u, uint16_t coef, int16_t yPrev) +{ + int32_t tmp; + int16_t y; + + tmp = (((int16_t)(u << 4) * coef) >> 16) + + (((int32_t)(65535U - coef) * yPrev) >> 16); + + // Overflow protection + if (tmp > 32767) { + tmp = 32767; + } else { + if (tmp < -32768) { + tmp = -32768; + } + } + + y = (int16_t)tmp; + + return y; +} + +// =========================================================== + /* Low pass filter fixed-point 32 bits: fixdt(1,32,16) + * Max: 32767.99998474121 + * Min: -32768 + * Res: 1.52587890625e-5 + * coef: [0,65535U] = fixdt(0,16,16) + * + * Call function example: + * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point) + * y = filtLowPass16(u, 52429, y); + */ +int32_t filtLowPass32(int32_t u, uint16_t coef, int32_t yPrev) +{ + int32_t q0; + int32_t q1; + int32_t y; + + q0 = (int32_t)(((int64_t)(u << 16) * coef) >> 16); + q1 = (int32_t)(((int64_t)(65535U - coef) * yPrev) >> 16); + + // Overflow protection + if ((q0 < 0) && (q1 < MIN_int32_T - q0)) { + y = MIN_int32_T; + } else if ((q0 > 0) && (q1 > MAX_int32_T - q0)) { + y = MAX_int32_T; + } else { + y = q0 + q1; + } + + return y; +} +// ===========================================================