Fix compass heading direction, add magnetic declination and heading offset

- Fix atan2 sign bug: atan2(-yHeading, xHeading) per Freescale AN4248 eq.22.
  The previous atan2(yHeading, xHeading) produced counter-clockwise angles,
  causing East/West to be swapped relative to compass convention.

- Add imuSetMagneticDeclination() (T:146, field "decl") to correct the
  offset between magnetic north and true north (iPhone shows true north).
  Value is persisted in imuConfig.json across reboots.

- Add imuSetHeadingOffset() (T:147, field "off") to compensate for sensor
  mounting orientation on the rover. Also persisted.

- Persist decl and hOff in imuConfig.json (version 2, backwards compatible).

- Fix calibration feedback for web interface: T:126 now includes calDone=1
  (success) or calDone=0 (fail) once after calibration completes, plus
  decl and hOff fields. Calibration start message explains polling procedure.

- Remove duplicate #include <nvs_flash.h> and unused Adafruit ICM20948
  includes that caused build failures when libraries were not installed.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

IMU.cpp

@@ -21,6 +21,10 @@ AK09918 magnetometer_;
 int16_t offset_x = -12, offset_y = 0, offset_z = 0;
 int16_t x, y, z;
 float magnetic_declination_deg = 0.0f;
+float heading_offset_deg = 0.0f;   // manual offset to align sensor X axis with rover forward
+
+// Last calibration result for web feedback: -1=none, 0=failed, 1=success
+static int8_t g_lastCalStatus = -1;
 
 #define Kp 4.50f   // proportional gain governs rate of convergence to accelerometer/magnetometer
 #define Ki 1.0f    // integral gain governs rate of convergence of gyroscope biases
@@ -219,6 +223,7 @@ void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
   magCalibrationProgress = 100;
 
   if (!hasEnoughMagCalibrationCoverage(magCalibrationSession)) {
+    g_lastCalStatus = 0;
     String resultJson = String("{\"T\":1002,\"status\":0,\"info\":\"Mag calibration failed. Rotate slower and cover more angles.\",\"magCal\":") +
                         (magCalibrationAvailable ? "1" : "0") +
                         ",\"saved\":" + (magCalibrationStored ? "1" : "0") + "}";
@@ -237,6 +242,7 @@ void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
   resetHeadingState();
   resetFilterState();
   magCalibrationStored = imuSaveMagnCalibration();
+  g_lastCalStatus = 1;
 
   String resultJson = String("{\"T\":1002,\"status\":1,\"info\":\"Mag calibration finished.\",\"magCal\":1,\"saved\":") +
                       (magCalibrationStored ? "1" : "0") +
@@ -294,7 +300,9 @@ bool computeTiltCompensatedHeading(float rollDeg, float pitchDeg, float mx, floa
     return false;
   }
 
-  *headingDeg = wrapDegrees360(atan2f(yHeading, xHeading) * kRadToDeg + magnetic_declination_deg);
+  // Negative yHeading: atan2(y,x) is CCW but compass convention is CW from North.
+  // See Freescale AN4248 eq.22: heading = atan2(-Bfy, Bfx).
+  *headingDeg = wrapDegrees360(atan2f(-yHeading, xHeading) * kRadToDeg + magnetic_declination_deg + heading_offset_deg);
   return true;
 }
 }  // namespace
@@ -524,6 +532,9 @@ bool imuLoadMagnCalibration()
   offset_x = imuDoc["offset_x"].as<int16_t>();
   offset_y = imuDoc["offset_y"].as<int16_t>();
   offset_z = imuDoc["offset_z"].as<int16_t>();
+  // Backwards compatible: version 1 files won't have these keys, defaults to 0.
+  magnetic_declination_deg = imuDoc["decl"] | 0.0f;
+  heading_offset_deg        = imuDoc["hOff"] | 0.0f;
 
   magCalibrationAvailable = true;
   magCalibrationStored = true;
@@ -539,11 +550,13 @@ bool imuSaveMagnCalibration()
     return false;
   }
 
-  StaticJsonDocument<128> imuDoc;
+  StaticJsonDocument<192> imuDoc;
   imuDoc["offset_x"] = offset_x;
   imuDoc["offset_y"] = offset_y;
   imuDoc["offset_z"] = offset_z;
-  imuDoc["version"] = 1;
+  imuDoc["decl"]     = magnetic_declination_deg;
+  imuDoc["hOff"]     = heading_offset_deg;
+  imuDoc["version"]  = 2;
 
   const size_t bytesWritten = serializeJson(imuDoc, configFile);
   configFile.println();
@@ -659,6 +672,35 @@ float invSqrt(float x)
   return y;
 }
 
