Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2088 → Rev 2089

/branches/dongfang_FC_rewrite/analog.c
50,6 → 50,8
int32_t groundPressure;
int16_t dHeight;
 
uint32_t gyroActivity;
 
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
* standing still. They are used for adjusting the gyro and acc. meter values
304,17 → 306,8
}
}
 
// Experimental gyro shake takeoff detect!
uint16_t gyroActivity = 0;
void measureGyroActivityAndUpdateGyro(uint8_t axis, int16_t newValue) {
int16_t tmp = gyro_ATT[axis];
gyro_ATT[axis] = newValue;
 
tmp -= newValue;
tmp = (tmp*tmp) >> 4;
 
if (gyroActivity + (uint16_t)tmp < 0x8000)
gyroActivity += tmp;
void measureGyroActivity(int16_t newValue) {
gyroActivity += abs(newValue);
}
 
#define GADAMPING 10
385,15 → 378,20
*/
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
 
measureGyroActivityAndUpdateGyro(0, tempOffsetGyro[PITCH]);
measureGyroActivityAndUpdateGyro(1, tempOffsetGyro[ROLL]);
dampenGyroActivity();
gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
 
measureGyroActivity(tempOffsetGyro[PITCH]);
measureGyroActivity(tempOffsetGyro[ROLL]);
 
// Yaw gyro.
if (staticParams.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);
}
 
void analog_updateAccelerometers(void) {
414,7 → 412,7
else
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
 
debugOut.analog[29] = acc[Z];
// debugOut.analog[29] = acc[Z];
}
 
void analog_updateAirPressure(void) {
/branches/dongfang_FC_rewrite/analog.h
195,7 → 195,7
 
extern int16_t magneticHeading;
 
extern uint16_t gyroActivity;
extern uint32_t gyroActivity;
 
/*
* Flag: Interrupt handler has done all A/D conversion and processing.
/branches/dongfang_FC_rewrite/attitude.c
76,8 → 76,9
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
// int16_t dynamicCalCount;
// uint16_t accVector;
 
uint16_t accVector;
// uint32_t gyroActivity;
 
/************************************************************************
* Set inclination angles from the acc. sensor data.
119,6 → 120,7
 
// reset gyro integrals to acc guessing
setStaticAttitudeAngles();
 
#ifdef USE_MK3MAG
attitude_resetHeadingToMagnetic();
#endif
209,63 → 211,6
}
}
 
void correctIntegralsByAcc0thOrder_old(void) {
uint8_t axis;
int32_t temp;
 
uint8_t ca = controlActivity >> 8;
uint8_t highControlActivity = (ca > staticParams.maxControlActivity);
 
if (highControlActivity) {
debugOut.digital[1] |= DEBUG_ACC0THORDER;
} else {
debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
}
 
if (accVector <= staticParams.maxAccVector) {
debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
 
uint8_t permilleAcc = staticParams.zerothOrderCorrection / 8;
int32_t accDerived;
 
/*
if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
permilleAcc /= 2;
debugFullWeight = 0;
}
 
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
permilleAcc /= 2;
debugFullWeight = 0;
*/
 
if (highControlActivity) { // reduce effect during stick control activity
permilleAcc /= 4;
if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity
permilleAcc /= 4;
}
}
 
