| /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: |