/branches/dongfang_FC_rewrite/attitude.c |
---|
155,7 → 155,8 |
************************************************************************/ |
int32_t getAngleEstimateFromAcc(uint8_t axis) { |
return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
int16_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256; |
return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
} |
void setStaticAttitudeAngles(void) { |
295,9 → 296,16 |
// Well actually the Z axis acc. check is not so silly. |
uint8_t axis; |
int32_t temp; |
debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
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 <= dynamicParams.maxAccVector) { |
debugOut.digital[0] |= DEBUG_ACC0THORDER; |
314,15 → 322,14 |
permilleAcc /= 2; |
debugFullWeight = 0; |
*/ |
uint8_t ca = controlActivity >> 8; |
if (ca > staticParams.maxControlActivity) { // reduce effect during stick control activity |
if (highControlActivity) { // reduce effect during stick control activity |
permilleAcc /= 4; |
debugOut.digital[1] |= DEBUG_ACC0THORDER; |
if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
permilleAcc /= 4; |
debugOut.digital[1] |= DEBUG_ACC0THORDER; |
} |
} else { |
debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
} |
/* |
/branches/dongfang_FC_rewrite/configuration.c |
---|
111,6 → 111,9 |
SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255); |
SET_POT_MM(dynamicParams.output1Timing, staticParams.outputFlash[1].timing,1,255); |
SET_POT(dynamicParams.levelCorrection[0], staticParams.levelCorrection[0]); |
SET_POT(dynamicParams.levelCorrection[1], staticParams.levelCorrection[1]); |
} |
const XLATION XLATIONS[] = { |
223,6 → 226,7 |
staticParams.minThrottle = 8; |
staticParams.maxThrottle = 230; |
staticParams.externalControl = 0; |
staticParams.motorSmoothing = 0; |
// IMU |
staticParams.gyroPIDFilterConstant = 1; |
247,6 → 251,7 |
staticParams.IFactor = 32; |
staticParams.yawIFactor = 100; |
staticParams.compassYawEffect = 128; |
staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128; |
// Servos |
staticParams.servoCount = 7; |
/branches/dongfang_FC_rewrite/configuration.h |
---|
41,6 → 41,7 |
uint8_t servoManualControl[2]; |
uint8_t motorSmoothing; |
uint8_t levelCorrection[2]; |
} dynamicParam_t; |
extern volatile dynamicParam_t dynamicParams; |
109,7 → 110,8 |
uint8_t externalControl; // for serial Control |
uint8_t maxAccVector; |
uint8_t maxControlActivity; |
uint8_t motorSmoothing; |
// IMU |
uint8_t gyroPIDFilterConstant; |
uint8_t gyroATTFilterConstant; |
132,6 → 134,7 |
uint8_t IFactor; // Value : 0-250 |
uint8_t yawIFactor; |
uint8_t compassYawEffect; // Value : 0-32 |
uint8_t levelCorrection[2]; |
// Servos |
uint8_t servoCount; |
/branches/dongfang_FC_rewrite/flight.c |
---|
87,7 → 87,7 |
/* Filter for motor value smoothing (necessary???) */ |
/************************************************************************/ |
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
switch (dynamicParams.motorSmoothing) { |
switch (staticParams.motorSmoothing) { |
case 0: |
return newvalue; |
case 1: |