Subversion Repositories FlightCtrl

#include <inttypes.h>
#include "attitude.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;
  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;
}