• 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/dongfang_FC_rewrite/ @ 1959

  → /branches/dongfang_FC_rewrite/ @ 1960

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 1959 → Rev 1960

/branches/dongfang_FC_rewrite/analog_fastirq.c
File deleted

/branches/dongfang_FC_rewrite/gyro.h
File deleted

/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
8,8 → 8,8
}
void gyro_setDefaults(void) {
staticParams.GyroD = 5;
staticParams.DriftComp = 3;
staticParams.GyroAccFactor = 1;
staticParams.GyroAccTrim = 5;
staticParams.gyroD = 5;
staticParams.driftCompDivider = 1;
staticParams.driftCompLimit = 3;
staticParams.zerothOrderCorrection = 1;
}

/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
56,7 → 56,6
}
}
startAnalogConversionCycle();
delay_ms_Mess(i < 10 ? 10 : 2);
}
delay_ms_Mess(70);
63,8 → 62,8
}
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.DriftComp = 200;
staticParams.GyroAccFactor = 25;
staticParams.GyroAccTrim = 1;
staticParams.gyroD = 3;
staticParams.driftCompDivider = 1;
staticParams.driftCompLimit = 200;
staticParams.zerothOrderCorrection = 25;
}

/branches/dongfang_FC_rewrite/analog.c
100,20 → 100,21
* standing still. They are used for adjusting the gyro and acc. meter values
* to be centered on zero.
*/
/*
volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512
* GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW };
volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512
* ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z };
*/
sensorOffset_t gyroOffset;
sensorOffset_t accOffset;
/*
* This allows some experimentation with the gyro filters.
* Should be replaced by #define's later on...
*/
volatile uint8_t GYROS_PID_FILTER;
volatile uint8_t GYROS_ATT_FILTER;
volatile uint8_t GYROS_D_FILTER;
volatile uint8_t ACC_FILTER;
/*
* Air pressure
245,6 → 246,7
}
void startAnalogConversionCycle(void) {
analogDataReady = 0;
// Stop the sampling. Cycle is over.
for (uint8_t i = 0; i < 8; i++) {
sensorInputs[i] = 0;
289,7 → 291,7
// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
// gyro with a wider range, and helps counter saturation at full control.
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {
if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
if (tempGyro < SENSOR_MIN_PITCHROLL) {
debugOut.digital[0] |= DEBUG_SENSORLIMIT;
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
304,19 → 306,19
// 2) Apply sign and offset, scale before filtering.
if (GYRO_REVERSED[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
}
// 3) Scale and filter.
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro) / GYROS_PID_FILTER;
tempOffsetGyro = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroPIDFilterConstant;
// 4) Measure noise.
measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
// 5) Differential measurement.
gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER;
gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
// 6) Done.
gyro_PID[axis] = tempOffsetGyro;
328,21 → 330,21
// 1) Apply sign and offset, scale before filtering.
if (GYRO_REVERSED[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
}
// 2) Filter.
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro) / GYROS_ATT_FILTER;
gyro_ATT[axis] = (gyro_ATT[axis] * (staticParams.gyroATTFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroATTFilterConstant;
}
// Yaw gyro.
rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
if (GYRO_REVERSED[YAW])
yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
else
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
debugOut.analog[3] = gyro_ATT[PITCH];
debugOut.analog[4] = gyro_ATT[ROLL];
353,11 → 355,11
// Pitch and roll axis accelerations.
for (uint8_t axis=0; axis<2; axis++) {
if (ACC_REVERSED[axis])
acc[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis];
acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis];
else
acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis];
acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis];
filteredAcc[axis] = (filteredAcc[axis] * (ACC_FILTER - 1) + acc[axis]) / ACC_FILTER;
filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
/*
stronglyFilteredAcc[PITCH] =
369,17 → 371,14
// Z acc.
if (ACC_REVERSED[Z])
acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
else
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
/*
stronglyFilteredAcc[Z] =
(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
*/
debugOut.analog[6] = acc[PITCH];
debugOut.analog[7] = acc[ROLL];
}
void analog_updateAirPressure(void) {
479,13 → 478,6
#define GYRO_OFFSET_CYCLES 32
uint8_t i, axis;
int32_t deltaOffsets[3] = { 0, 0, 0 };
// Set the filters... to be removed again, once some good settings are found.
GYROS_PID_FILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1;
GYROS_ATT_FILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1;
GYROS_D_FILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1;
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1;
gyro_calibrate();
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
497,16 → 489,17
}
for (axis = PITCH; axis <= YAW; axis++) {
gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
// DebugOut.Analog[20 + axis] = gyroOffset[axis];
gyroOffset.offsets[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
debugOut.analog[6+axis] = gyroOffset.offsets[axis];
}
// Noise is relativ to offset. So, reset noise measurements when changing offsets.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
accOffset[ROLL] = GetParamWord(PID_ACC_ROLL);
accOffset[Z] = GetParamWord(PID_ACC_Z);
accOffset_readFromEEProm();
// accOffset[PITCH] = getParamWord(PID_ACC_PITCH);
// accOffset[ROLL] = getParamWord(PID_ACC_ROLL);
// accOffset[Z] = getParamWord(PID_ACC_Z);
// Rough estimate. Hmm no nothing happens at calibration anyway.
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2);
524,61 → 517,35
*/
void analog_calibrateAcc(void) {
#define ACC_OFFSET_CYCLES 10
uint8_t i, axis;
int32_t deltaOffset[3] = { 0, 0, 0 };
int16_t filteredDelta;
// int16_t pressureDiff, savedRawAirPressure;
uint8_t i, axis;
int32_t deltaOffset[3] = { 0, 0, 0 };
int16_t filteredDelta;
// int16_t pressureDiff, savedRawAirPressure;
for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
delay_ms_Mess(10);
for (axis = PITCH; axis <= YAW; axis++) {
deltaOffset[axis] += acc[axis];
}
}
for (axis = PITCH; axis <= YAW; axis++) {
filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2)
/ ACC_OFFSET_CYCLES;
accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
}
// Save ACC neutral settings to eeprom
// setParamWord(PID_ACC_PITCH, accOffset[PITCH]);
// setParamWord(PID_ACC_ROLL, accOffset[ROLL]);
// setParamWord(PID_ACC_Z, accOffset[Z]);
accOffset_writeToEEProm();
for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
delay_ms_Mess(10);
for (axis = PITCH; axis <= YAW; axis++) {
deltaOffset[axis] += acc[axis];
}
}
for (axis = PITCH; axis <= YAW; axis++) {
filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2)
/ ACC_OFFSET_CYCLES;
accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
}
// Save ACC neutral settings to eeprom
SetParamWord(PID_ACC_PITCH, accOffset[PITCH]);
SetParamWord(PID_ACC_ROLL, accOffset[ROLL]);
SetParamWord(PID_ACC_Z, accOffset[Z]);
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
delay_ms_Mess(100);
// Set the feedback so that air pressure ends up in the middle of the range.
// (raw pressure high --> OCR0A also high...)
/*
OCR0A += ((rawAirPressure - 1024) / rangewidth) - 1;
delay_ms_Mess(1000);
pressureDiff = 0;
// DebugOut.Analog[16] = rawAirPressure;
#define PRESSURE_CAL_CYCLE_COUNT 5
for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) {
savedRawAirPressure = rawAirPressure;
OCR0A+=2;
delay_ms_Mess(500);
// raw pressure will decrease.
pressureDiff += (savedRawAirPressure - rawAirPressure);
savedRawAirPressure = rawAirPressure;
OCR0A-=2;
delay_ms_Mess(500);
// raw pressure will increase.
pressureDiff += (rawAirPressure - savedRawAirPressure);
}
rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2 * 2);
DebugOut.Analog[27] = rangewidth;
*/
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
delay_ms_Mess(100);
}

/branches/dongfang_FC_rewrite/analog.h
188,6 → 188,15
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
typedef struct {
uint8_t checksum;
uint8_t revisionNumber;
int16_t offsets[3];
} sensorOffset_t;
extern sensorOffset_t gyroOffset;
extern sensorOffset_t accOffset;
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
*/

/branches/dongfang_FC_rewrite/attitude.c
173,7 → 173,7
************************************************************************/
void attitude_setNeutral(void) {
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
correctionSum[PITCH] = correctionSum[ROLL] = 0;
217,7 → 217,6
// We are done reading variables from the analog module.
// Interrupt-driven sensor reading may restart.
analogDataReady = 0;
startAnalogConversionCycle();
}
249,7 → 248,7
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
uint8_t axis;
if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) {
if (/*!looping && */ (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE)) {
trigAxisCoupling();
} else {
ACRate[PITCH] = rate_ATT[PITCH];
300,10 → 299,8
debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
<= dynamicParams.UserParams[7]) {
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
if (1 /*controlActivity <= dynamicParams.maxControlActivityForAcc*/) {
uint8_t permilleAcc = staticParams.zerothOrderCorrection;
int32_t accDerived;
/*
376,8 → 373,8
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);
driftComp[axis] += deltaCorrection / staticParams.driftCompDivider;
CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit);
// DebugOut.Analog[11 + axis] = correctionSum[axis];
// DebugOut.Analog[16 + axis] = correctionSum[axis];
debugOut.analog[28 + axis] = driftComp[axis];
444,7 → 441,7
w = (w * dynamicParams.CompassYawEffect) / 32;
w = dynamicParams.CompassYawEffect - w;
*/
w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect)
w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect)
/ 32;
// As readable formula:
460,7 → 457,7
% 360) - 180;
v = (r * w) / v; // align to compass course
// limit yaw rate
w = 3 * dynamicParams.CompassYawEffect;
w = 3 * dynamicParams.compassYawEffect;
if (v > w)
v = w;
else if (v < -w)

/branches/dongfang_FC_rewrite/attitudeControl.c
15,7 → 15,7
// With static MINPROJECTION: 220 usec.
uint16_t AC_getThrottle(uint16_t throttle) {
int32_t projection;
uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
uint8_t effect = dynamicParams.attitudeControl; // Userparam 3
int16_t deltaThrottle, y;
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);

