Subversion Repositories FlightCtrl

Compare Revisions

Regard whitespace Rev 1664 → Rev 1645

/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
1,18 → 1,17
#include "ADXRS610_FC2.0.h"
#include "configuration.h"
 
const uint8_t GYRO_REVERSED[3] = {0,0,0};
const uint8_t ACC_REVERSED[3] = {0,1,0};
const uint8_t GYROS_REVERSE[2] = {0,0};
 
void gyro_calibrate(void) {}
 
void gyro_setDefaults(void) {
staticParams.GyroD = 5;
staticParams.DriftComp = 10;
staticParams.DriftComp = 1;
staticParams.GyroAccFactor = 1;
staticParams.GyroAccTrim = 5;
 
// Not used.
staticParams.AngleTurnOverPitch = 78;
staticParams.AngleTurnOverRoll = 78;
}
 
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.h
2,7 → 2,6
#define _ADXRS610_H
 
#include "sensors.h"
 
/*
* Configuration for the ADXR610 gyros, as they are oriented and wired on the FC 2.0 ME board.
*/
23,4 → 22,8
*/
#define GYRO_YAW_CORRECTION 1.0f
 
/*
* Yaw axis is reverse on the ME.
*/
#define GYRO_REVERSE_YAW yes
#endif
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
5,15 → 5,17
#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
 
const uint8_t GYRO_REVERSED[3] = {0,0,1};
const uint8_t ACC_REVERSED[3] = {0,1,0};
#define DAC_PITCH 0
#define DAC_ROLL 1
#define DAC_YAW 2
 
