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