Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1645 → Rev 1646

/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
1,17 → 1,18
#include "ADXRS610_FC2.0.h"
#include "configuration.h"
 
const uint8_t GYROS_REVERSE[2] = {0,0};
const uint8_t GYRO_REVERSED[3] = {0,0,0};
const uint8_t ACC_REVERSED[3] = {0,1,0};
 
void gyro_calibrate(void) {}
 
void gyro_setDefaults(void) {
staticParams.GyroD = 5;
staticParams.DriftComp = 1;
staticParams.DriftComp = 10;
staticParams.GyroAccFactor = 1;
staticParams.GyroAccTrim = 5;
 
// Not used.
staticParams.AngleTurnOverPitch = 78;
staticParams.AngleTurnOverRoll = 78;
}
 
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.h
2,6 → 2,7
#define _ADXRS610_H
 
#include "sensors.h"
 
/*
* Configuration for the ADXR610 gyros, as they are oriented and wired on the FC 2.0 ME board.
*/
22,8 → 23,4
*/
#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,17 → 5,15
#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
 
#define DAC_PITCH 0
#define DAC_ROLL 1
#define DAC_YAW 2
const uint8_t GYRO_REVERSED[3] = {0,0,1};
const uint8_t ACC_REVERSED[3] = {0,1,0};
 
// void gyro_init(void) {}
void gyro_calibrate(void) {
uint8_t i, numberOfAxesInRange = 0;
uint8_t i, axis, factor, numberOfAxesInRange = 0;
uint16_t timeout;
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
timeout = SetDelay(2000);
23,36 → 21,23
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;
if(rawGyroSum[PITCH] < PITCHROLL_MINLIMIT) DACValues[PITCH]--;
else if(rawGyroSum[PITCH] > PITCHROLL_MAXLIMIT) DACValues[PITCH]++;
else numberOfAxesInRange++;
for (axis=PITCH; axis<=YAW; axis++) {
if (axis==YAW) factor = GYRO_SUMMATION_FACTOR_YAW;
else factor = GYRO_SUMMATION_FACTOR_PITCHROLL;
 
if(rawGyroSum[axis] < factor * 510) DACValues[axis]--;
else if(rawGyroSum[axis] > limit * 515) DACValues[axis]++;
else numberOfAxesInRange++;
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]--;
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;
/* 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(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
60,8 → 45,8
while(twi_state) {
// Did it take too long?
if(CheckDelay(timeout)) {
printf("\r\n DAC or I2C Error1 check I2C, 3Vref, DAC, and BL-Ctrl");
break;
printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl");
break;
}
}
 
73,9 → 58,10
}
 
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.DriftComp = 32;
staticParams.GyroAccFactor = 5;
staticParams.GyroD = 3;
staticParams.DriftComp = 100;
staticParams.GyroAccFactor = 1;
staticParams.GyroAccTrim = 5;
 
// Not used.
staticParams.AngleTurnOverPitch = 85;
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.h
7,7 → 7,6
*/
 
#define GYRO_HW_NAME "ENC"
 
#define GYRO_HW_FACTOR 1.304f
 
/*
/branches/dongfang_FC_rewrite/analog.c
72,9 → 72,9
* 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[2], rawYawGyroSum;
volatile int16_t acc[2] = {0,0}, ZAcc = 0;
volatile int16_t filteredAcc[2] = {0,0};
volatile int16_t rawGyroSum[3];
volatile int16_t acc[3];
volatile int16_t filteredAcc[2]={0,0};
 
/*
* These 4 exported variables are zero-offset. The "PID" ones are used
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 = 0;
volatile int16_t yawGyro;
 
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
91,16 → 91,25
* standing still. They are used for adjusting the gyro and acc. meter values
* to be centered on zero.
*/
volatile int16_t gyroOffset[2], yawGyroOffset;
volatile int16_t accOffset[2], ZAccOffset;
volatile int16_t gyroOffset[3] = {
512 * GYRO_SUMMATION_FACTOR_PITCHROLL,
512 * GYRO_SUMMATION_FACTOR_PITCHROLL,
512 * GYRO_SUMMATION_FACTOR_YAW
};
 
volatile int16_t accOffset[3] = {
512 * ACC_SUMMATION_FACTOR_PITCHROLL,
512 * ACC_SUMMATION_FACTOR_PITCHROLL,
512 * ACC_SUMMATION_FACTOR_Z
};
 
/*
* This allows some experimentation with the gyro filters.
* Should be replaced by #define's later on...
*/
volatile uint8_t GYROS_FIRSTORDERFILTER;
volatile uint8_t GYROS_SECONDORDERFILTER;
volatile uint8_t GYROS_DFILTER;
volatile uint8_t GYROS_PID_FILTER;
volatile uint8_t GYROS_ATT_FILTER;
volatile uint8_t GYROS_D_FILTER;
volatile uint8_t ACC_FILTER;
 
/*
253,39 → 262,35
*/
switch(state++) {
case 7: // Z acc
#ifdef ACC_REVERSE_ZAXIS
ZAcc = -ZAccOffset - sensorInputs[AD_ACC_Z];
#else
ZAcc = sensorInputs[AD_ACC_Z] - ZAccOffset;
#endif
break;
if (ACC_REVERSED[Z])
acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
else
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
break;
case 10: // yaw gyro
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
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];
break;
case 11: // pitch axis acc.
#ifdef ACC_REVERSE_PITCHAXIS
acc[PITCH] = -accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
#else
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
#endif
if (ACC_REVERSED[PITCH])
acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
else
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
 
filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER;
 
measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);
break;
case 12: // roll axis acc.
#ifdef ACC_REVERSE_ROLLAXIS
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
#else
acc[ROLL] = -accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
#endif
if (ACC_REVERSED[ROLL])
acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
else
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER-1) + acc[ROLL]) / ACC_FILTER;
measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);
break;
314,9 → 319,9
filteredAirPressure = filterAirPressure(getAbsPressure(rawAirPressure));
}
DebugOut.Analog[12] = range;
DebugOut.Analog[13] = rawAirPressure;
DebugOut.Analog[14] = filteredAirPressure;
DebugOut.Analog[13] = range;
DebugOut.Analog[14] = rawAirPressure;
DebugOut.Analog[15] = filteredAirPressure;
break;
 
