Rev 1872 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
#include <inttypes.h>
#include "attitude.h"
#include "uart0.h"
#include "configuration.h"
#include "dongfangMath.h"
// For scope debugging only!
#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));
// Takes 380 - 400 usec. Way too slow.
// With static MINPROJECTION: 220 usec.
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]);
/*
if (projection < MINPROJECTION && projection >= 0) {
deltaThrottle = 0;
} else if (projection > -MINPROJECTION && projection < 0) {
// projection = -MINPROJECITON;
deltaThrottle = 0;
} else {
*/
deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2));
// DebugOut.Analog[13] = deltaThrottle;
return throttle + deltaThrottle;
}