Add offset value for imu

ubcuas · Jun 25, 2024 · 9666af9 · 9666af9
1 parent f580d22
commit 9666af9
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 5 deletions.
diff --git a/src/main/ConfigParse.cpp b/src/main/ConfigParse.cpp
@@ -31,6 +31,9 @@ void ConfigParser::createDefaultConfigFile(fs::SDFS fs){
     PID["KD"] = 0.0;
     PID["PID_ControlRate"] = 100.0;
 
+    //IMU Parameters
+    doc["IMU_HeadOffset"] = 180.0;
+
     // Print enable
     doc["Print_Enable"] = true;
     doc["BufferSize"] = 512;
@@ -137,6 +140,9 @@ ConfigParseStatus ConfigParser::parseConfigFile(fs::SDFS fs, ConfigData_t *confi
     configData->PID.KD = doc["PID"]["KD"];
     configData->PID.PID_ControlRate = doc["PID"]["PID_ControlRate"];
 
+    //IMU Parameters
+     configData->IMU_HeadOffset = doc["IMU_HeadOffset"];
+
     //Printing Parameters
     configData->Print_Enable = doc["Print_Enable"];
     configData->BufferSize = doc["BufferSize"];
@@ -185,6 +191,7 @@ void ConfigParser::getConfigNoSD(ConfigData_t *ConfigData){
     ConfigData->PID.KI = 0.0;
     ConfigData->PID.KD = 0.0;
     ConfigData->PID.PID_ControlRate = 100.0;
+    ConfigData->IMU_HeadOffset = 180.0;
 
     ConfigData->Print_Enable = true;
     ConfigData->BufferSize = 512;
@@ -228,6 +235,8 @@ void ConfigParser::printConfigData(ConfigData_t *configData){
     Serial.printf("PID KI: %f\n", configData->PID.KI);
     Serial.printf("PID KD: %f\n", configData->PID.KD);
     Serial.printf("PID Control Rate: %f\n", configData->PID.PID_ControlRate);
+
+    Serial.printf("IMU Head Offset: %lf(deg)\n", configData->IMU_HeadOffset);
 
     Serial.printf("Print Enable: %d\n", configData->Print_Enable);
     Serial.printf("Buffer Size: %d\n", configData->BufferSize);

diff --git a/src/main/ConfigParse.h b/src/main/ConfigParse.h
@@ -74,6 +74,7 @@ typedef struct{
     double ACC_Z_STD;
     double BARO_ALT_STD;
     double GPS_POS_STD;
+    double IMU_HeadOffset;
     PIDConfig_t PID;
     bool Print_Enable;
     uint16_t BufferSize;

diff --git a/src/main/main.ino b/src/main/main.ino
@@ -70,15 +70,15 @@ void setup()
   // Print the configuration data
   configParser_inst.printConfigData(&configData_inst);
 
+  // Initialize the WebStreamServer
+  webStreamServer_inst.init((const char *)configData_inst.SSID, (const char *)configData_inst.Password);
+  webStreamServer_inst.setCustomFunction([&]() { mySensor_inst.resetGPSReference(); });
+
   // Set up ESP-NOW communication
   if(InitESPNow(configData_inst.BottleID) == false){
     SERIAL_PORT.println("ESP-NOW init failed");
   }
 
-  // Initialize the WebStreamServer
-  webStreamServer_inst.init((const char *)configData_inst.SSID, (const char *)configData_inst.Password);
-  webStreamServer_inst.setCustomFunction([&]() { mySensor_inst.resetGPSReference(); });
-
   // Initialize the Sensors
   if(mySensor_inst.init() != Sensors::SENSORS_OK){
     SERIAL_PORT.println("Sensor init failed");
@@ -103,6 +103,8 @@ void setup()
   //Steering setup
   steering_setup(configData_inst.AcquireRate, configData_inst.PID.KP, configData_inst.PID.KI, configData_inst.PID.KD);
 
+  // set imu head offset
+  sensorData_inst.imuData.IMU_HeadOffset = configData_inst.IMU_HeadOffset;
   delay(1000);
   timeStart = millis();
 }

diff --git a/src/main/sensors.cpp b/src/main/sensors.cpp
@@ -364,7 +364,7 @@ SENSORS_Status_t sensors::readIMUData(){
               double yaw = -1 * atan2(t3, t4) * 180.0 / PI;
 
               // Add 180 degrees offset to yaw
-              yaw += YAW_OFFSET;
+              yaw += sensorData.imuData.IMU_HeadOffset;//YAW_OFFSET;
               if (yaw > 180.0) {
                 yaw -= 360.0;
               }

diff --git a/src/main/sensors.h b/src/main/sensors.h
@@ -70,6 +70,7 @@ typedef struct{
     Vector EulerAngles;
     biasStore IMUDmpBias;
     bool imuBiasFoundinEEPROM;
+    float IMU_HeadOffset;
 } imuData_t;
 
 typedef struct{