| /branches/dongfang_FC_rewrite/analog.c |
|---|
| 284,6 → 284,7 |
| // for various filters... |
| int16_t tempOffsetGyro, tempGyro; |
| debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
| for (uint8_t axis=0; axis<2; axis++) { |
| tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis]; |
| 301,8 → 302,6 |
| debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
| tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE |
| + SENSOR_MAX_PITCHROLL; |
| } else { |
| debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
| } |
| } |
| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 155,8 → 155,8 |
| ************************************************************************/ |
| int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
| return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
| //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
| return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];// + correctionTerm; |
| } |
| void setStaticAttitudeAngles(void) { |
| /branches/dongfang_FC_rewrite/flight.c |
|---|
| 391,7 → 391,7 |
| CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| // Add (P, D) parts minus stick pos. to the scaled-down I part. |
| term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
| /* |
| * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
| * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |