/branches/dongfang_FC_rewrite/analog.c |
---|
79,7 → 79,7 |
volatile int16_t rawGyroSum[3]; |
volatile int16_t acc[3]; |
volatile int16_t filteredAcc[2] = { 0,0 }; |
volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 }; |
// volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 }; |
/* |
* These 4 exported variables are zero-offset. The "PID" ones are used |
254,8 → 254,6 |
uint8_t i, axis; |
int16_t newrange; |
J5HIGH; |
// for various filters... |
int16_t tempOffsetGyro, tempGyro; |
273,8 → 271,10 |
else |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z]; |
/* |
stronglyFilteredAcc[Z] = |
(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
*/ |
break; |
295,10 → 295,11 |
filteredAcc[PITCH] = |
(filteredAcc[PITCH] * (ACC_FILTER - 1) + acc[PITCH]) / ACC_FILTER; |
/* |
stronglyFilteredAcc[PITCH] = |
(stronglyFilteredAcc[PITCH] * 99 + acc[PITCH] * 10) / 100; |
*/ |
measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1); |
break; |
310,8 → 311,10 |
filteredAcc[ROLL] = |
(filteredAcc[ROLL] * (ACC_FILTER - 1) + acc[ROLL]) / ACC_FILTER; |
/* |
stronglyFilteredAcc[ROLL] = |
(stronglyFilteredAcc[ROLL] * 99 + acc[ROLL] * 10) / 100; |
*/ |
measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1); |
break; |
482,10 → 485,6 |
// after full cycle stop further interrupts |
if (state) |
analog_start(); |
else |
J4LOW; |
J5LOW; |
} |
void analog_calibrate(void) { |
/branches/dongfang_FC_rewrite/analog.h |
---|
199,7 → 199,7 |
*/ |
extern volatile int16_t acc[3]; |
extern volatile int16_t filteredAcc[2]; |
extern volatile int32_t stronglyFilteredAcc[3]; |
// extern volatile int32_t stronglyFilteredAcc[3]; |
/* |
* Diagnostics: Gyro noise level because of motor vibrations. The variables |
/branches/dongfang_FC_rewrite/attitude.c |
---|
216,7 → 216,6 |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
analogDataReady = 0; |
J4HIGH; |
analog_start(); |
} |
239,6 → 238,8 |
angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
} |
// 480 usec with axis coupling - almost no time without. |
245,6 → 246,7 |
void integrate(void) { |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
trigAxisCoupling(); |
} else { |
357,19 → 359,26 |
void driftCorrection(void) { |
static int16_t timer = DRIFTCORRECTION_TIME; |
int16_t deltaCorrection; |
int16_t round; |
uint8_t axis; |
DebugOut.Analog[6] = (DRIFTCORRECTION_TIME + DRIFTCORRECTION_TIME/2) / DRIFTCORRECTION_TIME; |
DebugOut.Analog[7] = (-DRIFTCORRECTION_TIME + DRIFTCORRECTION_TIME/2) / DRIFTCORRECTION_TIME; |
if (!--timer) { |
timer = DRIFTCORRECTION_TIME; |
for (axis = PITCH; axis <= ROLL; axis++) { |
// Take the sum of corrections applied, add it to delta |
deltaCorrection = (correctionSum[axis] + DRIFTCORRECTION_TIME / 2) |
/ DRIFTCORRECTION_TIME; |
if (correctionSum[axis] >=0) |
round = DRIFTCORRECTION_TIME / 2; |
else |
round = -DRIFTCORRECTION_TIME / 2; |
deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
// Add the delta to the compensation. So positive delta means, gyro should have higher value. |
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
DebugOut.Analog[16 + axis] = correctionSum[axis]; |
DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
DebugOut.Analog[28 + axis] = driftComp[axis]; |
correctionSum[axis] = 0; |
381,18 → 390,9 |
* Main procedure. |
************************************************************************/ |
void calculateFlightAttitude(void) { |
// part1: 550 usec. |
// part1a: 550 usec. |
// part1b: 60 usec. |
getAnalogData(); |
// end part1b |
integrate(); |
// end part1a |
DebugOut.Analog[6] = stronglyFilteredAcc[PITCH]; |
DebugOut.Analog[7] = stronglyFilteredAcc[ROLL]; |
DebugOut.Analog[8] = stronglyFilteredAcc[Z]; |
DebugOut.Analog[3] = rate_PID[PITCH]; |
DebugOut.Analog[4] = rate_PID[ROLL]; |
DebugOut.Analog[5] = yawRate; |
401,7 → 401,6 |
correctIntegralsByAcc0thOrder(); |
driftCorrection(); |
#endif |
// end part1 |
} |
void updateCompass(void) { |
/branches/dongfang_FC_rewrite/attitude.h |
---|
67,8 → 67,8 |
* HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
* The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
*/ |
#define GYRO_DEG_FACTOR_PITCHROLL (int)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
#define GYRO_DEG_FACTOR_YAW (int)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
/* |
* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
107,7 → 107,7 |
/* |
* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
* to help cancelling out drift and vibration noise effects. The dynamic offsets themselves |
* to help canceling out drift and vibration noise effects. The dynamic offsets themselves |
* can be updated in flight by different ways, for example: |
* - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool |
* - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
/branches/dongfang_FC_rewrite/attitudeControl.c |
---|
5,7 → 5,7 |
#include "dongfangMath.h" |
// For scope debugging only! |
#include "rc.h" |
#include "output.h" |
// = cos^2(45 degs). |
const int32_t MINPROJECTION = (int32_t) MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2; |
14,15 → 14,15 |
// 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 = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
projection = 0; |
// (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; |
29,7 → 29,7 |
} else if (projection > -MINPROJECTION && projection < 0) { |
// projection = -MINPROJECITON; |
deltaThrottle = 0; |
} else |
} else { |
/* |
* We need delta throttle = constant/projection1 |
* (constant * MATH_UNIT_FACTOR^2) / projection. |
37,6 → 37,7 |
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; |
} |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
143,6 → 143,7 |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
// TODO: If no signal --> zero. |
uint8_t axis; |
int16_t tempThrottle; |
// takes almost no time... |
RC_update(); |
158,9 → 159,10 |
control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
// This can be a CPU time killer if the function implementations are inefficient. |
controlThrottle = AC_getThrottle(HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] |
+ EC_PRTY[CONTROL_THROTTLE])); |
tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
controlThrottle = AC_getThrottle(tempThrottle); |
controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
DebugOut.Analog[12] = control[PITCH]; |
218,6 → 220,7 |
lastCommand = COMMAND_NONE; |
} |
/* |
if (isCommandRepeated) |
DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED; |
else |
226,8 → 229,7 |
DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED; |
else |
DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED; |
// part1 end. |
*/ |
} |
// TODO: Integrate into command system. |
/branches/dongfang_FC_rewrite/dongfangMath.c |
---|
2,228 → 2,234 |
#include <inttypes.h> |
#include <avr/pgmspace.h> |
// For scope debugging only! |
#include "output.h" |
// Debug |
#include "uart0.h" |
const int16_t SIN_TABLE[] PROGMEM = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.05233595624294383 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.0697564737441253 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.08715574274765817 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.10452846326765346 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.12186934340514748 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.13917310096006544 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.15643446504023087 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.17364817766693033 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.1908089953765448 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.20791169081775931 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.224951054343865 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.24192189559966773 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.25881904510252074 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.27563735581699916 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.29237170472273677 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3090169943749474 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.32556815445715664 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3420201433256687 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.35836794954530027 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.374606593415912 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3907311284892737 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.40673664307580015 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.42261826174069944 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4383711467890774 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.45399049973954675 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4694715627858908 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.48480962024633706 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.49999999999999994 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5150380749100542 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5299192642332049 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5446390350150271 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5591929034707469 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.573576436351046 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5877852522924731 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6018150231520483 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6156614753256582 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6293203910498374 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6427876096865393 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6560590289905072 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6691306063588582 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6819983600624985 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6946583704589973 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7071067811865475 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7193398003386511 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7313537016191705 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7431448254773942 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.754709580222772 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.766044443118978 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7771459614569708 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.788010753606722 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7986355100472928 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8090169943749475 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8191520442889918 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8290375725550417 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8386705679454239 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8480480961564261 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8571673007021122 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8660254037844386 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8746197071393957 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8829475928589269 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8910065241883678 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.898794046299167 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9063077870366499 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9135454576426009 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9205048534524403 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9271838545667874 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9335804264972017 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9396926207859083 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9455185755993167 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9510565162951535 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9563047559630354 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9612616959383189 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9659258262890683 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9702957262759965 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9743700647852352 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9781476007338056 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.981627183447664 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.984807753012208 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9876883405951378 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9902680687415703 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.992546151641322 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), (int16_t) (1.0 |
* MATH_UNIT_FACTOR) }; |
(int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.05233595624294383 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.0697564737441253 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.08715574274765817 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.10452846326765346 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.12186934340514748 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.13917310096006544 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.15643446504023087 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.17364817766693033 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.1908089953765448 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.20791169081775931 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.224951054343865 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.24192189559966773 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.25881904510252074 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.27563735581699916 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.29237170472273677 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3090169943749474 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.32556815445715664 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3420201433256687 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.35836794954530027 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.374606593415912 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3907311284892737 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.40673664307580015 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.42261826174069944 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4383711467890774 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.45399049973954675 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4694715627858908 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.48480962024633706 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.49999999999999994 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5150380749100542 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5299192642332049 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5446390350150271 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5591929034707469 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.573576436351046 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5877852522924731 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6018150231520483 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6156614753256582 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6293203910498374 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6427876096865393 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6560590289905072 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6691306063588582 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6819983600624985 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6946583704589973 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7071067811865475 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7193398003386511 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7313537016191705 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7431448254773942 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.754709580222772 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.766044443118978 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7771459614569708 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.788010753606722 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7986355100472928 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8090169943749475 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8191520442889918 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8290375725550417 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8386705679454239 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8480480961564261 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8571673007021122 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8660254037844386 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8746197071393957 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8829475928589269 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8910065241883678 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.898794046299167 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9063077870366499 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9135454576426009 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9205048534524403 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9271838545667874 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9335804264972017 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9396926207859083 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9455185755993167 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9510565162951535 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9563047559630354 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9612616959383189 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9659258262890683 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9702957262759965 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9743700647852352 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9781476007338056 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.981627183447664 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.984807753012208 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9876883405951378 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9902680687415703 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.992546151641322 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), (int16_t) (1.0 |
* MATH_UNIT_FACTOR) }; |
const int16_t TAN_TABLE[] PROGMEM |
= { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03492076949174773 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.0524077792830412 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.06992681194351041 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.08748866352592401 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.10510423526567646 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.1227845609029046 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.14054083470239145 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.15838444032453627 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.17632698070846498 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.19438030913771848 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2125565616700221 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.23086819112556312 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.24932800284318068 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2679491924311227 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2867453857588079 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3057306814586604 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3249196962329063 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3443276132896652 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.36397023426620234 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3838640350354158 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4040262258351568 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4244748162096047 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4452286853085361 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4663076581549986 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.48773258856586144 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5095254494944288 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5317094316614788 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.554309051452769 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5773502691896257 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6008606190275604 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6248693519093275 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6494075931975106 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6745085168424267 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7002075382097097 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7265425280053608 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7535540501027942 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7812856265067173 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.809784033195007 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8390996311772799 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8692867378162265 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9004040442978399 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9325150861376615 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9656887748070739 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9999999999999999 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.0355303137905694 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.0723687100246826 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.1106125148291928 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.1503684072210094 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.19175359259421 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.234897156535051 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.2799416321930788 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.3270448216204098 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.3763819204711734 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.4281480067421144 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.4825609685127403 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.5398649638145825 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.6003345290410504 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.6642794823505174 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.7320508075688767 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.8040477552714236 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.8807264653463318 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.9626105055051504 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.050303841579296 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.1445069205095586 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.2460367739042164 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.355852365823752 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.4750868534162964 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.6050890646938005 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.7474774194546216 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.904210877675822 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.0776835371752527 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.2708526184841404 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.4874144438409087 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.7320508075688776 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.010780933535842 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.331475874284157 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.704630109478451 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (5.144554015970307 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (5.671281819617707 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (6.313751514675041 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (7.115369722384195 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (8.144346427974593 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (9.514364454222587 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (11.430052302761348 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (14.300666256711896 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (32767) }; |
= { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03492076949174773 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.0524077792830412 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.06992681194351041 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.08748866352592401 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.10510423526567646 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.1227845609029046 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.14054083470239145 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.15838444032453627 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.17632698070846498 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.19438030913771848 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2125565616700221 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.23086819112556312 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.24932800284318068 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2679491924311227 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2867453857588079 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3057306814586604 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3249196962329063 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3443276132896652 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.36397023426620234 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3838640350354158 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4040262258351568 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4244748162096047 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4452286853085361 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4663076581549986 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.48773258856586144 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5095254494944288 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5317094316614788 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.554309051452769 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5773502691896257 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6008606190275604 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6248693519093275 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6494075931975106 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6745085168424267 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7002075382097097 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7265425280053608 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7535540501027942 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7812856265067173 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.809784033195007 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8390996311772799 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8692867378162265 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9004040442978399 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9325150861376615 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9656887748070739 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9999999999999999 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.0355303137905694 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.0723687100246826 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.1106125148291928 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.1503684072210094 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.19175359259421 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.234897156535051 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.2799416321930788 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.3270448216204098 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.3763819204711734 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.4281480067421144 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.4825609685127403 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.5398649638145825 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.6003345290410504 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.6642794823505174 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.7320508075688767 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.8040477552714236 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.8807264653463318 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.9626105055051504 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.050303841579296 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.1445069205095586 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.2460367739042164 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.355852365823752 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.4750868534162964 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.6050890646938005 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.7474774194546216 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.904210877675822 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.0776835371752527 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.2708526184841404 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.4874144438409087 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.7320508075688776 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.010780933535842 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.331475874284157 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.704630109478451 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (5.144554015970307 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (5.671281819617707 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (6.313751514675041 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (7.115369722384195 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (8.144346427974593 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (9.514364454222587 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (11.430052302761348 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (14.300666256711896 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (32767) }; |
int16_t int_sin(int32_t arg) { |
int8_t sign; |
int16_t result; |
arg /= MATH_DRG_FACTOR; |
arg %= 360; |
if (arg < 0) { |
arg = -arg; |
sign = -1; |
} else { |
sign = 1; |
} |
if (arg >= 90) { |
arg = 180 - arg; |
} |
result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]); |
return (sign == 1) ? result : -result; |
int8_t sign; |
int16_t result; |
int16_t argp = arg / MATH_DRG_FACTOR; |
argp %= 360; |
if (argp < 0) { |
argp = -argp; |
sign = -1; |
} else { |
sign = 1; |
} |
if (argp >= 90) { |
argp = 180 - argp; |
} |
result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]); |
return (sign == 1) ? result : -result; |
} |
int16_t int_cos(int32_t arg) { |
if (arg > 90L * MATH_DRG_FACTOR) |
return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR); |
return int_sin(arg + 90L * MATH_DRG_FACTOR); |
if (arg > 90L * MATH_DRG_FACTOR) |
return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR); |
return int_sin(arg + 90L * MATH_DRG_FACTOR); |
} |
int16_t int_tan(int32_t arg) { |
int8_t sign = 1; |
int16_t result; |
arg /= MATH_DRG_FACTOR; |
if (arg >= 90) { |
arg = 180 - arg; |
sign = -1; |
} else if (arg < -90) { |
arg += 180; |
} else if (arg < 0) { |
arg = -arg; |
sign = -1; |
} |
result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]); |
return (sign == 1) ? result : -result; |
int8_t sign = 1; |
int16_t result; |
int16_t argp = arg / MATH_DRG_FACTOR; |
if (argp >= 90) { |
argp = 180 - argp; |
sign = -1; |
} else if (argp < -90) { |
argp += 180; |
} else if (argp < 0) { |
argp = -argp; |
sign = -1; |
} |
result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]); |
return (sign == 1) ? result : -result; |
} |
/branches/dongfang_FC_rewrite/externalControl.c |
---|
40,15 → 40,12 |
void EC_update() { |
if (externalControlActive) { |
externalControlActive--; |
EC_PRTY[CONTROL_PITCH] = (int16_t) externalControl.pitch |
* (int16_t) staticParams.StickP; |
EC_PRTY[CONTROL_ROLL] = externalControl.roll |
* (int16_t) staticParams.StickP; |
EC_PRTY[CONTROL_PITCH] = externalControl.pitch * (int16_t) staticParams.StickP; |
EC_PRTY[CONTROL_ROLL] = externalControl.roll * (int16_t) staticParams.StickP; |
EC_PRTY[CONTROL_THROTTLE] = externalControl.throttle; |
EC_PRTY[CONTROL_YAW] = externalControl.yaw; // No stickP or similar?????? |
} else { |
EC_PRTY[CONTROL_PITCH] = EC_PRTY[CONTROL_ROLL] = EC_PRTY[CONTROL_THROTTLE] |
= EC_PRTY[CONTROL_YAW] = 0; |
EC_PRTY[CONTROL_PITCH] = EC_PRTY[CONTROL_ROLL] = EC_PRTY[CONTROL_THROTTLE] = EC_PRTY[CONTROL_YAW] = 0; |
} |
} |
/branches/dongfang_FC_rewrite/flight.c |
---|
81,7 → 81,7 |
*/ |
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
// Some integral weight constant... |
92,22 → 92,23 |
/* Filter for motor value smoothing (necessary???) */ |
/************************************************************************/ |
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
switch(dynamicParams.UserParams[5]) { |
switch (dynamicParams.UserParams[5]) { |
case 0: |
return newvalue; |
case 1: |
return (oldvalue + newvalue) / 2; |
return (oldvalue + newvalue) / 2; |
case 2: |
if(newvalue > oldvalue) |
return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
else |
if (newvalue > oldvalue) |
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
else |
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
case 3: |
if(newvalue < oldvalue) |
return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
else |
if (newvalue < oldvalue) |
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
else |
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
default: return newvalue; |
default: |
return newvalue; |
} |
} |
123,7 → 124,8 |
controlMixer_initVariables(); |
} |
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
Ki = 10300 / _Ki; |
gyroPFactor = _gyroPFactor; |
gyroIFactor = _gyroIFactor; |
132,12 → 134,9 |
} |
void setNormalFlightParameters(void) { |
setFlightParameters(dynamicParams.IFactor + 1, |
dynamicParams.GyroP + 10, |
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
dynamicParams.GyroP + 10, |
dynamicParams.UserParams[6] |
); |
setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10, |
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
dynamicParams.GyroP + 10, dynamicParams.UserParams[6]); |
} |
void setStableFlightParameters(void) { |
144,18 → 143,17 |
setFlightParameters(33, 90, 120, 90, 120); |
} |
/************************************************************************/ |
/* Main Flight Control */ |
/************************************************************************/ |
void flight_control(void) { |
int16_t tmp_int; |
// Mixer Fractions that are combined for Motor Control |
// Mixer Fractions that are combined for Motor Control |
int16_t yawTerm, throttleTerm, term[2]; |
// PID controller variables |
int16_t PDPart[2], PDPartYaw, PPart[2]; |
static int32_t IPart[2] = {0,0}; |
static int32_t IPart[2] = { 0, 0 }; |
// static int32_t yawControlRate = 0; |
// Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
173,13 → 171,14 |
// Fire the main flight attitude calculation, including integration of angles. |
// We want that to kick as early as possible, not to delay new AD sampling further. |
calculateFlightAttitude(); |
controlMixer_update(); |
throttleTerm = controlThrottle; |
throttleTerm = controlThrottle; |
// This check removed. Is done on a per-motor basis, after output matrix multiplication. |
if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
if (throttleTerm < staticParams.MinThrottle + 10) |
throttleTerm = staticParams.MinThrottle + 10; |
else if (throttleTerm > staticParams.MaxThrottle - 20) |
throttleTerm = (staticParams.MaxThrottle - 20); |
/************************************************************************/ |
/* RC-signal is bad */ |
187,61 → 186,62 |
/* to the control mixer: An emergency autopilot control. */ |
/************************************************************************/ |
if(controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
RED_ON; |
beepRCAlarm(); |
if(emergencyFlightTime) { |
if (emergencyFlightTime) { |
// continue emergency flight |
emergencyFlightTime--; |
if(isFlying > 256) { |
// We're probably still flying. Descend slowly. |
throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
setStableFlightParameters(); |
emergencyFlightTime--; |
if (isFlying > 256) { |
// We're probably still flying. Descend slowly. |
throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
setStableFlightParameters(); |
} else { |
MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
} |
} else { |
// end emergency flight (just cut the motors???) |
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
} |
} else { |
} else { |
// signal is acceptable |
if(controlMixer_getSignalQuality() > SIGNAL_BAD) { |
if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
// Reset emergency landing control variables. |
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
// The time is in whole seconds. |
emergencyFlightTime = (uint16_t)staticParams.EmergencyGasDuration * 488; |
emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488; |
} |
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
// increment flight-time counter until overflow. |
if(isFlying != 0xFFFF) isFlying++; |
} else |
/* |
* When standing on the ground, do not apply I controls and zero the yaw stick. |
* Probably to avoid integration effects that will cause the copter to spin |
* or flip when taking off. |
*/ |
if(isFlying < 256) { |
IPart[PITCH] = IPart[ROLL] = 0; |
// TODO: Don't stomp on other modules' variables!!! |
// controlYaw = 0; |
PDPartYaw = 0; // instead. |
if(isFlying == 250) { |
// HC_setGround(); |
updateCompassCourse = 1; |
yawAngleDiff = 0; |
} |
} else { |
// Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
// Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
MKFlags |= (MKFLAG_FLY); |
if (isFlying != 0xFFFF) |
isFlying++; |
} else |
/* |
* When standing on the ground, do not apply I controls and zero the yaw stick. |
* Probably to avoid integration effects that will cause the copter to spin |
* or flip when taking off. |
*/ |
if (isFlying < 256) { |
IPart[PITCH] = IPart[ROLL] = 0; |
// TODO: Don't stomp on other modules' variables!!! |
// controlYaw = 0; |
PDPartYaw = 0; // instead. |
if (isFlying == 250) { |
// HC_setGround(); |
updateCompassCourse = 1; |
yawAngleDiff = 0; |
} |
} else { |
// Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
// Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
MKFlags |= (MKFLAG_FLY); |
} |
commands_handleCommands(); |
commands_handleCommands(); |
// if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
setNormalFlightParameters(); |
253,20 → 253,21 |
* by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
* This is the throttle part. |
*/ |
if(looping) { |
if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
if (looping) { |
if (throttleTerm > staticParams.LoopGasLimit) |
throttleTerm = staticParams.LoopGasLimit; |
} |
/************************************************************************/ |
/* Yawing */ |
/************************************************************************/ |
if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
ignoreCompassTimer = 1000; |
if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
updateCompassCourse = 1; |
} |
} |
// yawControlRate = controlYaw; |
// Trim drift of yawAngleDiff with controlYaw. |
273,17 → 274,17 |
// TODO: We want NO feedback of control related stuff to the attitude related stuff. |
// This seems to be used as: Difference desired <--> real heading. |
yawAngleDiff -= controlYaw; |
// limit the effect |
CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
/************************************************************************/ |
/* Compass is currently not supported. */ |
/************************************************************************/ |
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
updateCompass(); |
} |
#if defined (USE_NAVICTRL) |
/************************************************************************/ |
/* GPS is currently not supported. */ |
297,15 → 298,15 |
// end part 1: 750-800 usec. |
// start part 3: 350 - 400 usec. |
#define SENSOR_LIMIT (4096 * 4) |
/************************************************************************/ |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
for (axis=PITCH; axis<=ROLL; axis++) { |
if(looping & ((1<<4)<<axis)) { |
for (axis = PITCH; axis <= ROLL; axis++) { |
if (looping & ((1 << 4) << axis)) { |
PPart[axis] = 0; |
} else { // TODO: Where do the 44000 come from??? |
PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
316,26 → 317,27 |
* Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
* where pfactor is in [0..1]. |
*/ |
PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) |
+ (differential[axis] * (int16_t)dynamicParams.GyroD) / 16; |
PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] |
* gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
* (int16_t) dynamicParams.GyroD) / 16; |
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
} |
PDPartYaw = |
(int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
+ (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L |
/ CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 |
/ CONTROL_SCALING)); |
// limit control feedback |
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
/* |
* Compose throttle term. |
* If a Bl-Ctrl is missing, prevent takeoff. |
*/ |
if(missingMotor) { |
if (missingMotor) { |
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
if(isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
if (isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
isFlying = 1; // keep within lift off condition |
throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
} |
354,22 → 356,22 |
yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
// Limit yawTerm |
DebugOut.Digital[0] &= ~DEBUG_CLIP; |
if(throttleTerm > MIN_YAWGAS) { |
if (yawTerm < -throttleTerm/2) { |
if (throttleTerm > MIN_YAWGAS) { |
if (yawTerm < -throttleTerm / 2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
yawTerm = -throttleTerm/2; |
} else if (yawTerm > throttleTerm/2) { |
yawTerm = -throttleTerm / 2; |
} else if (yawTerm > throttleTerm / 2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
yawTerm = throttleTerm/2; |
yawTerm = throttleTerm / 2; |
} |
//CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
} else { |
if (yawTerm < -MIN_YAWGAS/2) { |
if (yawTerm < -MIN_YAWGAS / 2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
yawTerm = -MIN_YAWGAS/2; |
} else if (yawTerm > MIN_YAWGAS/2) { |
yawTerm = -MIN_YAWGAS / 2; |
} else if (yawTerm > MIN_YAWGAS / 2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
yawTerm = MIN_YAWGAS/2; |
yawTerm = MIN_YAWGAS / 2; |
} |
//CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
386,11 → 388,11 |
// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
DebugOut.Digital[1] &= ~DEBUG_CLIP; |
for (axis=PITCH; axis<=ROLL; axis++) { |
for (axis = PITCH; axis <= ROLL; axis++) { |
/* |
* Compose pitch and roll terms. This is finally where the sticks come into play. |
*/ |
if(gyroIFactor) { |
if (gyroIFactor) { |
// Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
// That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
// TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
401,12 → 403,13 |
IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
} |
tmp_int = (int32_t)((int32_t) dynamicParams.DynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
// TODO: From which planet comes the 16000? |
CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
// Add (P, D) parts minus stick pos. to the scaled-down I part. |
term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
/* |
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
421,7 → 424,6 |
} |
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
} |
// end part 3: 350 - 400 usec. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
433,13 → 435,14 |
DebugOut.Analog[14] = yawTerm; |
DebugOut.Analog[15] = throttleTerm; |
for(i = 0; i < MAX_MOTORS; i++) { |
for (i = 0; i < MAX_MOTORS; i++) { |
int16_t tmp; |
if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
tmp = ((int32_t) throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
tmp += ((int32_t) term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
tmp += ((int32_t) term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
tmp += ((int32_t) yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
tmp = throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
tmp += term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
tmp += term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
tmp += yawTerm * Mixer.Motor[i][MIX_YAW]; |
tmp = tmp >> 6; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
// Now we scale back down to a 0..255 range. |
tmp = motorFilters[i] / MOTOR_SCALING; |
451,36 → 454,36 |
// throttle should be limited. |
// --> WRONG. This caused motors to stall completely in tight maneuvers. |
// Apply to individual signals instead. |
CHECK_MIN_MAX(tmp, 8, 255); |
CHECK_MIN_MAX(tmp, 1, 255); |
motor[i].SetPoint = tmp; |
} |
else if (motorTestActive) { |
} else if (motorTestActive) { |
motor[i].SetPoint = motorTest[i]; |
} else { |
motor[i].SetPoint = 0; |
} |
if (i < 4) |
DebugOut.Analog[22+i] = motor[i].SetPoint; |
DebugOut.Analog[22 + i] = motor[i].SetPoint; |
} |
I2C_Start(TWI_STATE_MOTOR_TX); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugging |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!(--debugDataTimer)) { |
if (!(--debugDataTimer)) { |
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
/* |
DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
DebugOut.Analog[24] = controlYaw; |
DebugOut.Analog[25] = yawAngleDiff / 100L; |
DebugOut.Analog[26] = accNoisePeak[PITCH]; |
DebugOut.Analog[27] = accNoisePeak[ROLL]; |
DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
*/ |
DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
DebugOut.Analog[24] = controlYaw; |
DebugOut.Analog[25] = yawAngleDiff / 100L; |
DebugOut.Analog[26] = accNoisePeak[PITCH]; |
DebugOut.Analog[27] = accNoisePeak[ROLL]; |
DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
*/ |
} |
} |
/branches/dongfang_FC_rewrite/main.c |
---|
218,55 → 218,16 |
while (1) { |
if (runFlightControl) { // control interval |
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
if (!analogDataReady) { |
DebugOut.Digital[0] |= DEBUG_MAINLOOP_TIMER; |
} else { |
DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
if (analogDataReady) { |
// J4HIGH; |
J4HIGH; |
flight_control(); |
// J4LOW; |
J4LOW; |
/* |
* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
* the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
* the test throttle vector. If no testing, stop all motors. |
*/ |
// Obsoleted. |
// transmitMotorThrottleData(); |
RED_OFF; |
/* |
Does not belong here. Instead, external control should be ignored in |
controlMixer if there was no new data from there for some time. |
if(externalControlActive) externalControlActive--; |
else { |
externalControl.config = 0; |
externalStickPitch = 0; |
externalStickRoll = 0; |
externalStickYaw = 0; |
} |
*/ |
/* |
Does not belong here. |
if(RC_Quality) RC_Quality--; |
*/ |
/* Does not belong here. Well since we are not supporting navi right now anyway, leave out. |
#ifdef USE_NAVICTRL |
if(NCDataOkay) { |
if(--NCDataOkay == 0) // no data from NC |
{ // set gps control sticks neutral |
GPSStickPitch = 0; |
GPSStickRoll = 0; |
NCSerialDataOkay = 0; |
} |
} |
#endif |
*/ |
if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout |
if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout |
RED_ON; |
if (!I2CTimeout) { |
I2C_Reset(); |
279,8 → 240,6 |
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
usart0_TransmitTxData(); |
} else { |
DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER; |
} |
usart0_ProcessRxData(); |
/branches/dongfang_FC_rewrite/rc.c |
---|
74,54 → 74,54 |
* 16bit timer 1 is used to decode the PPM-Signal |
***************************************************************/ |
void RC_Init(void) { |
uint8_t sreg = SREG; |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// disable all interrupts before reconfiguration |
cli(); |
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
DDRD &= ~(1 << DDD6); |
PORTD |= (1 << PORTD6); |
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
DDRD &= ~(1 << DDD6); |
PORTD |= (1 << PORTD6); |
// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
// set as output |
DDRD |= (1 << DDD5) | (1 << DDD4) | (1 << DDD3); |
// low level |
PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3)); |
// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
// set as output |
DDRD |= (1 << DDD5) | (1 << DDD4) | (1 << DDD3); |
// low level |
PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3)); |
// PD3 can't be used if 2nd UART is activated |
// because TXD1 is at that port |
if (CPUType != ATMEGA644P) { |
DDRD |= (1 << PORTD3); |
PORTD &= ~(1 << PORTD3); |
} |
// PD3 can't be used if 2nd UART is activated |
// because TXD1 is at that port |
if (CPUType != ATMEGA644P) { |
DDRD |= (1 << PORTD3); |
PORTD &= ~(1 << PORTD3); |
} |
// Timer/Counter1 Control Register A, B, C |
// Timer/Counter1 Control Register A, B, C |
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0) |
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0) |
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
// Enable input capture noise cancler (bit: ICNC1=1) |
// Trigger on positive edge of the input capture pin (bit: ICES1=1), |
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs |
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) |
| (1 << WGM11) | (1 << WGM10)); |
TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0) |
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0) |
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
// Enable input capture noise cancler (bit: ICNC1=1) |
// Trigger on positive edge of the input capture pin (bit: ICES1=1), |
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs |
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) |
| (1 << WGM11) | (1 << WGM10)); |
TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
// Timer/Counter1 Interrupt Mask Register |
// Timer/Counter1 Interrupt Mask Register |
// Enable Input Capture Interrupt (bit: ICIE1=1) |
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
// Enable Overflow Interrupt (bit: TOIE1=0) |
TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1)); |
TIMSK1 |= (1 << ICIE1); |
// Enable Input Capture Interrupt (bit: ICIE1=1) |
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
// Enable Overflow Interrupt (bit: TOIE1=0) |
TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1)); |
TIMSK1 |= (1 << ICIE1); |
RC_Quality = 0; |
RC_Quality = 0; |
SREG = sreg; |
SREG = sreg; |
} |
/********************************************************************/ |
129,11 → 129,11 |
/********************************************************************/ |
/* t-Frame |
<-----------------------------------------------------------------------> |
____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | |
| | | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________| |
<-----><-------><------><--------> <------> <--- |
____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | |
| | | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________| |
<-----><-------><------><--------> <------> <--- |
t0 t1 t2 t4 tn t0 |
The PPM-Frame length is 22.5 ms. |
147,71 → 147,71 |
*/ |
ISR(TIMER1_CAPT_vect) |
{ // typical rate of 1 ms to 2 ms |
int16_t signal = 0, tmp; |
static int16_t index; |
static uint16_t oldICR1 = 0; |
int16_t signal = 0, tmp; |
static int16_t index; |
static uint16_t oldICR1 = 0; |
// 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
// at the time the edge was detected |
// 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
// at the time the edge was detected |
// calculate the time delay to the previous event time which is stored in oldICR1 |
// calculatiing the difference of the two uint16_t and converting the result to an int16_t |
// implicit handles a timer overflow 65535 -> 0 the right way. |
signal = (uint16_t) ICR1 - oldICR1; |
oldICR1 = ICR1; |
// calculate the time delay to the previous event time which is stored in oldICR1 |
// calculatiing the difference of the two uint16_t and converting the result to an int16_t |
// implicit handles a timer overflow 65535 -> 0 the right way. |
signal = (uint16_t) ICR1 - oldICR1; |
oldICR1 = ICR1; |
//sync gap? (3.52 ms < signal < 25.6 ms) |
if ((signal > 1100) && (signal < 8000)) { |
// if a sync gap happens and there where at least 4 channels decoded before |
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
if (index >= 4) { |
NewPpmData = 0; // Null means NewData for the first 4 channels |
} |
// synchronize channel index |
index = 1; |
} else { // within the PPM frame |
if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels |
// check for valid signal length (0.8 ms < signal < 2.1984 ms) |
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
if ((signal > 250) && (signal < 687)) { |
// shift signal to zero symmetric range -154 to 159 |
signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
// check for stable signal |
if (abs(signal - PPM_in[index]) < 6) { |
if (RC_Quality < 200) |
RC_Quality += 10; |
else |
RC_Quality = 200; |
} |
// If signal is the same as before +/- 1, just keep it there. |
if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
// In addition, if the signal is very close to 0, just set it to 0. |
if (signal >= -1 && signal <= 1) { |
tmp = 0; |
} else { |
tmp = PPM_in[index]; |
} |
} else |
tmp = signal; |
// calculate signal difference on good signal level |
if (RC_Quality >= 195) |
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
else |
PPM_diff[index] = 0; |
PPM_in[index] = tmp; // update channel value |
} |
index++; // next channel |
// demux sum signal for channels 5 to 7 to J3, J4, J5 |
// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
// channels are usually available at the receiver anyway. |
// if(index == 5) J3HIGH; else J3LOW; |
// if(index == 6) J4HIGH; else J4LOW; |
// if(CPUType != ATMEGA644P) // not used as TXD1 |
// { |
// if(index == 7) J5HIGH; else J5LOW; |
// } |
} |
} |
//sync gap? (3.52 ms < signal < 25.6 ms) |
if ((signal > 1100) && (signal < 8000)) { |
// if a sync gap happens and there where at least 4 channels decoded before |
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
if (index >= 4) { |
NewPpmData = 0; // Null means NewData for the first 4 channels |
} |
// synchronize channel index |
index = 1; |
} else { // within the PPM frame |
if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels |
// check for valid signal length (0.8 ms < signal < 2.1984 ms) |
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
if ((signal > 250) && (signal < 687)) { |
// shift signal to zero symmetric range -154 to 159 |
signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
// check for stable signal |
if (abs(signal - PPM_in[index]) < 6) { |
if (RC_Quality < 200) |
RC_Quality += 10; |
else |
RC_Quality = 200; |
} |
// If signal is the same as before +/- 1, just keep it there. |
if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
// In addition, if the signal is very close to 0, just set it to 0. |
if (signal >= -1 && signal <= 1) { |
tmp = 0; |
} else { |
tmp = PPM_in[index]; |
} |
} else |
tmp = signal; |
// calculate signal difference on good signal level |
if (RC_Quality >= 195) |
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
else |
PPM_diff[index] = 0; |
PPM_in[index] = tmp; // update channel value |
} |
index++; // next channel |
// demux sum signal for channels 5 to 7 to J3, J4, J5 |
// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
// channels are usually available at the receiver anyway. |
// if(index == 5) J3HIGH; else J3LOW; |
// if(index == 6) J4HIGH; else J4LOW; |
// if(CPUType != ATMEGA644P) // not used as TXD1 |
// { |
// if(index == 7) J5HIGH; else J5LOW; |
// } |
} |
} |
} |
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
222,23 → 222,23 |
// Internal. |
uint8_t RC_getStickCommand(void) { |
if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
// vertical is up |
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
return COMMAND_GYROCAL; |
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
return COMMAND_ACCCAL; |
return COMMAND_NONE; |
} else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
// vertical is down |
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
return COMMAND_STOP; |
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
return COMMAND_START; |
return COMMAND_NONE; |
} |
// vertical is around center |
return COMMAND_NONE; |
if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
// vertical is up |
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
return COMMAND_GYROCAL; |
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
return COMMAND_ACCCAL; |
return COMMAND_NONE; |
} else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
// vertical is down |
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
return COMMAND_STOP; |
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
return COMMAND_START; |
return COMMAND_NONE; |
} |
// vertical is around center |
return COMMAND_NONE; |
} |
/* |
245,39 → 245,40 |
* This must be called (as the only thing) for each control loop cycle (488 Hz). |
*/ |
void RC_update() { |
int16_t tmp1, tmp2; |
if (RC_Quality) { |
RC_Quality--; |
if (NewPpmData-- == 0) { |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP |
+ RCDiff(CH_PITCH) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP |
+ RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) |
* dynamicParams.UserParams[3] + 120; |
if (RC_PRTY[CONTROL_THROTTLE] < 0) |
RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
// exponential stick sensitivity in yawing rate |
tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1)) |
/ 512L; // expo y = ax + bx^2 |
tmp2 += (staticParams.StickYawP * tmp1) / 4; |
RC_PRTY[CONTROL_YAW] = tmp2; |
} |
uint8_t command = RC_getStickCommand(); |
if (lastRCCommand == command) { |
// Keep timer from overrunning. |
if (commandTimer < COMMAND_TIMER) |
commandTimer++; |
} else { |
// There was a change. |
lastRCCommand = command; |
commandTimer = 0; |
} |
} else { // Bad signal |
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
= RC_PRTY[CONTROL_YAW] = 0; |
} |
int16_t tmp1, tmp2; |
if (RC_Quality) { |
RC_Quality--; |
if (NewPpmData-- == 0) { |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP |
+ RCDiff(CH_PITCH) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP |
+ RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) |
* dynamicParams.UserParams[3] + 120; |
if (RC_PRTY[CONTROL_THROTTLE] < 0) |
RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
// exponential stick sensitivity in yawing rate |
tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1)) |
/ 512L; // expo y = ax + bx^2 |
tmp2 += (staticParams.StickYawP * tmp1) >> 2; |
RC_PRTY[CONTROL_YAW] = tmp2; |
} |
uint8_t command = RC_getStickCommand(); |
if (lastRCCommand == command) { |
// Keep timer from overrunning. |
if (commandTimer < COMMAND_TIMER) |
commandTimer++; |
} else { |
// There was a change. |
lastRCCommand = command; |
commandTimer = 0; |
} |
} else { // Bad signal |
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
= RC_PRTY[CONTROL_YAW] = 0; |
} |
} |
/* |
284,7 → 285,7 |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
int16_t* RC_getPRTY(void) { |
return RC_PRTY; |
return RC_PRTY; |
} |
/* |
291,28 → 292,28 |
* Get other channel value |
*/ |
int16_t RC_getVariable(uint8_t varNum) { |
if (varNum < 4) |
// 0th variable is 5th channel (1-based) etc. |
return RCChannel(varNum + 4) + POT_OFFSET; |
/* |
* Let's just say: |
* The RC variable 4 is hardwired to channel 5 |
* The RC variable 5 is hardwired to channel 6 |
* The RC variable 6 is hardwired to channel 7 |
* The RC variable 7 is hardwired to channel 8 |
* Alternatively, one could bind them to channel (4 + varNum) - or whatever... |
*/ |
return PPM_in[varNum + 1] + POT_OFFSET; |
if (varNum < 4) |
// 0th variable is 5th channel (1-based) etc. |
return RCChannel(varNum + 4) + POT_OFFSET; |
/* |
* Let's just say: |
* The RC variable 4 is hardwired to channel 5 |
* The RC variable 5 is hardwired to channel 6 |
* The RC variable 6 is hardwired to channel 7 |
* The RC variable 7 is hardwired to channel 8 |
* Alternatively, one could bind them to channel (4 + varNum) - or whatever... |
*/ |
return PPM_in[varNum + 1] + POT_OFFSET; |
} |
uint8_t RC_getSignalQuality(void) { |
if (RC_Quality >= 160) |
return SIGNAL_GOOD; |
if (RC_Quality >= 140) |
return SIGNAL_OK; |
if (RC_Quality >= 120) |
return SIGNAL_BAD; |
return SIGNAL_LOST; |
if (RC_Quality >= 160) |
return SIGNAL_GOOD; |
if (RC_Quality >= 140) |
return SIGNAL_OK; |
if (RC_Quality >= 120) |
return SIGNAL_BAD; |
return SIGNAL_LOST; |
} |
/* |
325,7 → 326,7 |
* of a stick, it may be useful. |
*/ |
void RC_calibrate(void) { |
// Do nothing. |
// Do nothing. |
} |
/* |
340,12 → 341,12 |
*/ |
uint8_t RC_getCommand(void) { |
if (commandTimer == COMMAND_TIMER) { |
// Stick has been held long enough; command committed. |
return lastRCCommand; |
} |
// Not yet sure what the command is. |
return COMMAND_NONE; |
if (commandTimer == COMMAND_TIMER) { |
// Stick has been held long enough; command committed. |
return lastRCCommand; |
} |
// Not yet sure what the command is. |
return COMMAND_NONE; |
} |
/* |
367,79 → 368,79 |
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL |
uint8_t RC_getArgument(void) { |
if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
// vertical is up |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 2; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 4; |
return 3; |
} else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
// vertical is down |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 8; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 6; |
return 7; |
} else { |
// vertical is around center |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 1; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 5; |
return 0; |
} |
if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
// vertical is up |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 2; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 4; |
return 3; |
} else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
// vertical is down |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 8; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 6; |
return 7; |
} else { |
// vertical is around center |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 1; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 5; |
return 0; |
} |
} |
uint8_t RC_getLooping(uint8_t looping) { |
// static uint8_t looping = 0; |
// static uint8_t looping = 0; |
if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig |
& CFG_LOOP_LEFT) { |
looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT); |
} else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) |
< staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT)); |
} |
if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig |
& CFG_LOOP_LEFT) { |
looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT); |
} else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) |
< staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT)); |
} |
if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_RIGHT) { |
looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT); |
} else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) |
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT)); |
} |
if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_RIGHT) { |
looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT); |
} else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) |
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT)); |
} |
if (RCChannel(CH_PITCH) > staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_UP) { |
looping |= (LOOPING_PITCH_AXIS | LOOPING_UP); |
} else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH) |
< staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP)); |
} |
if (RCChannel(CH_PITCH) > staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_UP) { |
looping |= (LOOPING_PITCH_AXIS | LOOPING_UP); |
} else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH) |
< staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP)); |
} |
if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_DOWN) { |
looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN); |
} else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) |
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN)); |
} |
if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_DOWN) { |
looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN); |
} else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) |
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN)); |
} |
return looping; |
return looping; |
} |
uint8_t RC_testCompassCalState(void) { |
static uint8_t stick = 1; |
// if pitch is centered or top set stick to zero |
if (RCChannel(CH_PITCH) > -20) |
stick = 0; |
// if pitch is down trigger to next cal state |
if ((RCChannel(CH_PITCH) < -70) && !stick) { |
stick = 1; |
return 1; |
} |
return 0; |
static uint8_t stick = 1; |
// if pitch is centered or top set stick to zero |
if (RCChannel(CH_PITCH) > -20) |
stick = 0; |
// if pitch is down trigger to next cal state |
if ((RCChannel(CH_PITCH) < -70) && !stick) { |
stick = 1; |
return 1; |
} |
return 0; |
} |
/* |
* Abstract controls are not used at the moment. |
/branches/dongfang_FC_rewrite/timer0.c |
---|
78,56 → 78,56 |
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor |
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate |
void timer0_init(void) { |
uint8_t sreg = SREG; |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// disable all interrupts before reconfiguration |
cli(); |
// Configure speaker port as output. |
// Configure speaker port as output. |
if (BoardRelease == 10) { // Speaker at PD2 |
DDRD |= (1 << DDD2); |
PORTD &= ~(1 << PORTD2); |
} else { // Speaker at PC7 |
DDRC |= (1 << DDC7); |
PORTC &= ~(1 << PORTC7); |
} |
if (BoardRelease == 10) { // Speaker at PD2 |
DDRD |= (1 << DDD2); |
PORTD &= ~(1 << PORTD2); |
} else { // Speaker at PC7 |
DDRC |= (1 << DDC7); |
PORTC &= ~(1 << PORTC7); |
} |
// set PB3 and PB4 as output for the PWM used as offset for the pressure sensor |
DDRB |= (1 << DDB4) | (1 << DDB3); |
PORTB &= ~((1 << PORTB4) | (1 << PORTB3)); |
// set PB3 and PB4 as output for the PWM used as offset for the pressure sensor |
DDRB |= (1 << DDB4) | (1 << DDB3); |
PORTB &= ~((1 << PORTB4) | (1 << PORTB3)); |
// Timer/Counter 0 Control Register A |
// Timer/Counter 0 Control Register A |
// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1) |
// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0) |
// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0) |
TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0)); |
TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00); |
// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1) |
// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0) |
// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0) |
TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0)); |
TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00); |
// Timer/Counter 0 Control Register B |
// Timer/Counter 0 Control Register B |
// set clock divider for timer 0 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz |
// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz |
// hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz |
// set clock divider for timer 0 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz |
// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz |
// hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz |
// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0) |
TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02)); |
TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00); |
// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0) |
TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02)); |
TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00); |
// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4 |
OCR0A = 0; // for PB3 |
OCR0B = 120; // for PB4 |
// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4 |
OCR0A = 0; // for PB3 |
OCR0B = 120; // for PB4 |
// init Timer/Counter 0 Register |
TCNT0 = 0; |
// init Timer/Counter 0 Register |
TCNT0 = 0; |
// Timer/Counter 0 Interrupt Mask Register |
// enable timer overflow interrupt only |
TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A)); |
TIMSK0 |= (1 << TOIE0); |
// Timer/Counter 0 Interrupt Mask Register |
// enable timer overflow interrupt only |
TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A)); |
TIMSK0 |= (1 << TOIE0); |
SREG = sreg; |
SREG = sreg; |
} |
/*****************************************************/ |
135,85 → 135,88 |
/*****************************************************/ |
ISR(TIMER0_OVF_vect) |
{ // 9765.625 Hz |
static uint8_t cnt_1ms = 1, cnt = 0; |
uint8_t Beeper_On = 0; |
static uint8_t cnt_1ms = 1, cnt = 0; |
uint8_t Beeper_On = 0; |
#ifdef USE_NAVICTRL |
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done |
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done |
#endif |
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz) |
cnt = 9; |
cnt_1ms ^= 1; |
if (!cnt_1ms) { |
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
DebugOut.Digital[0] |= DEBUG_MAINLOOP_TIMER; |
} |
CountMilliseconds++; // increment millisecond counter |
} |
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz) |
cnt = 9; |
cnt_1ms ^= 1; |
if (!cnt_1ms) { |
if (runFlightControl == 1) |
DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER; |
else |
DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
} |
CountMilliseconds++; // increment millisecond counter |
} |
// beeper on if duration is not over |
if (BeepTime) { |
BeepTime--; // decrement BeepTime |
if (BeepTime & BeepModulation) |
Beeper_On = 1; |
else |
Beeper_On = 0; |
} else { // beeper off if duration is over |
Beeper_On = 0; |
BeepModulation = 0xFFFF; |
} |
// beeper on if duration is not over |
if (BeepTime) { |
BeepTime--; // decrement BeepTime |
if (BeepTime & BeepModulation) |
Beeper_On = 1; |
else |
Beeper_On = 0; |
} else { // beeper off if duration is over |
Beeper_On = 0; |
BeepModulation = 0xFFFF; |
} |
// if beeper is on |
if (Beeper_On) { |
// set speaker port to high. |
if (BoardRelease == 10) |
PORTD |= (1 << PORTD2); // Speaker at PD2 |
else |
PORTC |= (1 << PORTC7); // Speaker at PC7 |
} else { // beeper is off |
// set speaker port to low |
if (BoardRelease == 10) |
PORTD &= ~(1 << PORTD2);// Speaker at PD2 |
else |
PORTC &= ~(1 << PORTC7);// Speaker at PC7 |
} |
// if beeper is on |
if (Beeper_On) { |
// set speaker port to high. |
if (BoardRelease == 10) |
PORTD |= (1 << PORTD2); // Speaker at PD2 |
else |
PORTC |= (1 << PORTC7); // Speaker at PC7 |
} else { // beeper is off |
// set speaker port to low |
if (BoardRelease == 10) |
PORTD &= ~(1 << PORTD2);// Speaker at PD2 |
else |
PORTC &= ~(1 << PORTC7);// Speaker at PC7 |
} |
#ifndef USE_NAVICTRL |
// update compass value if this option is enabled in the settings |
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
// update compass value if this option is enabled in the settings |
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
#ifdef USE_MK3MAG |
MK3MAG_Update(); // read out mk3mag pwm |
MK3MAG_Update(); // read out mk3mag pwm |
#endif |
} |
} |
#endif |
} |
// ----------------------------------------------------------------------- |
uint16_t SetDelay(uint16_t t) { |
return (CountMilliseconds + t - 1); |
return (CountMilliseconds + t - 1); |
} |
// ----------------------------------------------------------------------- |
int8_t CheckDelay(uint16_t t) { |
return (((t - CountMilliseconds) & 0x8000) >> 8); // check sign bit |
return (((t - CountMilliseconds) & 0x8000) >> 8); // check sign bit |
} |
// ----------------------------------------------------------------------- |
void Delay_ms(uint16_t w) { |
uint16_t t_stop = SetDelay(w); |
while (!CheckDelay(t_stop)) |
; |
uint16_t t_stop = SetDelay(w); |
while (!CheckDelay(t_stop)) |
; |
} |
// ----------------------------------------------------------------------- |
void Delay_ms_Mess(uint16_t w) { |
uint16_t t_stop; |
t_stop = SetDelay(w); |
while (!CheckDelay(t_stop)) { |
if (analogDataReady) { |
analogDataReady = 0; |
analog_start(); |
} |
} |
uint16_t t_stop; |
t_stop = SetDelay(w); |
while (!CheckDelay(t_stop)) { |
if (analogDataReady) { |
analogDataReady = 0; |
analog_start(); |
} |
} |
} |
/branches/dongfang_FC_rewrite/uart0.c |
---|
129,9 → 129,9 |
"GyroPitch(PID) ", |
"GyroRoll(PID) ", |
"GyroYaw ", //5 |
"FilteredAccP ", |
"FilteredAccR ", |
"FilteredAccZ ", |
"plusround ", |
"minusround ", |
"unused ", |
"AccPitch (angle)", |
"AccRoll (angle) ", //10 |
"UBat ", |
141,8 → 141,8 |
"Throttle Term ", //15 |
"0th O Corr pitch", |
"0th O Corr roll ", |
"DriftCompDelta P", |
"DriftCompDelta R", |
"unused ", |
"unused ", |
"dHeightThrottle ", //20 |
"hoverThrottle ", |
"M1 ", |