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 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;
    uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
    int16_t deltaThrottle;

        // part1 start: 150 usec
        // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high.
        projection = 0;

        // (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
        // part1 end.
        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
 */