// void gyro_init(void) {}
void gyro_calibrate(void) {
uint8_t i, axis, factor, numberOfAxesInRange = 0;
uint8_t i, numberOfAxesInRange = 0;
uint16_t timeout;
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
timeout = SetDelay(2000);
21,23 → 23,36
for(i = 140; i != 0; i--) {
// If all 3 axis are in range, shorten the remaining number of iterations.
if(numberOfAxesInRange == 3 && i > 10) i = 9;
numberOfAxesInRange = 0;
for (axis=PITCH; axis<=YAW; axis++) {
if (axis==YAW) factor = GYRO_SUMMATION_FACTOR_YAW;
else factor = GYRO_SUMMATION_FACTOR_PITCHROLL;
if(rawGyroSum[PITCH] < PITCHROLL_MINLIMIT) DACValues[PITCH]--;
else if(rawGyroSum[PITCH] > PITCHROLL_MAXLIMIT) DACValues[PITCH]++;
else numberOfAxesInRange++;
 
if(rawGyroSum[axis] < factor * 510) DACValues[axis]--;
else if(rawGyroSum[axis] > limit * 515) DACValues[axis]++;
if(rawGyroSum[ROLL] < PITCHROLL_MINLIMIT) DACValues[ROLL]--;
else if(rawGyroSum[ROLL] > PITCHROLL_MAXLIMIT) DACValues[ROLL]++;
else numberOfAxesInRange++;
/* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp).
if(DACValues[axis] < 10) {
DACValues[axis] = 10;
} else if(DACValues[axis] > 245) {
DACValues[axis] = 245;
if(rawYawGyroSum < GYRO_SUMMATION_FACTOR_YAW * 510) DACValues[DAC_YAW]--;
else if(rawYawGyroSum > GYRO_SUMMATION_FACTOR_YAW * 515) DACValues[DAC_YAW]++ ;
else numberOfAxesInRange++;
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[ROLL] = 10;
} else if(DACValues[DAC_ROLL] > 245) {
/* GyroDefectRoll = 1; */ DACValues[ROLL] = 245;
}
if(DACValues[DAC_YAW] < 10) {
/* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 10;
} else if(DACValues[DAC_YAW] > 245) {
/* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 245;
}
 
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission
45,7 → 60,7
while(twi_state) {
// Did it take too long?
if(CheckDelay(timeout)) {
printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl");
printf("\r\n DAC or I2C Error1 check I2C, 3Vref, DAC, and BL-Ctrl");
break;
}
}
59,9 → 74,8
 
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.DriftComp = 100;
staticParams.GyroAccFactor = 1;
staticParams.GyroAccTrim = 5;
staticParams.DriftComp = 32;
staticParams.GyroAccFactor = 5;
 
// Not used.
staticParams.AngleTurnOverPitch = 85;
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.h
7,6 → 7,7
*/
 
#define GYRO_HW_NAME "ENC"
 
#define GYRO_HW_FACTOR 1.304f
 
/*
/branches/dongfang_FC_rewrite/analog.c
72,8 → 72,8
* reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
* the offsets with the DAC.
*/
volatile int16_t rawGyroSum[3];
volatile int16_t acc[3];
volatile int16_t rawGyroSum[2], rawYawGyroSum;
volatile int16_t acc[2] = {0,0}, ZAcc = 0;
volatile int16_t filteredAcc[2]={0,0};
 
/*
84,7 → 84,7
volatile int16_t gyro_PID[2];
volatile int16_t gyro_ATT[2];
volatile int16_t gyroD[2];
volatile int16_t yawGyro;
volatile int16_t yawGyro = 0;
 
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
91,25 → 91,16
* 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 gyroOffset[2], yawGyroOffset;
volatile int16_t accOffset[2], ZAccOffset;
 
volatile int16_t accOffset[3] = {
512 * ACC_SUMMATION_FACTOR_PITCHROLL,
512 * ACC_SUMMATION_FACTOR_PITCHROLL,
512 * ACC_SUMMATION_FACTOR_Z
};
 
/*
* 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 GYROS_FIRSTORDERFILTER;
volatile uint8_t GYROS_SECONDORDERFILTER;
volatile uint8_t GYROS_DFILTER;
volatile uint8_t ACC_FILTER;
 
/*
262,35 → 253,39
*/
switch(state++) {
case 7: // Z acc
if (ACC_REVERSED[Z])
acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
else
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
#ifdef ACC_REVERSE_ZAXIS
ZAcc = -ZAccOffset - sensorInputs[AD_ACC_Z];
#else
ZAcc = sensorInputs[AD_ACC_Z] - ZAccOffset;
#endif
break;
case 10: // yaw gyro
rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
if (GYRO_REVERSED[YAW])
yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
else
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
rawYawGyroSum = sensorInputs[AD_GYRO_YAW];
#ifdef GYRO_REVERSE_YAW
yawGyro = rawYawGyroSum - yawGyroOffset;
#else
yawGyro = yawGyroOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3).
#endif
break;
case 11: // pitch axis acc.
if (ACC_REVERSED[PITCH])
acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
else
#ifdef ACC_REVERSE_PITCHAXIS
acc[PITCH] = -accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
#else
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
#endif
filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER;
 
filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER;
measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);
break;
case 12: // roll axis acc.
if (ACC_REVERSED[ROLL])
acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
else
#ifdef ACC_REVERSE_ROLLAXIS
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
#else
acc[ROLL] = -accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
#endif
filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER-1) + acc[ROLL]) / ACC_FILTER;
measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);
break;
319,9 → 314,9
filteredAirPressure = filterAirPressure(getAbsPressure(rawAirPressure));
}
DebugOut.Analog[13] = range;
DebugOut.Analog[14] = rawAirPressure;
DebugOut.Analog[15] = filteredAirPressure;
DebugOut.Analog[12] = range;
DebugOut.Analog[13] = rawAirPressure;
DebugOut.Analog[14] = filteredAirPressure;
break;
 
case 14:
345,7 → 340,7
}
 
// 2) Apply sign and offset, scale before filtering.
if (GYRO_REVERSED[axis]) {
if (GYROS_REVERSE[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
352,13 → 347,13
}
 
// 3) Scale and filter.
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER-1) + tempOffsetGyro) / GYROS_PID_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_D_FILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER;
gyroD[axis] = (gyroD[axis] * (GYROS_DFILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_DFILTER;
 
// 6) Done.
gyro_PID[axis] = tempOffsetGyro;
369,7 → 364,7
tempGyro = rawGyroSum[axis];
// 1) Apply sign and offset, scale before filtering.
if (GYRO_REVERSED[axis]) {
if (GYROS_REVERSE[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
376,7 → 371,7
}
// 2) Filter.
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER-1) + tempOffsetGyro) / GYROS_ATT_FILTER;
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_INTEGRALFILTER-1) + tempOffsetGyro) / GYROS_INTEGRALFILTER;
break;
case 16:
405,39 → 400,41
 
