Hallo,
ich programmiere einen STM32F103C Mikrocontroller. Ich habe eine ISR
Funktion, die mit 60µs Takt aufgerufen wird. Da der STM32F1 keinen FPU
hat, möchte ich die paar Gleitkomma Berechnungen mit Shifts realisieren,
klappt leider nicht.
Die Implementierung mit Float:
1 | volatile float angle_vel_L = 0.0f;
|
2 | void angleEstimate_L(void) {
|
3 | float angle_pL = angle_L + angle_vel_L * 0.000064f;
|
4 |
|
5 | float measured_angle_L = (float) ((hallCounterL << 6) - (hallCounterL << 2));
|
6 |
|
7 | float errAngle_L = measured_angle_L - angle_pL;
|
8 | angle_L = angle_pL + 0.03340962f * errAngle_L;
|
9 | angle_vel_L += 0.05676238f * errAngle_L;
|
10 |
|
11 | }
|
mit Integer:
1 | #define SCALE 28
|
2 |
|
3 | volatile int angle_L = 0;
|
4 | volatile int angle_vel_L = 0;
|
5 | void angleEstimate_L(void) {
|
6 | int angle_pL = angle_L + (angle_vel_L >> 14);
|
7 | int measured_angle_L = ((hallCounterL << 6) - (hallCounterL << 2)) << SCALE;
|
8 |
|
9 | int errAngle_L = measured_angle_L - angle_pL;
|
10 | angle_L = angle_pL + (errAngle_L >> 5);
|
11 | angle_vel_L += (errAngle_L >> 4);
|
12 |
|
13 | }
|
14 |
|
15 | int getAngle(void) {
|
16 | return angle_L >> SCALE;
|
17 | }
|
Weiß jemand vielleicht wo der Fehler liegt.
Vielen Dank im Voraus.