/branches/dongfang_FC_rewrite/commands.c
82,12 → 82,12
// A valid parameter-set (1..5) was chosen - use it.
setActiveParamSet(argument);
}
ParamSet_ReadFromEEProm(getActiveParamSet());
paramSet_readFromEEProm(getActiveParamSet());
attitude_setNeutral();
flight_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
} else if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE
} else if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE
| CFG_GPS_ACTIVE) && argument == 7) {
// If right stick is centered and down
// TODO: Out of here! State machine instead.

/branches/dongfang_FC_rewrite/configuration.c
49,16 → 49,20
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <util/delay.h>
#include <avr/eeprom.h>
#include <stddef.h>
#include <string.h>
#include "configuration.h"
#include "eeprom.h"
#include "sensors.h"
#include "rc.h"
int16_t variables[8] = {0,0,0,0,0,0,0,0};
dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
paramset_t staticParams;
channelMap_t channelMap;
mixerMatrix_t mixerMatrix;
dynamicParam_t dynamicParams; // = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
uint8_t CPUType = ATMEGA644;
uint8_t BoardRelease = 13;
uint8_t boardRelease = 13;
uint8_t requiredMotors;
/************************************************************************
* Map the parameter to pot values
* Replacing this code by the code below saved almost 1 kbyte.
68,76 → 72,43
uint8_t i;
#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
#define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}}
SET_POT(dynamicParams.MaxHeight,staticParams.MaxHeight);
SET_POT_MM(dynamicParams.HeightD,staticParams.HeightD,0,100);
SET_POT_MM(dynamicParams.HeightP,staticParams.HeightP,0,100);
SET_POT(dynamicParams.Height_ACC_Effect,staticParams.Height_ACC_Effect);
SET_POT(dynamicParams.CompassYawEffect,staticParams.CompassYawEffect);
SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,0,255);
SET_POT(dynamicParams.GyroI,staticParams.GyroI);
SET_POT(dynamicParams.GyroD,staticParams.GyroD);
SET_POT(dynamicParams.IFactor,staticParams.IFactor);
for (i=0; i<sizeof(staticParams.UserParams1); i++) {
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]);
SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200);
SET_POT(dynamicParams.gyroI, staticParams.gyroI);
SET_POT(dynamicParams.gyroD, staticParams.gyroD);
SET_POT(dynamicParams.IFactor, staticParams.IFactor);
SET_POT(dynamicParams.yawIFactor, staticParams.yawIFactor);
SET_POT(dynamicParams.compassYawEffect,staticParams.compassYawEffect);
SET_POT(dynamicParams.externalControl, staticParams.externalControl);
SET_POT(dynamicParams.axisCoupling1, staticParams.axisCoupling1);
SET_POT(dynamicParams.axisCoupling2, staticParams.axisCoupling2);
SET_POT(dynamicParams.axisCouplingYawCorrection, staticParams.axisCouplingYawCorrection);
SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability);
SET_POT(dynamicParams.maxControlActivityForAcc,staticParams.maxControlActivityForAcc);
SET_POT_MM(dynamicParams.heightP, staticParams.heightP,0,100);
SET_POT_MM(dynamicParams.heightD, staticParams.heightD,0,100);
SET_POT(dynamicParams.heightSetting,staticParams.heightSetting);
SET_POT(dynamicParams.heightACCEffect,staticParams.heightACCEffect);
SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl);
for (i=0; i<sizeof(staticParams.userParams); i++) {
SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]);
}
for (i=0; i<sizeof(staticParams.UserParams2); i++) {
SET_POT(dynamicParams.UserParams[i + sizeof(staticParams.UserParams1)], staticParams.UserParams2[i]);
}
SET_POT(dynamicParams.ServoPitchControl,staticParams.ServoPitchControl);
SET_POT(dynamicParams.LoopGasLimit,staticParams.LoopGasLimit);
SET_POT(dynamicParams.AxisCoupling1,staticParams.AxisCoupling1);
SET_POT(dynamicParams.AxisCoupling2,staticParams.AxisCoupling2);
SET_POT(dynamicParams.AxisCouplingYawCorrection,staticParams.AxisCouplingYawCorrection);
SET_POT(dynamicParams.DynamicStability,staticParams.DynamicStability);
SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255);
SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255);
#if defined (USE_NAVICTRL)
SET_POT(dynamicParams.NaviGpsModeControl,staticParams.NaviGpsModeControl);
SET_POT(dynamicParams.NaviGpsGain,staticParams.NaviGpsGain);
SET_POT(dynamicParams.NaviGpsP,staticParams.NaviGpsP);
SET_POT(dynamicParams.NaviGpsI,staticParams.NaviGpsI);
SET_POT(dynamicParams.NaviGpsD,staticParams.NaviGpsD);
SET_POT(dynamicParams.NaviGpsACC,staticParams.NaviGpsACC);
SET_POT_MM(dynamicParams.NaviOperatingRadius,staticParams.NaviOperatingRadius,10, 255);
SET_POT(dynamicParams.NaviWindCorrection,staticParams.NaviWindCorrection);
SET_POT(dynamicParams.NaviSpeedCompensation,staticParams.NaviSpeedCompensation);
#endif
SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl);
SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl);
SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl);
SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255);
SET_POT_MM(dynamicParams.output1Timing, staticParams.outputFlash[1].timing,1,255);
}
const XLATION XLATIONS[] = {
{offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)},
{offsetof(paramset_t, Height_ACC_Effect), offsetof(dynamicParam_t, Height_ACC_Effect)},
{offsetof(paramset_t, CompassYawEffect), offsetof(dynamicParam_t, CompassYawEffect)},
{offsetof(paramset_t, GyroI), offsetof(dynamicParam_t, GyroI)},
{offsetof(paramset_t, GyroD), offsetof(dynamicParam_t, GyroD)},
{offsetof(paramset_t, IFactor), offsetof(dynamicParam_t, IFactor)},
{offsetof(paramset_t, ServoPitchControl), offsetof(dynamicParam_t, ServoPitchControl)},
{offsetof(paramset_t, LoopGasLimit), offsetof(dynamicParam_t, LoopGasLimit)},
{offsetof(paramset_t, AxisCoupling1), offsetof(dynamicParam_t, AxisCoupling1)},
{offsetof(paramset_t, AxisCoupling2), offsetof(dynamicParam_t, AxisCoupling2)},
{offsetof(paramset_t, AxisCouplingYawCorrection), offsetof(dynamicParam_t, AxisCouplingYawCorrection)},
{offsetof(paramset_t, DynamicStability), offsetof(dynamicParam_t, DynamicStability)},
{offsetof(paramset_t, NaviGpsModeControl),
offsetof(dynamicParam_t, NaviGpsModeControl)},
{offsetof(paramset_t, NaviGpsGain), offsetof(dynamicParam_t, NaviGpsGain)},
{offsetof(paramset_t, NaviGpsP), offsetof(dynamicParam_t, NaviGpsP)},
{offsetof(paramset_t, NaviGpsI), offsetof(dynamicParam_t, NaviGpsI)},
{offsetof(paramset_t, NaviGpsD), offsetof(dynamicParam_t, NaviGpsD)},
{offsetof(paramset_t, NaviGpsACC), offsetof(dynamicParam_t, NaviGpsACC)},
{offsetof(paramset_t, NaviWindCorrection), offsetof(dynamicParam_t, NaviWindCorrection)},
{offsetof(paramset_t, NaviSpeedCompensation), offsetof(dynamicParam_t, NaviSpeedCompensation)},
{offsetof(paramset_t, ExternalControl), offsetof(dynamicParam_t, ExternalControl)}
// {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)},
};
const MMXLATION MMXLATIONS[] = {
{offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100},
{offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightP),0,150},
{offsetof(paramset_t, GyroP), offsetof(dynamicParam_t, GyroP),0,255},
{offsetof(paramset_t, J16Timing), offsetof(dynamicParam_t, J16Timing),1,255},
{offsetof(paramset_t, J17Timing), offsetof(dynamicParam_t, J17Timing),1,255},
{offsetof(paramset_t, NaviOperatingRadius), offsetof(dynamicParam_t, NaviOperatingRadius),10,255}
// {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100},
};
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
168,21 → 139,13
}
}
for (i=0; i<sizeof(staticParams.UserParams1); i++) {
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams1) + i));
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + i);
for (i=0; i<sizeof(staticParams.userParams); i++) {
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, userParams) + i));
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, userParams) + i);
if (src < 255) {
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
}
}
for (i=0; i<sizeof(staticParams.UserParams2); i++) {
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams2) + i));
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + sizeof(staticParams.UserParams1) + i);
if (src < 255) {
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
}
}
}
uint8_t getCPUType(void) { // works only after reset or power on when the registers have default values
203,7 → 166,7
*/
uint8_t getBoardRelease(void) {
uint8_t BoardRelease = 13;
uint8_t boardRelease = 13;
// the board release is coded via the pull up or down the 2 status LED
PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
213,16 → 176,16
switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
case 0x00:
BoardRelease = 10; // 1.0
boardRelease = 10; // 1.0
break;
case 0x01:
BoardRelease = 11; // 1.1 or 1.2
boardRelease = 11; // 1.1 or 1.2
break;
case 0x02:
BoardRelease = 20; // 2.0
boardRelease = 20; // 2.0
break;
case 0x03:
BoardRelease = 13; // 1.3
boardRelease = 13; // 1.3
break;
default:
break;
231,5 → 194,125
DDRB |= (1<<DDB1)|(1<<DDB0);
RED_ON;
GRN_OFF;
return BoardRelease;
return boardRelease;
}
void setOtherDefaults(void) {
// Height Control
staticParams.heightP = 10;
staticParams.heightD = 30;
staticParams.heightSetting = 251;
staticParams.heightMaxThrottleChange = 10;
staticParams.heightSlewRate = 4;
staticParams.heightACCEffect = 30;
// Control
staticParams.stickP = 8;
staticParams.stickD = 12;
staticParams.stickYawP = 12;
staticParams.stickThrottleD = 12;
staticParams.minThrottle = 8;
staticParams.maxThrottle = 230;
staticParams.externalControl = 0;
// IMU
staticParams.gyroPIDFilterConstant = 1;
staticParams.gyroATTFilterConstant = 1;
staticParams.gyroDFilterConstant = 1;
staticParams.accFilterConstant = 4;
staticParams.gyroP = 60;
staticParams.gyroI = 80;
staticParams.gyroD = 4;
// set by gyro code.
// staticParams.zerothOrderCorrection =
// staticParams.driftCompDivider =
// staticParams.driftCompLimit =
staticParams.axisCoupling1 = 90;
staticParams.axisCoupling2 = 67;
staticParams.axisCouplingYawCorrection = 0;
staticParams.dynamicStability = 50;
staticParams.IFactor = 32;
staticParams.compassYawEffect = 128;
// Servos
for (uint8_t i=0; i<2; i++) {
staticParams.servoConfigurations[i].manualControl = 128;
staticParams.servoConfigurations[i].compensationFactor = 0;
staticParams.servoConfigurations[i].minValue = 64;
staticParams.servoConfigurations[i].maxValue = 192;
staticParams.servoConfigurations[i].flags = 0;
}
staticParams.servoCount = 4;
// Battery warning and emergency flight
staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S
staticParams.emergencyThrottle = 35;
staticParams.emergencyFlightDuration = 30;
// Outputs
staticParams.outputFlash[0].bitmask = 0b01011111;
staticParams.outputFlash[0].timing = 15;
staticParams.outputFlash[1].bitmask = 0b11110011;
staticParams.outputFlash[1].timing = 15;
staticParams.outputFlags = 0;
}
/***************************************************/
/* Default Values for parameter set 1 */
/***************************************************/
void paramSet_default(uint8_t setnumber) {
setOtherDefaults();
gyro_setDefaults();
for (uint8_t i=0; i<8; i++) {
staticParams.userParams[i] = 0;
}
staticParams.bitConfig =
CFG_GYRO_SATURATION_PREVENTION |
CFG_HEADING_HOLD;
staticParams.bitConfig2 = 0;
memcpy(staticParams.name, "Default\0", 6);
}
/***************************************************/
/* Default Values for Mixer Table */
/***************************************************/
void mixerMatrix_default(void) { // Quadro
uint8_t i;
// mixerMatric.revision = EEMIXER_REVISION;
// clear mixer table (but preset throttle)
for (i = 0; i < 16; i++) {
mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
mixerMatrix.motor[i][MIX_PITCH] = 0;
mixerMatrix.motor[i][MIX_ROLL] = 0;
mixerMatrix.motor[i][MIX_YAW] = 0;
}
// default = Quadro
mixerMatrix.motor[0][MIX_PITCH] = +64;
mixerMatrix.motor[0][MIX_YAW] = +64;
mixerMatrix.motor[1][MIX_PITCH] = -64;
mixerMatrix.motor[1][MIX_YAW] = +64;
mixerMatrix.motor[2][MIX_ROLL] = -64;
mixerMatrix.motor[2][MIX_YAW] = -64;
mixerMatrix.motor[3][MIX_ROLL] = +64;
mixerMatrix.motor[3][MIX_YAW] = -64;
memcpy(mixerMatrix.name, "Quadro\0", 7);
}
void channelMap_default(void) {
channelMap.channels[CH_PITCH] = 2;
channelMap.channels[CH_ROLL] = 1;
channelMap.channels[CH_THROTTLE] = 3;
channelMap.channels[CH_YAW] = 4;
channelMap.channels[CH_POTS + 0] = 5;
channelMap.channels[CH_POTS + 1] = 6;
channelMap.channels[CH_POTS + 2] = 7;
channelMap.channels[CH_POTS + 3] = 8;
}

/branches/dongfang_FC_rewrite/configuration.h
4,40 → 4,46
#include <inttypes.h>
#include <avr/io.h>
#define MAX_CHANNELS 10
#define MAX_MOTORS 12
typedef struct {
/*PMM*/uint8_t HeightD;
/* P */uint8_t MaxHeight;
/*PMM*/uint8_t HeightP;
/* P */uint8_t Height_ACC_Effect;
/* P */uint8_t CompassYawEffect;
/* P */uint8_t GyroD;
/*PMM*/uint8_t GyroP;
/* P */uint8_t GyroI;
/* Never used */uint8_t StickYawP;
// IMU
/*PMM*/uint8_t gyroP;
/* P */uint8_t gyroD;
/* P */uint8_t gyroI;
/* P */uint8_t IFactor;
/* P */uint8_t UserParams[8];
/* P */uint8_t ServoPitchControl;
/* P */uint8_t LoopGasLimit;
/* P */uint8_t AxisCoupling1;
/* P */uint8_t AxisCoupling2;
/* P */uint8_t AxisCouplingYawCorrection;
/* P */uint8_t DynamicStability;
/* P */uint8_t ExternalControl;
/*PMM*/uint8_t J16Timing;
/*PMM*/uint8_t J17Timing;
/* P */uint8_t NaviGpsModeControl;
/* P */uint8_t NaviGpsGain;
/* P */uint8_t NaviGpsP;
/* P */uint8_t NaviGpsI;
/* P */uint8_t NaviGpsD;
/* P */uint8_t NaviGpsACC;
/*PMM*/uint8_t NaviOperatingRadius;
/* P */uint8_t NaviWindCorrection;
/* P */uint8_t NaviSpeedCompensation;
int8_t KalmanK;
int8_t KalmanMaxDrift;
int8_t KalmanMaxFusion;
uint8_t yawIFactor;
/* P */uint8_t compassYawEffect;
// Control
/* P */uint8_t externalControl;
/* P */uint8_t axisCoupling1;
/* P */uint8_t axisCoupling2;
/* P */uint8_t axisCouplingYawCorrection;
/* P */uint8_t dynamicStability;
uint8_t maxControlActivityForAcc;
// Height control
/*PMM*/uint8_t heightP;
/*PMM*/uint8_t heightD;
/* P */uint8_t heightSetting;
/* P */uint8_t heightACCEffect;
uint8_t attitudeControl;
// The rest...
/* P */uint8_t userParams[8];
/*PMM*/uint8_t output0Timing;
/*PMM*/uint8_t output1Timing;
uint8_t servoManualControl[2];
uint8_t motorSmoothing;
} dynamicParam_t;
extern dynamicParam_t dynamicParams;
typedef struct {
49,109 → 55,120
uint8_t sourceIdx, targetIdx;
} XLATION;
typedef struct {
uint8_t checksum;
uint8_t revisionNumber;
uint8_t channels[MAX_CHANNELS];
} channelMap_t;
extern channelMap_t channelMap;
typedef struct {
uint8_t checksum;
uint8_t revision;
int8_t name[12];
int8_t motor[MAX_MOTORS][4];
}__attribute__((packed)) mixerMatrix_t;
extern mixerMatrix_t mixerMatrix;
typedef struct {
uint8_t manualControl;
uint8_t compensationFactor;
uint8_t minValue;
uint8_t maxValue;
uint8_t flags;
} servo_t;
typedef struct {
uint8_t bitmask;
uint8_t timing;
} output_flash_t;
// values above 250 representing poti1 to poti4
typedef struct {
uint8_t ChannelAssignment[8]; // see upper defines for details
uint8_t GlobalConfig; // see upper defines for bitcoding
uint8_t HeightMinGas; // Value : 0-100
uint8_t HeightD; // Value : 0-250
uint8_t MaxHeight; // Value : 0-32
uint8_t HeightP; // Value : 0-32
uint8_t Height_Gain; // Value : 0-50
uint8_t Height_ACC_Effect; // Value : 0-250
uint8_t StickP; // Value : 1-6
uint8_t StickD; // Value : 0-64
uint8_t StickYawP; // Value : 1-20
uint8_t MinThrottle; // Value : 0-32
uint8_t MaxThrottle; // Value : 33-250
uint8_t GyroAccFactor; // Value : 1-64
uint8_t CompassYawEffect; // Value : 0-32
uint8_t GyroP; // Value : 10-250
uint8_t GyroI; // Value : 0-250
uint8_t GyroD; // Value : 0-250
uint8_t LowVoltageWarning; // Value : 0-250
uint8_t EmergencyGas; // Value : 0-250 //Gaswert bei Empüngsverlust
uint8_t EmergencyGasDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
uint8_t Unused0; //
// Global bitflags
uint8_t bitConfig; // see upper defines for bitcoding
uint8_t bitConfig2; // see upper defines for bitcoding
// Height Control
uint8_t heightP; // Value : 0-32
uint8_t heightD; // Value : 0-250
uint8_t heightSetting; // Value : 0-32
uint8_t heightMaxThrottleChange; // Value : 0-100
uint8_t heightSlewRate; // Value : 0-50
uint8_t heightACCEffect; // Value : 0-250
// Attitude Control
uint8_t attitudeControl;
// Control
uint8_t stickP; // Value : 1-6
uint8_t stickD; // Value : 0-64
uint8_t stickYawP; // Value : 1-20
uint8_t stickThrottleD;
uint8_t minThrottle; // Value : 0-32
uint8_t maxThrottle; // Value : 33-250
uint8_t externalControl; // for serial Control
uint8_t maxControlActivityForAcc;
// IMU
uint8_t gyroPIDFilterConstant;
uint8_t gyroATTFilterConstant;
uint8_t gyroDFilterConstant;
uint8_t accFilterConstant;
uint8_t gyroP; // Value : 10-250
uint8_t gyroI; // Value : 0-250
uint8_t gyroD; // Value : 0-250
uint8_t zerothOrderCorrection; // Value : 1-64
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung)
uint8_t driftCompLimit; // limit for gyrodrift compensation
uint8_t axisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung)
uint8_t axisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t axisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t dynamicStability; // PID limit for Attitude controller
uint8_t IFactor; // Value : 0-250
uint8_t UserParams1[4]; // Value : 0-250
/*
uint8_t UserParam2; // Value : 0-250
uint8_t UserParam3; // Value : 0-250
uint8_t UserParam4; // Value : 0-250
*/
uint8_t ServoPitchControl;
uint8_t ServoPitchComp;
uint8_t ServoPitchMin;
uint8_t ServoPitchMax; // Value : 0-250 // Anschlag
uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output
uint8_t LoopGasLimit; // Value: 0-250 max. Gas wührend Looping
uint8_t LoopThreshold; // Value: 0-250 Schwelle für Stickausschlag
uint8_t LoopHysteresis; // Value: 0-250 Hysterese für Stickausschlag
uint8_t AxisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung)
uint8_t AxisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AxisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t unused0;
uint8_t unused1;
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung)
uint8_t DriftComp; // limit for gyrodrift compensation
uint8_t DynamicStability; // PID limit for Attitude controller
uint8_t UserParams2[4]; // Value : 0-250
/*
uint8_t UserParam6; // Value : 0-250
uint8_t UserParam7; // Value : 0-250
uint8_t UserParam8; // Value : 0-250
*/
uint8_t J16Bitmask; // for the J16 Output
uint8_t J16Timing; // for the J16 Output
uint8_t J17Bitmask; // for the J17 Output
uint8_t J17Timing; // for the J17 Output
uint8_t NaviGpsModeControl; // Parameters for the Naviboard
uint8_t NaviGpsGain; // overall gain for GPS-PID controller
uint8_t NaviGpsP; // P gain for GPS-PID controller
uint8_t NaviGpsI; // I gain for GPS-PID controller
uint8_t NaviGpsD; // D gain for GPS-PID controller
uint8_t NaviGpsPLimit; // P limit for GPS-PID controller
uint8_t NaviGpsILimit; // I limit for GPS-PID controller
uint8_t NaviGpsDLimit; // D limit for GPS-PID controller
uint8_t NaviGpsACC; // ACC gain for GPS-PID controller
uint8_t NaviGpsMinSat; // number of sattelites neccesary for GPS functions
uint8_t NaviStickThreshold; // activation threshild for detection of manual stick movements
uint8_t NaviWindCorrection; // streng of wind course correction
uint8_t NaviSpeedCompensation; // D gain fefore position hold login
uint8_t NaviOperatingRadius; // Radius limit in m around start position for GPS flights
uint8_t NaviAngleLimitation; // limitation of attitude angle controlled by the gps algorithm
uint8_t NaviPHLoginTime; // position hold logintimeout
uint8_t ExternalControl; // for serial Control
uint8_t BitConfig; // see upper defines for bitcoding
uint8_t ServoPitchCompInvert; // Value : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen
uint8_t Reserved[4];
int8_t Name[12];
} paramset_t;
uint8_t yawIFactor;
uint8_t compassYawEffect; // Value : 0-32
#define PARAMSET_STRUCT_LEN sizeof(paramset_t)
// Servos
servo_t servoConfigurations[2]; // [PITCH, ROLL]
uint8_t servoCount;
extern paramset_t staticParams;
// Battery warning and emergency flight
uint8_t batteryVoltageWarning; // Value : 0-250
uint8_t emergencyThrottle; // Value : 0-250 //Gaswert bei Empüngsverlust /*
uint8_t emergencyFlightDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
typedef struct {
uint8_t Revision;
int8_t Name[12];
int8_t Motor[16][4];
}__attribute__((packed)) MixerTable_t;
// Outputs
output_flash_t outputFlash[2];
uint8_t outputDebugMask;
// bit0: Test all OFF
// bit1: Test all ON
// bit2: flash when beeping
uint8_t outputFlags;
extern MixerTable_t Mixer;
// User params
uint8_t userParams[8]; // Value : 0-250
// Name
uint8_t name[12];
} paramset_t;
extern paramset_t staticParams;
// MKFlags
#define MKFLAG_MOTOR_RUN (1<<0)
#define MKFLAG_FLY (1<<1)
#define MKFLAG_CALIBRATE (1<<2)
#define MKFLAG_START (1<<3)
#define MKFLAG_EMERGENCY_LANDING (1<<4)
#define MKFLAG_EMERGENCY_FLIGHT (1<<4)
#define MKFLAG_RESERVE1 (1<<5)
#define MKFLAG_RESERVE2 (1<<6)
#define MKFLAG_RESERVE3 (1<<7)
// bit mask for staticParams.GlobalConfig
// bit mask for staticParams.bitConfig
#define CFG_HEIGHT_CONTROL (1<<0)
#define CFG_HEIGHT_SWITCH (1<<1)
#define CFG_HEADING_HOLD (1<<2)
159,9 → 176,9
#define CFG_COMPASS_FIX (1<<4)
#define CFG_GPS_ACTIVE (1<<5)
#define CFG_AXIS_COUPLING_ACTIVE (1<<6)
#define CFG_ROTARY_RATE_LIMITER (1<<7)
#define CFG_GYRO_SATURATION_PREVENTION (1<<7)
// bit mask for staticParams.BitConfig
// bit mask for staticParams.bitConfig2
#define CFG_LOOP_UP (1<<0)
#define CFG_LOOP_DOWN (1<<1)
#define CFG_LOOP_LEFT (1<<2)
173,11 → 190,11
#define SYSCLK F_CPU
// Not really a part of configuration, but LEDs and HW version test are the same.
#define RED_OFF {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
#define RED_ON {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);}
#define RED_OFF {if((boardRelease == 10) || (boardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
#define RED_ON {if((boardRelease == 10) || (boardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);}
#define RED_FLASH PORTB ^= (1<<PORTB0)
#define GRN_OFF {if(BoardRelease < 12) PORTB &=~(1<<PORTB1); else PORTB |= (1<<PORTB1);}
#define GRN_ON {if(BoardRelease < 12) PORTB |= (1<<PORTB1); else PORTB &=~(1<<PORTB1);}
#define GRN_OFF {if(boardRelease < 12) PORTB &=~(1<<PORTB1); else PORTB |= (1<<PORTB1);}
#define GRN_ON {if(boardRelease < 12) PORTB |= (1<<PORTB1); else PORTB &=~(1<<PORTB1);}
#define GRN_FLASH PORTB ^= (1<<PORTB1)
// Mixer table
187,10 → 204,15
#define MIX_YAW 3
extern volatile uint8_t MKFlags;
extern uint8_t requiredMotors;
extern int16_t variables[8]; // The "Poti"s.
extern uint8_t BoardRelease;
extern uint8_t boardRelease;
extern uint8_t CPUType;
void channelMap_default(void);
void paramSet_default(uint8_t setnumber);
void mixerMatrix_default(void);
void configuration_applyVariablesToParams(void);
uint8_t getCPUType(void);
uint8_t getBoardRelease(void);