case 14:
340,7 → 345,7
}
 
// 2) Apply sign and offset, scale before filtering.
if (GYROS_REVERSE[axis]) {
if (GYRO_REVERSED[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
347,13 → 352,13
}
 
// 3) Scale and filter.
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PIDFILTER-1) + tempOffsetGyro) / GYROS_PIDFILTER;
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER-1) + tempOffsetGyro) / GYROS_PID_FILTER;
 
// 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;
gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER;
 
// 6) Done.
gyro_PID[axis] = tempOffsetGyro;
364,7 → 369,7
tempGyro = rawGyroSum[axis];
// 1) Apply sign and offset, scale before filtering.
if (GYROS_REVERSE[axis]) {
if (GYRO_REVERSED[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
371,7 → 376,7
}
// 2) Filter.
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_INTEGRALFILTER-1) + tempOffsetGyro) / GYROS_INTEGRALFILTER;
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER-1) + tempOffsetGyro) / GYROS_ATT_FILTER;
break;
case 16:
400,41 → 405,39
 
void analog_calibrate(void) {
#define GYRO_OFFSET_CYCLES 32
uint8_t i;
int32_t _pitchOffset = 0, _rollOffset = 0, _yawOffset = 0;
uint8_t i, axis;
int32_t deltaOffsets[3] = {0,0,0};
int16_t filteredDelta;
 
// Set the filters... to be removed again, once some good settings are found.
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;
GYROS_PID_FILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1;
GYROS_ATT_FILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1;
GYROS_D_FILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1;
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1;
 
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);
_pitchOffset += rawGyroSum[PITCH];
_rollOffset += rawGyroSum[ROLL];
_yawOffset += rawYawGyroSum;
for (axis=0; axis<=YAW; axis++) {
deltaOffsets[axis] += rawGyroSum[axis] - gyroOffset[axis];
}
}
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;
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.
 
for (axis=0; axis<=YAW; axis++) {
filteredDelta = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
gyroOffset[axis] += filteredDelta;
}
 
// Noise is relative to offset. So, reset noise measurements when changing offsets.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
 
accOffset[PITCH] = (int16_t)GetParamWord(PID_ACC_PITCH);
accOffset[ROLL] = (int16_t)GetParamWord(PID_ACC_ROLL);
ZAccOffset = (int16_t)GetParamWord(PID_ACC_TOP);
accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
accOffset[ROLL] = GetParamWord(PID_ACC_ROLL);
accOffset[Z] = GetParamWord(PID_ACC_Z);
 