void analog_calibrate(void) {
#define GYRO_OFFSET_CYCLES 32
uint8_t i, axis;
int32_t deltaOffsets[3] = {0,0,0};
int16_t filteredDelta;
uint8_t i;
int32_t _pitchOffset = 0, _rollOffset = 0, _yawOffset = 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;
GYROS_FIRSTORDERFILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1;
GYROS_SECONDORDERFILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1;
GYROS_DFILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1;
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1;
 
gyroOffset[PITCH] = gyroOffset[ROLL] = yawGyroOffset = 0;
 
gyro_calibrate();
 
// 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);
for (axis=0; axis<=YAW; axis++) {
deltaOffsets[axis] += rawGyroSum[axis] - gyroOffset[axis];
_pitchOffset += rawGyroSum[PITCH];
_rollOffset += rawGyroSum[ROLL];
_yawOffset += rawYawGyroSum;
}
}
 
for (axis=0; axis<=YAW; axis++) {
filteredDelta = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
gyroOffset[axis] += filteredDelta;
}
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;
 
// Noise is relative to offset. So, reset noise measurements when changing offsets.
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.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
 
accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
accOffset[ROLL] = GetParamWord(PID_ACC_ROLL);
accOffset[Z] = GetParamWord(PID_ACC_Z);
 
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);
}
 
/*
449,35 → 446,30
*/
void analog_calibrateAcc(void) {
#define ACC_OFFSET_CYCLES 10
uint8_t i, axis;
int32_t deltaOffset[3] = {0,0,0};
int16_t filteredDelta;
uint8_t i;
int32_t _pitchAxisOffset = 0, _rollAxisOffset = 0, _ZAxisOffset = 0;
// int16_t pressureDiff, savedRawAirPressure;
 
accOffset[PITCH] = accOffset[ROLL] = ZAccOffset = 0;
 
for(i=0; i < ACC_OFFSET_CYCLES; i++) {
Delay_ms_Mess(10);
for (axis=0; axis<=YAW; axis++) {
deltaOffset[axis] += acc[axis];
_pitchAxisOffset += acc[PITCH];
_rollAxisOffset += acc[ROLL];
_ZAxisOffset += ZAcc;
}
}
 
for (axis=0; 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]);
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.
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);
// 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...)
503,7 → 495,7
pressureDiff += (rawAirPressure - savedRawAirPressure);
}
 
DebugOut.Analog[16] = rangewidth =
DebugOut.Analog[15] = rangewidth =
(pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2);
*/
}
/branches/dongfang_FC_rewrite/analog.h
3,8 → 3,7
#include <inttypes.h>
 
//#include "invenSense.h"
//#include "ENC-03_FC1.3.h"
#include "ADXRS610_FC2.0.h"
#include "ENC-03_FC1.3.h"
 
/*
* How much low pass filtering to apply for gyro_PID.
11,7 → 10,7
* 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_PID_FILTER 1
#define GYROS_PIDFILTER 1
 
/*
* How much low pass filtering to apply for gyro_ATT.
18,7 → 17,7
* 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_ATT_FILTER 1
#define GYROS_INTEGRALFILTER 1
 
// Temporarily replaced by userparam-configurable variable.
// #define ACC_FILTER 4
29,10 → 28,7
The "Positive direction" is the rotation direction around an axis where
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:
- Nose down for pitch
- Left hand side down for roll
- Clockwise seen from above for yaw.
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.
104,9 → 100,6
#define GYRO_SUMMATION_FACTOR_PITCHROLL 4
#define GYRO_SUMMATION_FACTOR_YAW 2
 
#define ACC_SUMMATION_FACTOR_PITCHROLL 2
#define ACC_SUMMATION_FACTOR_Z 1
 
/*
Integration:
The HiResXXXX values are divided by 8 (in H&I firmware) before integration.
167,8 → 160,6
 
#define PITCH 0
#define ROLL 1
#define YAW 2
#define Z 2
/*
* The values that this module outputs
* These first 2 exported arrays are zero-offset. The "PID" ones are used
189,12 → 180,12
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
*/
extern volatile int16_t rawGyroSum[3];
extern volatile int16_t rawGyroSum[2], rawYawGyroSum;
 
