Initial commit

Aquapilot.cpp

@@ -0,0 +1,131 @@
+#include "Aquapilot.h"
+#include <cmath>
+
+Aquapilot::Aquapilot() {
+  curr_wp = nWaypoints = l_motor = r_motor = rudderAngle = 0;
+}
+
+void Aquapilot::updateCompass(double heading) {
+  Waypoint w = waypoints[curr_wp];
+  double bearing = calcBearing(latitude, longitude, w.lat, w.lon);
+  double relBearing = bearing - heading;
+
+  // Left motor
+  if (relBearing > 0)
+    l_motor = 255 * w.speed;
+  else
+    l_motor = (1.4167 * relBearing + 255) * w.speed;
+
+  // Right motor
+  if (relBearing < 0)
+    r_motor = 255 * w.speed;
+  else
+    r_motor = (-1.4167 * relBearing + 255) * w.speed;
+
+  // Rudder
+  rudderAngle = (relBearing + 180) / 360 * 255;
+
+  #ifdef DEBUG
+  cout << "------------------------------\n";
+  //cout << "Distance: " << dist << "\n";
+  cout << "Bearing: " << bearing << "\n";
+  cout << "Relative Bearing: " << relBearing << "\n";
+  cout << "Motors: " << l_motor << " " << r_motor << "\n";
+  cout << "------------------------------\n";
+  #endif
+}
+
+
+void Aquapilot::updateGPS(double lat, double lon, bool hasFix) {
+  // If we lose the GPS fix
+  if (!hasFix) {
+    l_motor = r_motor = 0; // Shutdown motors
+    return;
+  }
+
+  // Update the object's lat/lon for future use by the compass
+  latitude = lat;
+  longitude = lon;
+
+  // Current waypoint
+  Waypoint w = waypoints[curr_wp];
+
+  // Have we reached the waypoint yet?
+  double dist = distance(lat, lon, w.lat, w.lon);
+  if (dist < WAYPOINT_RADIUS) {
+
+    // Are there more waypoints remaining?
+    if (curr_wp < nWaypoints) {
+      w = waypoints[++curr_wp];
+    }
+    else {
+      l_motor = r_motor = 0;
+    }
+  }
+}
+
+void Aquapilot::addWaypoint(double lat, double lon, double speedFactor) {
+  waypoints[nWaypoints].lat = lat;
+  waypoints[nWaypoints].lon = lon;
+  waypoints[nWaypoints].speed = speedFactor;
+  nWaypoints++;
+}
+
+double Aquapilot::calcBearing(double lat1, double lon1, double lat2, double lon2) {
+  double y = sin(lon2 - lon1) * cos(lat2);
+  double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1);
+
+  return atan2(y, x) * 180 / 3.141592654;
+}
+
+double Aquapilot::distance(double lat1, double lon1, double lat2, double lon2)
+{
+  // Conversion factor from degrees to radians (pi/180)
+  const double toRadian = 0.01745329251;
+
+  // First coordinate (Radians)
+  double lat1_r = lat1 * toRadian;
+  double lon1_r = lon1 * toRadian;
+
+  // Second coordinate (Radians)
+  double lat2_r = lat2 * toRadian;  
+  double lon2_r = lon2 * toRadian;
+
+  // Delta coordinates 
+  double deltaLat_r = (lat2 - lat1) * toRadian;
+  double deltaLon_r = (lon2 - lon1) * toRadian;
+
+  // Distance
+  double a = sin(deltaLat_r/2)*sin(deltaLat_r/2) + cos(lat1_r) * cos(lat2_r) * sin(deltaLon_r/2) * sin(deltaLon_r/2);
+  double c = 2 * atan2(sqrt(a), sqrt(1-a));
+  double distance = 6371 * c * 1000;
+
+  return distance;
+}
+
+int Aquapilot::leftMotorSpeed() {
+  return l_motor;
+}
+
+int Aquapilot::rightMotorSpeed() {
+  return r_motor;
+}
+
+int Aquapilot::singleMotorSpeed() {
+  return waypoints[curr_wp].speed * 255;
+}
+
+int Aquapilot::rudder() {
+  return rudderAngle;
+}
+
+
+#ifdef DEBUG
+void Aquapilot::printWaypoints() {
+  for (int i = 0; i < nWaypoints; i++) {
+    Waypoint w = waypoints[i];
+    cout << "[" << i << "] Lat: " << w.lat << " Lon: " << w.lon << " Speed: " << w.speed << "\n";
+  }
+}
+#endif
+