/branches/dongfang_FC_rewrite/controlMixer.c
63,8 → 63,7
// uint16_t maxControl[2] = { 0, 0 };
uint16_t controlActivity = 0;
int16_t controls[4] = { 0, 0, 0, 0 };
//int16_t controlYaw = 0, controlThrottle = 0;
uint8_t looping = 0;
//uint8_t looping = 0;
// Internal variables for reading commands made with an R/C stick.
uint8_t lastCommand = COMMAND_NONE;
190,13 → 189,13
// controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
controlMixer_updateVariables();
configuration_applyVariablesToParams();
looping = RC_getLooping(looping);
controlMixer_updateVariables();
configuration_applyVariablesToParams();
//looping = RC_getLooping(looping);
} else { // Signal is not OK
// Could handle switch to emergency flight here.
// throttle is handled elsewhere.
looping = 0;
// looping = 0;
}
// part1a end.

/branches/dongfang_FC_rewrite/controlMixer.h
75,7 → 75,7
//extern int16_t controlYaw, controlThrottle;
extern uint16_t controlActivity;
//extern uint16_t maxControl[2];
extern uint8_t looping;
//extern uint8_t looping;
extern volatile uint8_t MKFlags;
extern uint16_t isFlying;

/branches/dongfang_FC_rewrite/eeprom.c
63,188 → 63,62
#define EEMEM __attribute__ ((section (".eeprom")))
#endif
#include <avr/eeprom.h>
#include <string.h>
#include "eeprom.h"
#include "printf_P.h"
#include "output.h"
// TODO: Get rid of these. They have nothing to do with eeprom.
#include "flight.h"
#include "rc.h"
#include "sensors.h"
#include <avr/eeprom.h>
// byte array in eeprom
uint8_t EEPromArray[E2END + 1] EEMEM;
paramset_t staticParams;
MixerTable_t Mixer;
/*
* Default for your own experiments here, so you don't have to reset them
* from MK-Tool all the time.
*/
void setDefaultUserParams(void) {
uint8_t i;
for (i = 0; i < sizeof(staticParams.UserParams1); i++) {
staticParams.UserParams1[i] = 0;
}
for (i = 0; i < sizeof(staticParams.UserParams2); i++) {
staticParams.UserParams2[i] = 0;
}
/*
* While we are still using userparams for flight parameters, do set
* some safe & meaningful default values.
*/
staticParams.UserParams1[3] = 12; // Throttle stick D=12
staticParams.UserParams2[0] = 0b11010101; // All gyro filter constants 2; acc. 4
staticParams.UserParams2[1] = 2; // H&I motor smoothing.
staticParams.UserParams2[2] = 120; // Yaw I factor
staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles.
}
void setOtherDefaults(void) {
/* Channel assignments were changed to the normal:
* Aileron/roll=1, elevator/pitch=2, throttle=3, yaw/rudder=4
*/
staticParams.ChannelAssignment[CH_PITCH] = 2;
staticParams.ChannelAssignment[CH_ROLL] = 1;
staticParams.ChannelAssignment[CH_THROTTLE] = 3;
staticParams.ChannelAssignment[CH_YAW] = 4;
staticParams.ChannelAssignment[CH_POTS + 0] = 5;
staticParams.ChannelAssignment[CH_POTS + 1] = 6;
staticParams.ChannelAssignment[CH_POTS + 2] = 7;
staticParams.ChannelAssignment[CH_POTS + 3] = 8;
staticParams.GlobalConfig = /* CFG_AXIS_COUPLING_ACTIVE | */ CFG_HEADING_HOLD; // CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;
staticParams.HeightMinGas = 30;
staticParams.MaxHeight = 251;
staticParams.HeightP = 10;
staticParams.HeightD = 30;
staticParams.Height_ACC_Effect = 30;
staticParams.Height_Gain = 4;
staticParams.StickP = 8;
staticParams.StickD = 12;
staticParams.StickYawP = 12;
staticParams.MinThrottle = 8;
staticParams.MaxThrottle = 230;
staticParams.CompassYawEffect = 128;
staticParams.GyroP = 80;
staticParams.GyroI = 100;
staticParams.LowVoltageWarning = 101; // 3.7 each for 3S
staticParams.EmergencyGas = 35;
staticParams.EmergencyGasDuration = 30;
staticParams.Unused0 = 0;
staticParams.IFactor = 32;
staticParams.ServoPitchControl = 100;
staticParams.ServoPitchComp = 40;
staticParams.ServoPitchCompInvert = 0;
staticParams.ServoPitchMin = 50;
staticParams.ServoPitchMax = 150;
staticParams.ServoRefresh = 5;
staticParams.LoopGasLimit = 50;
staticParams.LoopThreshold = 90;
staticParams.LoopHysteresis = 50;
staticParams.BitConfig = 0;
staticParams.AxisCoupling1 = 90;
staticParams.AxisCoupling2 = 67;
staticParams.AxisCouplingYawCorrection = 0;
staticParams.DynamicStability = 50;
staticParams.J16Bitmask = 95;
staticParams.J17Bitmask = 243;
staticParams.J16Timing = 15;
staticParams.J17Timing = 15;
staticParams.NaviGpsModeControl = 253;
staticParams.NaviGpsGain = 100;
staticParams.NaviGpsP = 90;
staticParams.NaviGpsI = 90;
staticParams.NaviGpsD = 90;
staticParams.NaviGpsPLimit = 75;
staticParams.NaviGpsILimit = 75;
staticParams.NaviGpsDLimit = 75;
staticParams.NaviGpsACC = 0;
staticParams.NaviGpsMinSat = 6;
staticParams.NaviStickThreshold = 8;
staticParams.NaviWindCorrection = 90;
staticParams.NaviSpeedCompensation = 30;
staticParams.NaviOperatingRadius = 100;
staticParams.NaviAngleLimitation = 100;
staticParams.NaviPHLoginTime = 4;
}
/***************************************************/
/* Default Values for parameter set 1 */
/* Read Parameter from EEPROM as byte */
/***************************************************/
void ParamSet_DefaultSet1(void) { // sport
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER
/* | CFG_AXIS_COUPLING_ACTIVE*/;
staticParams.EmergencyGasDuration = 200;
staticParams.J16Timing = 10;
staticParams.J17Timing = 10;
memcpy(staticParams.Name, "Sport\0", 6);
uint8_t getParamByte(uint16_t param_id) {
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
}
/***************************************************/
/* Default Values for parameter set 2 */
/* Write Parameter to EEPROM as byte */
/***************************************************/
void ParamSet_DefaultSet2(void) { // normal
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER
/* | CFG_AXIS_COUPLING_ACTIVE*/ ;
staticParams.Height_Gain = 3;
staticParams.EmergencyGasDuration = 200;
staticParams.J16Timing = 20;
staticParams.J17Timing = 20;
memcpy(staticParams.Name, "Normal\0", 7);
void setParamByte(uint16_t param_id, uint8_t value) {
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}
/***************************************************/
/* Default Values for parameter set 3 */
/* Read Parameter from EEPROM as word */
/***************************************************/
void ParamSet_DefaultSet3(void) { // beginner
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER
/* | CFG_AXIS_COUPLING_ACTIVE */;
staticParams.Height_Gain = 3;
staticParams.EmergencyGasDuration = 200;
staticParams.J16Timing = 30;
staticParams.J17Timing = 30;
memcpy(staticParams.Name, "Beginner\0", 9);
uint16_t getParamWord(uint16_t param_id) {
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN
+ param_id]);
}
/***************************************************/
/* Read Parameter from EEPROM as byte */
/* Write Parameter to EEPROM as word */
/***************************************************/
uint8_t GetParamByte(uint16_t param_id) {
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
void setParamWord(uint16_t param_id, uint16_t value) {
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}
/***************************************************/
/* Write Parameter to EEPROM as byte */
/***************************************************/
void SetParamByte(uint16_t param_id, uint8_t value) {
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
uint8_t calculateChecksum(uint8_t* data, uint16_t length) {
uint8_t result = 0;
for (uint16_t i=0; i<length; i++) {
result += data[i];
}
return result;
}
/***************************************************/
/* Read Parameter from EEPROM as word */
/***************************************************/
uint16_t GetParamWord(uint16_t param_id) {
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN
+ param_id]);
void writeChecksummedBlock(uint8_t revisionNumber, uint8_t* data, uint16_t length, uint16_t offset) {
uint8_t checksum = calculateChecksum(data+1, length-1);
data[0] = checksum;
data[1] = revisionNumber;
eeprom_write_block(data, &EEPromArray[offset], length);
}
/***************************************************/
/* Write Parameter to EEPROM as word */
/***************************************************/
void SetParamWord(uint16_t param_id, uint16_t value) {
eeprom_write_word(
(uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
uint8_t readChecksummedBlock(uint8_t revisionNumber, uint8_t* target, uint16_t length, uint16_t offset) {
eeprom_read_block(target, &EEPromArray[offset], length);
uint8_t checksum = calculateChecksum(target+1, length-1);
return (checksum != target[0] || revisionNumber != target[1]);
}
/***************************************************/
251,13 → 125,10
/* Read Parameter Set from EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_ReadFromEEProm(uint8_t setnumber) {
if ((1 > setnumber) || (setnumber > 5))
setnumber = 3;
eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber
- 1)], PARAMSET_STRUCT_LEN);
output_init();
uint8_t paramSet_readFromEEProm(uint8_t setnumber) {
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*sizeof(paramset_t);
output_init(); // what's that doing here??
return readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, sizeof(paramset_t), offset);
}
/***************************************************/
264,159 → 135,104
/* Write Parameter Set to EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_WriteToEEProm(uint8_t setnumber) {
if (setnumber > 5)
setnumber = 5;
if (setnumber < 1)
return;
eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber
- 1)], PARAMSET_STRUCT_LEN);
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAMSET_LENGTH],
PARAMSET_STRUCT_LEN);
eeprom_write_block(&staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_CHANNELS], 8); // backup the first 8 bytes that is the rc channel mapping
// set this parameter set to active set
setActiveParamSet(setnumber);
output_init();
void paramSet_writeToEEProm(uint8_t setnumber) {
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*sizeof(paramset_t);
writeChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, sizeof(paramset_t), offset);
// set this parameter set to active set
setActiveParamSet(setnumber);
output_init(); // what's that doing here??
}
/***************************************************/
/* Get active parameter set */
/***************************************************/
uint8_t getActiveParamSet(void) {
uint8_t setnumber;
setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]);
if (setnumber > 5) {
setnumber = 3;
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}
return (setnumber);
void paramSet_readOrDefault() {
uint8_t setnumber = getActiveParamSet();
// parameter version check
if (setnumber<1 ||setnumber>5 || paramSet_readFromEEProm(setnumber)) {
// if version check faild
printf("\n\rInit Parameter in EEPROM");
for (uint8_t i=6; i>0; i--) {
paramSet_default(i); // Fill staticParams Structure to default parameter set 1 (Sport)
paramSet_writeToEEProm(i);
}
// default-Setting is parameter set 3
setActiveParamSet(1);
}
printf("\n\rUsing Parameter Set %d", getActiveParamSet());
}
/***************************************************/
/* Set active parameter set */
/* MixerTable */
/***************************************************/
void setActiveParamSet(uint8_t setnumber) {
if (setnumber > 5)
setnumber = 5;
if (setnumber < 1)
setnumber = 1;
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
uint8_t mixerMatrix_readFromEEProm(void) {
return readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t));
}
/***************************************************/
/* Read MixerTable from EEPROM */
/***************************************************/
uint8_t MixerTable_ReadFromEEProm(void) {
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE])
== EEMIXER_REVISION) {
eeprom_read_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE],
sizeof(Mixer));
return 1;
} else
return 0;
void mixerMatrix_writeToEEProm(void) {
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t));
}
void mixerMatrix_readOrDefault(void) {
// load mixer table
if (mixerMatrix_readFromEEProm()) {
printf("\n\rGenerating default mixerMatrix");
mixerMatrix_default(); // Quadro
mixerMatrix_writeToEEProm();
}
// determine motornumber
requiredMotors = 0;
for (uint8_t i=0; i<MAX_MOTORS; i++) {
if (mixerMatrix.motor[i][MIX_THROTTLE])
requiredMotors++;
}
printf("\n\rMixer-Config: '%s' (%u Motors)",mixerMatrix.name, requiredMotors);
printf("\n\r==============================");
}
/***************************************************/
/* Write Mixer Table to EEPROM */
/* ChannelMap */
/***************************************************/
uint8_t MixerTable_WriteToEEProm(void) {
if (Mixer.Revision == EEMIXER_REVISION) {
eeprom_write_block((uint8_t *) &Mixer,
&EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
return 1;
} else
return 0;
uint8_t channelMap_readFromEEProm(void) {
return readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t));
}
void channelMap_writeToEEProm(void) {
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t));
}
void channelMap_readOrDefault(void) {
if (!channelMap_readFromEEProm()) {
printf("\n\rGenerating default channel map");
channelMap_default();
channelMap_writeToEEProm();
}
}
uint8_t accOffset_readFromEEProm(void) {
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t));
}
void accOffset_writeToEEProm(void) {
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t));
}
/***************************************************/
/* Default Values for Mixer Table */
/* Get active parameter set */
/***************************************************/
void MixerTable_Default(void) { // Quadro
uint8_t i;
Mixer.Revision = EEMIXER_REVISION;
// clear mixer table (but preset throttle)
for (i = 0; i < 16; i++) {
Mixer.Motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
Mixer.Motor[i][MIX_PITCH] = 0;
Mixer.Motor[i][MIX_ROLL] = 0;
Mixer.Motor[i][MIX_YAW] = 0;
}
// default = Quadro
Mixer.Motor[0][MIX_PITCH] = +64;
Mixer.Motor[0][MIX_YAW] = +64;
Mixer.Motor[1][MIX_PITCH] = -64;
Mixer.Motor[1][MIX_YAW] = +64;
Mixer.Motor[2][MIX_ROLL] = -64;
Mixer.Motor[2][MIX_YAW] = -64;
Mixer.Motor[3][MIX_ROLL] = +64;
Mixer.Motor[3][MIX_YAW] = -64;
memcpy(Mixer.Name, "Quadro\0", 7);
uint8_t getActiveParamSet(void) {
uint8_t setnumber;
setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]);
if (setnumber > 4) {
setActiveParamSet(setnumber = 0);
}
return setnumber;
}
/***************************************************/
/* Initialize EEPROM Parameter Sets */
/* Set active parameter set */
/***************************************************/
void ParamSet_Init(void) {
uint8_t Channel_Backup = 1, i, j;
// parameter version check
if (eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) {
// if version check faild
printf("\n\rInit Parameter in EEPROM");
eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE], 0xFF); // reset also mixer table
// check if channel mapping backup is valid
for (j = 0; j < 4 && Channel_Backup; j++) {
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS + 0]) >= 12)
Channel_Backup = 0;
}
// fill all 5 parameter settings
for (i = 1; i < 6; i++) {
switch (i) {
case 1:
ParamSet_DefaultSet1(); // Fill staticParams Structure to default parameter set 1 (Sport)
break;
case 2:
ParamSet_DefaultSet2(); // Kamera
break;
case 3:
ParamSet_DefaultSet3(); // Beginner
break;
default:
ParamSet_DefaultSet2(); // Kamera
break;
}
if (Channel_Backup) { // if we have a rc channel mapping backup in eeprom
// restore it
for (j = 0; j < 8; j++) {
staticParams.ChannelAssignment[j] = eeprom_read_byte(
&EEPromArray[EEPROM_ADR_CHANNELS + j]);
}
}
ParamSet_WriteToEEProm(i);
}
// default-Setting is parameter set 3
setActiveParamSet(1);
// update version info
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION);
}
// read active parameter set to staticParams stucture
ParamSet_ReadFromEEProm(getActiveParamSet());
printf("\n\rUsing Parameter Set %d", getActiveParamSet());
void setActiveParamSet(uint8_t setnumber) {
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}
// load mixer table
if (!MixerTable_ReadFromEEProm()) {
printf("\n\rGenerating default Mixer Table");
MixerTable_Default(); // Quadro
MixerTable_WriteToEEProm();
}
// determine motornumber
RequiredMotors = 0;
for (i = 0; i < 16; i++) {
if (Mixer.Motor[i][MIX_THROTTLE] > 0)
RequiredMotors++;
}
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name, RequiredMotors);
printf("\n\r==============================");
}

