Subversion Repositories FlightCtrl

Rev

Rev 2046 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2046 Rev 2047
Line 7... Line 7...
7 
// For scope debugging only!
 7 
// For scope debugging only!
8 
#include "output.h"
 8 
#include "output.h"
Line 9... Line 9...
9 
 
 9 
 
10 
// = cos^2(45 degs).
 10 
// = cos^2(45 degs).
11 
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
 11 
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
Line 12... Line 12...
12 
const int32_t MINPROJECTION = 1L << (MATH_UNIT_FACTOR_LOG * 2 - 9);
 12 
const int32_t MINPROJECTION = 1L << (LOG_MATH_UNIT_FACTOR * 2 - 9);
13 
 
 13 
 
14 
// Takes 380 - 400 usec. Way too slow.
 14 
// Takes 380 - 400 usec. Way too slow.
15 
// With static MINPROJECTION: 220 usec.
 15 
// With static MINPROJECTION: 220 usec.
Line 32... Line 32...
32 
      projection = MINPROJECTION;
 32 
      projection = MINPROJECTION;
33 
    } else if (projection > -MINPROJECTION && projection < 0) {
 33 
    } else if (projection > -MINPROJECTION && projection < 0) {
34 
      projection = -MINPROJECTION;
 34 
      projection = -MINPROJECTION;
35 
    } else {
 35 
    } else {
36 
    }
 36 
    }
37 
    y = ((int32_t) throttle << (MATH_UNIT_FACTOR_LOG * 2 - 8)) / projection - throttle;
 37 
    y = ((int32_t) throttle << (LOG_MATH_UNIT_FACTOR * 2 - 8)) / projection - throttle;
38 
  }
 38 
  }
39 
  deltaThrottle = ((int32_t)y * effect) >> 6;
 39 
  deltaThrottle = ((int32_t)y * effect) >> 6;
40 
  // debugOut.analog[8] = deltaThrottle;
 40 
  // debugOut.analog[8] = deltaThrottle;
41 
  return throttle + deltaThrottle;
 41 
  return throttle + deltaThrottle;
42 
}
 42 
}