+void imuSetMagneticDeclination(float deg)
+{
+  magnetic_declination_deg = deg;
+}
+
+float imuGetMagneticDeclination()
+{
+  return magnetic_declination_deg;
+}
+
+void imuSetHeadingOffset(float deg)
+{
+  heading_offset_deg = deg;
+}
+
+float imuGetHeadingOffset()
+{
+  return heading_offset_deg;
+}
+
+// Returns -1 if no result yet, 0 if last cal failed, 1 if last cal succeeded.
+// Clears the result after reading so it is reported only once.
+int8_t imuPopLastCalStatus()
+{
+  const int8_t s = g_lastCalStatus;
+  g_lastCalStatus = -1;
+  return s;
+}
+
 void calibrateMagn(void)
 {
   int16_t temp[9];

IMU.h

@@ -38,5 +38,10 @@ bool imuSaveMagnCalibration();
 float imuGetTemperature();
 void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
 float invSqrt(float x);
+void imuSetMagneticDeclination(float deg);
+float imuGetMagneticDeclination();
+void imuSetHeadingOffset(float deg);
+float imuGetHeadingOffset();
+int8_t imuPopLastCalStatus();
 
 #endif

IMU_ctrl.h

@@ -64,7 +64,9 @@ void startMagCalibration() {
 		jsonInfoHttp.clear();
 		jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
 		jsonInfoHttp["status"] = started ? 1 : 0;
-		jsonInfoHttp["info"] = started ? "Mag calibration started. Rotate the rover slowly through different angles until progress reaches 100." : "Mag calibration failed to start.";
+		jsonInfoHttp["info"] = started
+			? "Mag cal started (12s). Rotate rover slowly 360deg in horizontal plane. Poll {\"T\":126} to watch magCalProgress reach 100. Result: calDone=1 success, calDone=0 fail."
+			: "Mag calibration failed to start.";
 		jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
 		jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
 		jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
@@ -99,6 +101,14 @@ void getIMUData() {
 	jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
 	jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
 	jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
+	jsonInfoHttp["decl"] = imuGetMagneticDeclination();
+	jsonInfoHttp["hOff"] = imuGetHeadingOffset();
+
+	// Report calibration completion status once (cleared after read).
+	const int8_t calStatus = imuPopLastCalStatus();
+	if (calStatus >= 0) {
+		jsonInfoHttp["calDone"] = calStatus;
+	}
 
 	jsonInfoHttp["temp"] = temp;
 
@@ -128,3 +138,44 @@ void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) {
 		imuSaveMagnCalibration();
 		getIMUOffset();
 }
+
+// {"T":146,"decl":4.5}
+// Sets magnetic declination in degrees (positive = East of true North).
+// Find yours at: https://www.magnetic-declination.com/
+// Example: Germany ~+3 to +5, USA East Coast ~-13, Australia ~+12
+void setMagDeclination(float deg) {
+		imuSetMagneticDeclination(deg);
+		imuSaveMagnCalibration();  // persist across reboots
+
+		jsonInfoHttp.clear();
+		jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
+		jsonInfoHttp["decl"] = imuGetMagneticDeclination();
+		jsonInfoHttp["hOff"] = imuGetHeadingOffset();
+		jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0;
+		jsonInfoHttp["info"] = "Magnetic declination set and saved.";
+
+		String getInfoJsonString;
+		serializeJson(jsonInfoHttp, getInfoJsonString);
+		Serial.println(getInfoJsonString);
+}
+
+// {"T":147,"off":90.0}
+// Sets a fixed heading offset in degrees.
+// Use this to compensate for sensor mounting orientation on the rover.
+// Procedure: point rover to known direction (e.g. phone compass North = 0),
+// send {"T":126} to read current yaw, then set offset = 0 - current_yaw.
+void setHeadingOffset(float deg) {
+		imuSetHeadingOffset(deg);
+		imuSaveMagnCalibration();  // persist across reboots
+
+		jsonInfoHttp.clear();
+		jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
+		jsonInfoHttp["decl"] = imuGetMagneticDeclination();
+		jsonInfoHttp["hOff"] = imuGetHeadingOffset();
+		jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0;
+		jsonInfoHttp["info"] = "Heading offset set and saved.";
+
+		String getInfoJsonString;
+		serializeJson(jsonInfoHttp, getInfoJsonString);
+		Serial.println(getInfoJsonString);
+}

WAVE_ROVER.ino

@@ -12,16 +12,12 @@ StaticJsonDocument<512> jsonInfoHttp;
 #include <WiFi.h>
 #include <WebServer.h>
 #include <esp_now.h>
-#include <nvs_flash.h>
 #include <Adafruit_SSD1306.h>
 #include <INA219_WE.h>
 #include <ESP32Encoder.h>
 #include <PID_v2.h>
 #include <SimpleKalmanFilter.h>
 #include <math.h>
-#include <Adafruit_ICM20X.h>
-#include <Adafruit_ICM20948.h>
-#include <Adafruit_Sensor.h>
 
 
 // functions for barrery info.

json_cmd.h

@@ -119,6 +119,14 @@
 // {"T":145}
 #define CMD_CALI_MAG_START	145
 
+// set magnetic declination in degrees (find at magnetic-declination.com)
+// {"T":146,"decl":4.5}
+#define CMD_SET_MAG_DECLINATION	146
+
+// set fixed heading offset in degrees (compensates sensor mounting angle)
+// {"T":147,"off":90.0}
+#define CMD_SET_HEADING_OFFSET	147
+
 
 
 // LIGHT/GIMBAL/MOVTION CTRL

uart_ctrl.h

@@ -64,6 +64,12 @@ void jsonCmdReceiveHandler(){
 												jsonCmdReceive["cmd"]);break;
 	case CMD_CALI_MAG_START:
 												startMagCalibration();break;
+	case CMD_SET_MAG_DECLINATION:
+												setMagDeclination(
+												jsonCmdReceive["decl"]);break;
+	case CMD_SET_HEADING_OFFSET:
+												setHeadingOffset(
+												jsonCmdReceive["off"]);break;
 	case CMD_ARM_CTRL_UI: RoArmM2_uiCtrl(
 												jsonCmdReceive["E"],
 												jsonCmdReceive["Z"],