Navigation beacon

2024-02-05 | By M5Stack

License: General Public License Arduino M5Stack M5Stick

Thank you for the source code and project information provided by @Juan Forero

Story

The device presented in this project aims at providing a navigation aid for swimmers with impaired vision. The initial challenge was to provide help when swimming in a pool, but later, a more interesting challenge was presented by Marc. He exposed his interest in swimming beyond pools and into open waters. This seems to be a more interesting challenge to me, thus this was the direction I went into.

How can one find its way in open waters without visual feedback? For one, navigational cues will need to be provided either as audio feedback or a vibrational stimulus. I chose to provide an audio cue for this purpose, which is nothing but vibration in the air. Second, one must be able to define a destination to swim towards as well as the current location where the swimmer is to calculate navigation guides to reach the destination. I chose using a GPS device for this purpose given even the small ones provide an accuracy of about 5 meters, which should be enough to guide the swimmer. Finally, the system should allow the swimmer to choose a destination easily, preferably with an approach as simple as point-and-click. Here I chose to use a digital compass to set the direction where the swimmer wants to head.

The system is quite simple. It consists of an ESP32 (M5StickC Plus) as the main unit, a GPS module (Mini GPS/BDS Unit), and an electronic magnetometer (based on the GY-271 chip). Everything is packed together and not bigger than a big pen.

Beacon system

The operation of the system is simple: 1) Turn on the system, 2) point towards the direction to destination, 3) click the button to set the destination, 4) use the audio feedback to navigate to the destination.

How does it all come together? The system is constantly collecting data from the GPS and the digital compass. When the button is pressed, a destination location is defined as 100 meters from the current position (based on the GPS measurement) heading in the direction the device is pointing at (based on the compass measurement). The target location can be reset by pointing in a different direction and clicking the button again. The device will report on the screen the distance from the current position to the destination, and at the same time, a beeping sound will be generated depending on how far the target is.

At this point the system is ready for a preliminary test field overground, and later after a few iterations (on the waterproofing case) ready for the real end goal test... open waters!...

An update is to come!

Schematics

Parts and assembly

These are all the components required to assemble the system, it's as simple as connect and secure.

Code

navigation_beacon Arduino

This code is to be uploaded into the M5StickC Plus using the Arduino IDE. Just connect the M5StickC Plus to the computer, launch the Arduino IDE, set the port where the M5StickC Plus is connected to, and upload the code.

Copy Code

//-------------------------------------------------------------------------
// Swimming beacon for M5StickC Plus
//-------------------------------------------------------------------------


//-------------------------------------------------------------------------
// Declarations
//-------------------------------------------------------------------------

// ----- Libraries
#include <M5StickCPlus.h>
#include <TinyGPSPlus.h>
#include <HardwareSerial.h>
#include <QMC5883LCompass.h>
#include <Math3D.h>
#include <Wire.h>
#include <OneButton.h>


// ----- Communication parameters
static const int RXPin = 33, TXPin = 32;
static const int HATSDAPin = 0, HATSCLPin = 26;
static const uint32_t GPSBaud = 9600;

static const int n_avg = 4;

// ----- Variables UI
int ledPin = G10;
OneButton btnA = OneButton(M5_BUTTON_HOME, true);
OneButton btnB = OneButton(M5_BUTTON_RST,  true);

// ----- Sensor settings
QMC5883LCompass compass;            // QMC5883L compass on HAT
TinyGPSPlus gps;                    // TinyGPSPlus object
HardwareSerial ss(2);               // Serial communication to the GPS device

// ----- Variables GPS
int currentYear, currentMonth, currentDay;
int currentHour, currentMinute, currentSecond, currentCentisecond;
double currentLat, currentLng; 
double destinationLat, destinationLng;

// ----- Variables compass
double offset = 0;              // offset for compass
double declination = 13.9167;    // declination for Edmonton, AB, Canada
float magnetometerX;
float magnetometerY; 
float magnetometerZ;
float compassX;
float compassY; 
float compassZ;
float bearing;

// ----- Variables accelerometer
float accelerometerX;
float accelerometerY; 
float accelerometerZ;

// ----- Variables gyroscope
float gyroscopeX;
float gyroscopeY; 
float gyroscopeZ;

// ----- Variables AHRS
float pitch;
float roll;
float yaw;

// ----- Variables beacon navigation
double goal_distance = 0.10;         // 
int    earthRadius = 6371;           // mean radius of the earth 

