Für einen Autopiloten (Modellflugzeuge) verwende ich unter anderem die Funktion arm_sin_cos_f32(). Leider kann ich nach Aufruf dieser Funktion nicht mit den sin-cos-Werten weiterrechnen... Wenn ich stattdessen arm_sin_f32() und arm_cos_f32() nutze, funktioniert alles wunderbar!
1 | /* calculate orientation-setpoint (qSP) */
|
2 | /* using euler > quaternion conversion */
|
3 | |
4 | // funktioniert:
|
5 | sin_phiSP_half = arm_sin_f32(phiSP * 0.5); |
6 | sin_thetaSP_half = arm_sin_f32(thetaSP * 0.5); |
7 | sin_psiSP_half = arm_sin_f32(psiSP * 0.5); |
8 | cos_phiSP_half = arm_cos_f32(phiSP * 0.5); |
9 | cos_thetaSP_half = arm_cos_f32(thetaSP * 0.5); |
10 | cos_psiSP_half = arm_cos_f32(psiSP * 0.5); |
11 | |
12 | // funktioniert nicht:
|
13 | arm_sin_cos_f32((phiSP * 0.5), &sin_phiSP_half, &cos_phiSP_half); |
14 | arm_sin_cos_f32((thetaSP * 0.5), &sin_thetaSP_half, &cos_thetaSP_half); |
15 | arm_sin_cos_f32((psiSP * 0.5), &sin_psiSP_half, &cos_psiSP_half); |
16 | |
17 | qSP0 = (cos_phiSP_half * cos_thetaSP_half * cos_psiSP_half) + (sin_phiSP_half * sin_thetaSP_half * sin_psiSP_half); |
18 | qSP1 = (sin_phiSP_half * cos_thetaSP_half * cos_psiSP_half) - (cos_phiSP_half * sin_thetaSP_half * sin_psiSP_half); |
19 | qSP2 = (cos_phiSP_half * sin_thetaSP_half * cos_psiSP_half) + (sin_phiSP_half * cos_thetaSP_half * sin_psiSP_half); |
20 | qSP3 = (cos_phiSP_half * cos_thetaSP_half * sin_psiSP_half) - (sin_phiSP_half * sin_thetaSP_half * cos_psiSP_half); |