/branches/dongfang_FC_rewrite/attitudeControl.c |
---|
18,7 → 18,10 |
uint8_t effect = dynamicParams.attitudeControl; // Userparam 3 |
int16_t deltaThrottle, y; |
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
projection = (int32_t) cos_360(pitchAngleInDegrees) * (int32_t) cos_360(rollAngleInDegrees); |
projection >>= 8; |
if (projection < 0) { |