ardupilot goes into the water Part 10

Software...

There a several problems, when you do embedded software documentation. The most evident one is, that you don´t have so cute looking pictures as you have when documenting mechanics or hardware.
Despite that fact, i provide one:


This nice plate was photographed in the vicinity of a Buddhist temple on my last holiday in Thailand.

The wisdom of this sentence becomes evident, when you are a developer, especially if you are a software-flavoured developer.
A (now retired) customer of mine, who was specialized in mechanics and optics development once asked me:
"Do you know what´s the real hell on earth? "
I said "no".
"I will tell you: It is water and software combined in one device"

At that time we were involved in the software development for a laser-system that had water-cooling.
He was right!
Bringing the ArduPilot into the water was a bit like that project.

The baseline for the development of the ArduPilot software was the very first version of the ArduPilot, which was published as Version 1.0 and can be downloaded from Google-code. http://code.google.com/p/ardupilot/
The V1.0 zip file should be found here: http://ardupilot.googlecode.com/files/ArduPilot1.0.zip

For the installation of the Arduino platform, please have a closer look to this site:


here you will find a very good introduction, how to get all up and running. I also would suggest to read the whole Ardupilot Manual first, before starting any actions: The Manual can be found here

The following list contains the modifications and bug fixes that i have applied to the V1.0 code. The list may not be complete, because my book-keeping of the changes was not very accurate, when i was in the hot-phase of the development.

1) Some #defines for constants were added, because "magic numbers" inside the code may cause troubles, when used more than once. All constants are now shown in upper-case letters.
2) The data-structure for the waypoint-list was changed to a predefined array, so the initialization of the predefined array is done by the startup-code of arduino itself. The tab "Mission_setup" was no longer used and was therefore deleted.
The array that holds the waypoints is now located in the "ArduPilot" tab.
The structure for the waypoints looks like this:

typedef struct
{ float lon;
float lat;
}LONLAT;

The predefined waypoint array looks like this:
LONLAT wps[ ] =
{
10.02069619383977, 48.35006621372347,
10.02018762320307, 48.35061609619746,
10.021905, 48.350598 /* Home*/
};

3) The number of elements that are contained in the array is calculated upon startup. (one of my first recovery-swimming actions was caused by an erroneous waypoint, that the ship tried to reach, because the "#define waypoints" directive of the original version did not match with the real number of elements in the array) .

4) All code that had to do with "Return to launch" or "Manual Mode" was eliminated.

5) All code that had to do with altitude control was eliminated.

6) Some unused test variables have been deleted.

7) All calculations in the PID-control are done now on a one-second base. In the original code, the time betweeen the executions was calculated and the value was multiplied / divided by the Integrator and derivator part (the dt variable in the 1.0 code). When using a 1Hz update rate, the factor is one and so, i deleted the code, to avoid unnecessary floating point operations. In reality, the update rate, which is driven by the GPS jitters a little bit, because the NMEA-messages are different in length. But for the practical use, this is of no concern.
Attention, when using a GPS with a faster update-rate, this functionality has to be reactivated.

8) In the orginal PID-control, the "heading_output" variable, that keeps the result of the PID calculations was of type "int". The calculations are done in float. This led to a big inaccuracy, because i used k-values that were below 1.0. Maybe for the first implementations of the Ardupilot nobody noticed that, because for the EasyStar values of around 10 were used for the kp and the integrator/derivator were not used and so, the error caused by that was very small. I changed it to float and all was fine.

9) I changed some suspicious "int" to "float" assignments and did a casting, everywhere i found it necessary.

10) The integrator values are reset upon startup and on each waypoint switch and held to zero for 25 seconds after the switch. This cures two problems:
1. The NMEA-parser has still a bug, that sometimes produces erroneous readings upon startup.
2. after sharp turns, the integrator adds up and causes offset problems. The 25 seconds is the time needed for a full 180° turn of the actual ship (catamarane).
This time is set in the #define WP_TIMEOUT directive.

11) I often use #if 0 (1) directives to switch off or on code or data segments.
i find this more convenient than commenting the parts out.

12) I added #define "model" segments to switch between different parameter sets (catamarane and monokeel in the actual version). note, that only one can be active at at time

For the moment, thats all that can be told about the software. Please leave a comment if you have any further questions or suggestions.
Here is the .zip file that contains my actual code:



Not much pictures in this episode, but i add here a "guess-what" one.
More about that in the next episode...


E-mail me when people leave their comments –

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

Join diydrones