/branches/dongfang_FC_rewrite/eeprom.h
3,33 → 3,53
#include <inttypes.h>
#include "configuration.h"
#include "analog.h"
#define EEPROM_ADR_PARAM_BEGIN 0
#define PID_PARAM_REVISION 1 // byte
#define PID_ACTIVE_SET 2 // byte
#define PID_PRESSURE_OFFSET 3 // byte
#define PID_ACC_PITCH 4 // word
#define PID_ACC_ROLL 6 // word
#define PID_ACC_Z 8 // word
#define EEPROM_ADR_CHANNELS 80 // 8 bytes
#define EEPROM_ADR_PARAMSET_LENGTH 98 // word
//#define PID_ACC_PITCH 4 // word
//#define PID_ACC_ROLL 6 // word
//#define PID_ACC_Z 8 // word
#define EEPROM_ADR_ACCOFFSET 20
#define EEPROM_ADR_GYROOFFSET 30
#define EEPROM_ADR_CHANNELMAP 80
#define EEPROM_ADR_PARAMSET_BEGIN 100
#define EEPROM_ADR_MIXER_TABLE 1000 // 1000 - 1076
#define EEPARAM_REVISION 75 // is count up, if paramater stucture has changed (compatibility)
#define EEMIXER_REVISION 1 // is count up, if Mixer stucture has changed (compatibility)
extern void ParamSet_Init(void);
extern void ParamSet_ReadFromEEProm(uint8_t setnumber);
extern void ParamSet_WriteToEEProm(uint8_t setnumber);
#define CHANNELMAP_REVISION 0
#define EEPARAM_REVISION 0
#define EEMIXER_REVISION 0
#define SENSOROFFSET_REVISION 0
extern uint8_t MixerTable_ReadFromEEProm(void);
extern uint8_t MixerTable_WriteToEEProm(void);
extern void paramSet_readOrDefault(void);
extern void channelMap_readOrDefault(void);
extern void mixerMatrix_readOrDefault(void);
extern uint8_t GetParamByte(uint16_t param_id);
extern void SetParamByte(uint16_t param_id, uint8_t value);
extern uint16_t GetParamWord(uint16_t param_id);
extern void SetParamWord(uint16_t param_id, uint16_t value);
extern uint8_t paramSet_readFromEEProm(uint8_t setnumber);
extern void paramSet_writeToEEProm(uint8_t setnumber);
extern uint8_t channelMap_readFromEEProm(void);
extern void channelMap_writeToEEProm(void);
extern uint8_t mixerMatrix_eeadFromEEProm(void);
extern void mixerMatrix_writeToEEProm(void);
extern uint8_t accOffset_readFromEEProm(void);
extern void accOffset_writeToEEProm(void);
extern uint8_t gyroOffset_readFromEEProm(void);
extern void gyroOffset_writeToEEProm(void);
extern uint8_t getParamByte(uint16_t param_id);
extern void setParamByte(uint16_t param_id, uint8_t value);
extern uint16_t getParamWord(uint16_t param_id);
extern void setParamWord(uint16_t param_id, uint16_t value);
extern uint8_t getActiveParamSet(void);
extern void setActiveParamSet(uint8_t setnumber);

