Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2091 → Rev 2092

/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);