/*
* The acceleration values that this module outputs. They are zero based.
*/
extern volatile int16_t acc[3];
extern volatile int16_t acc[2], ZAcc;
extern volatile int16_t filteredAcc[2];
 
/*
/branches/dongfang_FC_rewrite/attitude.c
124,7 → 124,7
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L)
 
int16_t correctionSum[2] = {0,0};
int32_t correctionSum[2] = {0,0};
 
/*
* Experiment: Compensating for dynamic-induced gyro biasing.
165,7 → 165,6
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
 
dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0;
correctionSum[PITCH] = correctionSum[ROLL] = 0;
// Calibrate hardware.
analog_calibrate();
179,7 → 178,6
 
// update compass course to current heading
compassCourse = compassHeading;
 
// Inititialize YawGyroIntegral value with current compass heading
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
 
201,7 → 199,6
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.
240,6 → 237,10
ACYawRate = yawRate;
}
 
DebugOut.Analog[3] = ACRate[PITCH];
DebugOut.Analog[4] = ACRate[ROLL];
DebugOut.Analog[5] = ACYawRate;
 
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
272,7 → 273,7
/************************************************************************
* A kind of 0'th order integral correction, that corrects the integrals
* directly. This is the "gyroAccFactor" stuff in the original code.
* There is (there) also a drift compensation
* There is (there) also what I would call a "minus 1st order correction"
* - it corrects the differential of the integral = the gyro offsets.
* That should only be necessary with drifty gyros like ENC-03.
************************************************************************/
281,13 → 282,13
// are less than ....., or reintroduce Kalman.
// Well actually the Z axis acc. check is not so silly.
uint8_t axis;
int32_t correction;
if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) {
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;
int32_t accDerived[2];
if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
permilleAcc /= 2;
303,23 → 304,11
* Add to each sum: The amount by which the angle is changed just below.
*/
for (axis=PITCH; axis<=ROLL; axis++) {
accDerived = getAngleEstimateFromAcc(axis);
DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
accDerived[axis] = getAngleEstimateFromAcc(axis);
correctionSum[axis] += permilleAcc * (accDerived[axis] - angle[axis]);
// 1000 * the correction amount that will be added to the gyro angle in next line.
correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L;
 
correctionSum[axis] += angle[axis] - correction;
// There should not be a risk of overflow here, since the integrals do not exceed a few 100000.
// change = ((1000 - permilleAcc) * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis]
// = (1000 * angle[axis] - permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis]
// = (- permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000
// = permilleAcc * (accDerived - angle[axis]) / 1000
// Experiment: Do not acutally apply the correction. See if drift compensation does that.
// angle[axis] += correction / 1000;
angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived[axis]) / 1000L;
}
DebugOut.Digital[1] = debugFullWeight;
333,28 → 322,25
* (that happens in correctIntegralsByAcc0thOrder above) but rather the
* cause of it: Gyro drift, vibration and rounding errors.
* All the corrections made in correctIntegralsByAcc0thOrder over
* DRIFTCORRECTION_TIME cycles are summed up. This number is
* then divided by DRIFTCORRECTION_TIME to get the approx.
* MINUSFIRSTORDERCORRECTION_TIME cycles are summed up. This number is
* then divided by MINUSFIRSTORDERCORRECTION_TIME to get the approx.
* correction that should have been applied to each iteration to fix
* the error. This is then added to the dynamic offsets.
************************************************************************/
// 2 times / sec. = 488/2
#define DRIFTCORRECTION_TIME 256L
void driftCorrection(void) {
// 2 times / sec.
#define DRIFTCORRECTION_TIME 488/2
void driftCompensation(void) {
static int16_t timer = DRIFTCORRECTION_TIME;
int16_t deltaCorrection;
int16_t deltaCompensation;
uint8_t axis;
if (! --timer) {
timer = DRIFTCORRECTION_TIME;
for (axis=PITCH; axis<=ROLL; axis++) {
// Take the sum of corrections applied, add it to delta
deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME;
// Add the delta to the compensation. So positive delta means, gyro should have higher value.
dynamicOffset[axis] += deltaCorrection / staticParams.GyroAccTrim;
CHECK_MIN_MAX(dynamicOffset[axis], -staticParams.DriftComp, staticParams.DriftComp);
DebugOut.Analog[11 + axis] = correctionSum[axis];
DebugOut.Analog[28 + axis] = dynamicOffset[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;
}
}
}
365,18 → 351,9
void calculateFlightAttitude(void) {
getAnalogData();
integrate();
DebugOut.Analog[6] = ACRate[PITCH];
DebugOut.Analog[7] = ACRate[ROLL];
DebugOut.Analog[8] = ACYawRate;
DebugOut.Analog[3] = rate_PID[PITCH];
DebugOut.Analog[4] = rate_PID[ROLL];
DebugOut.Analog[5] = yawRate;
#ifdef ATTITUDE_USE_ACC_SENSORS
correctIntegralsByAcc0thOrder();
driftCorrection();
driftCompensation();
#endif
}
 
/branches/dongfang_FC_rewrite/configuration.h
75,7 → 75,7
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; //
uint8_t UfoArrangement; // x oder + Formation
uint8_t IFactor; // Value : 0-250
uint8_t UserParams1[4]; // Value : 0-250
/*
/branches/dongfang_FC_rewrite/eeprom.c
95,11 → 95,10
* While we are still using userparams for flight parameters, do set
* some safe & meaningful default values.
*/
staticParams.UserParams1[3] = 10; // Throttle stick D=10
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.
staticParams.UserParams1[4] = 0b01010101;
staticParams.UserParams1[5] = 2; // H&I motor smoothing.
staticParams.UserParams1[6] = 120;
staticParams.UserParams1[7] = 5;
}
 
void setOtherDefaults(void) {
132,7 → 131,7
staticParams.LowVoltageWarning = 94;
staticParams.EmergencyGas = 35;
staticParams.EmergencyGasDuration = 30;
staticParams.Unused0 = 0;
staticParams.UfoArrangement = 0;
staticParams.IFactor = 32;
staticParams.ServoPitchControl = 100;
staticParams.ServoPitchComp = 40;
147,6 → 146,7
staticParams.AxisCoupling1 = 90;
staticParams.AxisCoupling2 = 67;
staticParams.AxisCouplingYawCorrection = 0;
staticParams.GyroAccTrim = 2;
staticParams.DynamicStability = 50;
staticParams.J16Bitmask = 95;
staticParams.J17Bitmask = 243;
174,9 → 174,9
/* Default Values for parameter set 1 */
/***************************************************/
void ParamSet_DefaultSet1(void) { // sport
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
setOtherDefaults();
memcpy(staticParams.Name, "Sport\0",6);
}
 
184,10 → 184,11
/* Default Values for parameter set 2 */
/***************************************************/
void ParamSet_DefaultSet2(void) { // normal
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
setOtherDefaults();
staticParams.Height_Gain = 3;
staticParams.GyroAccTrim = 32;
staticParams.J16Timing = 20;
staticParams.J17Timing = 20;
memcpy(staticParams.Name, "Normal\0", 7);
197,13 → 198,14
/* Default Values for parameter set 3 */
/***************************************************/
void ParamSet_DefaultSet3(void) { // beginner
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
setOtherDefaults();
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;
staticParams.Height_Gain = 3;
staticParams.EmergencyGasDuration = 20;
staticParams.AxisCouplingYawCorrection = 70;
staticParams.GyroAccTrim = 32;
staticParams.J16Timing = 30;
staticParams.J17Timing = 30;
memcpy(staticParams.Name, "Beginner\0", 9);
/branches/dongfang_FC_rewrite/eeprom.h
10,7 → 10,7
#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 PID_ACC_TOP 8 // word
 
#ifdef USE_KILLAGREG
#define PID_MM3_X_OFF 11 // byte
/branches/dongfang_FC_rewrite/flight.c
541,7 → 541,7
// 12..15 are the controls.
// DebugOut.Analog[16] = pitchAxisAcc;
// DebugOut.Analog[17] = rollAxisAcc;
DebugOut.Analog[18] = HIRES_GYRO_INTEGRATION_FACTOR;
// DebugOut.Analog[18] = ZAxisAcc;
 
DebugOut.Analog[19] = throttleTerm;
DebugOut.Analog[20] = term[PITCH];
/branches/dongfang_FC_rewrite/invenSense.c
4,17 → 4,12
 
#include <avr/io.h>
 
/*
* Configuration for my prototype board with InvenSense gyros.
* The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll.
*/
const uint8_t GYRO_REVERSED[3] = {0,0,0};
const uint8_t ACC_REVERSED[3] = {0,0,1};
 
#define AUTOZERO_PORT PORTD
#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))) {
34,7 → 29,7
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.GyroAccFactor = 1;
staticParams.DriftComp = 10;
staticParams.DriftComp = 1;
 
// Not used.
staticParams.AngleTurnOverPitch = 85;
/branches/dongfang_FC_rewrite/invenSense.h
6,6 → 6,13
#define GYRO_HW_NAME "ISens"
/*
* Configuration for my prototype board with InvenSense gyros.
* The FC 1.3 board is installed upside down, therefore 2 acc. meters are reversed.
*/
#define ACC_REVERSE_ROLLAXIS yes
#define ACC_REVERSE_ZAXIS yes
 
/*
* The InvenSense gyros have a lower sensitivity than for example the ADXRS610s on the FC 2.0 ME,
* but they have a wider range too.
* 2mV/deg/s gyros and no amplifiers:
26,4 → 33,9
*/
#define GYRO_YAW_CORRECTION 0.97f
 
/*
* Yaw axis is reverse.
*/
#define GYRO_REVERSE_YAW yes
 
#endif
/branches/dongfang_FC_rewrite/makefile
22,8 → 22,8
RC = DSL
#RC = SPECTRUM
 
#GYRO=ENC-03_FC1.3
GYRO=ADXRS610_FC2.0
GYRO=ENC-03_FC1.3
#GYRO=ADXRS610_FC2.0
#GYRO=invenSense
 
#-------------------------------------------------------------------
/branches/dongfang_FC_rewrite/sensors.h
1,19 → 1,6
#ifndef _SENSORS_H
#define _SENSORS_H
extern const uint8_t GYROS_REVERSE[2];
 
#include <inttypes.h>
 
/*
* Whether (pitch, roll, yaw) gyros are reversed (see analog.h).
*/
extern const uint8_t GYRO_REVERSED[3];
 
/*
* Whether (pitch, roll, Z) acc. meters are reversed(see analog.h).
*/
extern const uint8_t ACC_REVERSED[3];
 
/*
* Common procedures for all gyro types.
* FC 1.3 hardware: Searching the DAC values that return neutral readings.
* FC 2.0 hardware: Nothing to do.
25,5 → 12,3
* Set some default FC parameters, depending on gyro type: Drift correction etc.
*/
void gyro_setDefaults(void);
 
#endif
/branches/dongfang_FC_rewrite/uart0.c
125,22 → 125,22
"AnglePitch ", //0
"AngleRoll ",
"AngleYaw ",
"GyroPitch(PID) ",
"GyroRoll(PID) ",
"GyroYaw ", //5
"GyroPitch(AC) ",
"GyroRoll(AC) ",
"GyroYaw(AC) ",
"AccPitch ",
"AccRoll ", //10
"CorrSumPitch ",
"CorrSumRoll ",
"GyroYaw(AC) ", //5
"Pitch_Raw ",
"HiResPitch_PID ",
"PitchGyroOffset ",
"Roll_Raw ",
"HiResRoll_PID ", //10
"rollGyroOffset ",
"Pressure range ",
"Pressure A/D ",
"Pressure Value ", //15
"Press. rangewidz",
"Pressure Value ",
"Press. rangewidz", //15
"Press. init A/D ",
"FACTOR ",
"Ext. Quality ",
"Looping ",
"throttleTerm ",
"pitchTerm ", //20
"rollTerm ",
150,8 → 150,8
"P+D Pitch ", //25
"Pitch acc noise ",
"Roll acc noise ",
"DynamicPitch ",
"DynamicRoll ",
"Pitch Corr ",
"Roll Corr ",
"Pitch Noise ", //30
"Roll Noise "
};