diff --git a/src/SUAS_Link_2024/src/main.cpp b/src/SUAS_Link_2024/src/main.cpp index 191284f..3994330 100644 --- a/src/SUAS_Link_2024/src/main.cpp +++ b/src/SUAS_Link_2024/src/main.cpp @@ -52,8 +52,13 @@ void setup() { peerInfo.encrypt = false; // Register parachute boards memcpy(peerInfo.peer_addr, ADDRESS_1, 6); + if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); } memcpy(peerInfo.peer_addr, ADDRESS_2, 6); if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); } + memcpy(peerInfo.peer_addr, ADDRESS_3, 6); + if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); } + memcpy(peerInfo.peer_addr, ADDRESS_4, 6); + if (esp_now_add_peer(&peerInfo) != ESP_OK) { Serial.println("Failed to add peer"); } digitalWrite(LED_RED, LOW); // Red LED off indicates comminucation established @@ -80,11 +85,7 @@ char buffer[100]; // For printing things void loop() { // Read drop point and bottle number from Pi (blocking) - // drop_data = recieveData(); - drop_data.lon = -123.0734507; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! - drop_data.lat = 49.2444132; - drop_data.heading = 0.0; - drop_data.bottleID = 2; + drop_data = recieveData(); bool notDropped = true; snprintf(buffer, sizeof(buffer), "Received data from Pi: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer); @@ -96,19 +97,12 @@ void loop() { // Serial.println("Wind: " + String(windspeed) + ", " + String(wind_heading)); // calc_des_drop_state(windspeed, wind_heading, drop_data, &des_drop_data); - calc_des_drop_state(0, 0, drop_data, &des_drop_data); // TODO: REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - // Serial.println(latitudeToMeters(des_drop_data.lat)-latitudeToMeters(drop_data.lat)); - // Serial.println(latitudeToMeters(des_drop_data.lon)-latitudeToMeters(drop_data.lon)); + calc_des_drop_state(0, 0, drop_data, &des_drop_data); // Send desired drop point to Pi snprintf(buffer, sizeof(buffer), "%.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID); PiSerial.print(buffer); - Serial.print("Sent desired drop point to Pi: "); Serial.print(buffer); - - // Send message to parachutes (try 3 times; this is time sensitive so nothing we can do if it fails) - for (int i = 0; i < 3; i++) { - broadcastMessage(des_drop_data); - } + Serial.print("Sent desired drop point to Pi (not): "); Serial.print(buffer); // Wait to get close enough to desired drop point digitalWrite(LED_RED, HIGH); @@ -119,25 +113,21 @@ void loop() { double lon = (double) msg.global_position_int.lon / 10000000.0; // Serial.println("Coords: " + String(lat, 8) + ", " + String(lon, 8)); - if (FAILSAFE_MODE) { - if (msg.servo_output_raw.servo13_raw > 1500) { // If servo 13 is high, then drop the right bottle - des_drop_data.bottleID = 1; - notDropped = false; - Serial.println("Failsafe mode: Right bottle dropped."); - } - if (msg.servo_output_raw.servo14_raw > 1500) { // If servo 14 is high, then drop the left bottle - des_drop_data.bottleID = 2; - notDropped = false; - Serial.println("Failsafe mode: Left bottle dropped."); - } - } - else { - double dist_from_drop_point = distance(lat, lon, des_drop_data.lat, des_drop_data.lon); - Serial.println("Dist: " + String(dist_from_drop_point)); - if (dist_from_drop_point < RELEASE_MARGIN) { - notDropped = false; - Serial.println("Reached drop point"); - } + if (msg.servo_output_raw.servo13_raw > 1500) { // If servo 13 is high, then drop the left bottle + des_drop_data.bottleID = 1; + notDropped = false; + Serial.println("Failsafe mode: Right bottle dropped."); + } + if (msg.servo_output_raw.servo14_raw > 1500) { // If servo 14 is high, then drop the right bottle + des_drop_data.bottleID = 2; + notDropped = false; + Serial.println("Failsafe mode: Left bottle dropped."); + } + double dist_from_drop_point = distance(lat, lon, des_drop_data.lat, des_drop_data.lon); + Serial.println("Dist: " + String(dist_from_drop_point)); + if (dist_from_drop_point < RELEASE_MARGIN) { + notDropped = false; + Serial.println("Reached drop point"); } } digitalWrite(LED_RED, LOW); diff --git a/src/SUAS_Link_2024/src/params.h b/src/SUAS_Link_2024/src/params.h index 0ab9454..63d4dc9 100644 --- a/src/SUAS_Link_2024/src/params.h +++ b/src/SUAS_Link_2024/src/params.h @@ -19,20 +19,20 @@ extern HardwareSerial CubeSerial; #define M_PER_LAT_DEG 111320.0 // 111320 meters per degree of latitude (short distance approx) -#define AIRCRAFT_SPEED 7.0 // m/s -#define DRIFT_FACTOR 0.0 // TODO: find this from testing, it's arbitrary rn!! +#define AIRCRAFT_SPEED 12.0 // m/s +#define DRIFT_FACTOR 1.0 // TODO: find this from testing, it's arbitrary rn!! #define RELEASE_DELAY 0.0 // s // TODO!!! -#define RELEASE_MARGIN 6.0 // Tolerance margin from desired drop point that we will trigger release +#define RELEASE_MARGIN 5.0 // Tolerance margin from desired drop point that we will trigger release #define WINDOW_SIZE 10 -#define FAILSAFE_MODE true +#define FAILSAFE_MODE false // MAC Address of responder - edit as required static const uint8_t ADDRESS_1[] = {0xD8, 0xBC, 0x38, 0xE4, 0x9E, 0x5C}; // Parachute #1 static const uint8_t ADDRESS_2[] = {0x94, 0xE6, 0x86, 0x92, 0xA7, 0xFC}; // Parachute #2 -// static const uint8_t ADDRESS_3[] = {0x, 0x, 0x, 0x, 0x, 0x}; -// static const uint8_t ADDRESS_4[] = {0x, 0x, 0x, 0x, 0x, 0x}; -// static const uint8_t ADDRESS_5[] = {0x, 0x, 0x, 0x, 0x, 0x}; +static const uint8_t ADDRESS_3[] = {0x94, 0xE6, 0x86, 0x92, 0xA7, 0xE4}; // Parachute #3 +static const uint8_t ADDRESS_4[] = {0x94, 0xE6, 0x86, 0x92, 0xA8, 0xCC}; // Parachute #4 +// static const uint8_t ADDRESS_ALL[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; #endif \ No newline at end of file diff --git a/src/main/ConfigParse.cpp b/src/main/ConfigParse.cpp index eac0097..89e9b00 100644 --- 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 index 1c8c5fa..71dc977 100644 --- 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/Reciever.cpp b/src/main/Reciever.cpp index 7d263e5..0c40809 100644 --- a/src/main/Reciever.cpp +++ b/src/main/Reciever.cpp @@ -27,7 +27,8 @@ bool InitESPNow(uint8_t Bottle_id) { // Callback function that will be executed when data is received void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { - memcpy(&myData, (datapacket*)incomingData, sizeof(myData)); + datapacket temp_storage; + memcpy(&temp_storage, (datapacket*)incomingData, sizeof(temp_storage)); Serial.print("Bytes received: "); Serial.println(len); Serial.print("lat: "); @@ -41,8 +42,9 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { Serial.println(); // Send a response - if (myData.bottleID == Bottle_ID){ + if (temp_storage.bottleID == Bottle_ID){ response.bottleID = Bottle_ID; + memcpy(&myData, (datapacket*)incomingData, sizeof(myData)); esp_err_t result = esp_now_send(mac, (uint8_t *) &response, sizeof(response)); if (result == ESP_OK) { Serial.println("Response sent"); diff --git a/src/main/SDCard.cpp b/src/main/SDCard.cpp index ed034ba..afedadf 100644 --- a/src/main/SDCard.cpp +++ b/src/main/SDCard.cpp @@ -90,9 +90,21 @@ namespace SDCard } } - // create a new base file + // Check if directory exists + if (!SD.exists(LOG_DIR)) { + // Create directory if it does not exist + if (SD.mkdir(LOG_DIR)) { + Serial.println("Directory created"); + } else { + Serial.println("Failed to create directory"); + SDStatus = SDCARD_ERROR; + return SDCARD_ERROR; + } + } + + // Create a new base file File file = SD.open((String(LOG_DIR) + "/" + String(LOG_FILE_BASE) + String(LOG_FILE_EXT)), FILE_WRITE); - if(!file){ + if (!file) { Serial.println("Failed to create new file"); SDStatus = SDCARD_ERROR; return SDCARD_ERROR; diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp index cac3408..8a84d05 100644 --- a/src/main/encoder_steering.cpp +++ b/src/main/encoder_steering.cpp @@ -12,6 +12,7 @@ QueueHandle_t steeringQueue = NULL; #define SERVO_CONTROL_PERIOD 10 // ms volatile int PID_Control_Period = SERVO_CONTROL_PERIOD; +bool servo_enable = false; void steering_setup(double acquireRate, double kp, double ki, double kd) { @@ -31,16 +32,6 @@ void steering_setup(double acquireRate, double kp, double ki, double kd) { PID_Control_Period = (1.0/acquireRate)*1000; // pid.setDeadband(DIST_PER_TICK); - // Initialize servos - Serial.println("Initializing servos"); - servo_1.attach(SERVO_1); - servo_1.setPeriodHertz(SERVO_FREQ); - servo_2.attach(SERVO_2); - servo_2.setPeriodHertz(SERVO_FREQ); - // No spin - servo_1.write(90); - servo_2.write(90); - // Create queue steeringQueue = xQueueCreate(1, sizeof(AngleData)); // this queue will take the yaw value if (steeringQueue == NULL) { @@ -67,7 +58,19 @@ void steering_setup(double acquireRate, double kp, double ki, double kd) { } else { Serial.println("Servo control task created"); } +} + +void init_servos() { + // Initialize servos + Serial.println("Initializing servos"); + servo_1.attach(SERVO_1); + servo_1.setPeriodHertz(SERVO_FREQ); + servo_2.attach(SERVO_2); + servo_2.setPeriodHertz(SERVO_FREQ); + // No spin + servo_1.write(90); + servo_2.write(90); } @@ -90,11 +93,22 @@ double angle_diff(double angle1, double angle2) { */ void servo_control(AngleData data) { - if (data.desiredForward == -1){ - do_nothing(); + // Serial,printf("Data Packet: %lf \t %lf \t %lf\n", data.currentYaw, data.desiredForward, data.desiredYaw); + + if (data.desiredForward == -1.0){ + if (servo_enable) { + servo_1.detach(); + servo_2.detach(); + servo_enable = false; + } return; } + if (!servo_enable) { + servo_enable = true; + init_servos(); + } + SteeringData des_lengths; SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)}; @@ -111,7 +125,7 @@ void servo_control(AngleData data) { des_lengths.l2 = output; } - Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done + // Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done // Servo 1 if (des_lengths.l1 - current_lengths.l1 > (DIST_PER_TICK)) { @@ -145,13 +159,13 @@ void servoControlTask(void *pvParameters) { // initialize the incoming data incomingData.desiredYaw = 0; incomingData.currentYaw = 0; - incomingData.desiredForward = 0; + incomingData.desiredForward = -1.0; while (true) { // Check if we have incoming data if (xQueueReceive(steeringQueue, &incomingData, 0) == pdTRUE) { // Successfully received data - Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); + // Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); } // Call servo_control function here servo_control(incomingData); // Modify parameters as needed diff --git a/src/main/encoder_steering.h b/src/main/encoder_steering.h index 8df0577..2fea534 100644 --- a/src/main/encoder_steering.h +++ b/src/main/encoder_steering.h @@ -39,6 +39,9 @@ extern double current_heading; extern volatile bool forward_1; extern volatile bool forward_2; +extern bool servo_enable; + +void init_servos(); void steering_setup(double acquireRate, double kp, double ki, double kd); double angle_diff(double angle1, double angle2); void servo_control(AngleData data); diff --git a/src/main/gpsLowLevel.h b/src/main/gpsLowLevel.h index a6104f3..341fa7d 100644 --- a/src/main/gpsLowLevel.h +++ b/src/main/gpsLowLevel.h @@ -26,7 +26,7 @@ typedef struct{ class gpsLowLevel { public: - gpsLowLevel(HardwareSerial *gpsSerial = &Serial2, uint32_t GPSBaudRate = 115200, uint8_t GPSRate = 5, uint8_t RXPin = 2, uint8_t TXPin = 4); + gpsLowLevel(HardwareSerial *gpsSerial = &Serial2, uint32_t GPSBaudRate = 115200, uint8_t GPSRate = 5, uint8_t RXPin = 4, uint8_t TXPin = 2); enum GPS_Status { GPS_OK, diff --git a/src/main/main.ino b/src/main/main.ino index dd93507..6b1b007 100644 --- 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(); } @@ -367,10 +369,11 @@ void ComputePID(){ if (myData.bottleID == configData_inst.BottleID){ target_lon = myData.lon; target_lat = myData.lat; + printf("Target received.\n"); } else { - target_lon = 0; - target_lat = 0; + target_lon = 1000.0; // 1000 deg lat or lon doesn't exist (I hope) + target_lat = 1000.0; } //To access kalman filter values for current x,y,&z direction MatrixXd X_Zaxis = myKalmanFilter_inst_Z.getState(); @@ -399,37 +402,38 @@ void ComputePID(){ setpoint = setpoint - 360; } - // Send the data packet to the queue (i.e. activate steering), if detect that parachute has fallen below HEIGHT_THRESH - // double height = X_Zaxis(0, 0); - double height = sensorData_inst.barometerData.Altitude - sensorData_inst.barometerData.AltitudeOffset; + // Send the data packet to the queue (i.e. activate steering), if detect that parachute has fallen below HEIGHT_THRESH (and if it was given a coordinate to go to lol) + double height = X_Zaxis(0, 0); + // double height = sensorData_inst.barometerData.Altitude - sensorData_inst.barometerData.AltitudeOffset; // Serial.println("\nHeight: " + String(height) + "\n"); - if (height <= configData_inst.HEIGHT_THRESH && height >= 1.0){ + if (height <= configData_inst.HEIGHT_THRESH && height >= 0.5 && target_lon != 1000.0 && target_lat != 1000.0){ // Make the Data packet + // Serial.printf("Height matched if condition. Value: %lf\n", height); AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0}; sendSteeringData(data); } else { - count_1 = 0; // Keep resetting encoders to zero until steering activated - count_2 = 0; + // count_1 = 0; // Keep resetting encoders to zero until steering activated + // count_2 = 0; AngleData data = {0, 0, -1}; // I'm using the desiredForward as a flag to turn off the servos (-1 = off) sendSteeringData(data); } double distance = GPS.distanceBetween(lon_now, lat_now, target_lon, target_lat); - // //Print to terminal - // snprintf(outputBuffer, BufferLen, "Yaw: %.3lf\nSetpoint: %.3lf\nDistance: %.3lf\nlattitude: %.6lf\nlongitude: %.6lf\nlat_now: %.6lf\nlon_now: %.6lf\n,x: %.6lf\ny: %.6lf\nz: %.6lf\n", - // sensorData_inst.imuData.EulerAngles.v2, - // setpoint, - // distance, - // sensorData_inst.gpsData.Latitude, - // sensorData_inst.gpsData.Longitude, - // lat_now, - // lon_now, - // X_Xaxis(0,0), - // X_Yaxis(0,0), - // X_Zaxis(0,0) - // ); - // SERIAL_PORT.print(outputBuffer); + //Print to terminal + snprintf(outputBuffer, BufferLen, "Yaw: %.3lf\nSetpoint: %.3lf\nDistance: %.3lf\nlattitude: %.6lf\nlongitude: %.6lf\nlat_now: %.6lf\nlon_now: %.6lf\n,x: %.6lf\ny: %.6lf\nz: %.6lf\n", + sensorData_inst.imuData.EulerAngles.v2, + setpoint, + distance, + sensorData_inst.gpsData.Latitude, + sensorData_inst.gpsData.Longitude, + lat_now, + lon_now, + X_Xaxis(0,0), + X_Yaxis(0,0), + X_Zaxis(0,0) + ); + SERIAL_PORT.print(outputBuffer); } diff --git a/src/main/sensors.cpp b/src/main/sensors.cpp index 947f486..7f16c12 100644 --- a/src/main/sensors.cpp +++ b/src/main/sensors.cpp @@ -183,30 +183,30 @@ SENSORS_Status_t sensors::initIMU(){ SERIAL_PORT.println(F("EEPROM.begin failed! Bias Save and Restore not possible")); } EEPROM.get(0, sensorData.imuData.IMUDmpBias); //read the bias if stored - if(isBiasStoreValid(&sensorData.imuData.IMUDmpBias)){ - SERIAL_PORT.println(F("Bias data in EEPROM is valid. Restoring it...")); - success &= (imu.setBiasGyroX(sensorData.imuData.IMUDmpBias.biasGyroX) == ICM_20948_Stat_Ok); - success &= (imu.setBiasGyroY(sensorData.imuData.IMUDmpBias.biasGyroY) == ICM_20948_Stat_Ok); - success &= (imu.setBiasGyroZ(sensorData.imuData.IMUDmpBias.biasGyroZ) == ICM_20948_Stat_Ok); - success &= (imu.setBiasAccelX(sensorData.imuData.IMUDmpBias.biasAccelX) == ICM_20948_Stat_Ok); - success &= (imu.setBiasAccelY(sensorData.imuData.IMUDmpBias.biasAccelY) == ICM_20948_Stat_Ok); - success &= (imu.setBiasAccelZ(sensorData.imuData.IMUDmpBias.biasAccelZ) == ICM_20948_Stat_Ok); - success &= (imu.setBiasCPassX(sensorData.imuData.IMUDmpBias.biasCPassX) == ICM_20948_Stat_Ok); - success &= (imu.setBiasCPassY(sensorData.imuData.IMUDmpBias.biasCPassY) == ICM_20948_Stat_Ok); - success &= (imu.setBiasCPassZ(sensorData.imuData.IMUDmpBias.biasCPassZ) == ICM_20948_Stat_Ok); - if (success) - { - SERIAL_PORT.println(F("Biases restored.")); - printBiases(&sensorData.imuData.IMUDmpBias); - sensorData.imuData.imuBiasFoundinEEPROM = true; - delay(2000); - } - else{ - SERIAL_PORT.println(F("Bias restore failed!")); - sensorData.imuData.imuBiasFoundinEEPROM = false; - delay(2000); - } - } + // if(isBiasStoreValid(&sensorData.imuData.IMUDmpBias)){ + // SERIAL_PORT.println(F("Bias data in EEPROM is valid. Restoring it...")); + // success &= (imu.setBiasGyroX(sensorData.imuData.IMUDmpBias.biasGyroX) == ICM_20948_Stat_Ok); + // success &= (imu.setBiasGyroY(sensorData.imuData.IMUDmpBias.biasGyroY) == ICM_20948_Stat_Ok); + // success &= (imu.setBiasGyroZ(sensorData.imuData.IMUDmpBias.biasGyroZ) == ICM_20948_Stat_Ok); + // success &= (imu.setBiasAccelX(sensorData.imuData.IMUDmpBias.biasAccelX) == ICM_20948_Stat_Ok); + // success &= (imu.setBiasAccelY(sensorData.imuData.IMUDmpBias.biasAccelY) == ICM_20948_Stat_Ok); + // success &= (imu.setBiasAccelZ(sensorData.imuData.IMUDmpBias.biasAccelZ) == ICM_20948_Stat_Ok); + // success &= (imu.setBiasCPassX(sensorData.imuData.IMUDmpBias.biasCPassX) == ICM_20948_Stat_Ok); + // success &= (imu.setBiasCPassY(sensorData.imuData.IMUDmpBias.biasCPassY) == ICM_20948_Stat_Ok); + // success &= (imu.setBiasCPassZ(sensorData.imuData.IMUDmpBias.biasCPassZ) == ICM_20948_Stat_Ok); + // if (success) + // { + // SERIAL_PORT.println(F("Biases restored.")); + // printBiases(&sensorData.imuData.IMUDmpBias); + // sensorData.imuData.imuBiasFoundinEEPROM = true; + // delay(2000); + // } + // else{ + // SERIAL_PORT.println(F("Bias restore failed!")); + // sensorData.imuData.imuBiasFoundinEEPROM = false; + // delay(2000); + // } + // } SERIAL_PORT.println("IMU init successful"); return status; } @@ -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 index d4023b5..f235d28 100644 --- a/src/main/sensors.h +++ b/src/main/sensors.h @@ -37,7 +37,7 @@ #define GPSBAUDRATE 115200 #define GPS_RX 4 #define GPS_TX 2 -#define GPS_RATE 5 +#define GPS_RATE 10 // BATTERY MEASUREMENT #define BATTERY_PIN 14 @@ -70,6 +70,7 @@ typedef struct{ Vector EulerAngles; biasStore IMUDmpBias; bool imuBiasFoundinEEPROM; + float IMU_HeadOffset; } imuData_t; typedef struct{