/*
* 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] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
// 1000 * the correction amount that will be added to the gyro angle in next line.
temp = attitude[axis];
attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
+ (int32_t) permilleAcc * accDerived) / 1000L;
correctionSum[axis] += attitude[axis] - temp;
}
} else {
// experiment: Kill drift compensation updates when not flying smooth.
// correctionSum[PITCH] = correctionSum[ROLL] = 0;
debugOut.digital[0] |= DEBUG_ACC0THORDER;
}
}
 
 
/************************************************************************
* A kind of 0'th order integral correction, that corrects the integrals
* directly. This is the "gyroAccFactor" stuff in the original code.
275,7 → 220,7
************************************************************************/
#define LOG_DIVIDER 12
#define DIVIDER (1L << LOG_DIVIDER)
void correctIntegralsByAcc0thOrder_new(void) {
void correctIntegralsByAcc0thOrder(void) {
// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
// are less than ....., or reintroduce Kalman.
// Well actually the Z axis acc. check is not so silly.
282,53 → 227,30
uint8_t axis;
int32_t temp;
 
// for debug LEDs, to be removed with that.
static uint8_t controlActivityFlash=1;
static uint8_t accFlash=1;
#define CF_MAX 10
// [1..n[=off [n..10]=on
// 1 -->1=on, 2=on, ..., 10=on
// 2 -->1=off,2=on, ..., 10=on
// 10-->1=off,2=off,..., 10=on
// 11-->1=off,2=off,..., 10=off
uint16_t ca = controlActivity >> 6;
uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance;
if (!controlActivityWeighted) controlActivityWeighted = 1;
uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance;
if (!accVectorWeighted) accVectorWeighted = 1;
uint16_t ca = gyroActivity >> 8;
debugOut.analog[14] = ca;
 
uint8_t accPart = staticParams.zerothOrderCorrection;
int32_t accDerived;
uint8_t gyroActivityWeighted = ca / staticParams.rateTolerance;
if (!gyroActivityWeighted) gyroActivityWeighted = 1;
 
debugOut.analog[14] = controlActivity;
debugOut.analog[15] = accVector;
uint8_t accPart = staticParams.zerothOrderCorrection / gyroActivityWeighted;
 
debugOut.analog[20] = controlActivityWeighted;
debugOut.analog[21] = accVectorWeighted;
debugOut.analog[24] = accVector;
debugOut.analog[15] = gyroActivityWeighted;
debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
 
accPart /= controlActivityWeighted;
accPart /= accVectorWeighted;
 
if (controlActivityFlash < controlActivityWeighted) {
debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
} else {
debugOut.digital[0] |= DEBUG_ACC0THORDER;
if (gyroActivityWeighted < 8) {
debugOut.digital[0] |= DEBUG_ACC0THORDER;
}
if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1;
 
if (accFlash < accVectorWeighted) {
debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
} else {
debugOut.digital[1] |= DEBUG_ACC0THORDER;
if (gyroActivityWeighted <= 2) {
debugOut.digital[1] |= DEBUG_ACC0THORDER;
}
if (++accFlash > CF_MAX+1) accFlash=1;
 
/*
* Add to each sum: The amount by which the angle is changed just below.
*/
for (axis = PITCH; axis <= ROLL; axis++) {
accDerived = getAngleEstimateFromAcc(axis);
int32_t accDerived = getAngleEstimateFromAcc(axis);
//debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
// 1000 * the correction amount that will be added to the gyro angle in next line.
temp = attitude[axis];
375,6 → 297,7
}
}
 
/*
void calculateAccVector(void) {
int16_t temp;
temp = filteredAcc[0] >> 3;
384,6 → 307,7
temp = filteredAcc[2] >> 3;
accVector += temp * temp;
}
*/
 
#ifdef USE_MK3MAG
void attitude_resetHeadingToMagnetic(void) {
478,15 → 402,11
************************************************************************/
void calculateFlightAttitude(void) {
getAnalogData();
calculateAccVector();
// calculateAccVector();
integrate();
 
#ifdef ATTITUDE_USE_ACC_SENSORS
if (staticParams.maxControlActivity) {
correctIntegralsByAcc0thOrder_old();
} else {
correctIntegralsByAcc0thOrder_new();
}
correctIntegralsByAcc0thOrder();
driftCorrection();
#endif
 
500,42 → 420,3
}
#endif
}
 
/*
* This is part of an experiment to measure average sensor offsets caused by motor vibration,
* and to compensate them away. It brings about some improvement, but no miracles.
* As long as the left stick is kept in the start-motors position, the dynamic compensation
* will measure the effect of vibration, to use for later compensation. So, one should keep
* the stick in the start-motors position for a few seconds, till all motors run (at the wrong
* speed unfortunately... must find a better way)
*/
/*
void attitude_startDynamicCalibration(void) {
dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0;
savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000;
}
 
void attitude_continueDynamicCalibration(void) {
// measure dynamic offset now...
dynamicCalPitch += hiResPitchGyro;
dynamicCalRoll += hiResRollGyro;
dynamicCalYaw += rawYawGyroSum;
dynamicCalCount++;
 
// Param6: Manual mode. The offsets are taken from Param7 and Param8.
if (dynamicParams.UserParam6 || 1) { // currently always enabled.
// manual mode
driftCompPitch = dynamicParams.UserParam7 - 128;
driftCompRoll = dynamicParams.UserParam8 - 128;
} else {
// use the sampled value (does not seem to work so well....)
driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount;
driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount;
driftCompYaw = -dynamicCalYaw / dynamicCalCount;
}
 
// keep resetting these meanwhile, to avoid accumulating errors.
setStaticAttitudeIntegrals();
yawAngle = 0;
}
*/
/branches/dongfang_FC_rewrite/configuration.c
154,7 → 154,6
// IMU
staticParams.gyroPIDFilterConstant = 1;
staticParams.gyroATTFilterConstant = 1;
staticParams.gyroDFilterConstant = 1;
staticParams.accFilterConstant = 10;
 
168,8 → 167,8
// staticParams.driftCompLimit =
staticParams.dynamicStability = 50;
staticParams.zerothOrderCorrectionAccTolerance = 60;
staticParams.zerothOrderCorrectionControlTolerance = 60;
staticParams.rateTolerance = 10;
staticParams.yawRateFactor = 4;
staticParams.IFactor = 52;
staticParams.yawIFactor = 100;
staticParams.compassYawCorrection = 64;
202,9 → 201,6
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS;
 
staticParams.naviMode = 0; // free.
 
staticParams.maxControlActivity = 0; // temporary!
staticParams.maxAccVector = 80;
}
 
/***************************************************/
/branches/dongfang_FC_rewrite/configuration.h
125,17 → 125,13
uint8_t imuReversedFlags;
uint8_t gyroPIDFilterConstant;
uint8_t gyroATTFilterConstant;
uint8_t gyroDFilterConstant;
uint8_t accFilterConstant;
 
uint8_t zerothOrderCorrection;
uint8_t zerothOrderCorrectionAccTolerance;
uint8_t zerothOrderCorrectionControlTolerance;
uint8_t rateTolerance;
uint8_t yawRateFactor;
 
uint8_t maxControlActivity; // old version for comparison.
uint8_t maxAccVector;
 
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung)
uint8_t driftCompLimit; // limit for gyrodrift compensation
 
/branches/dongfang_FC_rewrite/controlMixer.c
13,7 → 13,7
#include "output.h"
 
// uint16_t maxControl[2] = { 0, 0 };
uint16_t controlActivity = 0;
// uint16_t controlActivity = 0;
int16_t controls[4] = { 0, 0, 0, 0 };
 
// Internal variables for reading commands made with an R/C stick.
81,6 → 81,7
return rcQ > ecQ ? rcQ : ecQ;
}
 
