Error in navigation code

[GoogleCodeExporter]

What steps will reproduce the problem?
1. In Arduino, there in Commands tab, add the following Serial.print -lines:

void set_next_WP(struct Location *wp)

  ...

    // this is used to offset the shrinking longitude as we go towards the poles

        float rads = (abs(next_WP.lat)/t7) * 0.0174532925;

        scaleLongDown = cos(rads);
        scaleLongUp = 1.0f/cos(rads);

        Serial.print(" scaleLongDown: ");
        Serial.print(scaleLongDown);
        Serial.print("; scaleLongUp: ");
        Serial.print(scaleLongUp);
        Serial.print("; next_WP.lat: ");
        Serial.print(next_WP.lat);
        Serial.print("; rads: ");
        Serial.print(rads);
        Serial.print("; t7: ");
        Serial.print(t7);
        Serial.print("; ");

    // this is handy for the groundstation
  ...

2. Make and upload flight plan, start autopilot, wait for Gps-lock, switch to 
Auto.

3. Look serial data with Terminal. Somewhere in the texts there is something 
like this:

MSG <increment_WP_index> WP index is incremented to 1
MSG <set_next_wp> wp_index: 1
scaleLongDown: 1.00; scaleLongUp: 1.00; next_WP.lat: 627620672; rads: 0.00; t7: 
10000000.00;

What is the expected output? What do you see instead?

scaleLongDown and scaleLongUp are both always 1, rads is always 0. These should 
be something else, depending on latitude. 

What version of the product are you using? On what operating system?
APM 2.1.2, Windows 7

Please provide any additional information below.

It seems, that rads is calculated wrong. I tested code
float rads = (abs(float(next_WP.lat)/t7)) * 0.0174532925;

and this seems to show better numbers. (I am no expert in C-language...)
scaleLongDown: 0.47; scaleLongUp: 2.13; next_WP.lat: 627620672; rads: 1.08; t7: 
10000000.00;

I live quite north, latitude is about 62.7. Here one longitude degree is about 
half of latitude degree. 

I have had problems with navigation PIDs, plane made always strange extra 
curves. In the attached pictures you can see those curves.

In picture apm2.jpg there is screenshot from program, that I made to solve this 
problem. Log is from real flight with my plane. Plane (green dot) is coming to 
south. It has reached waypoint in east, and now it is going to waypoint in 
south-east.

In picture apm1.jpg there is zoomed to plane:
- Red line is gps course, plane is going to that direction.
- Brown line is Yaw, plane thinks it goes to that direction. This is correct.
- Green line is TargetBear, direction where the next waypoint SHOULD be.
- Black line is real nextpoint direction.
- Violet NavBear is always same as Target bear, so it is under green line

TargetBear  is not correct. So the plane is making a strange curve. Navbear is 
probably also wrong?

In picture apm3.jpg there is situation some moments later. Green line is still 
pointing to wrong direction.

In picture apm4.jpg there is one other flight with other PIDs. Green line again 
to wrong direction.

Both file logs and kml files are in attached files.

PS. To find this error was quite a hard job! First I thought that my navPIDs 
were wrong, but after tens of test flights with different PID  gains nothing 
changes to better. I have to make a program, which reads log and shows plane 
directions and positions. With this it was quite easy to find reason.

Original issue reported on code.google.com by alpo.has...@gmail.com on 3 Jul 2011 at 2:41

[GoogleCodeExporter]

Correct cos(62.762 degrees) should be about 0.45988
The corrected code above gives value 0.47, and this is same as cos(62 degrees)

How to change code to get correct values?

Original comment by alpo.has...@gmail.com on 3 Jul 2011 at 4:18

[GoogleCodeExporter]

One more typical route. Curves are in the same places.

Original comment by alpo.has...@gmail.com on 3 Jul 2011 at 4:51

[GoogleCodeExporter]

In the code line, is the abs really needed?
Cos in the 0 degrees (= equator) is 1, and then it reduces to both directions, 
and is 0 in the poles. Cos is always positive between poles.

Also Arduino manual says:
Warning

Because of the way the abs() function is implemented, avoid using other 
functions inside the brackets, it may lead to incorrect results.

abs(a++);   // avoid this - yields incorrect results

a++;          // use this instead -
abs(a);       // keep other math outside the function

Is it wise to check all APM codes?

I tested the following simple line:

float rads = next_WP.lat / t7 * 0.0174532925;

Now the flight seems much better: the green line points correctly to next 
waypoint. Today was 4-6 m/s wind from north-east, and going to that direction 
is not yet very good. Now I must adjust those PIDs.

Original comment by alpo.has...@gmail.com on 4 Jul 2011 at 5:08

[GoogleCodeExporter]

alpo.has...

Thank you for the bug report.  You correctly identified the location of the 
bug.  Your proposed fix would not work in the southern hemisphere...  The root 
cause was the use of the integer abs() function instead of the floating point 
fabs() function.

Bug fix has been uploaded to both the trunk and the branch.

Original comment by dewei...@gmail.com on 5 Jul 2011 at 7:45

• Changed state: Fixed

[GoogleCodeExporter]

Thank for the fix. It is now ok.

But I still don't agree that southern hemisphere :-)

I have code only for the latest version in Downloads section, 2.20

Local variable rads is used only in two lines.

If your latitude is for example -45.678, it is in radians -0.797, so 
rads = -0.797

Then you calculate two global variables from that value. These are used in 
other subroutines:

scaleLongDown = cos(-0.797) = 0.6988, so it is positive.
scaleLongUp = 1.0f/cos(-0.797) = 1.43, so it is also positive

Same variables in northern hemisphere:
rads = 0.797
scaleLongDown = cos(0.797) = 0.6988, so it is positive.
scaleLongUp = 1.0f/cos(0.797) = 1.43, so it is also positive

So both are positive in northern and southern hemisphere. Is fabs() really 
needed? Of course it is not an error, it can be there.

And here is the log from todays flight. Now everything seems as it should be :-)

Original comment by alpo.has...@gmail.com on 6 Jul 2011 at 7:04