Mowbot/Mowbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add GNSS navigation

Browse Source
This commit is contained in:
Thierry Fleury
2025-08-30 16:32:59 +02:00
parent bbf17063e9
commit 0f5be83056
11 changed files with 375 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
lib/ble/ble.cpp
View File

@@ -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 : ");
Serial.println(value);
log_d("Read cut value : %d", 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 : ");
Serial.println(newValue);
log_d("Write cut value : %d", 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=");
Serial.print(value.left);
Serial.print(" r=");
Serial.println(value.right);
log_d("Read move value : l=%d r=%d", value.left, 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=");
Serial.print(newValue->left);
Serial.print(" r=");
Serial.println(newValue->right);
log_d("Write move value : l=%d r=%d", newValue->left, 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 : ");
Serial.println(newValue);
TripController->RotateToBearing(newValue);
log_d("Write rotate to value : %.1f", newValue);
TripController->RotateToBearing(newValue, 20000, true);
}
};
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);
pRotateToBearing->setCallbacks(new RotateToBearingCallbacks());
pService->createCharacteristic(ROTATE_TO_BEARING_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
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.");
}

10
lib/compass/compass.cpp
View File

@@ -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;
if(heading > 2*PI)
heading -= 2*PI;
read.HeadingDegress = heading * 180/PI;
heading += TWO_PI;
if(heading > TWO_PI)
heading -= TWO_PI;
read.HeadingDegress = heading * RAD_TO_DEG;
return read.HeadingDegress;
}

47
lib/geo-util/geo-util.cpp
View File

@@ -1,7 +1,15 @@
#include <Arduino.h>
#include <geo-util.h>
float angle_difference(float source, float target) {
float delta = target - source;
const double EARTH_RADIUS = 6371000;
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;
}
@@ -10,3 +18,38 @@ float angle_difference(float source, float target) {
}
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.);
}

15
lib/geo-util/geo-util.h
View File

@@ -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

10
lib/gnss/gnss.cpp Normal file
View File

@@ -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;

26
lib/gnss/gnss.h Normal file
View File

@@ -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

65
lib/gnss/gnssServiceClass.cpp Normal file
View File

@@ -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()
{
}

12
lib/trip/trip.h
View File

@@ -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();
};

144
lib/trip/tripController.cpp
View File

@@ -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) {
float delta = angle_difference(Compass->GetHeading(), heading);
void TripControllerClass::RotateToBearing(float heading, uint timeout, bool stop) {
static _rotate_args params;
params.heading = heading;
params.timeout = timeout;
params.stop = stop;
while (delta < -5 || delta > 5)
{
Serial.print("Rotate delta : ");
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;
MotorsController->SetMoveSpeed(move);
delay(100);
delta = angle_difference(Compass->GetHeading(), heading);
_startTask(
_rotateTask,
&params
);
}
void TripControllerClass::GoTo(geo_pos &pos, u_int8_t speed, uint timeout, bool stop) {
static _goto_args params;
params.pos = pos;
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()

4
platformio.ini
View File

@@ -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"'
-DCORE_DEBUG_LEVEL=5
-DCONFIG_ARDUHAL_LOG_COLORS=1

13
src/main.cpp
View File

@@ -1,22 +1,21 @@
#include <Arduino.h>
#include "ble.h"
#include "compass.h"
#include "gnss.h"
#include "motors.h"
#include "trip.h"
void setup() {
Serial.begin(115200);
Serial.println("Booting...");
ulong lastUpdate;
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);
}