/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 " |
}; |