double current_course = 0;           // GPS based course
double destination_course = 0;       // GPS based course
double distance_to_destination = 0;  // GPS based distance to destination
double bearing_to_destination;       // difference between bearing and target direction
double course_to_destination;        // difference between course and target direction


//-------------------------------------------------------------------------
// Setup
//-------------------------------------------------------------------------
void setup() {
  
  // ----- M5StickC
  M5.begin();
  M5.Lcd.setRotation(0);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setCursor(0, 15);
  M5.Lcd.setTextColor(RED);
  M5.Lcd.println("Loading...");
  delay(500);

  // ----- IMU
  M5.IMU.Init();
  M5.Lcd.println("IMU     ok");
  delay(500);

  // ----- GPS
  ss.begin(GPSBaud, SERIAL_8N1, RXPin, TXPin);
  M5.Lcd.println("GPS     ok");
  delay(500);

  // ----- Compass
  Wire.begin(HATSDAPin, HATSCLPin);
  compass.init();
  M5.Lcd.println("Compass ok");
  delay(500);

  // ----- LED
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH);
  M5.Lcd.println("LED     ok");
  delay(500);
  
  // ----- Button handles
  btnA.attachClick(btnAClick);
  btnA.setDebounceTicks(40);  
  btnB.attachClick(btnBClick);
  btnB.setDebounceTicks(25);
  M5.Lcd.println("Buttons ok");
  delay(500);
} 


//-------------------------------------------------------------------------
// Main loop
//  run >> set destination >> run
//-------------------------------------------------------------------------
void loop() {
  delay(100);
  digitalWrite (ledPin, HIGH);
  
  // ----- Poll for button press
  btnA.tick();
  btnB.tick();

  // ----- Update data
  getMagnetometerValues();
  getAccelerometerValues();
  while (ss.available() > 0) {
    gps.encode(ss.read());
    readGPS();
  }

  // ----- Heading calculation
  headingFromCompass();
  headingFromGPS();

  // ----- Update navigation
  current_course = gps.course.deg();
  course_to_destination = gps.courseTo(currentLat, currentLng, destinationLat, destinationLng);
  distance_to_destination = gps.distanceBetween(currentLat, currentLng, destinationLat, destinationLng);

  course_to_destination = current_course - course_to_destination;
  bearing_to_destination = bearing - course_to_destination;
    
  // ----- Update data
  displayInfo_LCD();

}


//-------------------------------------------------------------------------
// Data acquisition functions
//-------------------------------------------------------------------------

// ----- Read GPS data
void readGPS() {

  //location
  if (gps.location.isValid()) {	
    currentLat = gps.location.lat();
    currentLng = gps.location.lng();
  } else {
    currentLat = 0;
    currentLng = 0;
  }
}

// ----- Read magnetometer data
void getMagnetometerValues() {
  
  //magnetometer data
  compass.read();
  magnetometerX = compass.getX();
  magnetometerY = compass.getY();
  magnetometerZ = compass.getZ();

  //calibrated data
  //MAG_calibrated = (MAG - b) * A;
  Vec3 b; // b = [-338.8024 -203.7892 116.4398];
  b.x = -338.8024; b.y = -203.7892; b.z =  116.4398;
  M3x3 A; // A = [0.9609 -0.0038 0.0084; -0.0038 0.9451 0.0017; 0.0084 0.0017 1.1012];
  A.a11 =  0.9609; A.a12 = -0.0038; A.a13 =  0.0084; 
  A.a21 = -0.0038; A.a22 =  0.9451; A.a23 =  0.0017;
  A.a31 =  0.0084; A.a32 =  0.0017; A.a33 =  1.1012;
  Vec3 MAG;
  MAG.x = magnetometerX; MAG.y = magnetometerY; MAG.z = magnetometerZ;
  Vec3 MAG_calibrated = Normalize(Rotate(Sub(MAG, b), A));

  //compass data
  compassX =  MAG_calibrated.y;
  compassY =  MAG_calibrated.x;
  compassZ = -MAG_calibrated.z;
}

// ----- Read accelerometer data
void getAccelerometerValues() {
  M5.IMU.getAccelData(&accelerometerX, &accelerometerY, &accelerometerZ);
}


//-------------------------------------------------------------------------
// Functions to perform calculations
//-------------------------------------------------------------------------

