Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2088 → Rev 2091

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