Add GNSS navigation

lib/ble/ble.cpp

@@ -3,6 +3,8 @@
 #include <BLEUtils.h>
 #include <BLESecurity.h>
 #include <BLEServer.h>
+#include "compass.h"
+#include "gnss.h"
 #include "motors.h"
 #include "trip.h"
 
@@ -13,7 +15,9 @@
 #define SERVICE_UUID            "da6bd24f-3f5e-44c8-8f68-dd615ab2d89d"
 #define CUT_UUID                "1971f36f-2c78-4f3d-8550-7f47d2d61bd3"
 #define MOVE_UUID               "9fe842b6-6b68-43e3-959e-d3896620a18f"
+#define TRIP_STOP_UUID          "17f9daa1-63a0-4808-9f77-b0f4fa4d054f"
 #define ROTATE_TO_BEARING_UUID  "1263cfb8-1212-4029-a2eb-7a223c3362e8"
+#define POSITION_UUID           "c1235944-2ca3-4a4f-99eb-31ea440e7515"
 
 
 BLESecurity *pSecurity = new BLESecurity();
@@ -22,8 +26,7 @@ class CutCallbacks : public BLECharacteristicCallbacks {
   public:
     void onRead(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
       int16_t value = MotorsController->GetCutSpeed();
-      Serial.print("Read cut value : ");
+      log_d("Read cut value : %d", value);
-      Serial.println(value);
       uint8_t data[2];
       data[0] = value;
       data[1] = value >> 8;
@@ -32,8 +35,7 @@ class CutCallbacks : public BLECharacteristicCallbacks {
 
     void onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
       int16_t newValue = *(int16_t*)pCharacteristic->getData();
-      Serial.print("Write cut value : ");
+      log_d("Write cut value : %d", newValue);
-      Serial.println(newValue);
       MotorsController->SetCutSpeed(newValue);
     }
 };
@@ -42,10 +44,7 @@ class MoveCallbacks : public BLECharacteristicCallbacks {
   public:
     void onRead(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
       MoveSpeed value = MotorsController->GetMoveSpeed();
-      Serial.print("Read move value : l=");
+      log_d("Read move value : l=%d r=%d", value.left, value.right);
-      Serial.print(value.left);
-      Serial.print(" r=");
-      Serial.println(value.right);
       uint8_t data[4];
       data[0] = value.left;
       data[1] = value.left >> 8;
@@ -59,34 +58,65 @@ class MoveCallbacks : public BLECharacteristicCallbacks {
       MoveSpeed *newValue = new MoveSpeed();
       newValue->left = (int16_t)data;
       newValue->right = (int16_t)(data >> 16);
-      Serial.print("Write move value : l=");
+      log_d("Write move value : l=%d r=%d", newValue->left, newValue->right);
-      Serial.print(newValue->left);
-      Serial.print(" r=");
-      Serial.println(newValue->right);
       MotorsController->SetMoveSpeed(*newValue);
     }
 };
 
-class RotateToBearingCallbacks : public BLECharacteristicCallbacks {
+class TripStopCallbacks : public BLECharacteristicCallbacks {
   public:
     void onRead(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
-      pCharacteristic->setValue(NULL);
+      uint8_t val = 0;
+      pCharacteristic->setValue(&val, 1);
+    }
+
+    void onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
+      uint8_t newValue = *pCharacteristic->getData();
+      if (newValue == 1) {
+        log_d("Stop trip");
+        TripController->Stop();
+      }
+    }
+};
+
+class HeadingCallbacks : public BLECharacteristicCallbacks {
+  public:
+    void onRead(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
+      float heading = Compass->GetHeading();
+      pCharacteristic->setValue(heading);
     }
     void onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
       float newValue = *(float*)pCharacteristic->getData();
-      Serial.print("Write rotate to value : ");
+      log_d("Write rotate to value : %.1f", newValue);
-      Serial.println(newValue);
+      TripController->RotateToBearing(newValue, 20000, true);
-      TripController->RotateToBearing(newValue);
+    }
+};
+
+class PositionCallbacks : public BLECharacteristicCallbacks {
+  public:
+    void onRead(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
+      geo_pos pos = GnssService->GetPosition();
+      double data[2] = { pos.lat, pos.lng };
+      pCharacteristic->setValue((uint8_t*)&data, 2 * sizeof(double));
+    }
+    void onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) {
+      double* data = (double*)pCharacteristic->getData();
+      geo_pos pos {
+        .lat = data[0],
+        .lng = data[1]
+      };
+      log_d("Go to lat: %.7f lng: %.7f", pos.lat, pos.lng);
+      TripController->GoTo(pos, 100, 120000, true);
     }
 };
 
 class ServerCallbacks : public BLEServerCallbacks {
   void onConnect(BLEServer *pServer) {
-    Serial.print("BLE client connected");
+    log_i("BLE client connected");
   }
 
   void onDisconnect(BLEServer *pServer) {
-    Serial.print("BLE client disconnected");
+    log_i("BLE client disconnected");
     pServer->startAdvertising();
   }
 };