// ----- Destination calculator (based on http://www.efalk.org/Navigation/math.html#toc0)
void calculateDestination(double latitude_start, double longitude_start, double distance, double course, double &latitude_end, double &longitude_end) {
  double Bs = radians(latitude_start);       // Latitude at start, radians
  double Ls = radians(longitude_start);      // Longitude at start, radians
  double D  = distance / earthRadius;        // Distance to destination, radians
  double hdg = radians(course);              // Heading to destination, radians

  double Dx = D * sin(hdg);
  double Dy = D * cos(hdg);
  double B = Bs + Dy;
  double L = Ls + Dx / cos(B);

  latitude_end = degrees(B);
  longitude_end = degrees(L);
}

// ----- Compass heading with tilt compensation
void headingFromCompass() {

  Vec3 CMP; // calibrated compass
  CMP.x = compassX;
  CMP.y = compassY;
  CMP.z = compassZ;

  Vec3 ACC;
  ACC.x = accelerometerX;
  ACC.y = accelerometerY;
  ACC.z = accelerometerZ;

  Vec3 ACCn = Normalize(ACC);
  float d = DotProd(CMP, ACCn);
  Vec3 MAGw = Sub(CMP, Mul(d, ACCn));

  bearing = degrees(atan2(MAGw.y, MAGw.x)) - offset - declination;
}

// ----- GPS heading
void headingFromGPS() {

  // to destination
  distance_to_destination = gps.distanceBetween(gps.location.lat(), gps.location.lng(), destinationLat, destinationLng);
  course_to_destination = gps.courseTo(gps.location.lat(), gps.location.lng(), destinationLat, destinationLng);
}


//-------------------------------------------------------------------------
// Display info
//-------------------------------------------------------------------------

void displayInfo_LCD() {
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(2);

  //directions
  M5.Lcd.setCursor(0, 15);
  M5.Lcd.setTextColor(YELLOW);
  M5.Lcd.println(F("Direction: ")); 
  M5.Lcd.setTextColor(WHITE);
  M5.Lcd.print(bearing_to_destination,1);
  M5.Lcd.println(F(" deg"));
  M5.Lcd.print(course_to_destination,1);
  M5.Lcd.println(F(" deg"));
  
  //distance
  M5.Lcd.setCursor(0, 200);
  M5.Lcd.setTextColor(YELLOW);
  M5.Lcd.println(F("Distance: ")); 
  M5.Lcd.setTextColor(WHITE);
  M5.Lcd.print(distance_to_destination,1);
  M5.Lcd.println(F(" m"));

  //vectors
  drawVector_angle(course_to_destination, RED, 50);
  drawVector_angle(bearing_to_destination, DARKGREY, 30);
}


//-------------------------------------------------------------------------
// Drawing functions
//-------------------------------------------------------------------------

// ----- Draw vector using ANGLE
void drawVector_angle(double angle, uint32_t color, int size) {

  int centerX = M5.Lcd.width()/2;
  int centerY = M5.Lcd.height()/2 + 10;

  float angleA = angle;
  float angleB = angle - 150;
  float angleC = angle + 150;

  Vec3 A;
  A.x = centerX - sin(radians(angleA)) * size;
  A.y = centerY - cos(radians(angleA)) * size;
  A.z = 0;

  Vec3 B;
  B.x = centerX - sin(radians(angleB)) * size;
  B.y = centerY - cos(radians(angleB)) * size;
  B.z = 0;

  Vec3 C;
  C.x = centerX - sin(radians(angleC)) * size;
  C.y = centerY - cos(radians(angleC)) * size;
  C.z = 0;

  M5.Lcd.fillTriangle(A.x, A.y, B.x, B.y, C.x, C.y, color);  
}


// -----------------------------------
// Button functions
// -----------------------------------

// ----- RST button > power off
void btnBClick() {
  M5.Axp.PowerOff();

  digitalWrite (ledPin, HIGH);

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(3);
  M5.Lcd.setTextColor(WHITE);
  M5.Lcd.setCursor(0, 20);

  M5.Lcd.println("home");

}

// ----- HOME button > change state
void btnAClick() {
  digitalWrite (ledPin, LOW);
  delay(200);
  calculateDestination(currentLat, currentLng, goal_distance, bearing, destinationLat, destinationLng);
  destination_course = current_course;
  delay(500);
  M5.Beep.beep();
  delay(500);
  M5.Beep.end();
  digitalWrite (ledPin, HIGH);
}