| 32,7 → 32,7 |
|---|
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| 229,24 → 229,15 |
|---|
| int16_t cospitch = int_cos(angle[PITCH]); |
| int16_t cosroll = int_cos(angle[ROLL]); |
| int16_t sinroll = int_sin(angle[ROLL]); |
| int16_t tanpitch = int_tan(angle[PITCH]); |
| |
| int16_t test; |
| ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
| * sinroll) >> MATH_UNIT_FACTOR_LOG); |
| |
| ACRate[ROLL] = rate_ATT[ROLL] + |
| (((((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) |
| >> MATH_UNIT_FACTOR_LOG) |
| * int_tan(angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
| |
| #define ANTIOVF 512 |
| ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
| * sinroll) / (int32_t) MATH_UNIT_FACTOR; |
| ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
| / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF |
| * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
| |
| test = rate_ATT[ROLL] + |
| (((rate_ATT[PITCH] * sinroll + yawRate * cosroll) >> 14) * tanpitch) >> 14; |
| |
| DebugOut.Analog[20] = ACRate[ROLL]; |
| DebugOut.Analog[21] = test; |
| |
| ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
| + ((int32_t) yawRate * cosroll) / cospitch; |
| } |