Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1644 → Rev 1645

/branches/dongfang_FC_rewrite/flight2.c
File deleted
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
1,6 → 1,8
#include "ADXRS610_FC2.0.h"
#include "configuration.h"
 
const uint8_t GYROS_REVERSE[2] = {0,0};
 
void gyro_calibrate(void) {}
 
void gyro_setDefaults(void) {
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.h
7,7 → 7,6
*/
 
#define GYRO_HW_NAME "ADXR"
 
#define GYRO_HW_FACTOR 1.2288f
 
/*
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
5,6 → 5,7
#include "configuration.h"
#include "timer0.h"
 
const uint8_t GYROS_REVERSE[2] = {0,0};
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515
 
25,12 → 26,12
numberOfAxesInRange = 0;
if(rawPitchGyroSum < PITCHROLL_MINLIMIT) DACValues[DAC_PITCH]--;
else if(rawPitchGyroSum > PITCHROLL_MAXLIMIT) DACValues[DAC_PITCH]++;
if(rawGyroSum[PITCH] < PITCHROLL_MINLIMIT) DACValues[PITCH]--;
else if(rawGyroSum[PITCH] > PITCHROLL_MAXLIMIT) DACValues[PITCH]++;
else numberOfAxesInRange++;
if(rawRollGyroSum < PITCHROLL_MINLIMIT) DACValues[DAC_ROLL]--;
else if(rawRollGyroSum > PITCHROLL_MAXLIMIT) DACValues[DAC_ROLL]++;
if(rawGyroSum[ROLL] < PITCHROLL_MINLIMIT) DACValues[ROLL]--;
else if(rawGyroSum[ROLL] > PITCHROLL_MAXLIMIT) DACValues[ROLL]++;
else numberOfAxesInRange++;
if(rawYawGyroSum < GYRO_SUMMATION_FACTOR_YAW * 510) DACValues[DAC_YAW]--;
37,15 → 38,15
else if(rawYawGyroSum > GYRO_SUMMATION_FACTOR_YAW * 515) DACValues[DAC_YAW]++ ;
else numberOfAxesInRange++;
if(DACValues[DAC_PITCH] < 10) {
/* GyroDefectNick = 1; */ DACValues[DAC_PITCH] = 10;
} else if(DACValues[DAC_PITCH] > 245) {
/* GyroDefectNick = 1; */ DACValues[DAC_PITCH] = 245;
if(DACValues[PITCH] < 10) {
/* GyroDefectNick = 1; */ DACValues[PITCH] = 10;
} else if(DACValues[PITCH] > 245) {
/* GyroDefectNick = 1; */ DACValues[PITCH] = 245;
}
if(DACValues[DAC_ROLL] < 10) {
/* GyroDefectRoll = 1; */ DACValues[DAC_ROLL] = 10;
/* GyroDefectRoll = 1; */ DACValues[ROLL] = 10;
} else if(DACValues[DAC_ROLL] > 245) {
/* GyroDefectRoll = 1; */ DACValues[DAC_ROLL] = 245;
/* GyroDefectRoll = 1; */ DACValues[ROLL] = 245;
}
if(DACValues[DAC_YAW] < 10) {
/* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 10;
/branches/dongfang_FC_rewrite/H_and_I_AxisCoupling.txt
68,9 → 68,9
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
 
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / CONTROL_SCALING);
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / CONTROL_SCALING);
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / CONTROL_SCALING) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / CONTROL_SCALING));
 
messwertNick er nu P-part....
 
/branches/dongfang_FC_rewrite/analog.c
65,41 → 65,34
#include "eeprom.h"
 
/*
* Arrays could have been used for the 2 * 3 axes, but despite some repetition,
* the code is easier to read without.
*
* For each A/D conversion cycle, each channel (eg. the yaw gyro, or the Z axis
* accelerometer) is sampled a number of times (see array channelsForStates), and
* the results for each channel are summed. Here are those for the gyros and the
* acc. meters. They are not zero-offset.
* For each A/D conversion cycle, each analog channel is sampled a number of times
* (see array channelsForStates), and the results for each channel are summed.
* Here are those for the gyros and the acc. meters. They are not zero-offset.
* They are exported in the analog.h file - but please do not use them! The only
* reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
* the offsets with the DAC.
*/
volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum;
volatile int16_t pitchAxisAcc = 0, rollAxisAcc = 0, ZAxisAcc = 0;
volatile int16_t filteredPitchAxisAcc = 0, filteredRollAxisAcc = 0;
volatile int16_t rawGyroSum[2], rawYawGyroSum;
volatile int16_t acc[2] = {0,0}, ZAcc = 0;
volatile int16_t filteredAcc[2] = {0,0};
 
// that float one - "Top" - is missing.
 
/*
* These 4 exported variables are zero-offset. The "filtered" ones are
* (if configured to with the GYROS_SECONDORDERFILTER define) low pass
* filtered versions of the other 2.
* They are derived from the "raw" values above, by zero-offsetting.
* These 4 exported variables are zero-offset. The "PID" ones are used
* in the attitude control as rotation rates. The "ATT" ones are for
* integration to angles.
*/
volatile int16_t hiResPitchGyro = 0, hiResRollGyro = 0;
volatile int16_t filteredHiResPitchGyro = 0, filteredHiResRollGyro = 0;
volatile int16_t pitchGyroD = 0, rollGyroD = 0;
volatile int16_t gyro_PID[2];
volatile int16_t gyro_ATT[2];
volatile int16_t gyroD[2];
volatile int16_t yawGyro = 0;
 
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
* standing still. They are used for adjusting the gyro and acc. meter values
* to be zero when the copter stands still.
* to be centered on zero.
*/
volatile int16_t pitchOffset, rollOffset, yawOffset;
volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset;
volatile int16_t gyroOffset[2], yawGyroOffset;
volatile int16_t accOffset[2], ZAccOffset;
 
/*
* This allows some experimentation with the gyro filters.
110,12 → 103,14
volatile uint8_t GYROS_DFILTER;
volatile uint8_t ACC_FILTER;
 
// Air pressure (no support right now).
// volatile int32_t AirPressure = 32000;
// volatile uint8_t average_pressure = 0;
// volatile int16_t StartAirPressure;
// volatile uint16_t ReadingAirPressure = 1023;
// volatile int16_t HeightD = 0;
/*
* Air pressure measurement.
*/
#define MIN_RAWPRESSURE 200
#define MAX_RAWPRESSURE (1023-MIN_RAWPRESSURE)
volatile uint8_t rangewidth = 53;
volatile uint16_t rawAirPressure;
volatile uint16_t filteredAirPressure;
 
/*
* Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
124,8 → 119,6
*/
volatile int16_t UBat = 100;
 
volatile int16_t filteredAirPressure;
 
/*
* Control and status.
*/
135,18 → 128,18
/*
* Experiment: Measuring vibration-induced sensor noise.
*/
volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak;
volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak;
volatile uint16_t gyroNoisePeak[2];
volatile uint16_t accNoisePeak[2];
 
// ADC channels
#define AD_GYRO_YAW 0
#define AD_GYRO_ROLL 1
#define AD_GYRO_YAW 0
#define AD_GYRO_ROLL 1
#define AD_GYRO_PITCH 2
#define AD_AIRPRESSURE 3
#define AD_UBAT 4
#define AD_ACC_Z 5
#define AD_ACC_ROLL 6
#define AD_ACC_PITCH 7
#define AD_UBAT 4
#define AD_ACC_Z 5
#define AD_ACC_ROLL 6
#define AD_ACC_PITCH 7
 
/*
* Table of AD converter inputs for each state.
228,13 → 221,8
}
}
 
 
#define ADCENTER (1023/2)
#define HALFRANGE 400
uint8_t stepsize = 53;
 
uint16_t getAbsPressure(int advalue) {
return (uint16_t)OCR0A * (uint16_t)stepsize + advalue;
return (uint16_t)OCR0A * (uint16_t)rangewidth + advalue;
}
 
uint16_t filterAirPressure(uint16_t rawpressure) {
241,22 → 229,21
return rawpressure;
}
 
/*****************************************************/
/* Interrupt Service Routine for ADC */
/*****************************************************/
// Runs at 312.5 kHz or 3.2 µs
// When all states are processed the interrupt is disabled
// and the update of further AD conversions is stopped.
 
/*****************************************************
* Interrupt Service Routine for ADC
* Runs at 312.5 kHz or 3.2 µs. When all states are
* processed the interrupt is disabled and further
* AD conversions are stopped.
*****************************************************/
ISR(ADC_vect) {
static uint8_t ad_channel = AD_GYRO_PITCH, state = 0;
static uint16_t sensorInputs[8] = {0,0,0,0,0,0,0,0};
 
uint8_t i;
int16_t step = OCR0A;
static uint8_t pressure_wait = 10;
uint8_t i, axis;
int16_t range;
// for various filters...
static int16_t pitchGyroFilter, rollGyroFilter, tempOffsetGyro;
int16_t tempOffsetGyro, tempGyro;
sensorInputs[ad_channel] += ADC;
 
267,9 → 254,9
switch(state++) {
case 7: // Z acc
#ifdef ACC_REVERSE_ZAXIS
ZAxisAcc = -ZAxisAccOffset - sensorInputs[AD_ACC_Z];
ZAcc = -ZAccOffset - sensorInputs[AD_ACC_Z];
#else
ZAxisAcc = sensorInputs[AD_ACC_Z] - ZAxisAccOffset;
ZAcc = sensorInputs[AD_ACC_Z] - ZAccOffset;
#endif
break;
276,84 → 263,116
case 10: // yaw gyro
rawYawGyroSum = sensorInputs[AD_GYRO_YAW];
#ifdef GYRO_REVERSE_YAW
yawGyro = rawYawGyroSum - yawOffset;
yawGyro = rawYawGyroSum - yawGyroOffset;
#else
yawGyro = yawOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3).
yawGyro = yawGyroOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3).
#endif
break;
case 11: // pitch axis acc.
#ifdef ACC_REVERSE_PITCHAXIS
pitchAxisAcc = -pitchAxisAccOffset - sensorInputs[AD_ACC_PITCH];
acc[PITCH] = -accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
#else
pitchAxisAcc = sensorInputs[AD_ACC_PITCH] - pitchAxisAccOffset;
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
#endif
filteredPitchAxisAcc = (filteredPitchAxisAcc * (ACC_FILTER-1) + pitchAxisAcc) / ACC_FILTER;
filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER;
 
measureNoise(pitchAxisAcc, &pitchAccNoisePeak, 1);
measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);
break;
case 12: // roll axis acc.
#ifdef ACC_REVERSE_ROLLAXIS
rollAxisAcc = sensorInputs[AD_ACC_ROLL] - rollAxisAccOffset;
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
#else
rollAxisAcc = -rollAxisAccOffset - sensorInputs[AD_ACC_ROLL];
acc[ROLL] = -accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
#endif
filteredRollAxisAcc = (filteredRollAxisAcc * (ACC_FILTER-1) + rollAxisAcc) / ACC_FILTER;
measureNoise(rollAxisAcc, &rollAccNoisePeak, 1);
filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER-1) + acc[ROLL]) / ACC_FILTER;
measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);
break;
 