Delay_ms_Mess(100);
}
 
/*
446,30 → 449,35
*/
void analog_calibrateAcc(void) {
#define ACC_OFFSET_CYCLES 10
uint8_t i;
int32_t _pitchAxisOffset = 0, _rollAxisOffset = 0, _ZAxisOffset = 0;
uint8_t i, axis;
int32_t deltaOffset[3] = {0,0,0};
int16_t filteredDelta;
// int16_t pressureDiff, savedRawAirPressure;
accOffset[PITCH] = accOffset[ROLL] = ZAccOffset = 0;
 
for(i=0; i < ACC_OFFSET_CYCLES; i++) {
Delay_ms_Mess(10);
_pitchAxisOffset += acc[PITCH];
_rollAxisOffset += acc[ROLL];
_ZAxisOffset += ZAcc;
for (axis=0; axis<=YAW; axis++) {
deltaOffset[axis] += acc[axis];
}
}
 
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, (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));
SetParamWord(PID_ACC_PITCH, accOffset[PITCH]);
SetParamWord(PID_ACC_ROLL, accOffset[ROLL]);
SetParamWord(PID_ACC_Z, accOffset[Z]);
 
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
// Delay_ms_Mess(100);
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...)
495,7 → 503,7
pressureDiff += (rawAirPressure - savedRawAirPressure);
}
 
DebugOut.Analog[15] = rangewidth =
DebugOut.Analog[16] = rangewidth =
(pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2);
*/
}
/branches/dongfang_FC_rewrite/analog.h
2,8 → 2,9
#define _ANALOG_H
#include <inttypes.h>
 
// #include "invenSense.h"
#include "ENC-03_FC1.3.h"
//#include "invenSense.h"
//#include "ENC-03_FC1.3.h"
#include "ADXRS610_FC2.0.h"
 
/*
* How much low pass filtering to apply for gyro_PID.
10,7 → 11,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_PIDFILTER 1
// #define GYROS_PID_FILTER 1
 
/*
* How much low pass filtering to apply for gyro_ATT.
17,10 → 18,10
* 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_INTEGRALFILTER 1
// #define GYROS_ATT_FILTER 1
 
// Temporarily replaced by userparam-configurable variable.
//#define ACC_FILTER 4
// #define ACC_FILTER 4
 
/*
About setting constants for different gyros:
28,7 → 29,10
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 the same as in FC1.0-1.3, and reverse otherwise.
direction is:
- Nose down for pitch
- Left hand side down for roll
- Clockwise seen from above for yaw.
Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
100,6 → 104,9
#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.
160,6 → 167,8
 
#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
180,12 → 189,12
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
*/
extern volatile int16_t rawGyroSum[2], rawYawGyroSum;
extern volatile int16_t rawGyroSum[3];
 
/*
* The acceleration values that this module outputs. They are zero based.
*/
extern volatile int16_t acc[2], ZAcc;
extern volatile int16_t acc[3];
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)
 
int32_t correctionSum[2] = {0,0};
int16_t correctionSum[2] = {0,0};
 