Aquapilot.h

@@ -0,0 +1,34 @@
+#ifndef AQUAPILOT_H
+#define AQUAPILOT_H
+
+#define MAX_WAYPOINTS 32
+#define WAYPOINT_RADIUS 5 // 5 meter radius
+
+class Waypoint {
+  public:
+  double lat;
+  double lon;
+  double speed;
+};
+
+class Aquapilot {
+  public:
+  Aquapilot();
+  void updateGPS(double lat, double lon, bool hasFix);
+  void updateCompass(double heading);
+  void addWaypoint(double lat, double lon, double speedFactor);
+  int leftMotorSpeed();
+  int rightMotorSpeed();
+  int singleMotorSpeed();
+  int rudder();
+
+  private:
+  int curr_wp, nWaypoints, l_motor, r_motor, rudderAngle;
+  double latitude, longitude;
+  Waypoint waypoints[MAX_WAYPOINTS];
+  double calcBearing(double lat1, double lon1, double lat2, double lon2);
+  double distance(double lat1, double lon1, double lat2, double lon2);
+
+};
+
+#endif

README.md

@@ -0,0 +1,29 @@
+The AquaPilot library handles the computations for navigation. It is intended to aid in the development of a boat-based autopilot system. It is initially fed waypoints by the user, and then is constantly fed updates from a GPS and compass module. The calculations are processed and then the appropriate servo rudder values are given (or motor speeds if the boat turns by using dual motors).
+
+
+USAGE
+
+Installation:
+The source files must be copied into the Arduino library directory in a folder named "Aquapilot" (NOT "Aquapilot-Master"). The AquaPilot library should then be available within the Arduino IDE.
+
+Adding Waypoints:
+First add lat/lon waypoints by calling addWaypoint(LAT, LON, SPEED) where LAT and LON are latitude and logitude floats in decimal form (NOT IN NMEA FORM!), and speed is a float between 0 and 1 inclusive (I.E. 0.5 would be half speed). Note that the AquaPilot does not automatically return to its start destination. If you need the boat to return, the final waypoint needs to be the starting waypoint. Waypoints are generally accurate within 5 meters. 
+
+Next, the AquaPilot needs to be constantly fed updates from both the GPS an the compass. The calculations are computed upon receiving data from the compass (as the compass is typically updated much more frequently than the GPS).
+
+Feeding GPS Data:
+When the Arduino receives a GPS upate, call updateGPS(LAT, LON, HAS_FIX), where LAT and LON are latitude and logitude coordinates in decimal form and HAS_FIX is a boolean value that is true when the GPS has a fix. HAS_FIX is a safety feature so that if the GPS loses fix, the motors outputs will be set to zero so the boat will stop. 
+
+Feeding Compass Data:
+When the Arduino receives a compass update, call updateCompass(ANGLE), where angle is the degrees (0 is NORTH, 90 is EAST).
+
+Using Rudder Mode:
+Once GPS and Compass data have been fed to the AquaPilot, the rudder value can be retrieved by calling rudder() (a 0-255 PWM value to turn the servo). The motor speed can also be retrieved by calling singleMotorSpeed() (a 0-255 PWM value to output to the motor).
+
+Using Dual-Motor Mode:
+Once GPS and Compass data have been fed to the AquaPilot, the engine values can be retieved by calling leftMotorSpeed() and rightMotorSpeed (both are 0-255 PWM values to balance the motors speeds to turn as needed).
+
+CONTACT
+You may contact me with any questions at joefortune11@gmail.com
+
+