case 13: // air pressure
if (sensorInputs[AD_AIRPRESSURE] < ADCENTER-HALFRANGE) {
if (pressure_wait) {
// A range switch was done recently. Wait for steadying.
pressure_wait--;
break;
}
range = OCR0A;
rawAirPressure = sensorInputs[AD_AIRPRESSURE];
if (rawAirPressure < MIN_RAWPRESSURE) {
// value is too low, so decrease voltage on the op amp minus input, making the value higher.
step -= ((HALFRANGE-sensorInputs[AD_AIRPRESSURE]) / stepsize + 1);
if (step<0) step = 0;
OCR0A = step;
// wait = ... (calculate something here .. calculate at what time the R/C filter is to within one sample off)
} else if (sensorInputs[AD_AIRPRESSURE] > ADCENTER+HALFRANGE) {
range -= (MAX_RAWPRESSURE - rawAirPressure) / rangewidth - 1;
if (range < 0) range = 0;
pressure_wait = (OCR0A - range) * 4;
OCR0A = range;
} else if (rawAirPressure > MAX_RAWPRESSURE) {
// value is too high, so increase voltage on the op amp minus input, making the value lower.
step += ((sensorInputs[AD_AIRPRESSURE] - HALFRANGE)/stepsize + 1);
if (step>254) step = 254;
OCR0A = step;
// wait = ... (calculate something here .. calculate at what time the R/C filter is to within one sample off)
range += (rawAirPressure - MIN_RAWPRESSURE) / rangewidth - 1;
if (range > 254) range = 254;
pressure_wait = (range - OCR0A) * 4;
OCR0A = range;
} else {
filteredAirPressure = filterAirPressure(getAbsPressure(sensorInputs[AD_AIRPRESSURE]));
filteredAirPressure = filterAirPressure(getAbsPressure(rawAirPressure));
}
DebugOut.Analog[12] = range;
DebugOut.Analog[13] = rawAirPressure;
DebugOut.Analog[14] = filteredAirPressure;
break;
 
case 14: // pitch gyro
rawPitchGyroSum = sensorInputs[AD_GYRO_PITCH];
// Filter already before offsetting. The offsetting resolution improvement obtained by divding by
// GYROS_FIRSTORDERFILTER _after_ offsetting is too small to be worth pursuing.
pitchGyroFilter = (pitchGyroFilter * (GYROS_FIRSTORDERFILTER-1) + rawPitchGyroSum * GYRO_FACTOR_PITCHROLL) / GYROS_FIRSTORDERFILTER;
// Offset to 0.
#ifdef GYROS_REVERSE_PITCH
tempOffsetGyro = pitchOffset - pitchGyroFilter;
#else
tempOffsetGyro = pitchGyroFilter - pitchOffset;
#endif
// Calculate the delta from last shot and filter it.
pitchGyroD = (pitchGyroD * (GYROS_DFILTER-1) + (tempOffsetGyro - hiResPitchGyro)) / GYROS_DFILTER;
// How we can overwrite the last value. This value is used for the D part of the PID controller.
hiResPitchGyro = tempOffsetGyro;
// Filter a little more. This value is used in integration to angles.
filteredHiResPitchGyro = (filteredHiResPitchGyro * (GYROS_SECONDORDERFILTER-1) + hiResPitchGyro) / GYROS_SECONDORDERFILTER;
measureNoise(hiResPitchGyro, &pitchGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING);
case 14:
case 15: // pitch or roll gyro.
axis = state - 15;
tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH - axis];
// DebugOut.Analog[6 + 3 * axis ] = tempGyro;
/*
* Process the gyro data for the PID controller.
*/
// 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 (tempGyro < SENSOR_MIN_PITCHROLL) {
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
}
else if (tempGyro > SENSOR_MAX_PITCHROLL) {
tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
}
}
 
// 2) Apply sign and offset, scale before filtering.
if (GYROS_REVERSE[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
}
 
// 3) Scale and filter.
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PIDFILTER-1) + tempOffsetGyro) / GYROS_PIDFILTER;
 
// 4) Measure noise.
measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
 
