Subversion Repositories FlightCtrl

 
  /*
   * Here is a dynamic calibration experiment: Adjust integrals and gyro offsets if the pilot appears to be always
   * pushing of pulling on the pitch or roll stick.
   */
  /*
    if(ADCycleCount >= dynamicParams.UserParam2 * 10) {
    // This algo works OK on the desk but it is a little sluggish and it oscillates. 
    // It does not very effectively cancel drift because of dynamics.
    
    minStickForAutoCal = dynamicParams.UserParam3 * 10;
    maxStickForAutoCal = dynamicParams.UserParam4 * 10;
    
    // If not already corrected to the limit, and dynamic calibration is enabled:
    if (abs(dynamicOffsetPitch - savedDynamicOffsetPitch) < dynamicParams.UserParam1 && !dynamicParams.UserParam6) {
    // The pilot pushes on the stick, the integral is > 0, and the gyro val is > 0. Looks like a value-too-high case, so increase the offset.
    if (filteredHiResPitchGyro > dynamicOffsetPitch && pitchAngle > 0 && maxStickPitch >= minStickForAutoCal && maxStickPitch <= maxStickForAutoCal) {
    dynamicOffsetPitch += (int8_t)(dynamicParams.UserParam7 - 128); // (adding something seems right...)
    pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L;
    } else if (filteredHiResPitchGyro < dynamicOffsetPitch && pitchAngle < 0 && maxStickPitch <= -minStickForAutoCal && maxStickPitch >= -maxStickForAutoCal) {
    dynamicOffsetPitch -= (int8_t)(dynamicParams.UserParam7 - 128); // (subtracting something seems right...)
    pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L;
    }
    } 
    
    // If not already corrected to the limit, and dynamic calibration is enabled:
    if (abs(dynamicOffsetRoll - savedDynamicOffsetRoll) <= dynamicParams.UserParam1 && !dynamicParams.UserParam6) {
    if (filteredHiResRollGyro > dynamicOffsetRoll && rollAngle > 0 && maxStickRoll >= minStickForAutoCal && maxStickRoll <= maxStickForAutoCal) {
    dynamicOffsetRoll += (int8_t)(dynamicParams.UserParam8 - 128);
    rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L;
    } else if (filteredHiResRollGyro < dynamicOffsetRoll && rollAngle < 0 && maxStickRoll <= -minStickForAutoCal && maxStickRoll >= -maxStickForAutoCal) {
    dynamicOffsetRoll -= (int8_t)(dynamicParams.UserParam8 - 128);
    rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L;
    }
    }
    ADCycleCount = 0;
    }
  */