Autonomous Navigation With an ESP8266

Display showing current heading (bearing), target heading, direction to turn and GPS position

We think its good to have drones at sea, we need them, we need them to be cheap, so we can do stuff in our oceans like iron fertilizes, grow crops, generate fresh water etc. To work on that we develop a simple ocean drone at low cost. The parts of which are easy to buy. Thanks to an active tinkering community the software development is now also very easy.

D1 Mini Pro esp8266 board

We use an ESP8266 module called the D1 Mini Pro because it is easy to program using the Arduino IDE and it has a respectable transmission range of up to 100 meters (Wifi). We plan to use an external transmitter (1,5 km range) but for testing this is enough. Once it navigates we can program it to stay close to the side of the pond we will test in.

NEO-6M

The GPS module is from amazon, the NEO-6M. It spits out the GPS data through a serial connection at 9600 Baud. It has the coordinates, spead over land and altitude if you need that data. It pinpointed our desk very precisely inside our home from about 4 satelites.

HMC 5883L

The compass module is the HMC 5883L three axis magnetometer. It has an I2C connection, for which there is an actual adafruit library, so it is easy to query. Its not tilt compensated so this might be a problem at sea. For the purpose of a first prototype to work from this will do.

Battery with charge controller

As a power source we want to use a floating solar panel, but for now we use this LiPo battery and charging module. We also add an oled screen for good measure. Servo’s can be controlled directly from the ESP8266 pins. We tested all the connected devices and we get input from them and can output to the screen. I2C connections can be shared, its a bus type protocol. The GPS is read by so called soft serial input, which can be any pin.

GPS and compass calculating a heading to a waypoint. Display shows bearing, heading, direction to turn, longitude and lattitude

The code below will show you the heading and turn direction (left or right) you will have to make to reach the coordinates (latto,lonto) given at the beginning of the script. Those coordinates can be found using Google Maps. They show as decimal coordinates in the address bar. The screen shows your coordinates as you move. The next steps are to make it possible to set waypoints (new destinations) and use the turn direction to actually control a small drone boat.

#include <SoftwareSerial.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_SSD1306.h>

    // Compass Assign a unique ID to this sensor at the same time 
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

    // Oled Display
#define OLED_RESET 0  // GPIO0
Adafruit_SSD1306 display(OLED_RESET);

    // GPS
String gpsline;
String gpspos;

float latfrom; // gps current coordinates
float lonfrom; // gps current coordinates
float latto = 52.107177; 
float lonto = 4.269356;
float heading; // the direction you should go
float bearing; // the direction you are pointed
float control; // the direction 1 = right -1 = left to turn

static const int RXPin = 0, TXPin = 15; // Pin D3
static const uint32_t GPSBaud = 9600;

// The serial connection to the GPS device
SoftwareSerial ss(RXPin, TXPin);

void setup()
{
    Serial.begin(9600);

    if( ! mag.begin())
    { 
        Serial.println("ERROR : Check the compass wiring");
        while(1);
    }

    // Oled
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  
    // initialize with the I2C addr 0x3C (for the 64x48)
    display.display();
    display.clearDisplay();
    
    //text display tests
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0,0);
    display.println("Tracker 0.1");
    display.display();
    delay(1000);

     // Start GPS
    ss.begin(GPSBaud);
  
}

// read compass
float getcompassheading() {
  
    sensors_event_t event; 
    mag.getEvent( & event);

    float currentheading = atan2(event.magnetic.y, event.magnetic.x);
  
    if(currentheading < 0)
        currentheading += 2 * PI;
    
    return currentheading * 180 / M_PI; 
  
    }

// calculate bearing from two float coordinates
float calculatebearing(float lat, float lon, float lat2, float lon2){

    float teta1 = radians(lat);
    float teta2 = radians(lat2);
    float delta1 = radians(lat2-lat);
    float delta2 = radians(lon2-lon);

    float y = sin(delta2) * cos(teta2);
    float x = cos(teta1) * sin(teta2) - sin(teta1) * cos(teta2) * cos(delta2);
    float brng = atan2(y, x);
    brng = degrees(brng); // radians to degrees
    brng = (((int)brng + 360) % 360); 

    return brng;

}

    // string function to parse GPS
    String getValue(String data, char separator, int index)
{
    int found = 0;
    int strIndex[] = {0, -1};
    int maxIndex = data.length()-1;

    for(int i=0; i <= maxIndex && found <= index; i ++ ){
        if(data.charAt(i) == separator || i == maxIndex){
            found ++ ;
            strIndex[0] = strIndex[1] + 1;
            strIndex[1] = (i == maxIndex) ? i + 1: i;
        }
    }

    return found > index ? data.substring(strIndex[0], strIndex[1]): "";
}

// Move the decimal point to where it should be
float cleancoordinate(String coordinate) {
    char ch = '.';
    String c1 = getValue(coordinate, '.', 0);
    String c2 = getValue(coordinate, '.', 1);    
    int clen = c1.length();
    String cbefore = c1.substring(0, clen-2); 
    String cafter = c1.substring(clen-2, clen);
    String result = cbefore + '.' + cafter + c2;
         
    return result.toFloat();
  
}

// translate difference between heading and bearing
// into a control signal go left or right
String getcontroldirection(float bearing1,float heading1) {

    //Serial.println("get control direction");

    //Serial.println(" bearing " + String(bearing1) + " heading " + String(heading1));

    float opposite = heading1 + 180;
    
    if(opposite > 360) {
        opposite = opposite - 360;
    }

    if(heading1 > 0 and heading1 < 180) {
      if(bearing1 < heading1 and bearing1 > 0) {
        
        control = 1;
        return "1 right";     
      }

      if(bearing1 > (heading1 + 180)) { 
        
        control = 1;
        return "2 right";        
      }
  
      if(bearing1 < (heading1 + 180) and bearing1 > heading1) {
          
          control = -1;
          return "3 left";      
      }

    } else {

      if(bearing1 < heading1 and bearing1 > opposite) {
        
        control = 1;
        return "4 right";  
      }

      if(bearing1 > heading1 and bearing1 < 360) {
        
        control = -1;
         return "5 left";
      }

      if(bearing1 > 0 and bearing1 < opposite) {
         
         control = -1;
         return "6 left";     
      }
    }
}

void loop()
{
        
        gpsline = ss.readStringUntil('\n');

        gpspos = gpsline.substring(1,6);

        bearing  = getcompassheading();

        // Only parse the GPRMC line of the GPS info
        if(gpspos == "GPRMC") {

            String slonfrom = gpsline.substring(19, 29);        
            float lonfrom = cleancoordinate(slonfrom);
            String slatfrom = gpsline.substring(32, 42);
            float latfrom = cleancoordinate(slatfrom);
   
            heading = calculatebearing(latfrom, lonfrom, latto, lonto);  
                   
            String advice = getcontroldirection(bearing,heading);
            
            display.clearDisplay();
            display.setCursor(0,0);
            display.println(String(bearing) + ' ' + advice);
            display.println(String(heading) + ' ' + String(control));
            display.println(slonfrom);
            display.println(slatfrom);
            display.display();  
            
        }

}

You can order a complete device for 80,- Euro ext btw/shipping if you mail us at info@greencheck.nl. Any requests are welcome.

The data can also be shared to your phone, by Wifi where you can have a screen to set a new target. This needs some development but should allow you to use Google Maps to set waypoints.

You could make Robocop glasses 😉

Next step is to add a 1,5 km transmitter to the mix, this will allow remote control from the beach or side of the water as we plan to use the controller in a floating device. We’re also looking at an RC interface.