/branches/dongfang_FC_rewrite/externalControl.c
40,8 → 40,8
void EC_update() {
if (externalControlActive) {
externalControlActive--;
EC_PRTY[CONTROL_PITCH] = 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 {
58,7 → 58,7
// Configured and heard from
return SIGNAL_OK;
if (!(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128))
if (!(externalControl.config & 0x01 && dynamicParams.externalControl > 128))
// External control is not even configured.
return NO_SIGNAL;

/branches/dongfang_FC_rewrite/flight.c
61,10 → 61,6
// Necessary for external control and motor test
#include "uart0.h"
// for scope debugging
// #include "rc.h"
#include "twimaster.h"
#include "attitude.h"
#include "controlMixer.h"
86,13 → 82,12
// Some integral weight constant...
uint16_t Ki = 10300 / 33;
uint8_t RequiredMotors = 0;
/************************************************************************/
/* Filter for motor value smoothing (necessary???) */
/************************************************************************/
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
switch (dynamicParams.UserParams[5]) {
switch (dynamicParams.motorSmoothing) {
case 0:
return newvalue;
case 1:
118,9 → 113,11
void flight_setNeutral() {
MKFlags |= MKFLAG_CALIBRATE;
// not really used here any more.
/*
dynamicParams.KalmanK = -1;
dynamicParams.KalmanMaxDrift = 0;
dynamicParams.KalmanMaxFusion = 32;
*/
controlMixer_initVariables();
}
136,10 → 133,10
void setNormalFlightParameters(void) {
setFlightParameters(
dynamicParams.IFactor,
dynamicParams.GyroP,
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
dynamicParams.GyroP,
dynamicParams.UserParams[6]
dynamicParams.gyroP,
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
dynamicParams.gyroP,
dynamicParams.yawIFactor
);
}
179,10 → 176,10
throttleTerm = controls[CONTROL_THROTTLE];
// 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 */
199,8 → 196,8
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
throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle
MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing
setStableFlightParameters();
} else {
MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.
207,15 → 204,15
}
} else {
// end emergency flight (just cut the motors???)
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING);
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT);
}
} else {
// signal is acceptable
if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
// Reset emergency landing control variables.
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing
// The time is in whole seconds.
emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488;
emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488;
}
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
229,51 → 226,37
* 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;
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);
}
} 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();
// if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
setNormalFlightParameters();
// }
} // end else (not bad signal case)
// end part1a: 750-800 usec.
/*
* Looping the H&I way basically is just a matter of turning off attitude angle measurement
* 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;
}
/************************************************************************/
/* Yawing */
/************************************************************************/
if (abs(controls[CONTROL_YAW]) > 4 * staticParams.StickYawP) { // yaw stick is activated
if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated
ignoreCompassTimer = 1000;
if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) {
updateCompassCourse = 1;
}
}
// yawControlRate = controlYaw;
// yawControlRate = controlYaw;
// Trim drift of yawAngleDiff with controlYaw.
// TODO: We want NO feedback of control related stuff to the attitude related stuff.
// This seems to be used as: Difference desired <--> real heading.
285,7 → 268,7
/************************************************************************/
/* Compass is currently not supported. */
/************************************************************************/
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
updateCompass();
}
310,21 → 293,16
// 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)) {
PPart[axis] = 0;
} else { // TODO: Where do the 44000 come from???
PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
}
PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
/*
* Now blend in the D-part - proportional to the Differential of the integral = the rate.
* 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);
}
343,7 → 321,7
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
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
throttleTerm = staticParams.minThrottle; // reduce gas to min to avoid lift of
}
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
381,7 → 359,7
}
// FIXME: Throttle may exceed maxThrottle (there is no check no more).
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING;
tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
if (yawTerm < -(tmp_int - throttleTerm)) {
yawTerm = -(tmp_int - throttleTerm);
debugOut.digital[0] |= DEBUG_CLIP;
407,7 → 385,7
IPart[axis] += PDPart[axis] - controls[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
tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability
* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
// TODO: From which planet comes the 16000?
443,10 → 421,10
int32_t tmp;
uint8_t throttle;
tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE];
tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH];
tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL];
tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW];
tmp = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW];
tmp = tmp >> 6;
motorFilters[i] = motorFilter(tmp, motorFilters[i]);
// Now we scale back down to a 0..255 range.
464,7 → 442,7
if (i < 4) debugOut.analog[22 + i] = throttle;
if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) {
if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
motor[i].SetPoint = throttle;
} else if (motorTestActive) {
motor[i].SetPoint = motorTest[i];
486,6 → 464,6
debugOut.analog[16] = gyroPFactor;
debugOut.analog[17] = gyroIFactor;
debugOut.analog[18] = dynamicParams.GyroD;
debugOut.analog[18] = dynamicParams.gyroD;
}
}

/branches/dongfang_FC_rewrite/flight.h
22,8 → 22,6
* same, they are unseperable anyway.
*/
extern uint8_t RequiredMotors;
// looping params
// extern long TurnOver180Nick, TurnOver180Roll;