Comments

  • Hi Warren,

    fine to hear that it works now.
    Yes, you have to replace the GPS waypoint data with your own.
    Or - you come to southern Germany on a particular lake to try it out :-)
    I created the waypoints in the following manner:
    1. create a path inside Google earth
    2. export it as KML (not KMZ)
    3. edit the KML file with a text editor and extract the latitude and longitude values, strip the altitude.
    (Using the Mission planning tool for the Ardupilot will also do, but if i remember right,you have to swap the values. The Mission planning tool has a file output).
    4. embed the data into your code.

    But before starting that, take some known waypoints from the shoreline of your lake with a GPS and look, if they match with the google-earth map. Depending on how good the material is, there are always some meters difference to the reality. It is good practice to keep 10 meters away from the shoreline. The last waypoint is important, because this is the "return home one" This one i have taken from GPS.

    I will provide you the code from my RC experiments. Actually i have commented out the RC- code because i returned to the more thrilling -no RC- variant.
    Please give me some days to find the original files.
  • Thank you Harald,
    Yes I am using IDE 18 and changing the filename let it compile correctly... thanks again.

    And yes, if you have the code for optional R/C control, I would love to try it out. Also, I should be able to safely remove your GPS waypoint data from the code you provided and replace it with my own right? Should I do this from Google maps or using the program provided for the Ardupilot?
  • Hi Morli,
    yes, you are right, it was a long time...
    but soon there will be at least one new episode.
    (still thinking, which story i will take first)
    Ardupilot Mega, RC-control, 2010 swimming, new lake, RC plane crashes, marathon training...
  • Admin
    Hi Harald,
    pls keep us posted with your progress. It has been long time since your last episode and interesting read. thanks & good luck with new setup.
  • HI JD,

    do you have ARDUINO 18 ?

    if yes, there is a Problem with the Tab called PID&Control.pde"
    Change the filename in the Ardupilot directory to "PID_Control.pde"

    seems that ARDUINO has some problems with weird chaarcters like "&" the filenames.
    I encountered this problem yesterday evening (coincidence?) when switching to ARDUINO18.

    I already have made the code for switching to RC control. If you are interested, please let me know.

    regards
    Harald
  • Harald,
    First off, thanks for sharing your project - I am trying a similar project with a model boat and the Ardupilot, planning GPS guidance around a nearby lake. I will only need a few waypoints to get around the lake, and I have a boat with propeller and rudder. I am using the Ardupilot v1 board and the em406 GPS module, but plan on keeping the option of R/C control via toggle switch available as I hope to not go swimming after it.

    I tried to compile your code and it came up with 2 errors:
    "reset_PIDs();" was not declared in the scope
    which was in the main Ardupilot tab of the sketch...and:
    "PID_heading" was not declared in the scope
    which was in the Autopilot tab.

    I searched through each tab to see if I could find anything that resembled those variables, but could not. Can you provide some insight on them?

    Thanks,
    JD
  • You are right in all points!
    The problem with GPS-only navigation is well known. Without movement, no velocity or heading vector.
    And the slower you move, the more inaccurate is the direction reading. his also depends heavily on the GPS model used. I first took a 5Hz one and it was completely crap, because it started to update the heading above a speed of five knots. For a ship, this was absolutely unusable. I have made very good experience with the EM406, which is very accurate and the heading information is very stable down to speeds of about 1 knot-. I also wanted to use a compass after my first "fightings" with my early ship models. For a dynamic model, i find a compass or Gyro absolutely necessary because the update rate of 1Hz and the latency of the readings make it nearly impossible to control it with a "simple" PID controller as the ArduPilot does. Maybe, a more sophisticated control algorithm will make it better (eg. a Kalman filter) but this will by far exceed the capabilities of the ArduPilot.

    Anyway, first try it with the EM406 and the Ardupilot and see, how the ship behaves. If it is not controllable at all, i would then move to a IMU-like approach with a GPS as reference for the waypoints and a compass for the heading-control.

    The Ardupilot can normally do what you want, depending how much waypoints you will spend for the mission. A U-shape is normally not a problem, because you can design nearly any arbitrary pattern with Google-Earth. I also did this for meander-lile missions
    The range for the waypoint-hit can be adjusted in the Ardupilot software by a #define switch, which is actually set to 10m. You can change it or adapt it to your needs.
    The motors speed of my vessel is actually not controlled. I set the ESC to a fixed value and thats it. The velocity is very stable around 5.5 km/h with that settings. For more sharp turns, you may reduce the speed prior to the waypoint.
  • I'm not familiar with Ardupilot but have have used a number of marine autopilots. The problem with steering to a GPS heading on a boat is that, no matter how fast the GPS is updated, there will be the following problems:
    1. A GPS calculates the vessel's heading based on a line from its last position to its current position. Consequently a GPS heading is not really the vessel's current heading but rather a historical track.
    2. The accuracy of that GPS heading degrades as the vessel moves slower, and looses all accuracy if the vessel stops. For faster moving vehicles (ie. aircraft) you can probably work around this, but for a slow moving boat this will always be a problem.

    Consequently a marine autopilot basically works in the following way:
    1. A waypoint is set for the vessel to steer to and the GPS calculates the heading to steer to reach that waypoint.
    2. The compass is used to maintain the vessel's heading.
    3. The heading to steer is updated every x seconds (or minutes) from the GPS to the autopilot.

    When a waypoint is reached (usually within a certain range of the waypoint), the heading to steer to the next waypoint is calculated and the vessel turns to that new heading using the compass. This usually results in a large arc where the vessel is off track for a considerable distance. We usually use 2 waypoints then at a turn so the vessel's track is "U" shaped rather than a single waypoint which results in a "V" shaped track (like in your earlier post).

    I don't know if Ardupilot is capable of doing this (or if doing it this way interests you) but I have worked on a number of research ships and conducted sonar surveys and this is typically the methodology we used. I should also note that I've grossly over simplified the process but hopefully I've given enough info be make my point clearly.
  • A digital compass would make it a bit better, when the update rate is higher. A GPS lacks in realtime reaction, because the Kalman filter needs a little time to adapt, when in sharp turns (some seconds latency i assume) . Faster GPS makes the turn rate faster, but the accuracy is worse. For my project, this was absolutely no problem, because the turnrate is only dependant on the ship´s geometry and not from the PID controller. When in an 180° turn the rudder is already at its extreme positions. The Problem comes up, when the ship is on its right course after the turn and the Integrator part is still "charged".
    The flow goes like this:
    PI controller before the turning point
    P controller while turning
    PI controller after the turn (x seconds after the turning point)
  • Wouldn't a digital compass eliminate the need for your workaround?
This reply was deleted.