// 5) Differential measurement.
gyroD[axis] = (gyroD[axis] * (GYROS_DFILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_DFILTER;
 
// 6) Done.
gyro_PID[axis] = tempOffsetGyro;
 
/*
* Now process the data for attitude angles.
*/
tempGyro = rawGyroSum[axis];
// 1) Apply sign and offset, scale before filtering.
if (GYROS_REVERSE[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
}
// 2) Filter.
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_INTEGRALFILTER-1) + tempOffsetGyro) / GYROS_INTEGRALFILTER;
break;
case 15: // Roll gyro. Works the same as pitch.
rawRollGyroSum = sensorInputs[AD_GYRO_ROLL];
rollGyroFilter = (rollGyroFilter * (GYROS_FIRSTORDERFILTER-1) + rawRollGyroSum * GYRO_FACTOR_PITCHROLL) / GYROS_FIRSTORDERFILTER;
#ifdef GYRO_REVERSE_ROLL
tempOffsetGyro = rollOffset - rollGyroFilter;
#else
tempOffsetGyro = rollGyroFilter - rollOffset;
#endif
rollGyroD = (rollGyroD * (GYROS_DFILTER-1) + (tempOffsetGyro - hiResRollGyro)) / GYROS_DFILTER;
hiResRollGyro = tempOffsetGyro;
filteredHiResRollGyro = (filteredHiResRollGyro * (GYROS_SECONDORDERFILTER-1) + hiResRollGyro) / GYROS_SECONDORDERFILTER;
measureNoise(hiResRollGyro, &rollGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING);
break;
case 16:
// battery
390,7 → 409,7
GYROS_DFILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1;
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1;
 
pitchOffset = rollOffset = yawOffset = 0;
gyroOffset[PITCH] = gyroOffset[ROLL] = yawGyroOffset = 0;
 
gyro_calibrate();
 
397,28 → 416,25
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
for(i=0; i < GYRO_OFFSET_CYCLES; i++) {
Delay_ms_Mess(10);
_pitchOffset += rawPitchGyroSum * GYRO_FACTOR_PITCHROLL;
_rollOffset += rawRollGyroSum * GYRO_FACTOR_PITCHROLL;
_pitchOffset += rawGyroSum[PITCH];
_rollOffset += rawGyroSum[ROLL];
_yawOffset += rawYawGyroSum;
}
pitchOffset = (_pitchOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
rollOffset = (_rollOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
yawOffset = (_yawOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
gyroOffset[PITCH] = (_pitchOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
gyroOffset[ROLL] = (_rollOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
yawGyroOffset = (_yawOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
filteredHiResPitchGyro = filteredHiResRollGyro = 0;
 
pitchAxisAccOffset = (int16_t)GetParamWord(PID_ACC_NICK);
rollAxisAccOffset = (int16_t)GetParamWord(PID_ACC_ROLL);
ZAxisAccOffset = (int16_t)GetParamWord(PID_ACC_TOP);
gyro_PID[PITCH] = gyro_PID[ROLL] = 0;
gyro_ATT[PITCH] = gyro_ATT[ROLL] = 0;
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
pitchGyroNoisePeak = rollGyroNoisePeak = 0;
gyroNoisePeak[PITCH] = gyroNoisePeak[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);
accOffset[PITCH] = (int16_t)GetParamWord(PID_ACC_PITCH);
accOffset[ROLL] = (int16_t)GetParamWord(PID_ACC_ROLL);
ZAccOffset = (int16_t)GetParamWord(PID_ACC_TOP);
}
 
/*
432,22 → 448,54
#define ACC_OFFSET_CYCLES 10
uint8_t i;
int32_t _pitchAxisOffset = 0, _rollAxisOffset = 0, _ZAxisOffset = 0;
// int16_t pressureDiff, savedRawAirPressure;
pitchAxisAccOffset = rollAxisAccOffset = ZAxisAccOffset = 0;
accOffset[PITCH] = accOffset[ROLL] = ZAccOffset = 0;
 
for(i=0; i < ACC_OFFSET_CYCLES; i++) {
Delay_ms_Mess(10);
_pitchAxisOffset += pitchAxisAcc;
_rollAxisOffset += rollAxisAcc;
_ZAxisOffset += ZAxisAcc;
_pitchAxisOffset += acc[PITCH];
_rollAxisOffset += acc[ROLL];
_ZAxisOffset += ZAcc;
}
 
// Save ACC neutral settings to eeprom
SetParamWord(PID_ACC_NICK, (uint16_t)((_pitchAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
SetParamWord(PID_ACC_PITCH, (uint16_t)((_pitchAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
SetParamWord(PID_ACC_ROLL, (uint16_t)((_rollAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
SetParamWord(PID_ACC_TOP, (uint16_t)((_ZAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
 
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
pitchAccNoisePeak = rollAccNoisePeak = 0;
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 - 512) / rangewidth;
// Delay_ms_Mess(500);
 
/*
pressureDiff = 0;
DebugOut.Analog[16] = rawAirPressure;
 
#define PRESSURE_CAL_CYCLE_COUNT 2
for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) {
savedRawAirPressure = rawAirPressure;
OCR0A++;
Delay_ms_Mess(200);
// raw pressure will decrease.
pressureDiff += (savedRawAirPressure - rawAirPressure);
 
savedRawAirPressure = rawAirPressure;
OCR0A--;
Delay_ms_Mess(200);
// raw pressure will increase.
pressureDiff += (rawAirPressure - savedRawAirPressure);
}
 
DebugOut.Analog[15] = rangewidth =
(pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2);
*/
}
/branches/dongfang_FC_rewrite/analog.h
2,34 → 2,33
#define _ANALOG_H
#include <inttypes.h>
 
//#include "invenSense.h"
// #include "invenSense.h"
#include "ENC-03_FC1.3.h"
 
 
/*
* How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro.
* How much low pass filtering to apply for gyro_PID.
* 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
* Temporarily replaced by userparam-configurable variable.
*/
//#define GYROS_FIRSTORDERFILTER 2
#define GYROS_PIDFILTER 1
 
/*
* How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro.
* How much low pass filtering to apply for gyro_ATT.
* 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
* Temporarily replaced by userparam-configurable variable.
*/
//#define GYROS_SECONDORDERFILTER 2
#define GYROS_INTEGRALFILTER 1
 
// Temporarily replaced by userparam-configurable variable.
//#define ACC_FILTER 4
 
/*
About setting constants right for different gyros:
About setting constants for different gyros:
Main parameters are positive directions and voltage/angular speed gain.
The "Positive direction" is the rotation direction around an axis where
the corresponding gyro gives a voltage > the no-rotation voltage.
the corresponding gyro outputs a voltage > the no-rotation voltage.
A gyro is considered, in this code, to be "forward" if its positive
direction is the same as in FC1.0/1.1/1.2/1.3, and reverse otherwise.
direction is the same as in FC1.0-1.3, and reverse otherwise.
Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
44,12 → 43,12
roll.
So:
HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
where V is 2 for FC1.0 and 4 for all others.
Assuming constant ADCValue, in the H&I code:
HiResXXXX = I * ADCValue.
gyro = I * ADCValue.
 
where I is 8 for FC1.0 and 16 for all others.
 
79,16 → 78,17
(only about half as sensitive as V1.3. But it will take about twice the
rotation rate!)
 
All together: HiResXXXX = I * H * rotation rate [units / (deg/s)].
All together: gyro = I * H * rotation rate [units / (deg/s)].
*/
 
/*
* A factor that the raw gyro values are multiplied by,
* before being zero-offset, filtered and passed to the attitude module.
* before being filtered and passed to the attitude module.
* A value of 1 would cause a little bit of loss of precision in the
* filtering (on the other hand the values are so noisy in flight that
* it will not really matter - but when testing on the desk it might be
* noticeable). 4 is fine for the default filtering.
* Experiment: Set it to 1.
*/
#define GYRO_FACTOR_PITCHROLL 4
 
111,12 → 111,12
constant is there.
 
The control loop executes every 2ms, and for each iteration
HiResXXXX is added to gyroIntegralXXXX.
Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this
explanation), we get:
gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
Assuming a constant rotation rate v and a zero initial gyroIntegral
(for this explanation), we get:
gyroIntegralXXXX =
N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR =
gyroIntegral =
N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
where N is the number of summations; N = t/2ms.
134,7 → 134,7
*/
 
/*
* The value of hiResXXXX for one deg/s = The hardware factor H * the number of samples * multiplier factor.
* The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
* Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
*/
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)
141,17 → 141,38
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)
 
/*
* Gyro saturation prevention.
*/
// How far from the end of its range a gyro is considered near-saturated.
#define SENSOR_MIN_PITCHROLL 32
// Other end of the range (calculated)
#define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
// Max. boost to add "virtually" to gyro signal at total saturation.
#define EXTRAPOLATION_LIMIT 2500
// Slope of the boost (calculated)
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
 
/*
* This value is subtracted from the gyro noise measurement in each iteration,
* making it return towards zero.
*/
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
 
#define PITCH 0
#define ROLL 1
/*
* The values that this module outputs
* These first 2 exported arrays are zero-offset. The "PID" ones are used
* in the attitude control as rotation rates. The "ATT" ones are for
* integration to angles. For the same axis, the PID and ATT variables
* generally have about the same values. There are just some differences
* in filtering, and when a gyro becomes near saturated.
* Maybe this distinction is not really necessary.
*/
extern volatile int16_t hiResPitchGyro, hiResRollGyro;
extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro;
extern volatile int16_t pitchGyroD, rollGyroD;
extern volatile int16_t gyro_PID[2];
extern volatile int16_t gyro_ATT[2];
extern volatile int16_t gyroD[2];
 
extern volatile uint16_t ADCycleCount;
extern volatile int16_t UBat;
extern volatile int16_t yawGyro;
159,25 → 180,14
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
*/
extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum;
extern volatile int16_t rawGyroSum[2], rawYawGyroSum;
 
/*
* The acceleration values that this module outputs
* The acceleration values that this module outputs. They are zero based.
*/
extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc;
extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc;
extern volatile int16_t acc[2], ZAcc;
extern volatile int16_t filteredAcc[2];
 
// Only for debugging! Not to be exported! Remove when finished.
// extern volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset;
 
// Air pressure measurement not supported right now.
// extern volatile int32_t AirPressure;
// extern volatile int16_t HeightD;
// extern volatile uint16_t ReadingAirPressure;
// extern volatile int16_t StartAirPressure;
// extern uint8_t PressureSensorOffset;
// extern int8_t ExpandBaro;
 
/*
* Flag: Interrupt handler has done all A/D conversion and processing.
*/
186,8 → 196,8
// Diagnostics: Gyro noise level because of motor vibrations. The variables
// only really reflect the noise level when the copter stands still but with
// its motors running.
extern volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak;
extern volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak;
extern volatile uint16_t gyroNoisePeak[2];
extern volatile uint16_t accNoisePeak[2];
 
// void SearchAirPressureOffset(void);
 
/branches/dongfang_FC_rewrite/attitude.c
84,11 → 84,11
* The variables are overwritten at each attitude calculation invocation - the values
* are not preserved or reused.
*/
int16_t pitchRate, rollRate, yawRate;
int16_t rate[2], yawRate;
 
// With different (less) filtering
int16_t pitchRate_PID, rollRate_PID;
int16_t pitchDifferential, rollDifferential;
int16_t rate_PID[2];
int16_t differential[2];
 
/*
* Gyro readings, after performing "axis coupling" - that is, the transfomation
99,13 → 99,13
* The variables are overwritten at each attitude calculation invocation - the values
* are not preserved or reused.
*/
int16_t ACPitchRate, ACRollRate, ACYawRate;
int16_t ACRate[2], ACYawRate;
 
/*
* Gyro integrals. These are the rotation angles of the airframe compared to the
* horizontal plane, yaw relative to yaw at start.
*/
int32_t pitchAngle, rollAngle, yawAngle;
int32_t angle[2], yawAngle;
 
int readingHeight = 0;
 
124,12 → 124,12
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L)
 
int32_t pitchCorrectionSum = 0, rollCorrectionSum = 0;
int32_t correctionSum[2] = {0,0};
 
/*
* Experiment: Compensating for dynamic-induced gyro biasing.
*/
int16_t dynamicOffsetPitch = 0, dynamicOffsetRoll = 0, dynamicOffsetYaw = 0;
int16_t dynamicOffset[2] = {0,0}, dynamicOffsetYaw = 0;
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
// int16_t dynamicCalCount;
144,21 → 144,16
* it is hardly worth the trouble.
************************************************************************/
 
int32_t getPitchAngleEstimateFromAcc(void) {
return GYRO_ACC_FACTOR * (int32_t)filteredPitchAxisAcc;
int32_t getAngleEstimateFromAcc(uint8_t axis) {
return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];
}
 
int32_t getRollAngleEstimateFromAcc(void) {
return GYRO_ACC_FACTOR * (int32_t)filteredRollAxisAcc;
}
 
void setStaticAttitudeAngles(void) {
#ifdef ATTITUDE_USE_ACC_SENSORS
pitchAngle = getPitchAngleEstimateFromAcc();
rollAngle = getRollAngleEstimateFromAcc();
angle[PITCH] = getAngleEstimateFromAcc(PITCH);
angle[ROLL] = getAngleEstimateFromAcc(ROLL);
#else
pitchAngle = 0;
rollAngle = 0;
angle[PITCH] = angle[ROLL] = 0;
#endif
}
 
169,13 → 164,13
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
 
dynamicOffsetPitch = dynamicOffsetRoll = 0;
dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0;
// Calibrate hardware.
analog_calibrate();
 
// reset gyro readings
pitchRate = rollRate = yawRate = 0;
rate[PITCH] = rate[ROLL] = yawRate = 0;
 
// reset gyro integrals to acc guessing
setStaticAttitudeAngles();
192,70 → 187,26
/************************************************************************
* Get sensor data from the analog module, and release the ADC
* TODO: Ultimately, the analog module could do this (instead of dumping
* the values into variables).
* the values into variables).
* The rate variable end up in a range of about [-1024, 1023].
* When scaled down by CONTROL_SCALING, the interval is about [-256, 256].
*************************************************************************/
void getAnalogData(void) {
// For the differential calculation. Diff. is not supported right now.
// int16_t d2Pitch, d2Roll;
pitchRate_PID = (hiResPitchGyro + dynamicOffsetPitch) / HIRES_GYRO_INTEGRATION_FACTOR;
pitchRate = (filteredHiResPitchGyro + dynamicOffsetPitch) / HIRES_GYRO_INTEGRATION_FACTOR;
pitchDifferential = pitchGyroD;
 
rollRate_PID = (hiResRollGyro + dynamicOffsetRoll) / HIRES_GYRO_INTEGRATION_FACTOR;
rollRate = (filteredHiResRollGyro + dynamicOffsetRoll) / HIRES_GYRO_INTEGRATION_FACTOR;
rollDifferential = rollGyroD;
 
uint8_t axis;
for (axis=PITCH; axis <=ROLL; axis++) {
rate_PID[axis] = (gyro_PID[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
rate[axis] = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
differential[axis] = gyroD[axis];
}
yawRate = yawGyro + dynamicOffsetYaw;
 
// We are done reading variables from the analog module. Interrupt-driven sensor reading may restart.
// We are done reading variables from the analog module.
// Interrupt-driven sensor reading may restart.
analogDataReady = 0;
analog_start();
}
 
/************************************************************************
* Axis coupling, H&I Style
* Currently not working (and there is a bug in it,
* which causes unstable flight in heading-hold mode).
************************************************************************/
void H_and_I_axisCoupling(void) {
int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0;
int16_t CouplingNickRoll = 0, CouplingRollNick = 0;
 
tmp13 = (rollRate * pitchAngle) / 2048L;
tmp13 *= dynamicParams.AxisCoupling2; // 65
tmp13 /= 4096L;
CouplingNickRoll = tmp13;
tmp14 = (pitchRate * rollAngle) / 2048L;
tmp14 *= dynamicParams.AxisCoupling2; // 65
tmp14 /= 4096L;
CouplingRollNick = tmp14;
tmp14 -= tmp13;
 
ACYawRate = yawRate + tmp14;
/*
if(!dynamicParams.AxisCouplingYawCorrection) ACYawRate = yawRate - tmp14 / 2; // force yaw
else ACYawRate
*/
tmpl = ((yawRate + tmp14) * pitchAngle) / 2048L;
tmpl *= dynamicParams.AxisCoupling1;
tmpl /= 4096L;
tmpl2 = ((yawRate + tmp14) * rollAngle) / 2048L;
tmpl2 *= dynamicParams.AxisCoupling1;
tmpl2 /= 4096L;
 
// if(abs(yawRate > 64)) {
// if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
// }
ACPitchRate = pitchRate - tmpl2 + tmpl / 100L;
ACRollRate = rollRate + tmpl - tmpl2 / 100L;
}
 
/*
* This is the standard flight-style coordinate system transformation
* (from airframe-local axes to a ground-based system). For some reason
263,49 → 214,38
* changed accordingly.
*/
void trigAxisCoupling(void) {
int16_t cospitch = int_cos(pitchAngle);
int16_t cosroll = int_cos(rollAngle);
int16_t sinroll = int_sin(rollAngle);
int16_t tanpitch = int_tan(pitchAngle);
int16_t cospitch = int_cos(angle[PITCH]);
int16_t cosroll = int_cos(angle[ROLL]);
int16_t sinroll = int_sin(angle[ROLL]);
int16_t tanpitch = int_tan(angle[PITCH]);
#define ANTIOVF 1024
ACPitchRate = ((int32_t)pitchRate * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR;
ACRollRate = rollRate + (((int32_t)pitchRate * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(rollAngle) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR));
ACYawRate = ((int32_t)pitchRate * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch;
ACRate[PITCH] = ((int32_t) rate[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR;
ACRate[ROLL] = rate[ROLL] + (((int32_t)rate[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR));
ACYawRate = ((int32_t) rate[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch;
}
 
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)) {
// The rotary rate limiter bit is abused for selecting axis coupling algorithm instead.
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER)
trigAxisCoupling();
else
H_and_I_axisCoupling();
trigAxisCoupling();
} else {
ACPitchRate = pitchRate;
ACRollRate = rollRate;
ACRate[PITCH] = rate[PITCH];
ACRate[ROLL] = rate[ROLL];
ACYawRate = yawRate;
}
 
DebugOut.Analog[3] = pitchRate;
// DebugOut.Analog[3 + 3] = ACPitchRate;
DebugOut.Analog[4] = rollRate;
// DebugOut.Analog[4 + 3] = ACRollRate;
DebugOut.Analog[5] = yawRate;
// DebugOut.Analog[5 + 3] = ACYawRate;
DebugOut.Analog[3] = ACRate[PITCH];
DebugOut.Analog[4] = ACRate[ROLL];
DebugOut.Analog[5] = ACYawRate;
 
/*
DebugOut.Analog[9] = int_cos(pitchAngle);
DebugOut.Analog[10] = int_sin(pitchAngle);
DebugOut.Analog[11] = int_tan(pitchAngle);
*/
 
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
* Limit yawGyroHeading proportional to 0 deg to 360 deg
*/
yawGyroHeading += ACYawRate;
 
// Why is yawAngle not wrapped 'round?
320,22 → 260,14
/*
* Pitch axis integration and range boundary wrap.
*/
pitchAngle += ACPitchRate;
if(pitchAngle > PITCHROLLOVER180) {
pitchAngle -= PITCHROLLOVER360;
} else if (pitchAngle <= -PITCHROLLOVER180) {
pitchAngle += PITCHROLLOVER360;
for (axis=PITCH; axis<=ROLL; axis++) {
angle[axis] += ACRate[axis];
if(angle[axis] > PITCHROLLOVER180) {
angle[axis] -= PITCHROLLOVER360;
} else if (angle[axis] <= -PITCHROLLOVER180) {
angle[axis] += PITCHROLLOVER360;
}
}
/*
* Pitch axis integration and range boundary wrap.
*/
rollAngle += ACRollRate;
if(rollAngle > PITCHROLLOVER180) {
rollAngle -= PITCHROLLOVER360;
} else if (rollAngle <= -PITCHROLLOVER180) {
rollAngle += PITCHROLLOVER360;
}
}
 
/************************************************************************
349,17 → 281,16
// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
// are less than ....., or reintroduce Kalman.
// Well actually the Z axis acc. check is not so silly.
if(!looping && //((ZAxisAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction
ZAxisAcc >= -dynamicParams.UserParams[7] && ZAxisAcc <= dynamicParams.UserParams[7]) {
uint8_t axis;
if(!looping && //((ZAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction
ZAcc >= -dynamicParams.UserParams[7] && ZAcc <= dynamicParams.UserParams[7]) {
DebugOut.Digital[0] = 1;
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
uint8_t debugFullWeight = 1;
int32_t accDerived[2];
int32_t accDerivedPitch = getPitchAngleEstimateFromAcc();
int32_t accDerivedRoll = getRollAngleEstimateFromAcc();
if((maxControlPitch > 64) || (maxControlRoll > 64)) { // reduce effect during stick commands
if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
permilleAcc /= 2;
debugFullWeight = 0;
}
372,13 → 303,14
/*
* Add to each sum: The amount by which the angle is changed just below.
*/
pitchCorrectionSum += permilleAcc * (accDerivedPitch - pitchAngle);
rollCorrectionSum += permilleAcc * (accDerivedRoll - rollAngle);
for (axis=PITCH; axis<=ROLL; axis++) {
accDerived[axis] = getAngleEstimateFromAcc(axis);
correctionSum[axis] += permilleAcc * (accDerived[axis] - angle[axis]);
// There should not be a risk of overflow here, since the integrals do not exceed a few 100000.
pitchAngle = ((int32_t)(1000 - permilleAcc) * pitchAngle + (int32_t)permilleAcc * accDerivedPitch) / 1000L;
rollAngle = ((int32_t)(1000 - permilleAcc) * rollAngle + (int32_t)permilleAcc * accDerivedRoll) / 1000L;
// There should not be a risk of overflow here, since the integrals do not exceed a few 100000.
angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived[axis]) / 1000L;
}
DebugOut.Digital[1] = debugFullWeight;
} else {
DebugOut.Digital[0] = 0;
400,20 → 332,16
void driftCompensation(void) {
static int16_t timer = DRIFTCORRECTION_TIME;
int16_t deltaCompensation;
uint8_t axis;
if (! --timer) {
timer = DRIFTCORRECTION_TIME;
deltaCompensation = ((pitchCorrectionSum + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME);
CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp);
dynamicOffsetPitch += deltaCompensation / staticParams.GyroAccTrim;
 
deltaCompensation = ((rollCorrectionSum + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME);
CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp);
dynamicOffsetRoll += deltaCompensation / staticParams.GyroAccTrim;
 
pitchCorrectionSum = rollCorrectionSum = 0;
 
DebugOut.Analog[28] = dynamicOffsetPitch;
DebugOut.Analog[29] = dynamicOffsetRoll;
for (axis=PITCH; axis<=ROLL; axis++) {
deltaCompensation = ((correctionSum[axis] + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME);
CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp);
dynamicOffset[axis] += deltaCompensation / staticParams.GyroAccTrim;
correctionSum[axis] = 0;
DebugOut.Analog[28 + axis] = dynamicOffset;
}
}
}
 
430,56 → 358,56
}
 
/*
void updateCompass(void) {
void updateCompass(void) {
int16_t w, v, r,correction, error;
if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) {
setCompassCalState();
setCompassCalState();
} else {
// get maximum attitude angle
w = abs(pitchAngle / 512);
v = abs(rollAngle / 512);
if(v > w) w = v;
correction = w / 8 + 1;
// calculate the deviation of the yaw gyro heading and the compass heading
if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180;
if(abs(yawRate) > 128) { // spinning fast
error = 0;
}
if(!badCompassHeading && w < 25) {
if(updateCompassCourse) {
beep(200);
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
compassCourse = (int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
updateCompassCourse = 0;
}
}
yawGyroHeading += (error * 8) / correction;
w = (w * dynamicParams.CompassYawEffect) / 32;
w = dynamicParams.CompassYawEffect - w;
if(w >= 0) {
if(!badCompassHeading) {
v = 64 + (maxControlPitch + maxControlRoll) / 8;
// calc course deviation
r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180;
v = (r * w) / v; // align to compass course
// limit yaw rate
w = 3 * dynamicParams.CompassYawEffect;
if (v > w) v = w;
else if (v < -w) v = -w;
yawAngle += v;
}
else
{ // wait a while
badCompassHeading--;
}
}
else { // ignore compass at extreme attitudes for a while
badCompassHeading = 500;
}
// get maximum attitude angle
w = abs(pitchAngle / 512);
v = abs(rollAngle / 512);
if(v > w) w = v;
correction = w / 8 + 1;
// calculate the deviation of the yaw gyro heading and the compass heading
if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180;
if(abs(yawRate) > 128) { // spinning fast
error = 0;
}
}
if(!badCompassHeading && w < 25) {
if(updateCompassCourse) {
beep(200);
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
compassCourse = (int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
updateCompassCourse = 0;
}
}
yawGyroHeading += (error * 8) / correction;
w = (w * dynamicParams.CompassYawEffect) / 32;
w = dynamicParams.CompassYawEffect - w;
if(w >= 0) {
if(!badCompassHeading) {
v = 64 + (maxControlPitch + maxControlRoll) / 8;
// calc course deviation
r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180;
v = (r * w) / v; // align to compass course
// limit yaw rate
w = 3 * dynamicParams.CompassYawEffect;
if (v > w) v = w;
else if (v < -w) v = -w;
yawAngle += v;
}
else
{ // wait a while
badCompassHeading--;
}
}
else { // ignore compass at extreme attitudes for a while
badCompassHeading = 500;
}
}
}
*/
 
/*
491,12 → 419,12
* speed unfortunately... must find a better way)
*/
/*
void attitude_startDynamicCalibration(void) {
void attitude_startDynamicCalibration(void) {
dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0;
savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000;
}
}
 
void attitude_continueDynamicCalibration(void) {
void attitude_continueDynamicCalibration(void) {
// measure dynamic offset now...
dynamicCalPitch += hiResPitchGyro;
dynamicCalRoll += hiResRollGyro;
505,18 → 433,18
// Param6: Manual mode. The offsets are taken from Param7 and Param8.
if (dynamicParams.UserParam6 || 1) { // currently always enabled.
// manual mode
dynamicOffsetPitch = dynamicParams.UserParam7 - 128;
dynamicOffsetRoll = dynamicParams.UserParam8 - 128;
// manual mode
dynamicOffsetPitch = dynamicParams.UserParam7 - 128;
dynamicOffsetRoll = dynamicParams.UserParam8 - 128;
} else {
// use the sampled value (does not seem to work so well....)
dynamicOffsetPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount;
dynamicOffsetRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount;
dynamicOffsetYaw = -dynamicCalYaw / dynamicCalCount;
// use the sampled value (does not seem to work so well....)
dynamicOffsetPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount;
dynamicOffsetRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount;
dynamicOffsetYaw = -dynamicCalYaw / dynamicCalCount;
}
// keep resetting these meanwhile, to avoid accumulating errors.
setStaticAttitudeIntegrals();
yawAngle = 0;
}
}
*/
/branches/dongfang_FC_rewrite/attitude.h
1,6 → 1,6
/*#######################################################################################
Attitude sense system (processing of gyro, accelerometer and altimeter data)
#######################################################################################*/
/*********************************************************************************/
/* Attitude sense system (processing of gyro, accelerometer and altimeter data) */
/*********************************************************************************/
 
#ifndef _ATTITUDE_H
#define _ATTITUDE_H
33,7 → 33,7
* Gyro readings are divided by this before being used in attitude control. This will scale them
* to match the scale of the stick control etc. variables. This is just a rough non-precision
* scaling - the below definitions make precise conversion factors.
* Will be about 4 for InvenSense, 8 for FC1.3 and 13 for ADXRS610 (hmmm - originally is was only 8???)
* Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610.
* The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees.
*/
#define HIRES_GYRO_INTEGRATION_FACTOR (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
81,13 → 81,13
/*
* Rotation rates
*/
extern int16_t pitchRate_PID, rollRate_PID, yawRate;
extern int16_t pitchDifferential, rollDifferential;
extern int16_t rate_PID[2], yawRate;
extern int16_t differential[2];
 
/*
* Attitudes calcualted by numerical integration of gyro rates
* Attitudes calculated by numerical integration of gyro rates
*/
extern int32_t pitchAngle, rollAngle, yawAngle;
extern int32_t angle[2], yawAngle;
 
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c
 
104,12 → 104,6
extern uint16_t badCompassHeading;
 
/*
* Height control
*/
extern int readingHeight;
extern int setPointHeight;
 
/*
* Interval wrap-over values for attitude integrals
*/
extern long turnOver180Pitch, turnOver180Roll;
127,8 → 121,7
* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that
* - Invent your own...
*/
extern int16_t dynamicOffsetPitch, dynamicOffsetRoll, dynamicOffsetYaw;
// extern int16_t savedDynamicOffsetPitch, savedDynamicOffsetRoll;
extern int16_t dynamicOffset[2], dynamicOffsetYaw;
 
/*
* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).
/branches/dongfang_FC_rewrite/control.c
259,14 → 259,14
if(stickThrottle < 0) stickThrottle = 0;
 
if(abs(stickPitch / STICK_GAIN) > maxStickPitch) {
maxStickPitch = abs(stickPitch) / STICK_GAIN;
if(abs(stickPitch / CONTROL_SCALING) > maxStickPitch) {
maxStickPitch = abs(stickPitch) / CONTROL_SCALING;
if(maxStickPitch > 100) maxStickPitch = 100;
}
else if (maxStickPitch) maxStickPitch--;
 
if(abs(stickRoll / STICK_GAIN) > maxStickRoll) {
maxStickRoll = abs(stickRoll) / STICK_GAIN;
if(abs(stickRoll / CONTROL_SCALING) > maxStickRoll) {
maxStickRoll = abs(stickRoll) / CONTROL_SCALING;
if(maxStickRoll > 100) maxStickRoll = 100;
}
else if (maxStickRoll) maxStickRoll--;
/branches/dongfang_FC_rewrite/control.h
1,6 → 1,6
/*#######################################################################################
Stick control interface
#######################################################################################*/
/*********************************************************************************/
/* Stick control interface */
/*********************************************************************************/
/*
OBSOLETED BY controlMixer.h. But this is how it looked - maybe somebody will find it simpler?
 
8,13 → 8,13
#define _CONTROL_H
 
#include <inttypes.h>
#define STICK_GAIN 4
#define CONTROL_SCALING 4
 
// defines for lookup staticParams.ChannelAssignment
#define CH_PITCH 0
#define CH_ROLL 1
#define CH_THROTTLE 2
#define CH_YAW 3
#define CH_ROLL 1
#define CH_THROTTLE 2
#define CH_YAW 3
#define CH_POTS 4
#define POT_OFFSET 110
 
/branches/dongfang_FC_rewrite/controlMixer.c
63,8 → 63,8
*/
#define COMMAND_TIMER 200
 
uint16_t maxControlPitch = 0, maxControlRoll = 0;
int16_t controlPitch = 0, controlRoll = 0, controlYaw = 0, controlThrottle = 0;
uint16_t maxControl[2] = {0,0};
int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0;
uint8_t looping = 0;
 
// Internal variables for reading commands made with an R/C stick.
128,8 → 128,6
uint8_t controlMixer_getSignalQuality(void) {
uint8_t rcQ = RC_getSignalQuality();
uint8_t ecQ = EC_getSignalQuality();
DebugOut.Analog[16] = rcQ;
DebugOut.Analog[17] = ecQ;
// This needs not be the only correct solution...
return rcQ > ecQ ? rcQ : ecQ;
}
140,6 → 138,7
void controlMixer_update(void) {
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
// TODO: If no signal --> zero.
uint8_t axis;
RC_update();
EC_update();
 
146,15 → 145,11
int16_t* RC_PRTY = RC_getPRTY();
int16_t* EC_PRTY = EC_getPRTY();
 
controlPitch = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
controlRoll = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];
controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
 
DebugOut.Analog[6] = controlPitch;
DebugOut.Analog[7] = controlRoll;
DebugOut.Analog[8] = controlYaw;
if (RC_getSignalQuality() >= SIGNAL_GOOD) {
controlMixer_updateVariables();
configuration_applyVariablesToParams();
178,15 → 173,12
/*
* Record maxima
*/
if(abs(controlPitch / STICK_GAIN) > maxControlPitch) {
maxControlPitch = abs(controlPitch) / STICK_GAIN;
if(maxControlPitch > 100) maxControlPitch = 100;
} else if (maxControlPitch) maxControlPitch--;
if(abs(controlRoll / STICK_GAIN) > maxControlRoll) {
maxControlRoll = abs(controlRoll) / STICK_GAIN;
if(maxControlRoll > 100) maxControlRoll = 100;
for (axis=PITCH; axis<=ROLL; axis++) {
if(abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
if(maxControl[axis] > 100) maxControl[axis] = 100;
} else if (maxControl[axis]) maxControl[axis]--;
}
else if (maxControlRoll) maxControlRoll--;
 
// Here we could blend in other sources.
uint8_t commandNow = RC_getCommand();
/branches/dongfang_FC_rewrite/controlMixer.h
93,9 → 93,9
/*
* Our output.
*/
extern int16_t controlPitch, controlRoll, controlYaw, controlThrottle;
extern int16_t control[2], controlYaw, controlThrottle;
// extern int16_t stickOffsetNick, stickOffsetRoll;
extern uint16_t maxControlPitch, maxControlRoll;
extern uint16_t maxControl[2];
extern uint8_t looping;
 
void controlMixer_initVariables(void);
117,7 → 117,12
 
uint8_t controlMixer_getSignalQuality(void);
 
#define STICK_GAIN 4
/*
* The motor throttles can be set in an [0..256[ interval.
* The calculation of motor throttle values from sensor and control information (basically
* flight.c) take place in an [0..256*CONTROL_SCALING[ interval for better precision.
*/
#define CONTROL_SCALING 4
 
/*
* Gets the argument for the current command (a number).
/branches/dongfang_FC_rewrite/eeprom.h
8,7 → 8,7
#define PID_PARAM_REVISION 1 // byte
#define PID_ACTIVE_SET 2 // byte
#define PID_PRESSURE_OFFSET 3 // byte
#define PID_ACC_NICK 4 // word
#define PID_ACC_PITCH 4 // word
#define PID_ACC_ROLL 6 // word
#define PID_ACC_TOP 8 // word
 
/branches/dongfang_FC_rewrite/flight.c
56,8 → 56,8
#include "flight.h"
 
// Only for debug. Remove.
#include "analog.h"
#include "rc.h"
//#include "analog.h"
//#include "rc.h"
 
// Necessary for external control and motor test
#include "uart0.h"
74,7 → 74,7
* These are no longer maintained, just left at 0. The original implementation just summed the acc.
* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
*/
int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
 
// MK flags
uint16_t isFlying = 0;
128,9 → 128,6
dynamicParams.KalmanMaxFusion = 32;
 
controlMixer_initVariables();
 
// TODO: Move off.
// RC_Quality = 100;
}
 
/************************************************************************/
150,10 → 147,10
}
/*
DebugOut.Analog[12] = Motor[0].SetPoint; // Front
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
DebugOut.Analog[14] = Motor[3].SetPoint; // Left
DebugOut.Analog[15] = Motor[2].SetPoint; // Right
DebugOut.Analog[] = Motor[0].SetPoint; // Front
DebugOut.Analog[] = Motor[1].SetPoint; // Rear
DebugOut.Analog[] = Motor[3].SetPoint; // Left
DebugOut.Analog[] = Motor[2].SetPoint; // Right
*/
// Start I2C Interrupt Mode
I2C_Start(TWI_STATE_MOTOR_TX);
248,12 → 245,11
void flight_control(void) {
int16_t tmp_int;
// Mixer Fractions that are combined for Motor Control
int16_t yawTerm, throttleTerm, pitchTerm, rollTerm;
int16_t yawTerm, throttleTerm, term[2];
 
// PID controller variables
int16_t PDPartPitch, PDPartRoll, PDPartYaw, PPartPitch, PPartRoll;
static int32_t IPartPitch = 0, IPartRoll = 0;
 
int16_t PDPart[2], PDPartYaw, PPart[2];
static int32_t IPart[2] = {0,0};
static int32_t setPointYaw = 0;
 
// Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.
261,17 → 257,12
// static int32_t CorrectionPitch, CorrectionRoll;
 
static uint16_t emergencyFlightTime;
 
// No support for altitude control right now.
// static uint8_t HeightControlActive = 0;
// static int16_t HeightControlGas = 0;
 
static int8_t debugDataTimer = 1;
 
// High resolution motor values for smoothing of PID motor outputs
static int16_t motorFilters[MAX_MOTORS];
 
uint8_t i;
uint8_t i, axis;
 
// Fire the main flight attitude calculation, including integration of angles.
calculateFlightAttitude();
328,23 → 319,21
* 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.
* TODO: What was the value of IPartPitch? At 1st run of this, it's 0 already.
*/
if(isFlying < 256) {
IPartPitch = 0;
IPartRoll = 0;
// TODO: Don't stomp on other modules' variables!!!
controlYaw = 0;
if(isFlying == 250) {
updateCompassCourse = 1;
yawAngle = 0;
setPointYaw = 0;
}
IPart[PITCH] = IPart[ROLL] = 0;
// TODO: Don't stomp on other modules' variables!!!
controlYaw = 0;
if(isFlying == 250) {
updateCompassCourse = 1;
yawAngle = 0;
setPointYaw = 0;
}
} else {
// DebugOut.Digital[1] = 0;
// 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);
// DebugOut.Digital[1] = 0;
// 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);
}
/*
399,9 → 388,6
*/
 
#if defined (USE_MK3MAG)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/************************************************************************/
/* GPS is currently not supported. */
/************************************************************************/
416,6 → 402,8
}
*/
#endif
 
#define SENSOR_LIMIT (4096 * 4)
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
422,34 → 410,28
/* and angular velocity (gyro signal) */
/************************************************************************/
// The P-part is the P of the PID controller. That's the angle integrals (not rates).
if(looping & LOOPING_PITCH_AXIS) {
PPartPitch = 0;
} else { // TODO: Where do the 44000 come from???
PPartPitch = pitchAngle * gyroIFactor / (44000 / STICK_GAIN); // P-Part - Proportional to Integral
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
}
 
/*
* 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;
 
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
}
// Now blend in the D-part - proportional to the Differential of the integral = the rate.
PDPartPitch = PPartPitch + (int32_t)((int32_t)pitchRate_PID * gyroPFactor / (256L / STICK_GAIN))
+ (pitchDifferential * (int16_t)dynamicParams.GyroD) / 16;
PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING)
+ (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
// The P-part is actually the I-part...
if(looping & LOOPING_ROLL_AXIS) {
PPartRoll = 0;
} else { // TODO: Where do the 44000 come from???
PPartRoll = (rollAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral
}
// Now blend in the P-part - proportional to the Differential of the integral = the rate
PDPartRoll = PPartRoll + (int32_t)((int32_t)rollRate_PID * gyroPFactor / (256L / STICK_GAIN))
+ (rollDifferential * (int16_t)dynamicParams.GyroD) / 16;
PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / STICK_GAIN)
+ (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / STICK_GAIN));
// limit control feedback
#define SENSOR_LIMIT (4096 * 4)
CHECK_MIN_MAX(PDPartPitch, -SENSOR_LIMIT, SENSOR_LIMIT);
CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
/*
467,79 → 449,71
* Height control was here.
*/
if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
throttleTerm *= STICK_GAIN;
throttleTerm *= CONTROL_SCALING;
 
/*
* Compose yaw term.
* The yaw term is limited: Absolute value is max. = the throttle term / 2.
* However, at low throttle the yaw term is limited to a fixed value,
* and at high throttle it is limited by the throttle reserve (the difference
* between current throttle and maximum throttle).
*/
#define MIN_YAWGAS (40 * STICK_GAIN) // yaw also below this gas value
yawTerm = PDPartYaw - setPointYaw * STICK_GAIN;
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
yawTerm = PDPartYaw - setPointYaw * CONTROL_SCALING;
// limit yawTerm
if(throttleTerm > MIN_YAWGAS) {
/*
* -throttle/2 < -20 <= yawTerm <= 20 < throttle/2
*/
CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
} else {
/*
* -20 <= yawTerm <= 20
*/
CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
}
tmp_int = staticParams.MaxThrottle * STICK_GAIN;
 
/*
* throttle-MaxThrottle <= yawTerm <= MaxThrottle-throttle
*/
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING;
CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
/*
* Compose pitch and roll terms. This is finally where the sticks come into play.
*/
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.
IPartPitch += PPartPitch - controlPitch; // Integrate difference between P part (the angle) and the stick pos.
IPartRoll += PPartRoll - controlRoll; // I-part for attitude control OK
} else {
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
IPartPitch += PDPartPitch - controlPitch; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
IPartRoll += PDPartRoll - controlRoll; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
}
// TODO: From which planet comes the 16000?
CHECK_MIN_MAX(IPartPitch, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
 
// Add (P, D) parts minus stick pos. to the scaled-down I part.
pitchTerm = PDPartPitch - controlPitch + IPartPitch / Ki; // PID-controller for pitch
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64;
 
CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
rollTerm = PDPartRoll - controlRoll + IPartRoll / Ki; // PID-controller for roll
for (axis=PITCH; axis<=ROLL; axis++) {
/*
* Compose pitch and roll terms. This is finally where the sticks come into play.
*/
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.
IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos.
} else {
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
// To keep up with a full stick PDPart should be about 156...
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.
}
// 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
/*
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
* (max. pitch or roll term is the throttle value).
* TODO: Why a growing function of yaw?
*/
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
}
 
/*
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
* (max. pitch or roll term is the throttle value).
* TODO: Why a growing function of yaw?
*/
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64;
CHECK_MIN_MAX(pitchTerm, -tmp_int, tmp_int);
CHECK_MIN_MAX(rollTerm, -tmp_int, tmp_int);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
for(i = 0; i < MAX_MOTORS; i++) {
int16_t tmp;
if(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered.
tmp = ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L;
tmp += ((int32_t)pitchTerm * Mixer.Motor[i][MIX_PITCH]) / 64L;
tmp += ((int32_t)rollTerm * Mixer.Motor[i][MIX_ROLL]) / 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;
motorFilters[i] = motorFilter(tmp, motorFilters[i]);
tmp = motorFilters[i] / STICK_GAIN;
tmp = motorFilters[i] / CONTROL_SCALING;
CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);
Motor[i].SetPoint = tmp;
}
547,21 → 521,21
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// Debugging
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
DebugOut.Analog[0] = (10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[1] = (10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
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[9] = setPointYaw;
DebugOut.Analog[10] = yawIFactor;
DebugOut.Analog[11] = gyroIFactor;
// DebugOut.Analog[9] = setPointYaw;
// DebugOut.Analog[10] = yawIFactor;
// DebugOut.Analog[11] = gyroIFactor;
// DebugOut.Analog[12] = RC_getVariable(0);
// DebugOut.Analog[13] = dynamicParams.UserParams[0];
DebugOut.Analog[14] = RC_getVariable(4);
DebugOut.Analog[15] = dynamicParams.UserParams[4];
// DebugOut.Analog[14] = RC_getVariable(4);
// DebugOut.Analog[15] = dynamicParams.UserParams[4];
/* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */
 
// 12..15 are the controls.
570,17 → 544,18
// DebugOut.Analog[18] = ZAxisAcc;
 
DebugOut.Analog[19] = throttleTerm;
DebugOut.Analog[20] = pitchTerm;
DebugOut.Analog[21] = rollTerm;
DebugOut.Analog[20] = term[PITCH];
DebugOut.Analog[21] = term[ROLL];
DebugOut.Analog[22] = yawTerm;
DebugOut.Analog[23] = PPartPitch; //
DebugOut.Analog[24] = IPartPitch /Ki; // meget meget lille.
DebugOut.Analog[25] = PDPartPitch; // omtrent lig ppart.
 
DebugOut.Analog[26] = pitchAccNoisePeak;
DebugOut.Analog[27] = rollAccNoisePeak;
DebugOut.Analog[23] = PPart[PITCH]; //
DebugOut.Analog[24] = IPart[PITCH] /Ki; // meget meget lille.
DebugOut.Analog[25] = PDPart[PITCH]; // omtrent lig ppart.
 
DebugOut.Analog[30] = pitchGyroNoisePeak;
DebugOut.Analog[31] = rollGyroNoisePeak;
DebugOut.Analog[26] = accNoisePeak[PITCH];
DebugOut.Analog[27] = accNoisePeak[ROLL];
 
DebugOut.Analog[30] = gyroNoisePeak[PITCH];
DebugOut.Analog[31] = gyroNoisePeak[ROLL];
}
}
/branches/dongfang_FC_rewrite/flight.h
1,6 → 1,6
/*#######################################################################################
Flight Control
#######################################################################################*/
/*********************************************************************************/
/* Flight Control */
/*********************************************************************************/
 
#ifndef _FLIGHT_H
#define _FLIGHT_H
9,6 → 9,9
#include "analog.h"
#include "configuration.h"
 
#define PITCH 0
#define ROLL 1
 
extern uint8_t RequiredMotors;
 
// looping params
/branches/dongfang_FC_rewrite/invenSense.c
8,6 → 8,8
#define AUTOZERO_DDR DDRD
#define AUTOZERO_BIT 5
 
const uint8_t GYROS_REVERSE[2] = {0,0};
 
void gyro_calibrate() {
// If port not already set to output and high, do it.
if (!(AUTOZERO_DDR & (1<<AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1<<AUTOZERO_BIT))) {
/branches/dongfang_FC_rewrite/main.c
102,8 → 102,8
output_init();
timer0_init();
timer2_init();
USART0_Init();
if(CPUType == ATMEGA644P) USART1_Init();
usart0_Init();
if(CPUType == ATMEGA644P) usart1_Init();
RC_Init();
analog_init();
I2C_init();
263,10 → 263,10
// Allow Serial Data Transmit if motors must not updated or motors are not running
if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
USART0_TransmitTxData();
usart0_TransmitTxData();
}
USART0_ProcessRxData();
usart0_ProcessRxData();
if(CheckDelay(timer)) {
if(UBat < staticParams.LowVoltageWarning) {
/branches/dongfang_FC_rewrite/menu.c
150,8 → 150,8
*/
case 2:// Attitude Menu Item
LCD_printfxy(0,0,"Attitude");
LCD_printfxy(0,1,"Nick: %5i", pitchAngle / GYRO_DEG_FACTOR_PITCHROLL);
LCD_printfxy(0,2,"Roll: %5i", pitchAngle / GYRO_DEG_FACTOR_PITCHROLL);
LCD_printfxy(0,1,"Nick: %5i", angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL);
LCD_printfxy(0,2,"Roll: %5i", angle[ROLL ] / GYRO_DEG_FACTOR_PITCHROLL);
LCD_printfxy(0,3,"Heading: %5i", compassHeading);
break;
case 3:// Remote Control Channel Menu Item
/branches/dongfang_FC_rewrite/rc.c
56,10 → 56,6
#include "controlMixer.h"
#include "configuration.h"
 
// for DebugOut only.
#include "uart0.h"
 
 
// The channel array is 1-based. The 0th entry is not used.
volatile int16_t PPM_in[MAX_CHANNELS];
volatile int16_t PPM_diff[MAX_CHANNELS];
122,7 → 118,6
SREG = sreg;
}
 
 
/********************************************************************/
/* Every time a positive edge is detected at PD6 */
/********************************************************************/
216,8 → 211,8
if(RC_Quality) {
RC_Quality--;
if (NewPpmData-- == 0) {
RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD;
RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD;
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - stickOffsetPitch + RCDiff(CH_PITCH) * staticParams.StickD;
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - stickOffsetRoll + RCDiff(CH_ROLL) * staticParams.StickD;
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[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);
280,8 → 275,8
// In HH, it s OK to trim the R/C. The effect should not be conteracted here.
stickOffsetPitch = stickOffsetRoll = 0;
} else {
stickOffsetPitch = RCChannel(CH_PITCH);
stickOffsetRoll = RCChannel(CH_ROLL);
stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;
stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP;
}
}
 
/branches/dongfang_FC_rewrite/sensors.h
1,14 → 1,7
extern const uint8_t GYROS_REVERSE[2];
 
/*
* Common procedures for all gyro types.
*/
 
/*
* For InvenSense, set a port bit for the AUTO_ZERO line.
* Moved to gyro_calibrate.
*/
// void gyro_init(void);
 
/*
* FC 1.3 hardware: Searching the DAC values that return neutral readings.
* FC 2.0 hardware: Nothing to do.
* InvenSense hardware: Output a pulse on the AUTO_ZERO line.
15,4 → 8,7
*/
void gyro_calibrate(void);
 
/*
* Set some default FC parameters, depending on gyro type: Drift correction etc.
*/
void gyro_setDefaults(void);
/branches/dongfang_FC_rewrite/spi.c
59,7 → 59,7
#include "timer0.h"
#include "analog.h"
#include "attitude.h"
#include "GPScontrol.h"
#include "GPSControl.h"
#include "flight.h"
 
//-----------------------------------------
126,11 → 126,11
#define SPI_RXSYNCBYTE2 0x55
 
typedef enum
{
SPI_SYNC1,
SPI_SYNC2,
SPI_DATA
} SPI_RXState_t;
{
SPI_SYNC1,
SPI_SYNC2,
SPI_DATA
} SPI_RXState_t;
 
 
// data exchange packets to and From NaviCtrl
200,15 → 200,15
cli(); // stop all interrupts to avoid writing of new data during update of that packet.
// update content of packet to NaviCtrl
ToNaviCtrl.IntegralNick = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
ToNaviCtrl.IntegralRoll = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
ToNaviCtrl.IntegralNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
ToNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
ToNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
ToNaviCtrl.GyroNick = pitchRate_PID; // TODO: Which one should it be??
ToNaviCtrl.GyroRoll = rollRate_PID;
ToNaviCtrl.GyroNick = rate_PID[PITCH]; // TODO: Which one should it be??
ToNaviCtrl.GyroRoll = rate_PID[ROLL];
ToNaviCtrl.GyroYaw = yawRate;
ToNaviCtrl.AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
ToNaviCtrl.AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0;
// naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0;
switch(ToNaviCtrl.Command) {
case SPI_CMD_USER:
260,7 → 260,7
compassCalState = 0;
}
ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime;
ToNaviCtrl.Param.Int[1] = readingHeight; // at address of Byte 2 and 3
ToNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3
ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit;
ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit;
ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit;
/branches/dongfang_FC_rewrite/spi.h
4,11 → 4,8
 
//#include <util/delay.h>
#include <inttypes.h>
 
 
#define SPI_PROTOCOL_COMP 1
 
 
#define SPI_CMD_USER 10
#define SPI_CMD_STICK 11
#define SPI_CMD_MISC 12
15,8 → 12,7
#define SPI_CMD_PARAMETER1 13
#define SPI_CMD_VERSION 14
 
typedef struct
{
typedef struct {
uint8_t Sync1;
uint8_t Sync2;
uint8_t Command;
28,8 → 24,7
int16_t GyroNick;
int16_t GyroRoll;
int16_t GyroYaw;
union
{
union {
int8_t sByte[12];
uint8_t Byte[12];
int16_t Int[6];
39,15 → 34,12
uint8_t Chksum;
} __attribute__((packed)) ToNaviCtrl_t;
 
 
 
#define SPI_CMD_OSD_DATA 100
#define SPI_CMD_GPS_POS 101
#define SPI_CMD_GPS_TARGET 102
#define SPI_KALMAN 103
 
typedef struct
{
typedef struct {
uint8_t Command;
int16_t GPSStickNick;
int16_t GPSStickRoll;
55,8 → 47,7
int16_t CompassHeading;
int16_t Status;
uint16_t BeepTime;
union
{
union {
int8_t Byte[12];
int16_t Int[6];
int32_t Long[3];
66,8 → 57,7
} __attribute__((packed)) FromNaviCtrl_t;
 
 
typedef struct
{
typedef struct {
uint8_t Major;
uint8_t Minor;
uint8_t Patch;
74,13 → 64,10
uint8_t Compatible;
} __attribute__((packed)) SPI_VersionInfo_t;
 
 
extern ToNaviCtrl_t ToNaviCtrl;
extern FromNaviCtrl_t FromNaviCtrl;
 
 
typedef struct
{
typedef struct {
int8_t KalmanK;
int8_t KalmanMaxDrift;
int8_t KalmanMaxFusion;
87,7 → 74,6
uint8_t SerialDataOkay;
} __attribute__((packed)) NCData_t;
 
 
extern uint8_t NCDataOkay;
extern uint8_t NCSerialDataOkay;
 
95,6 → 81,4
void SPI_StartTransmitPacket(void);
void SPI_TransmitByte(void);
 
 
 
#endif //_SPI_H
/branches/dongfang_FC_rewrite/uart0.c
86,7 → 86,7
uint8_t Request_DebugData = FALSE;
uint8_t Request_Data3D = FALSE;
uint8_t Request_DebugLabel = 255;
uint8_t Request_PPMChannels = FALSE;
uint8_t Request_PPMChannels = FALSE;
uint8_t Request_MotorTest = FALSE;
uint8_t DisplayLine = 0;
 
125,20 → 125,20
"AnglePitch ", //0
"AngleRoll ",
"AngleYaw ",
"GyroPitch ",
"GyroRoll ",
"GyroYaw ", //5
"ControlPitch ",
"ControlRoll ",
"ControlYaw ",
"SetPointYaw ",
"YawRateIFactor ", //10
"Gyro I Factor ",
" ",
" ",
"R/C Var 4 ",
"User Param 4 ", //15
"RCQuality ",
"GyroPitch(AC) ",
"GyroRoll(AC) ",
"GyroYaw(AC) ", //5
"Pitch_Raw ",
"HiResPitch_PID ",
"PitchGyroOffset ",
"Roll_Raw ",
"HiResRoll_PID ", //10
"rollGyroOffset ",
"Pressure range ",
"Pressure A/D ",
"Pressure Value ",
"Press. rangewidz", //15
"Press. init A/D ",
"Ext. Quality ",
"Looping ",
"throttleTerm ",
159,7 → 159,7
/****************************************************************/
/* Initialization of the USART0 */
/****************************************************************/
void USART0_Init (void) {
void usart0_Init (void) {
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
425,7 → 425,7
}
 
// --------------------------------------------------------------------------
void USART0_ProcessRxData(void) {
void usart0_ProcessRxData(void) {
// if data in the rxd buffer are not locked immediately return
if(!rxd_buffer_locked) return;
585,9 → 585,9
rxd_buffer_locked = FALSE;
}
 
//############################################################################
//Routine für die Serielle Ausgabe
//############################################################################
/************************************************************************/
/* Routine für die Serielle Ausgabe */
/************************************************************************/
int16_t uart_putchar (int8_t c) {
if (c == '\n')
uart_putchar('\r');
599,7 → 599,7
}
 
//---------------------------------------------------------------------------------------------
void USART0_TransmitTxData(void) {
void usart0_TransmitTxData(void) {
if(!txd_complete) return;
 
if(Request_VerInfo && txd_complete) {
621,7 → 621,7
Request_Display1 = FALSE;
}
if(Request_DebugLabel != 0xFF) { // Texte für die Analogdaten
if(Request_DebugLabel != 0xFF) { // Texte für die Analogdaten
uint8_t label[16]; // local sram buffer
memcpy_P(label, ANALOG_LABEL[Request_DebugLabel], 16); // read lable from flash to sram buffer
SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &Request_DebugLabel, sizeof(Request_DebugLabel), label, 16);
628,7 → 628,7
Request_DebugLabel = 0xFF;
}
if(ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen
if(ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen
SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame));
ConfirmFrame = 0;
}
641,8 → 641,8
if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete) {
SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D));
Data3D.AngleNick = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
Data3D.AngleRoll = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
Data3D.Heading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
Data3D_Timer = SetDelay(Data3D_Interval);
Request_Data3D = FALSE;
655,8 → 655,8
 
#ifdef USE_MK3MAG
if((CheckDelay(Compass_Timer)) && txd_complete) {
ToMk3Mag.Attitude[0] = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg
ToMk3Mag.Attitude[1] = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg
ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg
ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
ToMk3Mag.CalState = CompassCalState;
/branches/dongfang_FC_rewrite/uart0.h
11,9 → 11,9
//Baud rate of the USART
#define USART0_BAUD 57600
 
extern void USART0_Init (void);
extern void USART0_TransmitTxData(void);
extern void USART0_ProcessRxData(void);
extern void usart0_Init (void);
extern void usart0_TransmitTxData(void);
extern void usart0_ProcessRxData(void);
extern int16_t uart_putchar(int8_t c);
 
extern uint8_t RemotePollDisplayLine;
/branches/dongfang_FC_rewrite/uart1.c
70,7 → 70,7
/****************************************************************/
/* Initialization of the USART1 */
/****************************************************************/
void USART1_Init (void) {
void usart1_Init (void) {
// USART1 Control and Status Register A, B, C and baud rate register
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
134,9 → 134,7
/****************************************************************/
/* USART1 data register empty ISR */
/****************************************************************/
/*ISR(USART1_UDRE_vect)
{
 
/*ISR(USART1_UDRE_vect) {
}
*/
 
143,16 → 141,13
/****************************************************************/
/* USART1 transmitter ISR */
/****************************************************************/
/*ISR(USART1_TX_vect)
{
 
/*ISR(USART1_TX_vect) {
}
*/
/****************************************************************/
/* USART1 receiver ISR */
/****************************************************************/
ISR(USART1_RX_vect)
{
ISR(USART1_RX_vect) {
uint8_t c;
c = UDR1; // get data byte
#if defined (USE_MK3MAG)
/branches/dongfang_FC_rewrite/uart1.h
1,8 → 1,6
#ifndef _UART1_H
#define _UART1_H
 
extern void usart1_Init (void);
 
extern void USART1_Init (void);
 
 
#endif //_UART1_H
/branches/dongfang_FC_rewrite/userparams.txt
1,10 → 1,10
User param 4: Throttle stick D value.
Userparam 4: Throttle stick D value. Recommended: 12-15. Good fun to fly with.
 
Userparam 5: Filter control.
bits 0-1: Gyros 1st order
bits 2-3: Gyros 2nd order
bits 4-5: GyroD (in series with 1st order)
bits 6-7: Acc.
bits 0-1: Gyro signal for the flight control PID (filter constants 1-4)
bits 2-3: Gyro signal for the attitude angle integration (ATT) (filter constants 1-4)
bits 4-5: GyroD (filter constants 1-4)
bits 6-7: Acc. (filter constants 1-4)
 
Userparam6: Motor smoothing.
0: No filter
12,11 → 12,10
2: As H&I
3: Reverse H&I
 
Userparam7: Yaw I factor. Default 0 (may be nasty to fly!)
Userparam7: Yaw I factor. Default 0 (may be nasty to fly!). Recommended: ca. 100.
 
Userparam8: Acc integral correction Z axis limit. Default 0 (!)
 
 
Other params:
GyroAccFactor = 0'th order integral drift correction - acceleration sensor part as 1/1000s.
Default 5 (ENC-03)
25,7 → 24,11
DriftComp = Max offset correction per iteration (=per 1/2 second).
This limits the value _before_ division by GyroAccTrim. Default 32 (ENC-03)
 
Rotary rate limiter flag ON = dongfang axis coupling used. OFF = H&I axis coupling
(with the modification that it does not affect the rate values).
If axis coupling is off: No effect.
The rotary rate limiter was removed.
Rotary rate limiter flag ON = The gyro signals for flight control are extrapolated with
a sharp rise in "virtual" rotation rates when the gyros are near saturation. This
prevents (via the flight control) the copter from turning fast enough that the gyros
saturate. Saturation will have the effect that stick controls overwhelm the (now clipped)
gyro feedback in the flight control, and the copter flips VERY fast. Saturation is
normally undesirable but it provides for a very entertaining stunt!
The rotary rate limiter (beginner feature to make the copter _very_ irresponsive to
stick controls) was removed.