Subversion Repositories FlightCtrl

Rev

Rev 1866 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1866 Rev 1868
Line 30... Line 30...
30 
// + this list of conditions and the following disclaimer.
 30 
// + this list of conditions and the following disclaimer.
31 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 31 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32 
// +     from this software without specific prior written permission.
 32 
// +     from this software without specific prior written permission.
33 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
 33 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34 
// +     for non-commercial use (directly or indirectly)
 34 
// +     for non-commercial use (directly or indirectly)
35 
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 35 
// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
36 
// +     with our written permission
 36 
// +     with our written permission
37 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 37 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38 
// +     clearly linked as origin
 38 
// +     clearly linked as origin
39 
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 39 
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
40 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 40 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Line 227... Line 227...
227 
 */
 227 
 */
228 
void trigAxisCoupling(void) {
 228 
void trigAxisCoupling(void) {
229 
        int16_t cospitch = int_cos(angle[PITCH]);
 229 
        int16_t cospitch = int_cos(angle[PITCH]);
230 
        int16_t cosroll = int_cos(angle[ROLL]);
 230 
        int16_t cosroll = int_cos(angle[ROLL]);
231 
        int16_t sinroll = int_sin(angle[ROLL]);
 231 
        int16_t sinroll = int_sin(angle[ROLL]);
232 
        int16_t tanpitch = int_tan(angle[PITCH]);
 - 
 
Line -... Line 232...
- 
 
 232 
 
233 
 
 233 
        ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate
Line 234... Line -...
234 
        int16_t test;
 - 
 
235 
 
 234 
                    * sinroll) >> MATH_UNIT_FACTOR_LOG);
236 
 
 235 
 
237 
#define ANTIOVF 512
 236 
        ACRate[ROLL] = rate_ATT[ROLL] +
238 
        ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate
 - 
 
239 
                        * sinroll) / (int32_t) MATH_UNIT_FACTOR;
 - 
 
240 
        ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll
 237 
          (((((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll)
241 
                        / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF
 - 
 
242 
                        * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR));
 - 
 
243 
 
 - 
 
244 
        test = rate_ATT[ROLL] +
 - 
 
245 
          (((rate_ATT[PITCH] * sinroll + yawRate * cosroll) >> 14) * tanpitch) >> 14;
 - 
 
246 
 
 - 
 
Line 247... Line 238...
247 
        DebugOut.Analog[20] = ACRate[ROLL];
 238 
              >> MATH_UNIT_FACTOR_LOG)
248 
        DebugOut.Analog[21] = test;
 239 
              * int_tan(angle[PITCH])) >> MATH_UNIT_FACTOR_LOG);
249 
 
 240