/branches/dongfang_FC_rewrite/heightControl.c
46,60 → 46,60
*/
int32_t getHeight(void) {
return groundPressure - filteredAirPressure;
return groundPressure - filteredAirPressure;
}
void HC_setGround(void) {
groundPressure = filteredAirPressure;
// This should also happen when height control is enabled in-flight.
rampedTargetHeight = getHeight();
maxHeight = 0;
iHeight = 0;
groundPressure = filteredAirPressure;
// This should also happen when height control is enabled in-flight.
rampedTargetHeight = getHeight();
maxHeight = 0;
iHeight = 0;
}
void HC_update(void) {
int32_t height = getHeight();
static uint8_t setHeightLatch = 0;
if (height > maxHeight)
maxHeight = height;
if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
// If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
// DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40) {
// Switch is ON
if (setHeightLatch <= LATCH_TIME) {
if (setHeightLatch == LATCH_TIME) {
// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
targetHeight = height;
// DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
}
// Time not yet reached.
setHeightLatch++;
}
} else {
// Switch is OFF.
setHeightLatch = 0;
// DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
}
} else {
// Switch is not activated; take the "max-height" as the target height.
// DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
targetHeight = (uint16_t) dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100;
int32_t height = getHeight();
static uint8_t setHeightLatch = 0;
if (height > maxHeight)
maxHeight = height;
if (staticParams.bitConfig & CFG_HEIGHT_SWITCH) {
// If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
// DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
// Switch is ON
if (setHeightLatch <= LATCH_TIME) {
if (setHeightLatch == LATCH_TIME) {
// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
targetHeight = height;
// DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
}
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
heightRampingTimer = 0;
if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) {
rampedTargetHeight += staticParams.Height_Gain;
} else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) {
rampedTargetHeight -= staticParams.Height_Gain;
}
}
// height, in meters (so the division factor is: 100)
debugOut.analog[30] = height / 10;
// Time not yet reached.
setHeightLatch++;
}
} else {
// Switch is OFF.
setHeightLatch = 0;
// DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
}
} else {
// Switch is not activated; take the "max-height" as the target height.
// DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
targetHeight = (uint16_t) dynamicParams.heightSetting * 100; //getHeight() + 10 * 100;
}
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
heightRampingTimer = 0;
if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) {
rampedTargetHeight += staticParams.heightSlewRate;
} else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) {
rampedTargetHeight -= staticParams.heightSlewRate;
}
}
// height, in meters (so the division factor is: 100)
debugOut.analog[30] = height / 10;
}
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
109,73 → 109,73
// takes 180-200 usec (with integral term). That is too heavy!!!
// takes 100 usec without integral term.
uint16_t HC_getThrottle(uint16_t throttle) {
int32_t height = getHeight();
int32_t heightError = rampedTargetHeight - height;
static int32_t lastHeight;
int16_t dHeight = height - lastHeight;
lastHeight = height;
// DebugOut.Analog[20] = dHeight;
// DebugOut.Analog[21] = dynamicParams.MaxHeight;
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
// iHeight += heightError;
if (dHeight > 0) {
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
} else if (dHeight < 0) {
debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
}
/*
if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
*/
int16_t dThrottle = heightError * staticParams.HeightP / 1000
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
* staticParams.HeightD;
// the "minGas" is now a limit for how much up / down the throttle can be varied
if (dThrottle > staticParams.HeightMinGas)
dThrottle = staticParams.HeightMinGas;
else if (dThrottle < -staticParams.HeightMinGas)
dThrottle = -staticParams.HeightMinGas;
int32_t height = getHeight();
int32_t heightError = rampedTargetHeight - height;
static int32_t lastHeight;
int16_t dHeight = height - lastHeight;
lastHeight = height;
// DebugOut.Analog[20] = dHeight;
// DebugOut.Analog[21] = dynamicParams.MaxHeight;
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
// iHeight += heightError;
if (dHeight > 0) {
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
} else if (dHeight < 0) {
debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
}
/*
if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
*/
int16_t dThrottle = heightError * staticParams.heightP / 1000
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
* staticParams.heightD;
// the "minGas" is now a limit for how much up / down the throttle can be varied
if (dThrottle > staticParams.heightMaxThrottleChange)
dThrottle = staticParams.heightMaxThrottleChange;
else if (dThrottle < -staticParams.heightMaxThrottleChange)
dThrottle = -staticParams.heightMaxThrottleChange;
// TODO: Eliminate repetition.
if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH)
|| (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) {
// If switch is not in use --> Just apply height control.
// If switch is in use --> only apply height control when switch is also ON.
throttle += dThrottle;
}
}
/* Experiment: Find hover-throttle */
stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
* (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
- 1) + throttle) / HOVERTHROTTLEFILTER;
if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
&& stronglyFilteredHeightDiff > -3) {
hoverThrottle = stronglyFilteredThrottle;
debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
// DebugOut.Analog[18] = hoverThrottle;
} else
debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
debugOut.analog[20] = dThrottle;
debugOut.analog[21] = hoverThrottle;
return throttle;
if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) {
if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH)
|| (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
// If switch is not in use --> Just apply height control.
// If switch is in use --> only apply height control when switch is also ON.
throttle += dThrottle;
}
}
/* Experiment: Find hover-throttle */
stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
* (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
- 1) + throttle) / HOVERTHROTTLEFILTER;
if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
&& stronglyFilteredHeightDiff > -3) {
hoverThrottle = stronglyFilteredThrottle;
debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
// DebugOut.Analog[18] = hoverThrottle;
} else
debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
debugOut.analog[20] = dThrottle;
debugOut.analog[21] = hoverThrottle;
return throttle;
}
/*

/branches/dongfang_FC_rewrite/invenSense.c
33,8 → 33,8
}
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.GyroAccFactor = 1;
staticParams.DriftComp = 3;
staticParams.GyroAccTrim = 2;
staticParams.gyroD = 3;
staticParams.driftCompDivider = 2;
staticParams.driftCompLimit = 3;
staticParams.zerothOrderCorrection = 2;
}

/branches/dongfang_FC_rewrite/main.c
85,7 → 85,7
// analyze hardware environment
CPUType = getCPUType();
BoardRelease = getBoardRelease();
boardRelease = getBoardRelease();
// disable watchdog
MCUSR &= ~(1 << WDRF);
128,7 → 128,8
printf("\n\r===================================");
// Parameter Set handling
ParamSet_Init();
channelMap_readOrDefault();
paramSet_readOrDefault();
// Wait for a short time (otherwise the RC channel check won't work below)
// timer = SetDelay(500);
160,17 → 161,6
printf("\n\r===================================");
/*
if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
{
printf("\n\rCalibrating air pressure sensor..");
timer = SetDelay(1000);
SearchAirPressureOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
*/
#ifdef USE_NAVICTRL
printf("\n\rSupport for NaviCtrl");
#ifdef USE_RC_DSL
205,7 → 195,7
beep(2000);
printf("\n\rControl: ");
if (staticParams.GlobalConfig & CFG_HEADING_HOLD)
if (staticParams.bitConfig & CFG_HEADING_HOLD)
printf("HeadingHold");
else printf("Neutral (ACC-Mode)");
248,7 → 238,7
if (UBat <= UBAT_AT_5V) {
// Do nothing. The voltage on the input side of the regulator is <5V;
// we must be running off USB power. Keep it quiet.
} else if (UBat < staticParams.LowVoltageWarning) {
} else if (UBat < staticParams.batteryVoltageWarning) {
beepBatteryAlarm();
}