@@ -106,9 +136,17 @@ void setup_ble() {
     pService->createCharacteristic(MOVE_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
   pMoveSpeed->setCallbacks(new MoveCallbacks());
   // Rotate to bearing
+  BLECharacteristic *pTripStop =
+    pService->createCharacteristic(TRIP_STOP_UUID, BLECharacteristic::PROPERTY_WRITE);
+  pTripStop->setCallbacks(new TripStopCallbacks());
+  // Rotate to bearing
   BLECharacteristic *pRotateToBearing =
-    pService->createCharacteristic(ROTATE_TO_BEARING_UUID, BLECharacteristic::PROPERTY_WRITE);
+    pService->createCharacteristic(ROTATE_TO_BEARING_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
-  pRotateToBearing->setCallbacks(new RotateToBearingCallbacks());
+  pRotateToBearing->setCallbacks(new HeadingCallbacks());
+  // Position
+  BLECharacteristic *pPosition =
+    pService->createCharacteristic(POSITION_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+  pPosition->setCallbacks(new PositionCallbacks());
 
   pService->start();
 
@@ -122,5 +160,5 @@ void setup_ble() {
   pSecurity->setAuthenticationMode(ESP_LE_AUTH_REQ_SC_MITM_BOND);
   pSecurity->setStaticPIN(123456);
 
-  Serial.println("BLE control initialized.");
+  log_i("BLE control initialized.");
 }

lib/compass/compass.cpp

@@ -7,7 +7,7 @@ CompassClass::CompassClass(int sda, int scl) {
   compass = new DFRobot_QMC5883(&Wire, QMC5883_ADDRESS);
   while (!compass->begin())
   {
-    Serial.println("Could not find a valid 5883 sensor, check wiring!");
+    log_e("Could not find a valid 5883 sensor, check wiring!");
     delay(500);
   }
 
@@ -22,10 +22,10 @@ float CompassClass::GetHeading() {
   float heading = atan2(-read.YAxis ,read.XAxis);
   //heading += this->ICdeclinationAngle;
   if(heading < 0)
-    heading += 2*PI;
+    heading += TWO_PI;
-  if(heading > 2*PI)
+  if(heading > TWO_PI)
-    heading -= 2*PI;
+    heading -= TWO_PI;
-  read.HeadingDegress = heading * 180/PI;
+  read.HeadingDegress = heading * RAD_TO_DEG;
   return read.HeadingDegress;
 }
 

lib/geo-util/geo-util.cpp

@@ -1,7 +1,15 @@
+#include <Arduino.h>
 #include <geo-util.h>
 
-float angle_difference(float source, float target) {
+const double EARTH_RADIUS = 6371000;
-  float delta = target - source;
+
+double sin2(double angle) {
+  double _sin = sin(angle);
+  return _sin * _sin;
+}
+
+double angle_difference(double source, double target) {
+  double delta = target - source;
   if (delta > 180) {
     delta -= 360;
   }
@@ -9,4 +17,39 @@ float angle_difference(float source, float target) {
     delta += 360;
   }
   return delta;
+}
+
+double bearing_to(geo_pos &origin, geo_pos &target) {
+  double l1 = to_radians(origin.lat);
+  double l2 = to_radians(target.lat);
+  double dl = l2 - l1;
+  double dg = to_radians(target.lng - origin.lng);
+
+  return to_degrees(
+    atan2(
+      sin(dg) * cos(l2),
+      cos(l1) * sin(l2) - sin(l1) * cos(l2) * cos(dg)
+    )
+  );
+}
+
+double distance_to(geo_pos &origin, geo_pos &target) {
+  double l1 = to_radians(origin.lat);
+  double l2 = to_radians(target.lat);
+  
+  double dl = l2 - l1;
+  double dg = to_radians(target.lng - origin.lng);
+
+  double a = sin2(dl / 2) + cos(l1) * cos(l2) * sin2(dg / 2);
+  double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+
+  return EARTH_RADIUS * c;
+}
+
+double to_radians(double degrees) {
+  return degrees * DEG_TO_RAD;
+}
+
+double to_degrees(double radians) {
+  return fmod((radians * RAD_TO_DEG) + 360., 360.);
 }

lib/geo-util/geo-util.h

@@ -2,9 +2,22 @@
 #ifndef _GEO_UTIL_H_
 #define _GEO_UTIL_H_
 
+typedef struct {
+  double lat;
+  double lng;
+} geo_pos;
+
 /**
  * Return angle difference in degrees
  */
-float angle_difference(float source, float target);
+double angle_difference(double source, double target);
+
+double bearing_to(geo_pos &origin, geo_pos &target);
+
+double distance_to(geo_pos &origin, geo_pos &target);
+
+double to_radians(double degrees);
+
+double to_degrees(double radians);
 
 #endif

lib/gnss/gnss.cpp

@@ -0,0 +1,10 @@
+#include <gnss.h>
+#include <SparkFun_u-blox_GNSS_v3.h>
+
+
+void setup_gnss() {
+  Serial1.begin(115200, SERIAL_8N1, 9, 10);
+  GnssService = new GnssServiceClass(Serial1);
+}
+
+GnssServiceClass* GnssService;

lib/gnss/gnss.h

@@ -0,0 +1,26 @@
+#pragma once
+#ifndef _GNSS_H_
+#define _GNSS_H_
+
+#include <geo-util.h>
+#include <SparkFun_u-blox_GNSS_v3.h>
+
+class GnssServiceClass
+{
+private:
+  SFE_UBLOX_GNSS_SERIAL* gnss;
+  static void Update(void *);
+public:
+  GnssServiceClass(Stream &serialPort);
+  geo_pos GetPosition();
+  float GetMotionHeading();
+  float GetMotionSpeed();
+  ~GnssServiceClass();
+};
+
+
+void setup_gnss();
+
+extern GnssServiceClass* GnssService;
+
+#endif

lib/gnss/gnssServiceClass.cpp

@@ -0,0 +1,65 @@
+#include <gnss.h>
+
+GnssServiceClass::GnssServiceClass(Stream &serialPort)
+{
+  gnss = new SFE_UBLOX_GNSS_SERIAL();
+  while(!gnss->begin(serialPort)) {
+    log_e("Could not find a valid GNSS module, check wiring!");
+    delay(500);
+  }
+
+  if (gnss->getModuleInfo())
+  {
+    log_i("GNSS module connected");
+    log_d("FWVER: %u.%u", gnss->getFirmwareVersionHigh(), gnss->getFirmwareVersionLow());
+    log_d("Firmware: %s", gnss->getFirmwareType());
+    log_d("PROTVER: %u.%u", gnss->getProtocolVersionHigh(), gnss->getProtocolVersionLow());
+  }
+
+  gnss->setUART1Output(COM_TYPE_UBX);
+  gnss->setMeasurementRate(100, VAL_LAYER_RAM);
+  gnss->setNavigationRate(1, VAL_LAYER_RAM);
+  gnss->setAutoPVT(true, VAL_LAYER_RAM),
+  gnss->saveConfiguration();
+
+  gnss->assumeAutoPVT(true, true);
+
+  log_d("Measure rate: %u", gnss->getMeasurementRate());
+  log_d("Navigation rate: %u", gnss->getNavigationRate());
+
+  xTaskCreate(GnssServiceClass::Update, "GnssUpdate", 1024, this, 5, nullptr);
+}
+
+void GnssServiceClass::Update(void * paramter) {
+  GnssServiceClass* gnssService = (GnssServiceClass*)paramter;
+  for(;;) {
+    gnssService->gnss->checkUblox();
+    gnssService->gnss->checkCallbacks();
+    delay(10);
+  }
+}
+
+
+geo_pos GnssServiceClass::GetPosition() {
+  //gnss->getPVT();
+  geo_pos result = {
+    .lat = gnss->getLatitude() / 10000000.,
+    .lng = gnss->getLongitude() / 10000000./*,
+    .alt = gnss->getAltitude() / 1000.*/
+  };
+  return result;
+}
+
+float GnssServiceClass::GetMotionHeading() {
+  //gnss->getPVT();
+  return gnss->getHeading() / 100000.;
+}
+
+float GnssServiceClass::GetMotionSpeed() {
+  //gnss->getPVT();
+  return gnss->getGroundSpeed() / 1000.;
+}
+
+GnssServiceClass::~GnssServiceClass()
+{
+}

lib/trip/trip.h

@@ -2,13 +2,21 @@
 #ifndef _TRIP_H_
 #define _TRIP_H_
 
+#include <Arduino.h>
+#include "geo-util.h"
+
 class TripControllerClass
 {
 private:
-  /* data */
+  TaskHandle_t _currentTask;
+  void _startTask(TaskFunction_t task, void *);
+  static void _rotateTask(void *);
+  static void _gotoTask(void *);
 public:
   TripControllerClass(/* args */);
-  void RotateToBearing(float heading);
+  void RotateToBearing(float heading, uint timeout, bool stop);
+  void GoTo(geo_pos &pos, u_int8_t speed, uint timeout, bool stop);
+  void Stop();
   ~TripControllerClass();
 };
 

lib/trip/tripController.cpp

@@ -1,28 +1,142 @@
 #include <compass.h>
 #include <geo-util.h>
+#include <gnss.h>
 #include <motors.h>
 #include <trip.h>
 
-TripControllerClass::TripControllerClass(/* args */)
+struct _rotate_args {
-{
+  float heading;
+  uint timeout;
+  bool stop;
+};
+
+struct _goto_args {
+  geo_pos pos;
+  u_int8_t speed;
+  uint timeout;
+  bool stop;
+};
+
+TripControllerClass::TripControllerClass() {
+  _currentTask = nullptr;
 }
 
-void TripControllerClass::RotateToBearing(float heading) {
+void TripControllerClass::RotateToBearing(float heading, uint timeout, bool stop) {
-  float delta = angle_difference(Compass->GetHeading(), heading);
+  static _rotate_args params;
+  params.heading = heading;
+  params.timeout = timeout;
+  params.stop = stop;
   
-  while (delta < -5 || delta > 5)
+  _startTask(
-  {
+    _rotateTask,
-    Serial.print("Rotate delta : ");
+    &params
-    Serial.println(delta);
+  );
-    MoveSpeed move;
+}
-    move.left = delta > 0 ? 20 + delta / 2 : -20 + delta / 2;
+
-    move.right = delta > 0 ? -20 - delta / 2 : 20 - delta / 2;
+void TripControllerClass::GoTo(geo_pos &pos, u_int8_t speed, uint timeout, bool stop) {
-    MotorsController->SetMoveSpeed(move);
+  static _goto_args params;
-    delay(100);
+  params.pos = pos;
-    delta = angle_difference(Compass->GetHeading(), heading);
+  params.speed = speed;
+  params.timeout = timeout;
+  params.stop = stop;
+  
+  _startTask(
+    _gotoTask,
+    &params
+  );
+}
+
+void TripControllerClass::Stop() {
+  if (_currentTask != nullptr) {
+    vTaskDelete(_currentTask);
+    _currentTask = nullptr;
+    log_i("Trip stopped !");
   }
   MotorsController->SetMoveSpeed({0 , 0});
-  Serial.println("Rotation completed !");
+}
+
+void TripControllerClass::_startTask(TaskFunction_t task, void * param) {
+  if (_currentTask != nullptr) {
+    vTaskDelete(_currentTask);
+  }
+  xTaskCreate(task, "CurrentTrip", 2048, param, 5, &_currentTask);
+}
+
+void TripControllerClass::_rotateTask(void * param) {
+  _rotate_args* args = (_rotate_args*)param;
+  int16_t delta = angle_difference(Compass->GetHeading(), args->heading);
+  
+  u_int32_t startTime = millis();
+  while ((delta < -5 || delta > 5) && millis() <= startTime + args->timeout)
+  {
+    MoveSpeed move = {
+      .left = (int16_t)(delta > 0 ? 20 + delta / 2 : -20 + delta / 2),
+      .right = (int16_t)(delta > 0 ? -20 - delta / 2 : 20 - delta / 2)
+    };
+    MotorsController->SetMoveSpeed(move);
+    delay(100);
+    delta = angle_difference(Compass->GetHeading(), args->heading);
+  }
+  if (args->stop) {
+    MotorsController->SetMoveSpeed({0 , 0});
+  }
+  log_i("Rotation completed !");
+  
+  while (args->stop)
+   delay(1000);
+}
+
+void TripControllerClass::_gotoTask(void * param) {
+  _goto_args* args = (_goto_args*)param;
+  geo_pos origin = GnssService->GetPosition();
+  double bearing = bearing_to(origin, args->pos);
+  double distance = distance_to(origin, args->pos);
+  log_d("Bearing : %.1f distance : %.2f");
+
+  uint32_t startTime = millis();
+
+  _rotate_args rotate_args = {
+    .heading = bearing,
+    .timeout = 15000,
+    .stop = false
+  };
+  _rotateTask(&rotate_args);
+  int bearingDelta = 0;
+  int correction = 0;
+  const int maxCorrection = args->speed / 2;
+  origin = GnssService->GetPosition();
+  distance = distance_to(origin, args->pos);
+  double lastDistance = distance;
+  MoveSpeed move = {
+    .left = args->speed,
+    .right = args->speed
+  };
+
+  while ((distance > 0.3 || distance <= lastDistance) && millis() <= startTime + args->timeout)
+  {
+    lastDistance = distance;
+    correction = constrain(
+      map(bearingDelta, -90, 90, -maxCorrection, maxCorrection),
+      -maxCorrection, maxCorrection);
+    move.left = args->speed + correction;
+    move.right = args->speed - correction;
+
+    MotorsController->SetMoveSpeed(move);
+    delay(100);
+    origin = GnssService->GetPosition();
+    bearing = bearing_to(origin, args->pos);
+    if (millis() >= startTime + 2000) { // wait before try to correct
+      bearingDelta = angle_difference(GnssService->GetMotionHeading(), bearing);
+    }
+    distance = distance_to(origin, args->pos);
+  }
+  if (args->stop) {
+    MotorsController->SetMoveSpeed({0 , 0});
+  }
+  log_i("Goto completed !");
+
+  while (args->stop)
+   delay(1000);
 }
 
 TripControllerClass::~TripControllerClass()

platformio.ini

@@ -13,8 +13,12 @@ platform = espressif32
 board = esp32-c3-devkitm-1
 framework = arduino
 monitor_speed = 115200
+monitor_raw = yes
 upload_speed = 921600
 
 lib_deps = dfrobot/DFRobot_QMC5883@^1.0.0
+  sparkfun/SparkFun u-blox GNSS v3@^3.1.11
 
-build_flags = -D HOSTNAME='"Mowbot"'
+build_flags = -D HOSTNAME='"Mowbot"'
+  -DCORE_DEBUG_LEVEL=5
+  -DCONFIG_ARDUHAL_LOG_COLORS=1

src/main.cpp

@@ -1,22 +1,21 @@
 #include <Arduino.h>
 #include "ble.h"
 #include "compass.h"
+#include "gnss.h"
 #include "motors.h"
 #include "trip.h"
 
-void setup() {
+ulong lastUpdate;
-  Serial.begin(115200);
-  Serial.println("Booting...");
 
+void setup() {
   setup_ble();
   setup_motors();
   setup_compass();
   setup_trip_control();
+  setup_gnss();
+
+  lastUpdate = millis();
 }
 
 void loop() {
-  //Serial.print(".");
-  //Serial.print("Degress = ");
-  //Serial.println(Compass->GetHeading());
-  delay(1000);
 }