Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1877 → Rev 1887

/branches/dongfang_FC_rewrite/attitudeControl.c
8,8 → 8,8
#include "output.h"
 
// = cos^2(45 degs).
const int32_t FACTORSQUARED = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2));
const int32_t MINPROJECTION = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2-1));
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
const int32_t MINPROJECTION = 1L << (MATH_UNIT_FACTOR_LOG * 2 - 9);
 
// Takes 380 - 400 usec. Way too slow.
// With static MINPROJECTION: 220 usec.
16,22 → 16,25
uint16_t AC_getThrottle(uint16_t throttle) {
int32_t projection;
uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
int16_t deltaThrottle;
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
/*
int16_t deltaThrottle, y;
 
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
projection >>= 8;
 
if (projection < 0) {
// Case not yet considered!
y = 0;
} else {
if (projection < MINPROJECTION && projection >= 0) {
deltaThrottle = 0;
projection = MINPROJECTION;
} else if (projection > -MINPROJECTION && projection < 0) {
// projection = -MINPROJECITON;
deltaThrottle = 0;
projection = -MINPROJECTION;
} else {
*/
deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2));
// DebugOut.Analog[13] = deltaThrottle;
}
y = ((int32_t) throttle << (MATH_UNIT_FACTOR_LOG * 2 - 8)) / projection
- throttle;
}
deltaThrottle = ((int32_t)y * effect) >> 6;
DebugOut.Analog[8] = deltaThrottle;
return throttle + deltaThrottle;
}