Hello Alec and Shirley here. We have almost finished a program for our boat using AI. Neither of us have coded before but we did take a months course on the internet on coding before starting this venture and it's taken months and it is still not finished as we have to add feedback for the rudder and mast position . Not sure about making it public on GitHub yet but we can do a text version of what we have so far. At the moment it has an SD card and an Oled on it for testing. We also have a flowery notebookLM description of what this code does
https://notebooklm.google.com/notebook/d5379604-a77b-4666-a839-7f20a2e9dadc. This is the code any suggestions or ideas, we would be grateful for. Repository: alfromuk/mighty.git
Files analyzed: 21
Estimated tokens: 11.4k
Directory structure:
└── alfromuk-mighty.git/
├── platformio.ini
├── include/
│ ├── BoatController.h
│ ├── CompassManager.h
│ ├── DisplayManager.h
│ ├── GPSManager.h
│ ├── NavigationManager.h
│ ├── PinConfig.h
│ ├── RudderController.h
│ ├── SailController.h
│ ├── SDLogger.h
│ └── WindSensor.h
└── src/
├── BoatController.cpp
├── CompassManager.cpp
├── DisplayManager.cpp
├── GPSManager.cpp
├── main.cpp
├── NavigationManager.cpp
├── RudderController.cpp
├── SailController.cpp
├── SDLogger.cpp
└── WindSensor.cpp
================================================
FILE: platformio.ini
================================================
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
;
https://docs.platformio.org/page/projectconf.html[env:esp32dev]
platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200
lib_deps =
mikalhart/TinyGPSPlus@^1.0.3
madhephaestus/ESP32Encoder@^0.10.1
sparkfun/SparkFun BNO08x Cortex Based IMU@^1.0.6
davidarmstrong/WMM_Tinier@^1.0.3
adafruit/Adafruit SSD1306@^2.5.16
adafruit/Adafruit GFX Library@^1.12.5
================================================
FILE: include/BoatController.h
================================================
#pragma once
#include "GPSManager.h"
#include "CompassManager.h"
#include "WindSensor.h"
#include "NavigationManager.h"
#include "RudderController.h"
#include "SailController.h"
#include "DisplayManager.h"
#include "SDLogger.h"
#include <Preferences.h>
#include <LittleFS.h>
#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include <freertos/task.h>
class BoatController {
public:
// Core Functions
void begin();
void printTasks();
CompassManager compass;
NavigationManager nav;
// Mission Loader for the Field Walk
void loadFieldTestMission();
private:
// Core Managers
GPSManager gps;
//CompassManager compass;
WindSensor wind;
//NavigationManager nav;
// Hardware Controllers
RudderController rudder;
SailController sail;
// Peripherals
DisplayManager display;
SDLogger sdLogger;
// Timing and State
Preferences preferences;
int lastMinute;
int lastMotorMoveMinute; // Tracks the last time the worm-drive was triggered
long timeOffset;
unsigned long lastSyncMillis;
unsigned long lastLogTime;
// Decision Averaging Buckets
float headingErrorAccumulator; // Sum of errors over the 30-sec/5-min window
int errorSampleCount; // Number of GPS samples collected
// Persistence and Internal Logic
void syncTime();
void saveState();
void loadState();
void logData();
// FreeRTOS Mutex and Tasks
SemaphoreHandle_t navMutex;
TaskHandle_t navigatorTaskHandle;
TaskHandle_t pilotTaskHandle;
static void navigatorTask(void* pvParameters);
static void pilotTask(void* pvParameters);
};
================================================
FILE: include/CompassManager.h
================================================
#ifndef COMPASSMANAGER_H
#define COMPASSMANAGER_H
#include <SparkFun_BNO08x_Arduino_Library.h>
#include <Arduino.h>
struct Quaternion {
float i, j, k, real;
};
class CompassManager {
public:
CompassManager();
void begin();
void update();
//void startCalibration();
float getHeadingTrue() const;
float getPitch() const;
float getRoll() const;
bool isHardwareOK() const;
// --- NEW FOR FIELD TESTING ---
void startCalibration(); // Enables the calibration engine
void saveCalibration(); // Saves offsets to BNO085 Flash
uint8_t getCalibrationStatus(); // Returns 0 (Unreliable) to 3 (High)
private:
BNO08x myIMU;
Quaternion quat;
bool hardwareStatus = false;
float declination = -1.0f; // Adjust based on your local magnetic north
};
#endif
================================================
FILE: include/DisplayManager.h
================================================
#ifndef DISPLAYMANAGER_H
#define DISPLAYMANAGER_H
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
class DisplayManager {
public:
DisplayManager();
void begin();
// Updates display with navigation and motor data
// xte is cross track error (distance).
void update(double xte, float distance, bool rightOfLine, float rudderDeg, int rudderDir, float sailDeg, int sailDir);
private:
Adafruit_SSD1306 display;
bool initialized;
};
#endif
================================================
FILE: include/GPSManager.h
================================================
#ifndef GPSMANAGER_H
#define GPSMANAGER_H
#include <TinyGPSPlus.h>
struct GPSData {
double latitude;
double longitude;
float speedKmph;
float courseDeg;
bool valid;
};
class GPSManager {
public:
void begin(HardwareSerial &serialPort, uint32_t baud = 9600);
void update();
GPSData getData();
double courseTo(double targetLat, double targetLon);
double distanceTo(double targetLat, double targetLon);
TinyGPSPlus& getRawGPS() { return gps; }
private:
TinyGPSPlus gps;
HardwareSerial* gpsSerial;
};
#endif
================================================
FILE: include/NavigationManager.h
================================================
#pragma once
#include "GPSManager.h"
#include <math.h>
class NavigationManager {
public:
static constexpr int maxWaypoints = 20;
static constexpr float WAYPOINT_RADIUS_METERS = 5.0f;
NavigationManager();
void begin();
// The main brain: processes GPS, Heading, and Wind
void update(const GPSData& pos, float heading, float windAngle);
// Getters for your Motor/Laptop logging
float getHeadingError() const;
double getXTE() const;
bool isInIrons() const;
// Waypoint Logic
bool isAtWaypoint(const GPSData& pos) const;
bool hasNextWaypoint() const;
void advanceWaypoint(const GPSData& pos); // Updated to take pos to set new 'start'
// Storage & Setup
void resetWaypoints();
void addWaypoint(double lat, double lon);
void saveWaypoints();
void loadWaypoints();
void printWaypoints() const;
private:
// Internal Math Helpers
double distanceTo(double lat1, double lon1, double lat2, double lon2) const;
double calculateXTE(double sLat, double sLon, double tLat, double tLon, double pLat, double pLon);
double toRadians(double degree);
// Waypoint Storage
double latitudes[maxWaypoints];
double longitudes[maxWaypoints];
int waypointCount;
int currentIndex;
// The "Rail" (XTE) Navigation
double startLat, startLon; // Coordinates where the current leg began
double xte; // Distance from the rail in meters
// Steering & Environment
float headingError;
bool inIrons;
float lastWindRelative; // For hysteresis
bool firstLock; // To initialize the very first 'start' point
};
================================================
FILE: include/PinConfig.h
================================================
#ifndef PINCONFIG_H
#define PINCONFIG_H
#include <Arduino.h>
// ============================================================
// 1. SPI BUS (BNO085 IMU)
// ============================================================
// Standard ESP32 SPI Pins: SCK (18), MISO (19), MOSI (23)
// These three additional pins are required for the BNO085:
#define BNO_CS 5 // Chip Select
#define BNO_INT 4 // Interrupt (Tells ESP32 data is ready)
#define BNO_RST 2 // Reset (Allows ESP32 to reboot the sensor)
// ============================================================
// 1.5 SD CARD & DISPLAY
// ============================================================
#define SD_CS_PIN 15 // SD Card Chip Select
#define OLED_SDA 21 // Display I2C SDA
#define OLED_SCL 22 // Display I2C SCL
// ============================================================
// 2. SERIAL PORTS (GPS)
// ============================================================
// Using Serial2 for the GPS Neo-6M
#define GPS_RX_PIN 16 // ESP32 RX2 <--- Connect to GPS TX
#define GPS_TX_PIN 17 // ESP32 TX2 <--- Connect to GPS RX
// ============================================================
// 3. MOTOR CONFIGURATION (LEDC PWM)
// ============================================================
#define PWM_FREQ_MOTOR 20000 // 20 kHz for silent operation
#define PWM_RESOLUTION 8 // 8-bit (0-255)
// --- Rudder Motor (Channel 0) ---
#define RUDDER_PWM_PIN 25
#define RUDDER_IN1_PIN 26
#define RUDDER_IN2_PIN 27
#define RUDDER_STBY_PIN 14
// --- Sail Motor (Channel 1) ---
#define SAIL_PWM_PIN 32
#define SAIL_IN1_PIN 33
#define SAIL_IN2_PIN 13
#define SAIL_STBY_PIN 14 // Changed from 4 to 14 to avoid BNO_INT conflict
// ============================================================
// 4. WIND SENSOR (Rotary Encoder)
// ============================================================
// Note: GPIO 34 and 35 are input-only.
// Use external 10k pull-up resistors to 3.3V on these pins.
#define WIND_SENSOR_PIN_A 34
#define WIND_SENSOR_PIN_B 35
// ============================================================
// 5. STATUS LED
// ============================================================
#define STATUS_LED_PIN 12 // Useful for "Heartbeat" or Watchdog status
#endif
================================================
FILE: include/RudderController.h
================================================
#ifndef RUDDERCONTROLLER_H
#define RUDDERCONTROLLER_H
#include <Arduino.h>
class RudderController
{
public:
void begin(uint8_t in1Pin, uint8_t in2Pin, uint8_t pwmPin, uint8_t stbyPin);
// Updates the rudder based on heading error (-180 to 180)
void update(float headingError);
void escapeIrons();
void stop();
// For logging/display
float currentDeg = 0.0f;
int currentDir = 0;
private:
unsigned long lastUpdateTime = 0;
uint8_t pinIn1, pinIn2, pinPWM, pinStby;
const uint8_t ledcChannel = 0;
// PID Constants
float Kp = 2.0; // Proportional gain (e.g., 4 * 45deg error = 180 PWM)
void applyPWM(int pwm, int direction);
};
#endif
================================================
FILE: include/SailController.h
================================================
#ifndef SAILCONTROLLER_H
#define SAILCONTROLLER_H
#include <Arduino.h>
class SailController
{
public:
SailController();
void begin(uint8_t in1Pin, uint8_t in2Pin, uint8_t pwmPin, uint8_t stbyPin);
// Adjusts sail based on apparent wind angle (0-180)
void setAngleForWind(float apparentWindAngle);
// Stop the sail motor
void stop();
// For logging/display
float currentDeg = 0.0f;
int currentDir = 0;
private:
unsigned long lastUpdateTime = 0;
uint8_t pinIn1, pinIn2, pinPWM, pinStby;
// CRITICAL FIX: Define the LEDC Channel for this controller
const uint8_t ledcChannel = 1;
// Helper to drive the motor
void applyPWM(int pwm, int direction);
};
#endif
================================================
FILE: include/SDLogger.h
================================================
#ifndef SDLOGGER_H
#define SDLOGGER_H
#include <Arduino.h>
#include <SPI.h>
#include <SD.h>
class SDLogger {
public:
SDLogger();
void begin();
// Logs the navigation data and motor states to SD card.
// Change this line in SDLogger.h
void logNavigationData(time_t now, double lat, double lon, double xte, float distance, bool rightOfLine, float rudderDeg, int rudderDir, float sailDeg, int sailDir, uint8_t magAccuracy);
private:
bool initialized;
};
#endif
================================================
FILE: include/WindSensor.h
================================================
#ifndef WINDSENSOR_H
#define WINDSENSOR_H
#include <Arduino.h>
class WindSensor {
public:
void begin(uint8_t pinA, uint8_t pinB);
void update();
float getApparentWindAngle();
float getSmoothedWindAngle();
void calibrate();
private:
volatile int32_t position = 0;
int32_t zeroOffset = 0;
uint8_t pinA, pinB;
static WindSensor* instance;
static void handleInterruptA();
static void handleInterruptB();
void handleInterrupt(uint8_t pin);
static const size_t bufferSize = 30;
float angleBuffer[bufferSize];
size_t bufferIndex = 0;
size_t sampleCount = 0;
void addSample(float angle);
float getVectorAverage();
};
#endif
================================================
FILE: src/BoatController.cpp
================================================
#include "BoatController.h"
#include "PinConfig.h"
#include <Arduino.h>
#include <time.h>
#include <Wire.h>
void BoatController::begin()
{
Serial.println("Initializing BoatController...");
// 1. Persistence
preferences.begin("boat", false);
loadState();
// 2. Filesystem
if(!LittleFS.begin(true)){
Serial.println("LittleFS Mount Failed");
}
// 3. Hardware Initialization
gps.begin(Serial1, 9600);
compass.begin(); // This now calls startCalibration() internally
wind.begin(WIND_SENSOR_PIN_A, WIND_SENSOR_PIN_B);
nav.begin();
rudder.begin(RUDDER_IN1_PIN, RUDDER_IN2_PIN, RUDDER_PWM_PIN, RUDDER_STBY_PIN);
sail.begin(SAIL_IN1_PIN, SAIL_IN2_PIN, SAIL_PWM_PIN, SAIL_STBY_PIN);
display.begin();
sdLogger.begin();
lastMinute = -1;
lastMotorMoveMinute = -1;
lastLogTime = 0;
headingErrorAccumulator = 0;
errorSampleCount = 0;
// FreeRTOS Mutex and Tasks Initialization
navMutex = xSemaphoreCreateMutex();
if (navMutex != NULL) {
Serial.println("navMutex created successfully.");
xTaskCreatePinnedToCore(
navigatorTask,
"NavigatorTask",
4096,
this,
1,
&navigatorTaskHandle,
0 // PRO_CPU
);
xTaskCreatePinnedToCore(
pilotTask,
"PilotTask",
4096,
this,
1,
&pilotTaskHandle,
1 // APP_CPU
);
} else {
Serial.println("Failed to create navMutex!");
}
}
void BoatController::loadFieldTestMission() {
nav.resetWaypoints();
nav.addWaypoint(52.358297, -2.700906);
//Waypoint 2 (DMS: 52.21.28, 02.41.57)
nav.addWaypoint(52.357817, -2.699360);
//Waypoint 3 (DMS: 52.21.25, 02.42.01)
nav.addWaypoint(52.356944444, -2.700277778);
// Waypoint 4 (DMS: 52.21.23, 02.42.01)
nav.addWaypoint(52.356388889, -2.700277778);
//Waypoint 5 (Back to Start DMS: 52.21.29, 02.42.02)
nav.addWaypoint(52.358055556, -2.700555556);
nav.saveWaypoints();
Serial.println(">>> MISSION READY: 2 Waypoints Uploaded to Flash.");
nav.printWaypoints();
}
void BoatController::navigatorTask(void* pvParameters) {
BoatController* boat = static_cast<BoatController*>(pvParameters);
while (true) {
boat->gps.update();
boat->compass.update();
boat->wind.update();
boat->syncTime();
// 10-Minute State Save (600,000 ms)
static unsigned long lastSaveMillis = 0;
if (millis() - lastSaveMillis > 600000) {
lastSaveMillis = millis();
boat->saveState();
Serial.println(">>> 10-MIN UPDATE: State Saved to Preferences.");
}
// 6-Hour Voyage Log (21,600,000 ms)
static unsigned long lastLogMillis = 0;
if (millis() - lastLogMillis > 21600000) {
lastLogMillis = millis();
boat->logData();
Serial.println(">>> 6-HOUR UPDATE: Data Logged to Voyage.csv.");
}
xSemaphoreTake(boat->navMutex, portMAX_DELAY);
GPSData pos = boat->gps.getData();
float heading = boat->compass.getHeadingTrue();
float windAngle = boat->wind.getSmoothedWindAngle();
boat->nav.update(pos, heading, windAngle);
if (boat->nav.isAtWaypoint(pos)) {
Serial.println("********** WAYPOINT REACHED **********");
boat->nav.advanceWaypoint(pos);
}
// Get navigation and motor state for display/logging
double xte = boat->nav.getXTE();
float dist = fabs(xte);
bool rightOfLine = (xte > 0);
float rDeg = boat->rudder.currentDeg;
int rDir = boat->rudder.currentDir;
float sDeg = boat->sail.currentDeg;
int sDir = boat->sail.currentDir;
// Rate limit OLED and SD to 1Hz
static unsigned long lastDisplayLogMillis = 0;
if (millis() - lastDisplayLogMillis > 1000) {
lastDisplayLogMillis = millis();
// 1. Get the current calibration status
uint8_t calStat = boat->compass.getCalibrationStatus();
// 2. Log to SD (Time must be fetched)
time_t now = boat->timeOffset + millis() / 1000;
// Note: Added 'calStat' at the very end of this line
boat->sdLogger.logNavigationData(now, pos.latitude, pos.longitude, xte, dist, rightOfLine, rDeg, rDir, sDeg, sDir, calStat);
// 3. Update OLED
boat->display.update(xte, dist, rightOfLine, rDeg, rDir, sDeg, sDir);
}
xSemaphoreGive(boat->navMutex);
vTaskDelay(pdMS_TO_TICKS(50)); // 20Hz
}
}
void BoatController::pilotTask(void* pvParameters) {
BoatController* boat = static_cast<BoatController*>(pvParameters);
while (true) {
float headingError = 0;
bool inIrons = false;
float apparentWind = boat->wind.getApparentWindAngle();
if (xSemaphoreTake(boat->navMutex, pdMS_TO_TICKS(500)) == pdTRUE) {
headingError = boat->nav.getHeadingError();
inIrons = boat->nav.isInIrons();
xSemaphoreGive(boat->navMutex);
if (inIrons) {
boat->rudder.escapeIrons();
} else {
boat->rudder.update(headingError);
}
boat->sail.setAngleForWind(apparentWind);
} else {
boat->rudder.stop();
boat->sail.stop();
Serial.println("Mutex timeout, motors stopped");
}
vTaskDelay(pdMS_TO_TICKS(20)); // 50Hz
}
}
void BoatController::syncTime() {
TinyGPSPlus& rawGps = gps.getRawGPS();
if (rawGps.date.isValid() && rawGps.time.isValid()) {
struct tm tm;
tm.tm_year = rawGps.date.year() - 1900;
tm.tm_mon = rawGps.date.month() - 1;
tm.tm_mday = rawGps.date.day();
tm.tm_hour = rawGps.time.hour();
tm.tm_min = rawGps.time.minute();
tm.tm_sec = rawGps.time.second();
tm.tm_isdst = 0;
time_t gpsTime = mktime(&tm);
if (timeOffset == 0 || abs(gpsTime - (timeOffset + (long)(millis() / 1000))) > 2) {
timeOffset = gpsTime - millis() / 1000;
Serial.println("Time Synced with GPS");
}
}
}
void BoatController::saveState() {
preferences.putLong("time", timeOffset + millis() / 1000);
GPSData pos = gps.getData();
if (pos.valid) {
preferences.putDouble("lat", pos.latitude);
preferences.putDouble("lon", pos.longitude);
}
}
void BoatController::loadState() {
long savedTime = preferences.getLong("time", 0);
if (savedTime > 0) {
timeOffset = savedTime - millis() / 1000;
}
}
void BoatController::logData() {
File file = LittleFS.open("/voyage.csv", FILE_APPEND);
if (!file) return;
time_t now = timeOffset + millis() / 1000;
struct tm * ti = localtime(&now);
GPSData pos = gps.getData();
// Capture Accuracy and Heading
float currentHeading = compass.getHeadingTrue();
uint8_t magAccuracy = compass.getCalibrationStatus();
// CSV format: Date, Time, Lat, Lon, Heading, Accuracy(0-3)
file.printf("%02d/%02d/%04d,%02d:%02d:%02d,%.9f,%.9f,%.1f,%u\n",
ti->tm_mday, ti->tm_mon + 1, ti->tm_year + 1900,
ti->tm_hour, ti->tm_min, ti->tm_sec,
pos.latitude, pos.longitude,
currentHeading, magAccuracy
);
file.close();
// Serial monitor feedback for the walk
Serial.printf("[LOG] T:%02d:%02d:%02d Accu:%u Head:%.1f\n",
ti->tm_hour, ti->tm_min, ti->tm_sec, magAccuracy, currentHeading);
}
================================================
FILE: src/CompassManager.cpp
================================================
#include "CompassManager.h"
#include "PinConfig.h"
#include <esp_task_wdt.h>
CompassManager::CompassManager() : quat{0.0f, 0.0f, 0.0f, 1.0f}, hardwareStatus(false) {}
void CompassManager::begin() {
Serial.println(">>> Initializing BNO085 (SparkFun SPI)...");
esp_task_wdt_reset();
pinMode(BNO_INT, INPUT_PULLUP);
if (myIMU.beginSPI(BNO_CS, BNO_INT, BNO_RST) == false) {
Serial.println("!!! BNO085 SPI Failure.");
hardwareStatus = false;
} else {
Serial.println(">>> BNO085 SPI Online.");
hardwareStatus = true;
myIMU.enableRotationVector(20);
// Automatically start the calibration engine on boot
startCalibration();
}
}
void CompassManager::startCalibration() {
if (!hardwareStatus) {
Serial.println("!!! Cannot calibrate. BNO085 not online.");
return;
}
Serial.println(">>> CALIBRATION SEQUENCE STARTING");
// Enable dynamic calibration for Accel, Gyro, and Mag
myIMU.setCalibrationConfig(SH2_CAL_ACCEL | SH2_CAL_GYRO | SH2_CAL_MAG);
Serial.println(">>> Please rotate the boat in figure-8 motions around all 3 axes.");
Serial.println(">>> Calibration will run for 30 seconds...");
unsigned long startTime = millis();
while (millis() - startTime < 30000) {
// Continuously pump data to prevent SPI stall during calibration
if (digitalRead(BNO_INT) == LOW) {
myIMU.wasReset();
// Pull data to clear internal buffers
quat.i = myIMU.getQuatI();
quat.j = myIMU.getQuatJ();
quat.k = myIMU.getQuatK();
quat.real = myIMU.getQuatReal();
}
if ((millis() - startTime) % 5000 < 10) {
Serial.print(">>> Calibrating... ");
Serial.print(30 - ((millis() - startTime) / 1000));
Serial.println(" seconds remaining.");
delay(10); // Prevent multi-prints in the same millisecond
}
delay(1);
}
Serial.println(">>> Calibration window complete. Saving to NVRAM...");
if (myIMU.saveCalibration()) {
Serial.println(">>> BNO085 Calibration SAVED successfully.");
} else {
Serial.println("!!! BNO085 Calibration SAVE FAILED.");
}
}
void CompassManager::update() {
if (!hardwareStatus || digitalRead(BNO_INT) == HIGH) return;
myIMU.wasReset();
quat.i = myIMU.getQuatI();
quat.j = myIMU.getQuatJ();
quat.k = myIMU.getQuatK();
quat.real = myIMU.getQuatReal();
}
void CompassManager::saveCalibration() {
if (hardwareStatus) {
// 1. Tell the BNO085 to save current offsets to its internal flash (NVRAM)
if (myIMU.saveCalibration()) {
Serial.println(">>> BNO085: Offsets saved to sensor memory.");
} else {
Serial.println("!!! BNO085: Failed to save calibration.");
}
// 2. Effectively "end" calibration by disabling the dynamic calibration bits
// This sets the calibration config to 0, 0, 0 for Accel, Gyro, and Mag
// Correct way to disable all calibration
myIMU.setCalibrationConfig(0);
Serial.println(">>> BNO085: Dynamic calibration disabled.");
}
}
uint8_t CompassManager::getCalibrationStatus() {
if (!hardwareStatus) return 0;
// This returns a value from 0 to 3:
// 0 = Unreliable
// 1 = Low Accuracy
// 2 = Medium Accuracy
// 3 = High Accuracy
return myIMU.getQuatAccuracy();
}
float CompassManager::getHeadingTrue() const {
if (!hardwareStatus) return 0.0f;
float y = 2.0f * (quat.real * quat.k + quat.i * quat.j);
float x = 1.0f - 2.0f * (quat.j * quat.j + quat.k * quat.k);
float heading = atan2f(y, x) * 180.0f / (float)PI;
float trueHeading = heading + declination;
if (trueHeading >= 360.0f) trueHeading -= 360.0f;
if (trueHeading < 0.0f) trueHeading += 360.0f;
return trueHeading;
}
// ... (getPitch and getRoll remain the same) ...
================================================
FILE: src/DisplayManager.cpp
================================================
#include "DisplayManager.h"
#include "PinConfig.h"
// Define OLED address, default is 0x3C
#define OLED_ADDR 0x3C
DisplayManager::DisplayManager() : display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1), initialized(false) {}
void DisplayManager::begin() {
Wire.begin(OLED_SDA, OLED_SCL);
// SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
Serial.println("SSD1306 allocation failed");
initialized = false;
return;
}
initialized = true;
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("AutoBoat Booting...");
display.display();
}
void DisplayManager::update(double xte, float distance, bool rightOfLine, float rudderDeg, int rudderDir, float sailDeg, int sailDir) {
if (!initialized) return;
display.clearDisplay();
display.setCursor(0, 0);
// Navigation Data
display.println("--- Navigation ---");
display.print("XTE: ");
display.print(xte, 2);
display.println(" m");
display.print("Dist: ");
display.print(distance, 1);
display.println(" m");
display.print("Side: ");
display.println(rightOfLine ? "Right of Line" : "Left of Line");
// Motor Data
display.println("--- Motors ---");
display.print("Rudder: ");
display.print(rudderDeg, 1);
display.print(" deg ");
display.println((rudderDir > 0) ? "(R)" : ((rudderDir < 0) ? "(L)" : "(-)"));
display.print("Sail: ");
display.print(sailDeg, 1);
display.print(" deg ");
display.println((sailDir > 0) ? "(Out)" : ((sailDir < 0) ? "(In)" : "(-)"));
display.display();
}
================================================
FILE: src/GPSManager.cpp
================================================
#include "GPSManager.h"
void GPSManager::begin(HardwareSerial &serialPort, uint32_t baud) {
gpsSerial = &serialPort;
// Serial port initialization is handled in main.cpp
}
void GPSManager::update() {
while (gpsSerial->available() > 0) {
// READ FROM HARDWARE: Read byte from GPS serial stream
gps.encode(gpsSerial->read());
}
}
GPSData GPSManager::getData() {
GPSData data;
data.latitude = gps.location.lat();
data.longitude = gps.location.lng();
data.speedKmph = gps.speed.kmph();
data.courseDeg = gps.course.deg();
data.valid = gps.location.isValid() && gps.location.age() < 2000;
return data;
}
double GPSManager::courseTo(double targetLat, double targetLon) {
return TinyGPSPlus::courseTo(gps.location.lat(), gps.location.lng(), targetLat, targetLon);
}
double GPSManager::distanceTo(double targetLat, double targetLon) {
return TinyGPSPlus::distanceBetween(gps.location.lat(), gps.location.lng(), targetLat, targetLon);
}
================================================
FILE: src/main.cpp
================================================
#include <Arduino.h>
#include <Wire.h>
#include "BoatController.h"
#include "PinConfig.h"
BoatController boat;
void setup() {
Serial.begin(115200);
delay(1000);
Serial.println("Autonomous Boat System Booting...");
// 1. Calibrate the Compass (BNO085)
// This will pause the program for 30 seconds while you spin the boat in all orientations.
// Remove or comment this out after you have calibrated once
boat.compass.startCalibration();
// 2. Initialize GPS Serial Port
Serial1.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
// 3. Initialize Boat Hardware and Persistence
boat.begin();
// 4. Load the Specific Field Test Waypoints
boat.loadFieldTestMission();
Serial.println("Setup Complete. Standing by for GPS Lock...");
}
void loop() {
// Main loop is not needed as FreeRTOS tasks handle navigation and pilot
vTaskDelete(NULL);
}
================================================
FILE: src/NavigationManager.cpp
================================================
#include <Arduino.h>
#include "NavigationManager.h"
#include <TinyGPSPlus.h>
#include <Preferences.h>
#include <math.h>
Preferences prefs;
NavigationManager::NavigationManager() :
waypointCount(0),
currentIndex(0),
headingError(0),
inIrons(false),
lastWindRelative(180),
xte(0.0),
firstLock(true) {
memset(latitudes, 0, sizeof(latitudes));
memset(longitudes, 0, sizeof(longitudes));
}
void NavigationManager::begin() {
prefs.begin("waypoints", false);
loadWaypoints();
}
void NavigationManager::update(const GPSData& pos, float heading, float windAngle) {
if (waypointCount == 0 || !pos.valid) return;
// Initialize the very first start point if this is our first valid GPS lock
if (firstLock) {
startLat = pos.latitude;
startLon = pos.longitude;
firstLock = false;
}
double targetLat = latitudes[currentIndex];
double targetLon = longitudes[currentIndex];
// 1. Calculate the 'Rail' Error (XTE)
xte = calculateXTE(startLat, startLon, targetLat, targetLon, pos.latitude, pos.longitude);
// 2. Point-and-Shoot: Direct bearing to the target
double directCourse = TinyGPSPlus::courseTo(pos.latitude, pos.longitude, targetLat, targetLon);
// 3. Correction: For every 1 meter of error, aim 2 degrees back to center
// Limit to 30 degrees to avoid sailing directly sideways
float correction = (float)xte * 2.0f;
correction = constrain(correction, -30.0f, 30.0f);
float correctedCourse = (float)directCourse - correction;
// 4. Normalize heading error (-180 to 180)
headingError = correctedCourse - heading;
headingError = fmodf(headingError + 540.0f, 360.0f) - 180.0f;
// 5. Wind / Irons Logic
float targetCourse = (float)directCourse;
float windRelative = fmodf(windAngle - targetCourse + 540.0f, 360.0f) - 180.0f;
windRelative = fabsf(windRelative);
if (windRelative < 25 || (lastWindRelative < 35 && windRelative < 35)) {
inIrons = true;
} else {
inIrons = false;
}
lastWindRelative = windRelative;
}
// Math helper for XTE
double NavigationManager::calculateXTE(double sLat, double sLon, double tLat, double tLon, double pLat, double pLon) {
double R = 6371000.0; // Earth radius in meters
double d13 = TinyGPSPlus::distanceBetween(sLat, sLon, pLat, pLon) / R;
double brng13 = toRadians(TinyGPSPlus::courseTo(sLat, sLon, pLat, pLon));
double brng12 = toRadians(TinyGPSPlus::courseTo(sLat, sLon, tLat, tLon));
return asin(sin(d13) * sin(brng13 - brng12)) * R;
}
double NavigationManager::toRadians(double degree) {
return degree * M_PI / 180.0;
}
float NavigationManager::getHeadingError() const { return headingError; }
double NavigationManager::getXTE() const { return xte; }
bool NavigationManager::isInIrons() const { return inIrons; }
bool NavigationManager::isAtWaypoint(const GPSData& pos) const {
if (waypointCount == 0 || !pos.valid) return false;
return distanceTo(pos.latitude, pos.longitude, latitudes[currentIndex], longitudes[currentIndex]) < WAYPOINT_RADIUS_METERS;
}
bool NavigationManager::hasNextWaypoint() const {
return (currentIndex + 1 < waypointCount);
}
void NavigationManager::advanceWaypoint(const GPSData& pos) {
if (hasNextWaypoint()) {
// The waypoint we just finished becomes the NEW start point for the next leg
startLat = latitudes[currentIndex];
startLon = longitudes[currentIndex];
currentIndex++;
prefs.putInt("currentIndex", currentIndex);
Serial.print("Advancing. New Target WP: "); Serial.println(currentIndex + 1);
}
}
// Storage functions updated for Double precision
void NavigationManager::saveWaypoints() {
prefs.putInt("count", waypointCount);
prefs.putInt("currentIndex", currentIndex);
char key[10];
for (int i = 0; i < waypointCount; i++) {
snprintf(key, sizeof(key), "lat%d", i);
prefs.putDouble(key, latitudes[i]);
snprintf(key, sizeof(key), "lon%d", i);
prefs.putDouble(key, longitudes[i]);
}
}
void NavigationManager::loadWaypoints() {
waypointCount = min(prefs.getInt("count", 0), maxWaypoints);
currentIndex = prefs.getInt("currentIndex", 0);
if (currentIndex >= waypointCount) currentIndex = 0;
char key[10];
for (int i = 0; i < waypointCount; i++) {
snprintf(key, sizeof(key), "lat%d", i);
latitudes[i] = prefs.getDouble(key, 0.0);
snprintf(key, sizeof(key), "lon%d", i);
longitudes[i] = prefs.getDouble(key, 0.0);
}
}
void NavigationManager::resetWaypoints() {
waypointCount = 0;
currentIndex = 0;
prefs.putInt("count", 0);
prefs.putInt("currentIndex", 0);
}
void NavigationManager::addWaypoint(double lat, double lon) {
if (waypointCount >= maxWaypoints) return;
latitudes[waypointCount] = lat;
longitudes[waypointCount] = lon;
waypointCount++;
}
double NavigationManager::distanceTo(double lat1, double lon1, double lat2, double lon2) const {
return TinyGPSPlus::distanceBetween(lat1, lon1, lat2, lon2);
}
void NavigationManager::printWaypoints() const {
Serial.println(">>> WAYPOINT LIST (TASKS) <<<");
if (waypointCount == 0) {
Serial.println("No waypoints loaded.");
return;
}
for (int i = 0; i < waypointCount; i++) {
Serial.print("WP ");
Serial.print(i + 1);
if (i == currentIndex) Serial.print(" [ACTIVE]");
Serial.print(": ");
Serial.print(latitudes[i], 9);
Serial.print(", ");
Serial.println(longitudes[i], 9);
}
Serial.println("-----------------------------");
}
================================================
FILE: src/RudderController.cpp
================================================
#include <Arduino.h>
#include "RudderController.h"
#include "PinConfig.h"
#include <driver/ledc.h>
void RudderController::begin(uint8_t in1, uint8_t in2, uint8_t pwm, uint8_t stby)
{
pinIn1 = in1;
pinIn2 = in2;
pinPWM = pwm;
pinStby = stby;
pinMode(pinIn1, OUTPUT);
pinMode(pinIn2, OUTPUT);
pinMode(pinStby, OUTPUT);
digitalWrite(pinStby, HIGH); // Enable the motor driver
// Setup ESP32 PWM (Channel 0, 20kHz, 8-bit resolution)
ledcSetup(ledcChannel, PWM_FREQ_MOTOR, PWM_RESOLUTION);
ledcAttachPin(pinPWM, ledcChannel);
lastUpdateTime = millis();
stop();
}
void RudderController::update(float headingError)
{
// Simple Proportional Control
// headingError is the difference between target and current heading (-180 to 180).
// Calculate PWM output
// We want a higher PWM for larger errors.
int pwm = (int)(headingError * Kp);
// Determine direction
// If error is positive, we need to turn towards positive (Right/Starboard)
// Assuming Motor Direction 1 is Starboard, -1 is Port.
int direction = 0;
if (pwm > 10) { // Deadband
direction = 1;
} else if (pwm < -10) {
direction = -1;
}
// Apply PWM
applyPWM(abs(pwm), direction);
}
void RudderController::stop() {
applyPWM(0, 0);
}
void RudderController::applyPWM(int pwm, int direction)
{
// Constrain PWM to 0-255 range
int safePWM = constrain(abs(pwm), 0, 255);
// Calculate simulated degrees moved based on open loop PWM duration
unsigned long now = millis();
float dt = (now - lastUpdateTime) / 1000.0f;
lastUpdateTime = now;
// Estimate degrees based on PWM speed. Max 255 PWM = ~15 deg/sec.
float degPerSec = (safePWM / 255.0f) * 15.0f;
currentDeg += (direction * degPerSec * dt);
// Boundary constraint (-45 to 45 deg)
currentDeg = constrain(currentDeg, -45.0f, 45.0f);
currentDir = direction;
if (direction > 0)
{
digitalWrite(pinIn1, HIGH);
digitalWrite(pinIn2, LOW);
}
else if (direction < 0)
{
digitalWrite(pinIn1, LOW);
digitalWrite(pinIn2, HIGH);
}
else
{
digitalWrite(pinIn1, LOW);
digitalWrite(pinIn2, LOW);
safePWM = 0;
}
ledcWrite(ledcChannel, safePWM);
}
void RudderController::escapeIrons()
{
// Moves the rudder to one side to get the boat moving
applyPWM(200, 1);
delay(1500); // Note: This blocks other sensors for 1.5s
applyPWM(0, 0); // Stop
}
================================================
FILE: src/SailController.cpp
================================================
#include "SailController.h"
#include "PinConfig.h"
#include <driver/ledc.h>
SailController::SailController() {}
void SailController::begin(uint8_t in1, uint8_t in2, uint8_t pwm, uint8_t stby)
{
pinIn1 = in1;
pinIn2 = in2;
pinPWM = pwm;
pinStby = stby;
pinMode(pinIn1, OUTPUT);
pinMode(pinIn2, OUTPUT);
pinMode(pinStby, OUTPUT);
// Enable the driver
digitalWrite(pinStby, HIGH);
// Setup ESP32 PWM
ledcSetup(ledcChannel, PWM_FREQ_MOTOR, PWM_RESOLUTION);
ledcAttachPin(pinPWM, ledcChannel);
lastUpdateTime = millis();
stop();
}
void SailController::stop() {
applyPWM(0, 0);
}
void SailController::setAngleForWind(float apparentWindAngle) {
// Map apparent wind (0-180) to sail position (0-100%)
// 0 deg (Head to wind) -> Sail in (Tight, 0%)
// 180 deg (Downwind) -> Sail out (Loose, 100%)
// Note: Since this hardware configuration (H-Bridge) lacks a position feedback sensor
// defined in PinConfig.h, we cannot implement a closed-loop position control here.
// We would need a potentiometer or encoder on the winch.
// Placeholder logic:
// float targetPosition = (abs(apparentWindAngle) / 180.0f) * 100.0f;
// Serial.printf("Sail Target: %.1f%%\n", targetPosition);
}
void SailController::applyPWM(int pwm, int direction)
{
// Constrain PWM
int safePWM = constrain(abs(pwm), 0, 255);
// Calculate simulated degrees moved based on open loop PWM duration
unsigned long now = millis();
float dt = (now - lastUpdateTime) / 1000.0f;
lastUpdateTime = now;
// Estimate degrees based on PWM speed. Max 255 PWM = ~20 deg/sec.
float degPerSec = (safePWM / 255.0f) * 20.0f;
currentDeg += (direction * degPerSec * dt);
// Boundary constraint (0 to 90 deg)
currentDeg = constrain(currentDeg, 0.0f, 90.0f);
currentDir = direction;
if (direction > 0)
{
digitalWrite(pinIn1, HIGH);
digitalWrite(pinIn2, LOW);
}
else if (direction < 0)
{
digitalWrite(pinIn1, LOW);
digitalWrite(pinIn2, HIGH);
}
else
{
digitalWrite(pinIn1, LOW);
digitalWrite(pinIn2, LOW);
safePWM = 0;
}
ledcWrite(ledcChannel, safePWM);
}
================================================
FILE: src/SDLogger.cpp
================================================
#include "SDLogger.h"
#include "PinConfig.h"
SDLogger::SDLogger() : initialized(false) {}
void SDLogger::begin() {
Serial.println("Initializing SD card...");
// The BNO085 uses SPI. We will share the bus.
// Ensure BNO CS is not active while initializing SD.
pinMode(BNO_CS, OUTPUT);
digitalWrite(BNO_CS, HIGH);
if (!SD.begin(SD_CS_PIN)) {
Serial.println("SD Card initialization failed!");
initialized = false;
return;
}
initialized = true;
Serial.println("SD Card initialized.");
// Create header if file doesn't exist
if (!SD.exists("/nav_log.csv")) {
File file = SD.open("/nav_log.csv", FILE_WRITE);
if (file) {
// Update the header string (Line ~25)
file.println("time,latitude,longitude,XTE,distance,side_of_line,rudder_deg,rudder_dir,sail_deg,sail_dir,mag_acc");
file.close();
}
}
}
// 1. ADD 'uint8_t magAccuracy' TO THE END OF THIS LINE
void SDLogger::logNavigationData(time_t now, double lat, double lon, double xte, float distance, bool rightOfLine, float rudderDeg, int rudderDir, float sailDeg, int sailDir, uint8_t magAccuracy) {
if (!initialized) return;
// Convert time to string
struct tm * ti = localtime(&now);
char timeStr[24];
sprintf(timeStr, "%04d-%02d-%02d %02d:%02d:%02d",
ti->tm_year + 1900, ti->tm_mon + 1, ti->tm_mday,
ti->tm_hour, ti->tm_min, ti->tm_sec
);
File file = SD.open("/nav_log.csv", FILE_APPEND);
if (!file) {
Serial.println("Failed to open nav_log.csv for appending");
return;
}
// 2. REPLACE THE OLD PRINTF WITH THIS UPDATED VERSION
// Note the ,%u at the end of the quotes and magAccuracy at the end of the list
file.printf("%s,%.9f,%.9f,%.2f,%.2f,%s,%.2f,%d,%.2f,%d,%u\n",
timeStr,
lat, lon,
xte, distance,
rightOfLine ? "Right" : "Left",
rudderDeg, rudderDir,
sailDeg, sailDir,
magAccuracy // <--- This matches the %u above
);
file.close();
}
================================================
FILE: src/WindSensor.cpp
================================================
#include "WindSensor.h"
#include <math.h>
#include <ESP32Encoder.h>
// The encoder object MUST be declared globally or statically
static ESP32Encoder encoder;
// Define the steps per revolution for your encoder
// The RM22SC0010B10F1C00 is 1024 steps per revolution
#define STEPS_PER_REV 1024.0
WindSensor *WindSensor::instance = nullptr;
void WindSensor::begin(uint8_t a, uint8_t b)
{
// We no longer need to use pinMode(INPUT) as the library handles it
// Set up the encoder. It handles all the interrupt logic internally.
encoder.attachFullQuad(a, b);
// Set initial position and enable counting
encoder.setCount(0);
encoder.clearCount();
// Set the max count to prevent overflow, though int32_t is large
encoder.setFilter(1023); // Set debounce filter for cleaner readings
// Initialize buffer
bufferIndex = 0;
sampleCount = 0;
for(int i=0; i<bufferSize; i++) angleBuffer[i] = 0.0;
}
void WindSensor::calibrate()
{
// The zero offset is now set by resetting the library's internal counter
encoder.clearCount();
zeroOffset = 0;
}
float WindSensor::getApparentWindAngle()
{
// 1. Get the raw count from the library
// READ FROM HARDWARE: Get raw encoder count
int32_t raw_steps = encoder.getCount();
// 2. Calculate the position within a single 360-degree rotation
// Use the fmodf() function for accurate floating-point modulus
float steps_f = fmodf((float)raw_steps, STEPS_PER_REV);
// 3. Ensure the result is positive (0 to 1024)
if (steps_f < 0)
{
steps_f += STEPS_PER_REV;
}
// 4. Convert steps to angle (0.0 to 360.0)
float angle = (steps_f / STEPS_PER_REV) * 360.0;
addSample(angle);
return angle;
}
void WindSensor::update()
{
// The library handles updates in the background, so this function remains empty,
// but the main loop calls it for consistency.
// However, calling getApparentWindAngle updates the buffer.
getApparentWindAngle();
}
void WindSensor::addSample(float angle) {
angleBuffer[bufferIndex] = angle;
bufferIndex = (bufferIndex + 1) % bufferSize;
if (sampleCount < bufferSize) sampleCount++;
}
float WindSensor::getSmoothedWindAngle() {
return getVectorAverage();
}
float WindSensor::getVectorAverage() {
if (sampleCount == 0) return 0.0;
float sumSin = 0;
float sumCos = 0;
for (size_t i = 0; i < sampleCount; i++) {
float rad = angleBuffer[i] * (float)PI / 180.0f;
sumSin += sinf(rad);
sumCos += cosf(rad);
}
float avgRad = atan2f(sumSin, sumCos);
float avgDeg = avgRad * 180.0 / PI;
if (avgDeg < 0) avgDeg += 360.0;
return avgDeg;