checking in all tuning and changes we did on the boat

Arduino/controller/AutoPilot.cpp

@@ -13,14 +13,15 @@
 #define METERS_TO_NAUTICAL_MILES 0.000539957  // Conversion factor: 1 meter = 0.000539957 nautical miles
 #define PI std::acos(-1.0)
 
-AutoPilot::AutoPilot(SerialType* ser) {  
+AutoPilot::AutoPilot(SerialType* ser) {
   serial = ser;
 
 #if defined(ARDUINO_ARCH_ESP32)  // For Arduino Nano ESP32
   mutex = xSemaphoreCreateRecursiveMutex();
   if (mutex == NULL) {
     serial->println("mutex creation failed");
-    while(1);
+    while (1)
+      ;
   }
 #endif
 
@@ -50,24 +51,27 @@ AutoPilot::AutoPilot(SerialType* ser) {
   heading_long_average = 0.0;
   heading_short_average_change = 0.0;
   heading_long_average_change = 0.0;
+  heading_short_average_change_max = 0.0;
   heading_average_initialized = false;
   heading_short_average_size = 0;
   heading_long_average_size = 0;
 
   start_motor = 0;
   motor_started = false;
 
-  // for (int i = 0; i < 3; i++) {
-  //   filtered_magnetometer_data[i] = 0.0;
-  //   filtered_accelerometer_data[i] = 0.0;
-  // }
+  for (int i = 0; i < HEADING_AVERAGE_SIZE; i++) {
+    heading_average[i] = 0.0;
+  }
+  heading_average_count = 0;
+
   waypoint_set = false;
   waypoint_lat = 0.0;
   waypoint_lon = 0.0;
   location_lat = 0.0;
   location_lon = 0.0;
   course = 0.0;
 
+  steer_angle = 0.0;
   speed = 0.0;
   distance = 0.0;
   destinationChanged = true;
@@ -79,7 +83,7 @@ AutoPilot::~AutoPilot() {
   if (mutex != NULL) {
     vSemaphoreDelete(mutex);
   }
-#endif  
+#endif
 }
 
 void AutoPilot::setStartMotor(int start_motor) {
@@ -119,7 +123,6 @@ time_t AutoPilot::getDateTime() {
   time_t value = this->dateTime;
   this->unlock();
   return value;
-
 }
 
 int AutoPilot::getMotorStopTime() {
@@ -206,7 +209,7 @@ int AutoPilot::setMode(int mode) {
   if ((mode <= 1) || (mode == 2 && this->waypoint_set)) {
     this->mode = mode;
     if (this->mode == 1) {
-      this->heading_desired = this->heading_short_average;
+      this->heading_desired = this->heading_long_average;
       this->bearing = this->heading_desired;
     }
     this->modeChanged = true;
@@ -269,26 +272,55 @@ void AutoPilot::setHeading(float heading) {
   }
   this->heading = heading;
   if (this->heading_average_initialized) {
-    this->heading_long_average = this->heading_long_average + ((this->heading - this->heading_long_average) / this->heading_long_average_size);
-    this->heading_short_average = this->heading_short_average + ((this->heading - this->heading_short_average) / this->heading_short_average_size);
-    this->heading_long_average_change = this->heading_long_average_change + (((this->heading - this->heading_long_average) - this->heading_long_average_change) / this->heading_long_average_size);
-    this->heading_short_average_change = this->heading_short_average_change + (((this->heading - this->heading_short_average) - this->heading_short_average_change) / this->heading_short_average_size);
+    int j = this->heading_average_count % HEADING_AVERAGE_SIZE;
+    this->heading_average[j] = heading;
+    if (this->heading_average_count < HEADING_AVERAGE_SIZE) {
+      this->heading_average_count++;
+    } else {
+      this->heading_average_count = 0;
+    }
+    float max_heading = 0.0;
+    float min_heading = 360.0;
+    float sum_heading = 0.0;
+    for (int k = 0; k < HEADING_AVERAGE_SIZE; k++) {
+      if (this->heading_average[k] > max_heading) {
+        max_heading = this->heading_average[k];
+      }
+      if (this->heading_average[k] < min_heading) {
+        min_heading = this->heading_average[k];
+      }
+      sum_heading += this->heading_average[k];
+    }
+    this->heading_long_average = (sum_heading - min_heading - max_heading) / (HEADING_AVERAGE_SIZE - 2);
+
+    // char buff[100];
+    // sprintf(buff,"j: %d val: %.2f max: %.2f min: %.2f sum: %.2f, heading: %.2f", j,this->heading_average[j], max_heading, min_heading, sum_heading, this->heading_long_average);
+    // serial->println(buff);
+
+  this->heading_short_average = this->heading_short_average + ((this->heading - this->heading_short_average) / this->heading_short_average_size);
+  this->heading_short_average_change = this->heading_short_average_change + (((this->heading - this->heading_short_average) - this->heading_short_average_change) / this->heading_short_average_size);
+  float change = abs(this->heading - this->heading_short_average);
+  if (this->heading_short_average_change_max < change) {
+    this->heading_short_average_change_max = change;
   } else {
-    this->heading_average_initialized = true;
-    this->heading_long_average = heading;
-    this->heading_short_average = heading;
-  }
-  if (this->heading_long_average_size < COMPASS_LONG_AVERAGE_MAX_SIZE) {
-    this->heading_long_average_size += 1;
-  }
-  if (this->heading_short_average_size < COMPASS_SHORT_AVERAGE_MAX_SIZE) {
-    this->heading_short_average_size += 1;
-  }
-  if (this->mode == 1) {
-    // TODO do we use shot average or long average?
-    this->bearing_correction = this->getCourseCorrection(this->bearing, this->heading_short_average);
+    this->heading_short_average_change_max -= (this->heading_short_average_change_max - change) / 100.;
   }
-  this->unlock();
+}
+else {
+  this->heading_average_initialized = true;
+  this->heading_long_average = heading;
+  this->heading_long_average_size = 1;
+  this->heading_short_average = heading;
+}
+
+if (this->heading_short_average_size < COMPASS_SHORT_AVERAGE_MAX_SIZE) {
+  this->heading_short_average_size += 1;
+}
+if (this->mode == 1) {
+  // TODO do we use shot average or long average?
+  this->bearing_correction = this->getCourseCorrection(this->bearing, this->heading_long_average);
+}
+this->unlock();
 }
 
 float AutoPilot::getHeadingLongAverage() {
@@ -319,9 +351,15 @@ float AutoPilot::getHeadingShortAverageChange() {
   return value;
 }
 
+float AutoPilot::getHeadingShortAverageChangeMax() {
+  this->lock();
+  float value = this->heading_short_average_change_max;
+  this->unlock();
+  return value;
+}
 float AutoPilot::getHeadingLongAverageSize() {
   this->lock();
-  float value =this->heading_long_average_size;
+  float value = this->heading_long_average_size;
   this->unlock();
   return value;
 }
@@ -455,6 +493,19 @@ float AutoPilot::getDistance() {
   return value;
 }
 
+float AutoPilot::getSteerAngle() {
+  this->lock();
+  float value = this->steer_angle;
+  this->unlock();
+  return value;
+}
+
+void AutoPilot::setSteerAngle(float steer_angle) {
+  this->lock();
+  this->steer_angle = steer_angle;
+  this->unlock();
+}
+
 void AutoPilot::printAutoPilot() {
   this->lock();
   serial->print("Date&Time: ");

Arduino/controller/AutoPilot.h

@@ -14,6 +14,7 @@
 #endif
 #include <TimeLib.h> // Include the Time library if needed
 
+#define HEADING_AVERAGE_SIZE 100
 class AutoPilot {
 private:
 #if defined(ARDUINO_ARCH_ESP32)  // For Arduino Nano ESP32
@@ -39,6 +40,9 @@ class AutoPilot {
   float heading_long_average;          // long running average forr the heading (most stable)
   float heading_short_average_change;  // rate of change for the short average
   float heading_long_average_change;   // rage of change for the long average
+  float heading_short_average_change_max;
+  float heading_average[HEADING_AVERAGE_SIZE];
+  int heading_average_count;
   bool heading_average_initialized;
   int heading_short_average_size;
   int heading_long_average_size;
@@ -59,6 +63,8 @@ class AutoPilot {
 
   bool destinationChanged; // flag indicating if the destination (waypoint/heading desired) has changed
   bool modeChanged; // flag indicating that the mode has changed
+  float steer_angle;
+
   SerialType *serial;
 
   float toRadians(float degrees);
@@ -102,6 +108,8 @@ class AutoPilot {
   float getHeadingShortAverageChange();
   float getHeadingLongAverageSize();
   float getHeadingShortAverageSize();
+  float getHeadingShortAverageChangeMax();
+  float getHeadingFilteredAverage();
   // float* getFilteredAccelerometerData();
   // void setFilteredAccelerometerData(float data[3]);
   // float* getFilteredMagentometerData();
@@ -117,6 +125,8 @@ class AutoPilot {
   float getSpeed();
   void setSpeed(float speed);
   float getDistance();
+  float getSteerAngle();
+  void setSteerAngle(float steer_angle);
   void printAutoPilot();
 
 };

Arduino/controller/calibrate.h

@@ -7,14 +7,14 @@
 
 // Hard-iron magnetic calibration settings
 const float magnetic_hard_iron[3] = {
-  -30.70, 20.52, 63.24
+  40.21, 30.01, -26.28
 };
 
 // Soft-iron magnetic calibration settings
 const float magnetic_soft_iron[3][3] = {
-  { 1.010, 0.015, 0.004 },
-  { 0.015, 0.998, -0.035 },
-  { 0.004, -0.035, 0.993 }
+  { 0.965, 0.009, -0.020 },
+  { 0.009, 1.030, -0.028 },
+  { -0.020, -0.028, 1.007 }
 };
 
 const float  acceleration_calibration[3][3] = {

Arduino/controller/command.ino

@@ -27,6 +27,7 @@ char telnet_buffer[BUF_SIZE];
 int telnet_count = BUF_SIZE;
 
 void process_adjust_bearing(CustomClientType& client, char buffer[]);
+void process_steer_angle(CustomClientType& client, char buffer[]);
 void process_mode(CustomClientType& client, char buffer[]);
 void process_print(CustomClientType& client);
 void process_quit(CustomClientType& client);
@@ -65,6 +66,13 @@ void process_adjust_bearing(CustomClientType& client, char buffer[]) {
   DEBUG_PRINTLN(bearing_adjustment);
 }
 
+void process_steer_angle(CustomClientType& client, char buffer[]) {
+  float steer_angle = atof(&buffer[1]);
+  autoPilot.setSteerAngle(steer_angle);
+  client.println("ok");
+  DEBUG_PRINT("set steer angle ");
+  DEBUG_PRINTLN(steer_angle);
+}
 
 void process_mode(CustomClientType& client, char buffer[]) {
   int new_mode = atoi(&buffer[1]);
@@ -210,6 +218,9 @@ void process_telnet(CustomClientType& client, char buffer[]) {
     case 'q':
       process_quit(client);
       break;
+    case 's':
+      process_steer_angle(client, buffer);
+      break;
     case 'w':
       process_waypoint(client, buffer);
       break;

Arduino/controller/compass.ino

@@ -12,9 +12,9 @@ Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(54321);
 
 float mag_data[3];
 float accel_data[3];
-
-float filtered_magnetometer_data[3]; // // Filtered Magnetometer measurements
-float filtered_accelerometer_data[3]; // // Filtered Accelerometer measurements
+int count = 0;
+static float filtered_magnetometer_data[3]; // // Filtered Magnetometer measurements
+static float filtered_accelerometer_data[3]; // // Filtered Accelerometer measurements
 
 void setup_compass() {
   for (int i = 0; i < 3; i++) {
@@ -104,37 +104,40 @@ void check_compass() {
 
     read_magnetometer();
 
-    //float* previous_mag_data = autoPilot.getFilteredMagentometerData();
     for (int i = 0; i < 3; i++) {
-      //mag_data[i] = (mag_data[i] * LOW_PASS_FILTER_COEFFICIENT) + (previous_mag_data[i] * (1.0 - LOW_PASS_FILTER_COEFFICIENT));
       mag_data[i] = (mag_data[i] * LOW_PASS_FILTER_COEFFICIENT) + (filtered_magnetometer_data[i] * (1.0 - LOW_PASS_FILTER_COEFFICIENT));
       filtered_magnetometer_data[i] = mag_data[i];
     }
-    //autoPilot.setFilteredMagnetometerData(mag_data);
 
     read_accelletometer();
 
-    //float* previous_accel_data = autoPilot.getFilteredAccelerometerData();
     for (int i = 0; i < 3; i++) {
-      //accel_data[i] = (accel_data[i] * LOW_PASS_FILTER_COEFFICIENT) + (previous_accel_data[i] * (1.0 - LOW_PASS_FILTER_COEFFICIENT));
       accel_data[i] = (accel_data[i] * LOW_PASS_FILTER_COEFFICIENT) + (filtered_accelerometer_data[i] * (1.0 - LOW_PASS_FILTER_COEFFICIENT));
       filtered_accelerometer_data[i] = accel_data[i];
 
     }
-    //autoPilot.setFilteredAccelerometerData(accel_data);
 
-    double roll = atan2(accel_data[1], accel_data[2]);
-    double pitch = atan2((accel_data[0] * -1.0), sqrt(accel_data[1] * accel_data[1] + accel_data[2] * accel_data[2]));
+    double pitch = atan2(accel_data[1] * -1.0 , sqrt(sq(accel_data[0]) + sq(accel_data[2])));
+    double roll = atan2(accel_data[0] , accel_data[2]);
 
-    double x = accel_data[0] * -1.0;
-    //double y = std::asin(x);
-    //double z = asin(x);
-    //double pitch = asin(accel_data[0] * -1.0);
-    // double roll = (double)asin((double)accel_data[1] / cos((double)pitch));
+    // double pitch = atan2((accel_data[0] * -1.0), sqrt(accel_data[1] * accel_data[1] + accel_data[2] * accel_data[2]));
+    // double roll = atan2(accel_data[1], accel_data[2]);
 
+    // if (count >= 50) {
+    //   count = 0;
+    //   char buff[100];
+    //   sprintf(buff, "Pitch: %.6f Roll: %.6f", pitch, roll);
+    //   DEBUG_PRINTLN(buff);
+    // } else {
+    //   count++;
+    // }
     // //  Calculating tilt compensated heading
     double Xh = mag_data[0] * cos((double)pitch) + mag_data[2] * sin((double)pitch);
     double Yh = mag_data[0] * sin((double)roll) * sin((double)pitch) + mag_data[1] * cos((double)roll) - mag_data[2] * sin((double)roll) * cos((double)pitch);
+
+    // For now forget the compensation since it seems to be crazy :-(
+    Xh = mag_data[0];
+    Yh = mag_data[1];
     float heading = (atan2(Xh, Yh)) * 180 / PI;
     if (heading < 0) {
       heading += 360;