A problem with the roll and pitch output in the rocket flight computer.

[Johann]

Hello, I encountered an issue with my flight computer output; it only shows zero for both roll and pitch. However, the IMU sensor seems to be functioning correctly.
void initQuaternionFilter() {
// Read the initial accelerometer and gyroscope data
myIMU.readAccelData(myIMU.accelCount);
myIMU.readGyroData(myIMU.gyroCount);
myIMU.readMagData(myIMU.magCount);
// Calculate the initial roll, pitch, and yaw angles
float roll = atan2(myIMU.ay, myIMU.az);
float pitch = atan2(-myIMU.ax, sqrt(myIMU.ay * myIMU.ay + myIMU.az * myIMU.az));
float yaw = atan2(-myIMU.my * cos(roll) + myIMU.mz * sin(roll), myIMU.mx * cos(pitch) + myIMU.my * sin(roll) * sin(pitch) + myIMU.mz * cos(roll) * sin(pitch));
// Initialize the quaternion with the initial roll, pitch, and yaw angles
float q0 = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2);
float q1 = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2);
float q2 = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2);
float q3 = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2);
// Set the initial quaternion values using the MahonyQuaternionUpdate function
MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD, myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.mx, myIMU.my, myIMU.mz, 0.0f);
}
bool readIMUData() {
// If intPin goes high, all data registers have new data
if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
// Read accelerometer data
myIMU.readAccelData(myIMU.accelCount);
myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;
myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;
myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;
// Read gyroscope data
myIMU.readGyroData(myIMU.gyroCount);
myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
// Read magnetometer data
myIMU.readMagData(myIMU.magCount);
myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes * myIMU.factoryMagCalibration[0] - myIMU.magBias[0];
myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes * myIMU.factoryMagCalibration[1] - myIMU.magBias[1];
myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes * myIMU.factoryMagCalibration[2] - myIMU.magBias[2];
// Update time for quaternion filter
myIMU.updateTime();
// Store sensor data and angles in the circular buffer
buffer[bufferHead].ax = 1000 * myIMU.ax;
buffer[bufferHead].ay = 1000 * myIMU.ay;
buffer[bufferHead].az = 1000 * myIMU.az;
buffer[bufferHead].gx = myIMU.gx;
buffer[bufferHead].gy = myIMU.gy;
buffer[bufferHead].gz = myIMU.gz;
buffer[bufferHead].mx = myIMU.mx;
buffer[bufferHead].my = myIMU.my;
buffer[bufferHead].mz = myIMU.mz;
buffer[bufferHead].temperature = ((float)myIMU.tempCount) / 333.87 + 21.0;

// Update the circular buffer head index
bufferHead = (bufferHead + 1) % BUFFER_SIZE;
return true; // Return true to indicate successful data read
}
// Perform Mahony quaternion update
MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD, myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my, myIMU.mx, myIMU.mz, myIMU.deltat);
// Calculate Euler angles (yaw, pitch, roll) from the updated quaternion
myIMU.yaw = atan2(2.0f * (*(getQ() + 1) * *(getQ() + 2) + *getQ() * *(getQ() + 3)), *getQ() * *getQ() + *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) - *(getQ() + 3) * *(getQ() + 3));
myIMU.pitch = -asin(2.0f * (*(getQ() + 1) * *(getQ() + 3) - *getQ() * *(getQ() + 2)));
myIMU.roll = atan2(2.0f * (*getQ() * *(getQ() + 1) + *(getQ() + 2) * *(getQ() + 3)), *getQ() * *getQ() - *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) + *(getQ() + 3) * *(getQ() + 3));
myIMU.pitch *= RAD_TO_DEG;
myIMU.yaw *= RAD_TO_DEG;
myIMU.yaw -= 5.333; // Declination adjustment
myIMU.roll *= RAD_TO_DEG;
// Print the yaw, pitch, and roll values
Serial.print("Yaw: ");
Serial.print(myIMU.yaw);
Serial.print("\tPitch: ");
Serial.print(myIMU.pitch);
Serial.print("\tRoll: ");
Serial.println(myIMU.roll);
buffer[bufferHead].yaw = myIMU.yaw;
buffer[bufferHead].pitch = myIMU.pitch;
buffer[bufferHead].roll = myIMU.roll;
return false; // Return false if no new data is available
}


// Function to adjust fins based on IMU data
bool adjustFins() {
unsigned long startTime = millis();
const unsigned long timeout = 500; // 500 milliseconds timeout
// Check if there is valid data in the buffer
if (bufferHead != bufferTail) {
// Get the latest IMU data from the buffer
IMUData data = buffer[bufferTail];
bufferTail = (bufferTail + 1) % BUFFER_SIZE;
// Calculate pitch, roll, and yaw errors
float pitchError = calculatePitchError(desiredPitch);
float rollError = calculateRollError(desiredRoll);
float yawError = calculateYawError(desiredYaw);
// PID controller coefficients
float Kp = 1.0; // Proportional gain
float Ki = 0.0; // Integral gain
float Kd = 0.0; // Derivative gain
// Calculate integral and derivative terms
integralPitchError += pitchError;
integralRollError += rollError;
integralYawError += yawError;
float derivativePitchError = pitchError - previousPitchError;
float derivativeRollError = rollError - previousRollError;
float derivativeYawError = yawError - previousYawError;
// Limit the integral windup
float integratorLimit = 50.0;
integralPitchError = constrain(integralPitchError, -integratorLimit, integratorLimit);
integralRollError = constrain(integralRollError, -integratorLimit, integratorLimit);
integralYawError = constrain(integralYawError, -integratorLimit, integratorLimit);
// Calculate servo angles using PID controller
servoXAngle = map(Kp * pitchError + Ki * integralPitchError + Kd * derivativePitchError, -90, 90, 0, 180);
servoYAngle = map(Kp * rollError + Ki * integralRollError + Kd * derivativeRollError, -90, 90, 0, 180);
// Add yaw error correction to one of the servo angles (e.g., servoYAngle)
servoYAngle += map(Kp * yawError + Ki * integralYawError + Kd * derivativeYawError, -90, 90, -45, 45);
// Update servo positions
servoX.write(servoXAngle);
servoY.write(servoYAngle);
// Update previous errors for next iteration
previousPitchError = pitchError;
previousRollError = rollError;
previousYawError = yawError;
// Print errors for debugging (optional)
Serial.print("Pitch Error: ");
Serial.print(pitchError);
Serial.print("\tRoll Error: ");
Serial.print(rollError);
Serial.print("\tYaw Error: ");
Serial.println(yawError);
return true; // Return true to indicate the adjustment was performed
} else if (millis() - startTime >= timeout) {
// Timeout reached, return false
Serial.println("Timeout reached while reading IMU data for fin adjustment.");
return false;
}
return false;
}

float calculatePitchError(float desiredPitch) {
return desiredPitch - buffer[bufferHead].pitch;
}
float calculateRollError(float desiredRoll) {
return desiredRoll - buffer[bufferHead].roll;
}
float calculateYawError(float desiredYaw) {
return desiredYaw - buffer[bufferHead].yaw;
}