• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

FlightCtrlBL-CtrlMK3MagNaviCtrlProjects

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → /branches/ @ 1871

  → /branches/ @ 1872

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 1871 → Rev 1872

/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 ",