/branches/dongfang_FC_rewrite/menu.c
126,9 → 126,9
case 0:// Version Info Menu Item
LCD_printfxy(0,0,"+ MikroKopter +")
;
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",BoardRelease/10,BoardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a')
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",boardRelease/10,boardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a')
;
LCD_printfxy(0,2,"Setting: %d %s", getActiveParamSet(), Mixer.Name)
LCD_printfxy(0,2,"Setting: %d %s", getActiveParamSet(), mixerMatrix.name)
;
if (I2CTimeout < 6) {
LCD_printfxy(0,3,"I2C Error!!!");
164,10 → 164,10
LCD_printfxy(0,3,"C7:%4i C8:%4i ",PPM_in[7],PPM_in[8]);
break;
case 4:// Remote Control Mapping Menu Item
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[staticParams.ChannelAssignment[CH_PITCH]],PPM_in[staticParams.ChannelAssignment[CH_ROLL]]);
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]],PPM_in[staticParams.ChannelAssignment[CH_YAW]]);
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[staticParams.ChannelAssignment[CH_POTS]],PPM_in[staticParams.ChannelAssignment[CH_POTS+1]]);
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[staticParams.ChannelAssignment[CH_POTS+2]],PPM_in[staticParams.ChannelAssignment[CH_POTS+3]]);
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[channelMap.channels[CH_PITCH]], PPM_in[channelMap.channels[CH_ROLL]]);
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[channelMap.channels[CH_THROTTLE]],PPM_in[channelMap.channels[CH_YAW]]);
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[channelMap.channels[CH_POTS]], PPM_in[channelMap.channels[CH_POTS+1]]);
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[channelMap.channels[CH_POTS+2]], PPM_in[channelMap.channels[CH_POTS+3]]);
break;
/*
case 5:// Gyro Sensor Menu Item

/branches/dongfang_FC_rewrite/output.c
69,10 → 69,10
void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask,
uint8_t manual) {
if (timing > 250 && manual > 230) {
// "timing" is set to "manual" and the value is very high --> Set to the value in bitmask bit 7.
// "timing" is set to "manual (a variable)" and the value is very high --> Set to the value in bitmask bit 7.
OUTPUT_SET(port, bitmask & 128);
} else if (timing > 250 && manual < 10) {
// "timing" is set to "manual" and the value is very low --> Set to the negated value in bitmask bit 7.
// "timing" is set to "manual" (a variable) and the value is very low --> Set to the negated value in bitmask bit 7.
OUTPUT_SET(port, !(bitmask & 128));
} else if (!flashCnt[port]--) {
// rotating mask over bitmask...
89,37 → 89,29
static int8_t delay = 0;
if (!delay--) { // 10 ms intervals
delay = 4;
if (beepModulation != BEEP_MODULATION_NONE) {
if (staticParams.outputFlags & 4 && beepModulation != BEEP_MODULATION_NONE) {
// alarm
flashingLight(0, 25, 0x55, 25);
flashingLight(1, 25, 0xAA, 25);
} else {
flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask,
dynamicParams.J16Timing);
flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask,
dynamicParams.J17Timing);
flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing);
flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing);
}
}
}
void output_update(void) {
if (!DIGITAL_DEBUG_MASK) {
flashingLights();
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_ON) {
if (staticParams.outputFlags & 1) {
OUTPUT_SET(0, 1);
OUTPUT_SET(1, 1);
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_OFF) {
} else if (staticParams.outputFlags & 2) {
OUTPUT_SET(0, 0);
OUTPUT_SET(1, 0);
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_0) {
OUTPUT_SET(0, 1);
OUTPUT_SET(1, 0);
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_1) {
OUTPUT_SET(0, 0);
OUTPUT_SET(1, 1);
} else if (!staticParams.outputDebugMask) {
flashingLights();
} else {
OUTPUT_SET(0, debugOut.digital[0] & DIGITAL_DEBUG_MASK);
OUTPUT_SET(1, debugOut.digital[1] & DIGITAL_DEBUG_MASK);
OUTPUT_SET(0, debugOut.digital[0] & staticParams.outputDebugMask);
OUTPUT_SET(1, debugOut.digital[1] & staticParams.outputDebugMask);
}
}

/branches/dongfang_FC_rewrite/output.h
70,7 → 70,7
* Set to 0 for using outputs as the usual flashing lights.
* Set to one of the DEBUG_... defines h for using the outputs as debug lights.
*/
#define DIGITAL_DEBUG_MASK DEBUG_ACC0THORDER
#define DIGITAL_DEBUG_MASK 0
void output_init(void);
void output_update(void);

