Suggestion - apply angular rate change algorithm to control better. Reduce I sum impact if rotational speed is too high. Reduce faster when approaching neutral attitude faster. Still allow I build up for unstable frames .
Current code - for angle variations, existing I method sums up and does not start to reduce until passing through neutral point. Momentum and the time for I to decay after this causes overshoot. A lot for high I values.
To mitigate we are all reducing I to minimise the impact and hide the issue.
For pure stable hover it is not a big issue, but as soon as start using controlled inputs such as level mode flying / GPS it is not optimal.
I have been running with this for a couple of month, but only recently had time to test fully and post. It's better than I expect. Almost perfectly matching wanted to achieve
Particularly important to have best stable mode setup now we are going with GPS hold and RTH.
Benefits I've noticed from testing:
-More precise feel flying in stable/angle mode.
-Almost eliminate wobble / overshoot situations caused by I
-Can run much higher values of I - stronger/faster reaction to deviations
-No impact to unbalanced frames - actually - is better. Can use stronger I and it sums up faster
-Should be largely frame/sensor independant - as its based upon rate of change
-Very tolerant of bad PI tuning
Negatives:
-Very small amount of memory
-Very small increase in processing time during stable mode
-Reduces speed of rotation. In practice not really seeing the impact.
I think negatives are worth accepting as this is core funtionality
Try it.......!
Put level p=0 and double level I to previously unflyable levels and see how it performs
For a quick tryout, replace this line near end of multiwii.pde:
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
With this
// DEV - start of test code - use with precaution.
int16_t errorAngleDelta=0;
int16_t Iratecomp=0;
static int16_t olderrorAngle[4];
errorAngleDelta=olderrorAngle[axis]-errorAngle;
olderrorAngle[axis]=errorAngle;
Iratecomp=constrain(errorAngleDelta*50,-2000,+2000); // 50&2000 from empirical testing for an acceptable rate.
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle-Iratecomp,-10000,+10000); // WindUp //16 bits is ok here
// DEV - start of test code - use with precaution.
If happy feel free to implement an optimised version!
Finally tested today with GPS Hold and RTH. No issues
Love to hear some testers feedback.
Suggest putting it on a switch so can switch of/on and compare the difference. It helps for
if rcData[AUX1] > 1700 {
int16_t errorAngleDelta=0;
int16_t Iratecomp=0;
static int16_t olderrorAngle[4];
errorAngleDelta=olderrorAngle[axis]-errorAngle;
olderrorAngle[axis]=errorAngle;
Iratecomp=constrain(errorAngleDelta*50,-2000,+2000); // 50&2000 from empirical testing for an acceptable rate.
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle-Iratecomp,-10000,+10000); // WindUp //16 bits is ok here
}
else{
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
}
.
