Blogs

Special Request for a new "get_bearing" function in ArduPilot source

Posted by Paul Mather on May 22, 2010 at 10:00pm

I'd like to ask any of the math wizards out there if you can write and post an improved "get_bearing" function for the navigation tab of the ArduPilot source.

SimulationParis.gif

The more I test the ArduPilot code with my emulator, the more nervous I get. You'll notice in the screenshot of my 43MPH flight around Paris on my emulator, the ArduPilot decided to take a few loops around waypoints and the larger the distance between the waypoints, the greater the error in bearing.

Here's the existing get_bearing function

long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}

E-mail me when people leave their comments –
Follow

You need to be a member of diydrones to add comments!

Join diydrones

Comments

  • Paul Mather May 24, 2010 at 2:24pm
    No physical properties whatsoever. The ArduPilot outputs the string below. The emulator reads the THH, RER and PSET commands and turns the plane accordingly (pitch, yaw not roll and throttle). Cross track was disabled (I'm assuming XTRACK_GAIN 00 means disabled) for the first test in the OP. The last screenshot has XTRACK_GAIN 3000 and the results are better but still bouncing and weaving.

    Serial.print("!!!");
    Serial.print("STT:");
    Serial.print((int)control_mode);
    Serial.print(",WPN:");
    Serial.print((int)wp_index);
    Serial.print(",DST:");
    Serial.print(wp_distance);
    Serial.print(",RER:");
    Serial.print(nav_roll/100);
    Serial.print(",PSET:");
    Serial.print(nav_pitch/100);
    Serial.print(",THH:");
    Serial.print(servo_throttle);
    Serial.print(",WPR:");
    Serial.print((int)wp_radius);
    Serial.print(",ALT:");
    Serial.print(current_loc.alt/100,DEC);
    Serial.print(",ALH:");
    Serial.print(next_WP.alt/100,DEC);
    Serial.println(",***");
    print_telemetry = false;
  • Developer
    Jason Short May 24, 2010 at 1:42pm
    Is Crosstrack disabled? I would start there and turn off any I or D gains. P could be turned to 1 and you should see sharp angles no? Or does your simulator include the physical properties of the plane?
    Jason
  • T3
    Brian Wolfe May 24, 2010 at 1:35pm
    I agree with Ryan. It's almost as if the GPS lag isn't being considered and the controller over shoots the heading to the next waypoint then has insufficient gain to zero the error so has to wait for the error to build as the WP gets closer. I think upping the P gain will help reduce the heading error sooner, but doesn’t address the initial overshoot problem. Eventually the P gain will become unstable but this should manifest itself has a higher frequency oscilation. I’m not explaining myself very well but maybe you catch my drift.
  • Developer
    Ryan Beall May 24, 2010 at 1:11pm
    There is still something fundamentally wrong with the controller or sim. It just isn't making sense. The whole "rainbow" effect just doesn't make sense to me. Every time it hits a waypoint. Its commanding it way off from the logical course and then it corrects back? The SE corner is totally befuddling me? Is this a right hand pattern or left hand pattern? Even with homing, you would expect this behavior? I would try and track down what is happeing right at the waypoint switch. It is like everytime it is making a switch it is commanding a due north turn for a split second and then picks back up where it is suposed to? Hence why it works ok for some parts but not so well in others.
  • lionel May 24, 2010 at 12:21pm
    OK, sorry guys, I over reacted but was quite afraid that:
    - Someone from the DIYDrones community would try this for real
    - Someone from Paris would load the data and actually try it, the site would take the blame (I know the Frenchies and especially the Parisians, having some 30+ years of experience leaving there before managing to escape ;) )

    Kind regards,
    Lionel
  • hopslink May 24, 2010 at 7:57am
    Besides, La Tour Virtuelle is just a twist on Virtual Sparkfun.

  • Admin
    Morli May 24, 2010 at 7:47am
    Just kidding. Lionel , Happy K is also author of few gr8 codes here and If i am not wrong , coder for original/updated F/W Remzibi OSD.
  • Admin
    Morli May 24, 2010 at 7:40am
    Happy, I almost believed you were going to fly to/in Paris :)))))))))))
  • Paul Mather May 24, 2010 at 6:54am
    lionel, come on now. I'm flying my plane on an emulator. I have no plans to visit France. I live in Ohio, USA. There aren't many landmarks people would recognize here....
  • lionel May 24, 2010 at 5:03am
    Nobody knows where one plans to spend holidays: I reside in Europe but will fly my UAV around Montreal this summer :-)
    Never the less, first step is to check for:
    - The local regulation
    - The open mindness of the authorities
This reply was deleted.