/branches/dongfang_FC_rewrite/rc.c
214,8 → 214,8
}
}
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
#define COMMAND_THRESHOLD 85
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
249,19 → 249,19
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_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;
* staticParams.stickThrottleD + 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))
tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1))
/ 512L; // expo y = ax + bx^2
tmp2 += (staticParams.StickYawP * tmp1) >> 2;
tmp2 += (staticParams.stickYawP * tmp1) >> 2;
RC_PRTY[CONTROL_YAW] = tmp2;
}
uint8_t command = RC_getStickCommand();
392,6 → 392,7
}
}
/*
uint8_t RC_getLooping(uint8_t looping) {
// static uint8_t looping = 0;
429,6 → 430,7
return looping;
}
*/
uint8_t RC_testCompassCalState(void) {
static uint8_t stick = 1;

/branches/dongfang_FC_rewrite/rc.h
2,9 → 2,8
#define _RC_H
#include <inttypes.h>
#include "configuration.h"
#define MAX_CHANNELS 10
// Number of cycles a command must be repeated before commit.
#define COMMAND_TIMER 200

/branches/dongfang_FC_rewrite/timer0.c
86,7 → 86,7
// Configure speaker port as output.
if (BoardRelease == 10) { // Speaker at PD2
if (boardRelease == 10) { // Speaker at PD2
DDRD |= (1 << DDD2);
PORTD &= ~(1 << PORTD2);
} else { // Speaker at PC7
160,7 → 160,8
if (beepTime) {
beepTime--; // decrement BeepTime
if (beepTime & beepModulation)
beeper_On = 1;
// beeper_On = 1;
beeper_On = 0; // shut up in dev!!!
else
beeper_On = 0;
} else { // beeper off if duration is over
171,13 → 172,13
// if beeper is on
if (beeper_On) {
// set speaker port to high.
if (BoardRelease == 10)
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)
if (boardRelease == 10)
PORTD &= ~(1 << PORTD2);// Speaker at PD2
else
PORTC &= ~(1 << PORTC7);// Speaker at PC7
185,7 → 186,7
#ifndef USE_NAVICTRL
// update compass value if this option is enabled in the settings
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
#ifdef USE_MK3MAG
MK3MAG_Update(); // read out mk3mag pwm
#endif
216,7 → 217,7
t_stop = setDelay(w);
while (!checkDelay(t_stop)) {
if (analogDataReady) {
analogDataReady = 0;
analog_update();
startAnalogConversionCycle();
}
}

/branches/dongfang_FC_rewrite/twimaster.c
198,8 → 198,7
// Master Transmit
case 0: // TWI_STATE_MOTOR_TX
// skip motor if not used in mixer
while ((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write
< MAX_MOTORS))
while ((mixerMatrix.motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS))
motor_write++;
if (motor_write >= MAX_MOTORS) { // writing finished, read now
motor_write = 0;
312,7 → 311,7
}
for (i = 0; i < MAX_MOTORS; i++) {
if (!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0)
if (!motor[i].Present && mixerMatrix.motor[i][MIX_THROTTLE] > 0)
printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1);
motor[i].Error = 0;
}

/branches/dongfang_FC_rewrite/twimaster.h
2,6 → 2,7
#define _I2C_MASTER_H
#include <inttypes.h>
#include "configuration.h"
#define TWI_STATE_MOTOR_TX 0
#define TWI_STATE_MOTOR_RX 3
21,7 → 22,6
uint8_t MaxPWM; // read back from BL
}__attribute__((packed)) MotorData_t;
#define MAX_MOTORS 12
extern MotorData_t motor[MAX_MOTORS];
extern volatile uint16_t I2CTimeout;

/branches/dongfang_FC_rewrite/uart0.c
129,8 → 129,8
"GyroPitch(PID) ",
"GyroRoll(PID) ",
"GyroYaw ", //5
"AccPitch (raw) ",
"AccRoll (raw) ",
"OffsPitch ",
"OffsRoll ",
"AttitudeControl ",
"AccPitch (angle)",
"AccRoll (angle) ", //10
521,13 → 521,13
case 'n':// "Get Mixer Table
while (!txd_complete)
; // wait for previous frame to be sent
SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer));
SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &mixerMatrix, sizeof(mixerMatrix));
break;
case 'm':// "Set Mixer Table
if (pRxData[0] == EEMIXER_REVISION) {
memcpy(&Mixer, (uint8_t*) pRxData, sizeof(Mixer));
MixerTable_WriteToEEProm();
memcpy(&mixerMatrix, (uint8_t*) pRxData, sizeof(mixerMatrix));
mixerMatrix_writeToEEProm();
while (!txd_complete)
; // wait for previous frame to be sent
tempchar1 = 1;
543,7 → 543,7
case 'q':// request settings
if (pRxData[0] == 0xFF) {
pRxData[0] = GetParamByte(PID_ACTIVE_SET);
pRxData[0] = getParamByte(PID_ACTIVE_SET);
}
// limit settings range
if (pRxData[0] < 1)
551,7 → 551,7
else if (pRxData[0] > 5)
pRxData[0] = 5; // limit to 5
// load requested parameter set
ParamSet_ReadFromEEProm(pRxData[0]);
paramSet_readFromEEProm(pRxData[0]);
tempchar1 = pRxData[0];
tempchar2 = EEPARAM_REVISION;
while (!txd_complete)
568,7 → 568,7
== EEPARAM_REVISION)) // check for setting to be in range and version of settings
{
memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams));
ParamSet_WriteToEEProm(pRxData[0]);
paramSet_writeToEEProm(pRxData[0]);
tempchar1 = getActiveParamSet();
beepNumber(tempchar1);
} else {
734,8 → 734,8
if((checkDelay(Compass_Timer)) && txd_complete) {
ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
ToMk3Mag.UserParam[0] = dynamicParams.userParams[0];
ToMk3Mag.UserParam[1] = dynamicParams.userParams[1];
ToMk3Mag.CalState = compassCalState;
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
// the last state is 5 and should be send only once to avoid multiple flash writing