Subversion Repositories FlightCtrl

#include <inttypes.h>
#include "attitude.h"
#include "uart0.h"
#include "configuration.h"
#include "dongfangMath.h"

// For scope debugging only!
#include "rc.h"

// = cos^2(45 degs).
const int32_t MINPROJECTION =  (int32_t)MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2;

// Takes 380 - 400 usec. Way too slow.
// With static MINPROJECTION: 220 usec.
uint16_t AC_getThrottle(uint16_t throttle) {
  int32_t projection;

  // part1 start: 150 usec
  // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high.
  projection = (int32_t)int_cos(angle[PITCH]) * (int32_t)int_cos(angle[ROLL]);
  // part1 end.

  uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
  int16_t deltaThrottle;

  if (projection < MINPROJECTION && projection >= 0) {
    // projection = MINPROJECTION;
    deltaThrottle = 0;
  } else if (projection >- MINPROJECTION && projection<0) {
    // projection = -MINPROJECITON;
    deltaThrottle = 0;
  } else
    /*
     * We need delta throttle = constant/projection1
     * (constant * MATH_UNIT_FACTOR^2) / projection.
     */
    deltaThrottle = ((int32_t)effect * (int32_t)MATH_UNIT_FACTOR * (int32_t)MATH_UNIT_FACTOR) / (projection / 10)  - effect * 10;
  // DebugOut.Analog[13] = deltaThrottle;

  return throttle + deltaThrottle;
}
/*
har: R = e * k/p
vil  R = e * ( 1 - k/p )
       = e - ek/p
*/