Implement save/restore of compass calibration values to EEPROM.

Author: bertrik

motor_controller/boat_vehicle.ino

@@ -50,6 +50,9 @@ GET HEADING
 GET WD
 */
 
+#include <Arduino.h>
+#include <EEPROM.h>
+
 // function to get sign of a value
 #define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
 
@@ -125,6 +128,7 @@ volatile bool newDataAvailable = false;
 void setup() {
   // Initialize serial communication
   Serial.begin(115200);
+  EEPROM.begin();
 
   // Initialize motor control pins
   pinMode(motorAPin1, OUTPUT);
@@ -154,9 +158,14 @@ void setup() {
   //use precalibration values to start with:
   //compass.setCalibrationOffsets(-69.00, 523.00, -907.00);
   //compass.setCalibrationScales(0.97, 1.00, 1.03);
+
+#if 0 // old hardcoded calibration
   compass.setCalibrationOffsets(294.00,-176.00,-794.00);
   compass.setCalibrationScales(0.90,0.80,1.54);
-
+#else // new calibration as restored from EEPROM
+  EEPROM.get(0, calibVal);
+  restoreCompassCalibration(calibVal);
+#endif
   compass.setSmoothing(4,true); //set smoothing to max (1..10)
 
   // Initialize DRDY pin and interrupt
@@ -300,10 +309,8 @@ void setupTimer1PWM() {
   OCR1B = 0;
 }
 
-
-FloatArray calibrateCompass(){
+void calibrateCompass(FloatArray &data) {
   //float calib[6];
-  FloatArray data;
   //make sure vehicle moves
   speedCommand=255;//128;//max speed
   nav_state=go_left;//to turn on itself
@@ -327,7 +334,12 @@ FloatArray calibrateCompass(){
   compass.setSmoothing(3,true);  
   //Serial.println("CALIBRATION DONE.");
   nav_state=stop_engine;
-  return data;
+}
+
+static void restoreCompassCalibration(FloatArray &data)
+{
+  compass.setCalibrationOffsets(data.values[0], data.values[1], data.values[2]);
+  compass.setCalibrationScales(data.values[3], data.values[4], data.values[5]);
 }
 
 void handlePWMcommand(String command) {
@@ -431,7 +443,8 @@ void handleWDcommand(String command) {
 void handleCALIBcommand() {
   if (motor_enable == true) {
     mode_pwm=false;
-    calibVal = calibrateCompass();//can take 10sec
+    calibrateCompass(calibVal);//can take 10sec
+    EEPROM.put(0, calibVal);
     Serial.println("OK");
   } else { // motor is not enable, calibration will fail because of vehicle not moving
     Serial.println("ERR");