Rev 2047 |
Blame |
Last modification |
View Log
| RSS feed
#include <inttypes.h>
#include "attitude.h"
#include "uart0.h"
#include "configuration.h"
#include "dongfangMath.h"
#include "controlMixer.h"
// For scope debugging only!
#include "output.h"
// = cos^2(45 degs).
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
const int32_t MINPROJECTION = 1L << (LOG_MATH_UNIT_FACTOR * 2 - 9);
// Takes 380 - 400 usec. Way too slow.
// With static MINPROJECTION: 220 usec.
void AC_getPRTY(int16_t* PRTY) {
int16_t throttle = PRTY[CONTROL_THROTTLE];
int32_t projection;
uint8_t effect = dynamicParams.attitudeControl; // Userparam 3
int16_t deltaThrottle, y;
int16_t rollAngleInDegrees = attitude[ROLL]/GYRO_DEG_FACTOR_PITCHROLL;
int16_t pitchAngleInDegrees = attitude[PITCH]/GYRO_DEG_FACTOR_PITCHROLL;
projection = (int32_t) cos_360(pitchAngleInDegrees) * (int32_t) cos_360(rollAngleInDegrees);
projection >>= 8;
if (projection < 0) {
// Case not yet considered!
y = 0;
} else {
if (projection < MINPROJECTION && projection >= 0) {
projection = MINPROJECTION;
} else if (projection > -MINPROJECTION && projection < 0) {
projection = -MINPROJECTION;
} else {
}
y = ((int32_t) throttle << (LOG_MATH_UNIT_FACTOR * 2 - 8)) / projection - throttle;
}
deltaThrottle = ((int32_t)y * effect) >> 6;
// debugOut.analog[8] = deltaThrottle;
PRTY[CONTROL_THROTTLE] = throttle;
}