interfisch
/
hoverboard-firmware-hack-foc-serial-esc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added fixed-point low pass filter functions 

Preparation of the low pass filter functions. They will be used to replace floating point filters.
This commit is contained in:
EmanuelFeru 2019-10-06 22:43:08 +02:00
parent 437990bfa3
commit 0527bd770c
8 changed files with 143 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.c
View File

@ -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: '<Root>/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: '<S1>/Low_Pass_Filter1' */
/* Sum: '<S2>/Sum1' incorporates:
@ -127,16 +128,25 @@ int16_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW)
* Sum: '<S2>/Sum5'
* UnitDelay: '<S2>/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: '<S2>/UnitDelay3' */
localDW->UnitDelay3_DSTATE = rtb_Sum1;
rty_y_0 = (int16_T)tmp;
/* Update for UnitDelay: '<S2>/UnitDelay3' incorporates:
* Sum: '<S2>/Sum1'
*/
localDW->UnitDelay3_DSTATE = (int16_T)tmp;
/* End of Outputs for SubSystem: '<S1>/Low_Pass_Filter1' */
/* DataTypeConversion: '<S1>/Data Type Conversion1' */
return (int16_T)(rtb_Sum1 >> 4);
return rty_y_0;
}
/* Model step function */

14
01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.h
View File

@ -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
*
* '<Root>' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control'
* '<S1>' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass'
* '<S2>' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass/Low_Pass_Filter1'
* '<Root>' : 'BLDCmotorControl_FOC_R2017b_fixdt'
* '<S1>' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass'
* '<S2>' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Low_Pass_Filter1'
*/
#endif /* RTW_HEADER_filtLowPass_h_ */

36
01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c
View File

@ -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: '<Root>/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: '<S1>/Low_Pass_Filter1' */
/* Sum: '<S2>/Sum1' incorporates:
@ -127,16 +129,24 @@ int16_T filtLowPass_l(int32_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW)
* Sum: '<S2>/Sum5'
* UnitDelay: '<S2>/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: '<S2>/UnitDelay3' */
localDW->UnitDelay3_DSTATE = rtb_Sum1;
/* Update for UnitDelay: '<S2>/UnitDelay3' incorporates:
* Sum: '<S2>/Sum1'
*/
localDW->UnitDelay3_DSTATE = rty_y_0;
/* End of Outputs for SubSystem: '<S1>/Low_Pass_Filter1' */
/* DataTypeConversion: '<S1>/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: '<Root>/coef'
* Inport: '<Root>/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: '<Root>/filtLowPass' */
}

18
01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h
View File

@ -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; /* '<Root>/u' */
int16_T u; /* '<Root>/u' */
uint16_T coef; /* '<Root>/coef' */
} ExtU;
/* External outputs (root outports fed by signals with auto storage) */
typedef struct {
int16_T y; /* '<Root>/y' */
int32_T y; /* '<Root>/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
*
* '<Root>' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control'
* '<S1>' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass'
* '<S2>' : 'BLDCmotorControl_FOC_R2017b_fixdt/BLDC_controller/F04_Field_Oriented_Control/filtLowPass/Low_Pass_Filter1'
* '<Root>' : 'BLDCmotorControl_FOC_R2017b_fixdt'
* '<S1>' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass'
* '<S2>' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Low_Pass_Filter1'
*/
#endif /* RTW_HEADER_filtLowPass_h_ */

BIN
01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx
View File

Binary file not shown.

6
Inc/defines.h
View File

@ -157,4 +157,8 @@ typedef struct {
uint16_t l_tx2;
uint16_t temp;
uint16_t l_rx2;
} adc_buf_t;
} 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);

14
README.md
View File

@ -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.

67
Src/main.c
View File

@ -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;
}
// ===========================================================