/*
* Experiment: Compensating for dynamic-induced gyro biasing.
165,6 → 165,7
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
 
dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0;
correctionSum[PITCH] = correctionSum[ROLL] = 0;
// Calibrate hardware.
analog_calibrate();
178,6 → 179,7
 
// update compass course to current heading
compassCourse = compassHeading;
 
// Inititialize YawGyroIntegral value with current compass heading
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
 
199,6 → 201,7
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.
237,10 → 240,6
ACYawRate = yawRate;
}
 
DebugOut.Analog[3] = ACRate[PITCH];
DebugOut.Analog[4] = ACRate[ROLL];
DebugOut.Analog[5] = ACYawRate;
 
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
273,7 → 272,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 what I would call a "minus 1st order correction"
* There is (there) also a drift compensation
* - it corrects the differential of the integral = the gyro offsets.
* That should only be necessary with drifty gyros like ENC-03.
************************************************************************/
282,13 → 281,13
// are less than ....., or reintroduce Kalman.
// Well actually the Z axis acc. check is not so silly.
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]) {
int32_t correction;
if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= 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 accDerived;
if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
permilleAcc /= 2;
304,11 → 303,23
* Add to each sum: The amount by which the angle is changed just below.
*/
for (axis=PITCH; axis<=ROLL; axis++) {
accDerived[axis] = getAngleEstimateFromAcc(axis);
correctionSum[axis] += permilleAcc * (accDerived[axis] - angle[axis]);
accDerived = getAngleEstimateFromAcc(axis);
DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
// 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.
angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived[axis]) / 1000L;
// 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;
}
DebugOut.Digital[1] = debugFullWeight;
322,25 → 333,28
* (that happens in correctIntegralsByAcc0thOrder above) but rather the
* cause of it: Gyro drift, vibration and rounding errors.
* All the corrections made in correctIntegralsByAcc0thOrder over
* MINUSFIRSTORDERCORRECTION_TIME cycles are summed up. This number is
* then divided by MINUSFIRSTORDERCORRECTION_TIME to get the approx.
* DRIFTCORRECTION_TIME cycles are summed up. This number is
* then divided by DRIFTCORRECTION_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.
#define DRIFTCORRECTION_TIME 488/2
void driftCompensation(void) {
// 2 times / sec. = 488/2
#define DRIFTCORRECTION_TIME 256L
void driftCorrection(void) {
static int16_t timer = DRIFTCORRECTION_TIME;
int16_t deltaCompensation;
int16_t deltaCorrection;
uint8_t axis;
if (! --timer) {
timer = DRIFTCORRECTION_TIME;
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;
// 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];
correctionSum[axis] = 0;
DebugOut.Analog[28 + axis] = dynamicOffset;
}
}
}
351,9 → 365,18
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();
driftCompensation();
driftCorrection();
#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 UfoArrangement; // x oder + Formation
uint8_t Unused0; //
uint8_t IFactor; // Value : 0-250
uint8_t UserParams1[4]; // Value : 0-250
/*
/branches/dongfang_FC_rewrite/eeprom.c
95,10 → 95,11
* While we are still using userparams for flight parameters, do set
* some safe & meaningful default values.
*/
staticParams.UserParams1[4] = 0b01010101;
staticParams.UserParams1[5] = 2; // H&I motor smoothing.
staticParams.UserParams1[6] = 120;
staticParams.UserParams1[7] = 5;
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.
}
 
void setOtherDefaults(void) {
131,7 → 132,7
staticParams.LowVoltageWarning = 94;
staticParams.EmergencyGas = 35;
staticParams.EmergencyGasDuration = 30;
staticParams.UfoArrangement = 0;
staticParams.Unused0 = 0;
staticParams.IFactor = 32;
staticParams.ServoPitchControl = 100;
staticParams.ServoPitchComp = 40;
146,7 → 147,6
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,11 → 184,10
/* 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);
198,14 → 197,13
/* 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_TOP 8 // word
#define PID_ACC_Z 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] = ZAxisAcc;
DebugOut.Analog[18] = HIRES_GYRO_INTEGRATION_FACTOR;
 
DebugOut.Analog[19] = throttleTerm;
DebugOut.Analog[20] = term[PITCH];
/branches/dongfang_FC_rewrite/invenSense.c
4,12 → 4,17
 
#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))) {
29,7 → 34,7
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.GyroAccFactor = 1;
staticParams.DriftComp = 1;
staticParams.DriftComp = 10;
 
// Not used.
staticParams.AngleTurnOverPitch = 85;
/branches/dongfang_FC_rewrite/invenSense.h
4,15 → 4,8
#include "sensors.h"
 
#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:
33,9 → 26,4
*/
#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,6 → 1,19
extern const uint8_t GYROS_REVERSE[2];
#ifndef _SENSORS_H
#define _SENSORS_H
 
#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.
12,3 → 25,5
* 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) ", //5
"Pitch_Raw ",
"HiResPitch_PID ",
"PitchGyroOffset ",
"Roll_Raw ",
"HiResRoll_PID ", //10
"rollGyroOffset ",
"GyroYaw(AC) ",
"AccPitch ",
"AccRoll ", //10
"CorrSumPitch ",
"CorrSumRoll ",
"Pressure range ",
"Pressure A/D ",
"Pressure Value ",
"Press. rangewidz", //15
"Pressure Value ", //15
"Press. rangewidz",
"Press. init A/D ",
"Ext. Quality ",
"Looping ",
"FACTOR ",
"throttleTerm ",
"pitchTerm ", //20
"rollTerm ",
150,8 → 150,8
"P+D Pitch ", //25
"Pitch acc noise ",
"Roll acc noise ",
"Pitch Corr ",
"Roll Corr ",
"DynamicPitch ",
"DynamicRoll ",
"Pitch Noise ", //30
"Roll Noise "
};