the mag-routine is not working as expected with my copter. So I had a look at the sourcecode and I hope I found the reason, why big differences betweend maghold and heading result in almost no reaction to the copter.
Here's the sourcecode:
#if MAG
if (abs(rcCommand[YAW]) <70 && magMode) {
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
if ( smallAngle25 ) rcCommand[YAW] -= dif*P8[PIDMAG]/30; // 18 deg
} else magHold = heading;
#endif
If I understand it right, the magHold-value should be set to actual heading, if the rcCommand[YAW]-value will be greater than 70 - that means, if I use the Yaw-Stick at the transmitter, the magHold should be actualized. BUT...
Let's assume, I use the default P8[PIDMAG] of 40 (4.0). And the magHold is 90 degree. Due to drift/wind/any other reason, the heading changed from 90 degree to 60 degree - a really noticable difference. The Yaw-stick at the transmitter is at neutral position, the copter is at horizontal position. Then the "new" rcCommand[YAW] will be:
0+30*40/30 = 40
The resulting value is so low, this will have almost no effect to the copter (at least at my copter). So we have to increase P8[PIDMAG], e.g. to 100 (10.0). Same assuments again, the "new" rcCommand[YAW] will be:
0+30*100/30 = 100
This is a value that would result in a (really slow) YAW-movement clockwise.
But it looks like the routine will be called again, before (!) the rcCommand[YAW] will be filled with the actual YAW-stick-position from the transmitter. So the rcCommand[YAW] will be NOT 0 at next call, but it's still 100. So the magHold will be set to the actual heading (60 in this case) and the copter will not try to rotate back to the 90 degree-position, but will try to stay at 60 degree.
Has anybode good ideas to fix this issue? Perhaps an additional boolean variable like "rcCommandYAWSetByMAG" would be an idea? It could be set to true anytime the rcCommand[YAW] is set automatically by the MAG-Routine and to false when the values from the RC are read.
The new code could be:
#if MAG
if ((abs(rcCommand[YAW]) <70 && magMode) || rcCommandYAWSetByMAG) {
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
if (rcCommandYAWSetByMAG) {
if ( smallAngle25 ) rcCommand[YAW] = -dif*P8[PIDMAG]/30;
} else {
if ( smallAngle25 ) rcCommand[YAW] -= dif*P8[PIDMAG]/30 // not sure, why the stick-YAW is needed in the calculation?
}
rcCommandYAWSetByMAG = 1;
} else magHold = heading;
#endif
Not implemented/tested yet - any comments/suggestions/better ideas? And where have I to reset the rcCommandYAWSetByMAG-variable in source code to make sure it's false after the values from the RC are read?
Regards,
Olaf.