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:
parent
437990bfa3
commit
0527bd770c
|
@ -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 */
|
||||
|
|
|
@ -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_ */
|
||||
|
||||
|
|
|
@ -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' */
|
||||
}
|
||||
|
|
|
@ -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_ */
|
||||
|
||||
|
|
Binary file not shown.
|
@ -158,3 +158,7 @@ typedef struct {
|
|||
uint16_t temp;
|
||||
uint16_t l_rx2;
|
||||
} 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
14
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.
|
||||
|
||||
|
|
67
Src/main.c
67
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;
|
||||
}
|
||||
// ===========================================================
|
||||
|
|
Loading…
Reference in New Issue