Published December 18, 2023

Navigation beacon

Navigation system to find your way in the absence of visual cues, such as swimming in open waters under low (or absent) visibility.

Beginner361

Things used in this project

Hardware components

M5Stack M5StickC ESP32-PICO Mini IoT Development Board

GY-271 HMC5883L 3 Axis Sensor Compass Magnetometer Module

M5Stack Mini GPS/BDS Unit

M5Stack M5StickC 18650C

M5Stack M5StickC Proto Hat

Software apps and online services

Arduino IDE

Story

"What were the needs or pain points that you attended to and identified when you were solving problems faced by the Contest Masters?"

The device presented in this projects 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 on a more interesting challenge was presented by Marc. He exposed his interest on swimming beyond pools and into open waters. This seems to be a more interesting challenge to me, thus this that 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 to this, one must be able to define a destination where to swim towards as well as the current location where the swimmer is at in order 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. And 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 want to head to.

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 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 the 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 on after a few iterations (on the waterproofing case) ready for the real end goal test... open waters!...

An update is to come!

Code

navigation_beacon

//-------------------------------------------------------------------------
// 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);
}

Credits

Juan Forero

1 project • 1 follower