/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c |
---|
10,10 → 10,10 |
} |
void gyro_setDefaultParameters(void) { |
staticParams.accQuadrant = 4; |
staticParams.imuReversedFlags = IMU_REVERSE_ACC_XY; |
IMUConfig.accQuadrant = 4; |
IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_XY; |
staticParams.gyroD = 5; |
staticParams.driftCompDivider = 1; |
staticParams.driftCompLimit = 3; |
staticParams.zerothOrderCorrection = 1; |
IMUConfig.driftCompDivider = 1; |
IMUConfig.driftCompLimit = 3; |
IMUConfig.zerothOrderCorrection = 1; |
} |
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
---|
9,7 → 9,7 |
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510 |
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515 |
void I2C_OutputAmplifierOffsets() { |
void I2C_OutputAmplifierOffsets(void) { |
uint16_t timeout = setDelay(2000); |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
// Wait for I2C to finish transmission. |
64,7 → 64,7 |
delay_ms_with_adc_measurement(70, 0); |
} |
void gyro_init() { |
void gyro_init(void) { |
if (gyroAmplifierOffset_readFromEEProm()) { |
printf("gyro amp invalid, recalibrate."); |
gyroAmplifierOffset.offsets[PITCH] = |
76,10 → 76,11 |
} |
void gyro_setDefaultParameters(void) { |
staticParams.accQuadrant = 4; |
staticParams.imuReversedFlags = IMU_REVERSE_ACC_XY; |
IMUConfig.gyroQuadrant = 0; |
IMUConfig.accQuadrant = 4; |
IMUConfig.imuReversedFlags = IMU_REVERSE_GYRO_YAW | IMU_REVERSE_ACC_XY; |
staticParams.gyroD = 3; |
staticParams.driftCompDivider = 1; |
staticParams.driftCompLimit = 200; |
staticParams.zerothOrderCorrection = 25; |
IMUConfig.driftCompDivider = 1; |
IMUConfig.driftCompLimit = 200; |
IMUConfig.zerothOrderCorrection = 120; |
} |
/branches/dongfang_FC_rewrite/analog.c |
---|
1,6 → 1,7 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <stdlib.h> |
#include "analog.h" |
#include "attitude.h" |
346,11 → 347,11 |
} |
// 2.1: Transform axes. |
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
for (uint8_t axis=0; axis<2; axis++) { |
// 3) Filter. |
tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / staticParams.gyroPIDFilterConstant; |
tempOffsetGyro[axis] = (gyro_PID[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant; |
// 4) Measure noise. |
measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
376,7 → 377,7 |
/* |
* Now process the data for attitude angles. |
*/ |
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
dampenGyroActivity(); |
gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
386,12 → 387,12 |
measureGyroActivity(tempOffsetGyro[ROLL]); |
// Yaw gyro. |
if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
measureGyroActivity(yawGyro*staticParams.yawRateFactor); |
measureGyroActivity(yawGyro*IMUConfig.yawRateFactor); |
} |
void analog_updateAccelerometers(void) { |
400,14 → 401,14 |
acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
} |
rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY); |
rotate(acc, IMUConfig.accQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_XY); |
for(uint8_t axis=0; axis<3; axis++) { |
filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
filteredAcc[axis] = (filteredAcc[axis] * (IMUConfig.accFilterConstant - 1) + acc[axis]) / IMUConfig.accFilterConstant; |
measureNoise(acc[axis], &accNoisePeak[axis], 1); |
} |
// Z acc. |
if (staticParams.imuReversedFlags & 8) |
if (IMUConfig.imuReversedFlags & 8) |
acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
else |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
611,7 → 612,7 |
accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
int16_t min,max; |
if (axis==Z) { |
if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
// TODO: This assumes a sensitivity of +/- 2g. |
min = (256-200) * ACC_OVERSAMPLING_Z; |
max = (256+200) * ACC_OVERSAMPLING_Z; |
/branches/dongfang_FC_rewrite/attitude.c |
---|
1,5 → 1,6 |
#include <stdlib.h> |
#include <avr/io.h> |
#include <stdlib.h> |
#include "attitude.h" |
#include "dongfangMath.h" |
227,14 → 228,15 |
uint8_t axis; |
int32_t temp; |
uint16_t ca = gyroActivity >> 8; |
uint16_t ca = gyroActivity >> 9; |
debugOut.analog[14] = ca; |
uint8_t gyroActivityWeighted = ca / staticParams.rateTolerance; |
uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance; |
if (!gyroActivityWeighted) gyroActivityWeighted = 1; |
uint8_t accPart = staticParams.zerothOrderCorrection / gyroActivityWeighted; |
uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted; |
debugOut.analog[28] = IMUConfig.rateTolerance; |
debugOut.analog[15] = gyroActivityWeighted; |
debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
287,8 → 289,8 |
round = -DRIFTCORRECTION_TIME / 2; |
deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
// Add the delta to the compensation. So positive delta means, gyro should have higher value. |
driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider; |
CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit); |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
// DebugOut.Analog[16 + axis] = correctionSum[axis]; |
// debugOut.analog[28 + axis] = driftComp[axis]; |
/branches/dongfang_FC_rewrite/configuration.c |
---|
11,6 → 11,7 |
ParamSet_t staticParams; |
channelMap_t channelMap; |
mixerMatrix_t mixerMatrix; |
IMUConfig_t IMUConfig; |
volatile DynamicParams_t dynamicParams; |
uint8_t CPUType; |
152,11 → 153,6 |
staticParams.externalControl = 0; |
staticParams.motorSmoothing = 0; |
// IMU |
staticParams.gyroPIDFilterConstant = 1; |
staticParams.gyroDFilterConstant = 1; |
staticParams.accFilterConstant = 10; |
staticParams.gyroP = 60; |
staticParams.gyroI = 80; |
staticParams.gyroD = 4; |
167,8 → 163,6 |
// staticParams.driftCompLimit = |
staticParams.dynamicStability = 50; |
staticParams.rateTolerance = 10; |
staticParams.yawRateFactor = 4; |
staticParams.IFactor = 52; |
staticParams.yawIFactor = 100; |
staticParams.compassYawCorrection = 64; |
201,6 → 195,14 |
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS; |
staticParams.naviMode = 0; // free. |
staticParams.airpressureWindowLength = 0; |
staticParams.airpressureDWindowLength = 24; |
staticParams.heightControlMaxIntegralIn = 125; |
staticParams.heightControlMaxIntegralOut = 75; |
staticParams.heightControlMaxThrottleChange = 75; |
staticParams.heightControlTestOscPeriod = 0; |
staticParams.heightControlTestOscAmplitude = 0; |
} |
/***************************************************/ |
208,10 → 210,9 |
/***************************************************/ |
void paramSet_default(uint8_t setnumber) { |
setOtherDefaults(); |
gyro_setDefaultParameters(); |
for (uint8_t i=0; i<8; i++) { |
staticParams.userParams[i] = 0; |
staticParams.userParams[i] = i; |
} |
staticParams.bitConfig = |
220,6 → 221,16 |
memcpy(staticParams.name, "Default\0", 6); |
} |
void IMUConfig_default(void) { |
IMUConfig.gyroPIDFilterConstant = 1; |
IMUConfig.gyroDFilterConstant = 1; |
IMUConfig.accFilterConstant = 10; |
IMUConfig.rateTolerance = 120; |
IMUConfig.yawRateFactor = 4; |
gyro_setDefaultParameters(); |
} |
/***************************************************/ |
/* Default Values for Mixer Table */ |
/***************************************************/ |
/branches/dongfang_FC_rewrite/configuration.h |
---|
114,12 → 114,7 |
uint8_t timing; |
} output_flash_t; |
// values above 250 representing poti1 to poti4 |
typedef struct { |
// Global bitflags |
uint8_t bitConfig; // see upper defines for bitcoding |
// IMU |
uint8_t gyroQuadrant; |
uint8_t accQuadrant; |
uint8_t imuReversedFlags; |
130,11 → 125,19 |
uint8_t zerothOrderCorrection; |
uint8_t rateTolerance; |
uint8_t yawRateFactor; |
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
uint8_t driftCompLimit; // limit for gyrodrift compensation |
} IMUConfig_t; |
extern IMUConfig_t IMUConfig; |
// values above 250 representing poti1 to poti4 |
typedef struct { |
// Global bitflags |
uint8_t bitConfig; // see upper defines for bitcoding |
// uint8_t axisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
// uint8_t axisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
// uint8_t axisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
275,6 → 278,7 |
extern volatile uint8_t MKFlags; |
extern uint16_t isFlying; |
void IMUConfig_default(void); |
void channelMap_default(void); |
void paramSet_default(uint8_t setnumber); |
void mixerMatrix_default(void); |
/branches/dongfang_FC_rewrite/eeprom.c |
---|
124,6 → 124,28 |
} |
/***************************************************/ |
/* Read IMU Config from EEPROM */ |
/***************************************************/ |
uint8_t IMUConfig_readFromEEprom(void) { |
return readChecksummedBlock(IMUCONFIG_REVISION, (uint8_t*)&IMUConfig, EEPROM_ADR_IMU_CONFIG, sizeof(IMUConfig_t)); |
} |
/***************************************************/ |
/* Write IMU Config to EEPROM */ |
/***************************************************/ |
void IMUConfig_writeToEEprom(void) { |
writeChecksummedBlock(IMUCONFIG_REVISION, (uint8_t*)&IMUConfig, EEPROM_ADR_IMU_CONFIG, sizeof(IMUConfig_t)); |
} |
void IMUConfig_readOrDefault(void) { |
if(IMUConfig_readFromEEprom()) { |
printf("\n\rwriting default IMU config"); |
IMUConfig_default(); |
IMUConfig_writeToEEprom(); |
} |
} |
/***************************************************/ |
/* MixerTable */ |
/***************************************************/ |
void mixerMatrix_writeToEEProm(void) { |
133,7 → 155,7 |
void mixerMatrix_readOrDefault(void) { |
// load mixer table |
if (readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t))) { |
printf("writing default mixerMatrix"); |
printf("\n\rwriting default mixerMatrix"); |
mixerMatrix_default(); // Quadro |
mixerMatrix_writeToEEProm(); |
} |
157,7 → 179,7 |
void channelMap_readOrDefault(void) { |
if (readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t))) { |
printf("writing default channel map"); |
printf("\n\rwriting default channel map"); |
channelMap_default(); |
channelMap_writeToEEProm(); |
wdt_enable(WDTO_500MS); |
/branches/dongfang_FC_rewrite/eeprom.h |
---|
20,17 → 20,20 |
#define EEPROM_ADR_GYROOFFSET 32 |
#define EEPROM_ADR_GYROAMPLIFIER 48 |
#define EEPROM_ADR_CHANNELMAP 64 |
#define EEPROM_ADR_MIXER_TABLE 128 |
#define EEPROM_ADR_IMU_CONFIG 100 |
#define EEPROM_ADR_MIXER_TABLE 128 |
#define EEPROM_ADR_PARAMSET_BEGIN 256 |
#define CHANNELMAP_REVISION 0 |
#define EEPARAM_REVISION 3 |
#define EEPARAM_REVISION 4 |
#define EEMIXER_REVISION 0 |
#define SENSOROFFSET_REVISION 0 |
#define IMUCONFIG_REVISION 0 |
void paramSet_readOrDefault(void); |
void channelMap_readOrDefault(void); |
void mixerMatrix_readOrDefault(void); |
void IMUConfig_readOrDefault(void); |
uint8_t paramSet_readFromEEProm(uint8_t setnumber); |
void paramSet_writeToEEProm(uint8_t setnumber); |
/branches/dongfang_FC_rewrite/flight.c |
---|
197,7 → 197,7 |
// In HH mode, the I part is summed from P and D of gyros minus stick. |
if (gyroIFactor) { |
int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); |
if (axis == 0) debugOut.analog[28] = iDiff; |
//if (axis == 0) debugOut.analog[28] = iDiff; |
PDPart += iDiff; |
IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
} else { |
/branches/dongfang_FC_rewrite/invenSense.c |
---|
36,8 → 36,8 |
void gyro_setDefaultParameters(void) { |
staticParams.gyroD = 3; |
staticParams.driftCompDivider = 2; |
staticParams.driftCompLimit = 5; |
staticParams.zerothOrderCorrection = 1; |
staticParams.imuReversedFlags = IMU_REVERSE_ACC_Z; |
IMUConfig.driftCompDivider = 2; |
IMUConfig.driftCompLimit = 5; |
IMUConfig.zerothOrderCorrection = 1; |
IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_Z; |
} |
/branches/dongfang_FC_rewrite/main.c |
---|
65,6 → 65,7 |
#endif |
// Parameter Set handling |
IMUConfig_readOrDefault(); |
channelMap_readOrDefault(); |
mixerMatrix_readOrDefault(); |
paramSet_readOrDefault(); |
/branches/dongfang_FC_rewrite/makefile |
---|
20,18 → 20,18 |
#EXT = MK3MAG |
#EXT = |
#GYRO=ENC-03_FC1.3 |
#GYRO_HW_NAME=ENC |
#GYRO_HW_FACTOR=1.304f |
GYRO=ENC-03_FC1.3 |
GYRO_HW_NAME=ENC |
GYRO_HW_FACTOR=1.304f |
GYRO_PITCHROLL_CORRECTION=0.7f |
GYRO_YAW_CORRECTION=0.9f |
#GYRO=ADXRS610_FC2.0 |
#GYRO_HW_NAME=ADXR |
#GYRO_HW_FACTOR=1.2288f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
GYRO=ADXRS610_FC2.0 |
GYRO_HW_NAME=ADXR |
GYRO_HW_FACTOR=1.2288f |
GYRO_PITCHROLL_CORRECTION=1.0f |
GYRO_YAW_CORRECTION=1.0f |
#GYRO=invenSense |
#GYRO_HW_NAME=Isense |
#GYRO_HW_FACTOR=0.6827f |
/branches/dongfang_FC_rewrite/uart0.c |
---|
116,7 → 116,7 |
"EFT ", //25 |
"naviPitch ", |
"naviRoll ", |
"i part contrib ", |
"tolerance ", |
"Gyro Act Cont. ", |
"GPS altitude ", //30 |
"GPS vert accura " |
500,6 → 500,14 |
request_PPMChannels = TRUE; |
break; |
case 'i':// IMU configuration |
tempchar[0] = IMUCONFIG_REVISION; |
tempchar[1] = sizeof(IMUConfig); |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig)); |
break; |
case 'q':// request settings |
if (pRxData[0] == 0xFF) { |
pRxData[0] = getParamByte(PID_ACTIVE_SET); |