Subversion Repositories FlightCtrl

#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;
}