/*
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
int16_t tmp = controls[index];
controls[index] = newValue;
89,11 → 90,12
tmp /= 2;
tmp = tmp * tmp;
// tmp += (newValue >= 0) ? newValue : -newValue;
/*
 
/ *
if (controlActivity + (uint16_t)tmp >= controlActivity)
controlActivity += tmp;
else controlActivity = 0xffff;
*/
* /
if (controlActivity + (uint16_t)tmp < 0x8000)
controlActivity += tmp;
}
105,6 → 107,7
tmp >>= CADAMPING;
controlActivity = tmp;
}
*/
 
/*
* Update the variables indicating stick position from the sum of R/C, GPS and external control
174,33 → 177,11
 
// Commit results to global variable and also measure control activity.
controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);
updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);
updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);
dampenControlActivity();
controls[CONTROL_PITCH] = tempPRTY[CONTROL_PITCH];
controls[CONTROL_ROLL] = tempPRTY[CONTROL_ROLL];
controls[CONTROL_YAW] = tempPRTY[CONTROL_YAW];
// dampenControlActivity();
// We can safely do this even with a bad signal - the variables will not have been updated then.
configuration_applyVariablesToParams();
// part1a end.
/* This is not really necessary with the dead-band feature on all sticks (see rc.c)
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (controlYaw > 2) controlYaw-= 2;
else if (controlYaw< -2) controlYaw += 2;
else controlYaw = 0;
}
*/
/*
* Record maxima. Predecessor of the control activity stuff.
for (axis = PITCH; axis <= ROLL; axis++) {
if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
if (maxControl[axis] > 100)
maxControl[axis] = 100;
} else if (maxControl[axis])
maxControl[axis]--;
}
*/
}
/branches/dongfang_FC_rewrite/heightControl.c
181,10 → 181,6
debugOut.analog[12] = dThrottleD;
debugOut.analog[13] = heightError/10;
 
//debugOut.analog[27] = dynamicParams.heightP;
//debugOut.analog[28] = dynamicParams.heightI;
//debugOut.analog[29] = dynamicParams.heightD;
 
int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD;
 
if (dThrottle > staticParams.heightControlMaxThrottleChange)
/branches/dongfang_FC_rewrite/makefile
273,7 → 273,7
AVRDUDE_RESET = -u -U lfuse:r:m
 
#avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) -B 10
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) -B 1
 
# Uncomment the following if you want avrdude's erase cycle counter.
# Note that this counter needs to be initialized first using -Yn,
/branches/dongfang_FC_rewrite/uart0.c
102,8 → 102,8
"heightI ",
"heightD ",
"heightErr/10 ",
"controlActivity ",
"accVector ", //15
"gyroAvtivity ",
"GActivityDivider", //15
"NaviMode ",
"NaviStatus ",
"NaviStickP ",
117,7 → 117,7
"naviPitch ",
"naviRoll ",
"i part contrib ",
"Acc Z ",
"Gyro Act Cont. ",
"GPS altitude ", //30
"GPS vert accura "
};