stable flight in theory been achieved

src/controller/drone controller/src/main.cpp

@@ -6,12 +6,22 @@
 // Oled Screen stuff
 U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);
 
-const int MAXPWMVALUE = 1000;
-const int MINPWMVALUE = 2000;
+const int MAXPWMVALUE = 2000;
+const int MINPWMVALUE = 1000;
 const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8B, 0x18};
 // 8c:bf:ea:cc:8b:18
 //=====================================================================================//
 //  Struct declarations
+typedef struct filteredJoystickData
+{
+  int PWMCH1;
+  int PWMCH2;
+  int PWMCH3;
+  int PWMCH4;
+  int killSwitch = 1; // 1 = throttle cut, 0 = normal operation
+} filteredJoystickData;
+filteredJoystickData filteredJoystick; // declare the struct as JoystickData
+
 typedef struct struct_message
 {
   int PWMCH1;
@@ -32,6 +42,16 @@ struct hardJoystickValues
   int RXD; // Right joystick X axis down
 };
 
+// uncalibrated offsets these are calibrated in setup() function
+int offsetPWMCH1 = 0; // Resting value for PWMCH1 (right joystick Y-axis)
+int offsetPWMCH2 = 0; // Resting value for PWMCH2 (right joystick X-axis)
+int offsetPWMCH3 = 0; // Resting value for PWMCH3 (left joystick X-axis)
+int offsetPWMCH4 = 0; // Resting value for PWMCH4 (left joystick Y-axis)
+// Define deadzone threshold
+const int deadzone = 200;
+const int pwmDeadzone = 50; // Much smaller deadzone in PWM space
+
+
 //=====================================================================================//
 // declarations
 int normalizePot(int pin, int minValue);
@@ -40,37 +60,51 @@ int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD
 void espNow();
 void readAllMultiPlexer();
 void espNowLoop();
-void printPWMValues();
+void printFilteredPWMValues();
+void printUnfilteredPWMValues();
 int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
 hardJoystickValues GUIParser();
 void MUXSetup();
 void killSwitch();
 void OLEDSetup();
+void deadZonedJoysticks(); // Apply deadzone to joystick values
+void calibrateJoystickOffsets();
+void printOffsets();
+void debugPWM1();
+void debugAllChannels();
 
+
+//===========================================================================//
 void setup()
 {
+  Serial.begin(9600);
   OLEDSetup();
   espNow();
   MUXSetup(); // Setup the multiplexer
-  Serial.begin(9600);
+  calibrateJoystickOffsets();
 }
 
 void loop()
 {
   // Debugging
-  //  readAllMultiPlexer();
-  //  printPWMValues();
+  // readAllMultiPlexer();
+  // printFilteredPWMValues();
+  // printUnfilteredPWMValues();
+  // printOffsets();
+  debugAllChannels();
 
   // Set values to send
-  JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0));  // Right joystick Y
-  JoystickData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0));  // Right joystick X
-  JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0));  // left joystick Y
-  JoystickData.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0));  // left joystick X
+  JoystickData.PWMCH1 = analogReadMultiPlexer(0, 0, 0, 0, A0); // Right joystick Y
+  JoystickData.PWMCH2 = analogReadMultiPlexer(1, 0, 0, 0, A0); // Right joystick X
+  JoystickData.PWMCH3 = analogReadMultiPlexer(0, 0, 0, 1, A0); // left joystick Y
+  JoystickData.PWMCH4 = analogReadMultiPlexer(1, 0, 0, 1, A0); // left joystick X
+
+  deadZonedJoysticks(); // Apply deadzone to joystick values
+
   bool buttonRight = abs(analogReadMultiPlexer(0, 0, 1, 0, A0) / 4095); // right button
   bool buttonLeft = abs(analogReadMultiPlexer(1, 0, 1, 0, A0) / 4095);  // left button
 
   killSwitch();
-  GUIParser();
   espNowLoop();
   delay(10); // delay to avoid hammering the radio and to save power/heat
 }
@@ -139,7 +173,7 @@ void espNow()
 void espNowLoop()
 {
   // Send message via ESP-NOW
-  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&JoystickData, sizeof(JoystickData));
+  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&filteredJoystick, sizeof(filteredJoystick));
   if (result == ESP_OK)
   {
     // Serial.println("Sent with success");
@@ -166,15 +200,6 @@ void MUXSetup()
 hardJoystickValues GUIParser()
 {
 
-  // Define joystick offsets (calibrated resting values)
-  const int offsetPWMCH1 = 1090; // Resting value for PWMCH1 (right joystick Y-axis)
-  const int offsetPWMCH2 = 1072; // Resting value for PWMCH2 (right joystick X-axis)
-  const int offsetPWMCH3 = 1043; // Resting value for PWMCH3 (left joystick X-axis)
-  const int offsetPWMCH4 = 1476; // Resting value for PWMCH4 (left joystick Y-axis)
-
-  // Define deadzone threshold
-  const int deadzone = 120;
-
   // Adjust joystick values by subtracting offsets
   int adjustedPWMCH1 = JoystickData.PWMCH1 - offsetPWMCH1;
   int adjustedPWMCH2 = JoystickData.PWMCH2 - offsetPWMCH2;
@@ -205,6 +230,108 @@ hardJoystickValues GUIParser()
   return {LXU, LXD, 0, 0}; // Return the values as a struct
 }
 
+void deadZonedJoysticks()
+{
+  // Convert raw values to PWM first, then check deadzone
+  int pwmCH1 = mapPot(JoystickData.PWMCH1);
+  int pwmCH2 = mapPot(JoystickData.PWMCH2);
+  int pwmCH3 = mapPot(JoystickData.PWMCH3);
+  int pwmCH4 = mapPot(JoystickData.PWMCH4);
+
+  // Convert offsets to PWM too
+  int pwmOffsetCH1 = mapPot(offsetPWMCH1);
+  int pwmOffsetCH2 = mapPot(offsetPWMCH2);
+  int pwmOffsetCH3 = mapPot(offsetPWMCH3);
+  int pwmOffsetCH4 = mapPot(offsetPWMCH4);
+
+  // Calculate adjusted values in PWM space
+  int adjustedPWMCH1 = pwmCH1 - pwmOffsetCH1;
+  int adjustedPWMCH2 = pwmCH2 - pwmOffsetCH2;
+  int adjustedPWMCH3 = pwmCH3 - pwmOffsetCH3;
+  int adjustedPWMCH4 = pwmCH4 - pwmOffsetCH4;
+
+  // // Apply deadzone
+  // if (abs(adjustedPWMCH1) < deadzone)
+  // filteredJoystick.PWMCH1 = 1500; // abs to avoid negatives
+  // else
+  // filteredJoystick.PWMCH1 =   JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0));  // Right joystick Y
+  // if (abs(adjustedPWMCH2) < deadzone)
+  // filteredJoystick.PWMCH2 = 1500;
+  // else
+  // filteredJoystick.PWMCH2 = JoystickData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0));  // Right joystick X
+  // if (abs(adjustedPWMCH3) < deadzone)
+  // filteredJoystick.PWMCH3 = 1500;
+  // else
+  // filteredJoystick.PWMCH3 = JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0));  // Left joystick Y
+  // if (abs(adjustedPWMCH4) < deadzone)
+  // filteredJoystick.PWMCH4 = 1500;
+  // else
+  // filteredJoystick.PWMCH4 = JoystickData.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0));  // Left joystick X
+
+  // Apply deadzone and set filtered values
+  // This is the same as above but more compact
+
+  filteredJoystick.PWMCH1 = (abs(adjustedPWMCH1) < pwmDeadzone) ? 1500 : pwmCH1;
+  filteredJoystick.PWMCH2 = (abs(adjustedPWMCH2) < pwmDeadzone) ? 1500 : pwmCH2;
+  filteredJoystick.PWMCH3 = (abs(adjustedPWMCH3) < pwmDeadzone) ? 1500 : pwmCH3;
+  filteredJoystick.PWMCH4 = (abs(adjustedPWMCH4) < pwmDeadzone) ? 1500 : pwmCH4;
+
+  // Copy kill switch state
+  filteredJoystick.killSwitch = JoystickData.killSwitch;
+}
+
+void calibrateJoystickOffsets()
+{
+  delay(5000); // Wait for everything to stabilize
+
+  long sum[4] = {0, 0, 0, 0};
+  const int samples = 500;
+
+  for (int i = 0; i < samples; i++)
+  {
+    sum[0] += analogReadMultiPlexer(0, 0, 0, 0, A0);
+    sum[1] += analogReadMultiPlexer(1, 0, 0, 0, A0);
+    sum[2] += analogReadMultiPlexer(0, 0, 0, 1, A0);
+    sum[3] += analogReadMultiPlexer(1, 0, 0, 1, A0);
+    delay(10); // Longer delay for stability
+  }
+
+  offsetPWMCH1 = sum[0] / samples;
+  offsetPWMCH2 = sum[1] / samples;
+  offsetPWMCH3 = sum[2] / samples;
+  offsetPWMCH4 = sum[3] / samples + 1000; // Hardware quirk of the controller
+}
+
+void killSwitch()
+{
+  // Set the kill switch to 1 to stop the motors
+  if (abs(analogReadMultiPlexer(0, 0, 1, 0, A0) / 4095) == 1) // Right button
+  {
+    JoystickData.killSwitch = 1; // Activate kill switch
+    Serial.println("Kill switch activated, motors stopped.");
+    u8g2.clearBuffer();
+    u8g2.drawStr(25, 15, "Kill Switch: ON");
+  }
+  else if (abs(analogReadMultiPlexer(1, 0, 1, 0, A0) / 4095) == 1) // Left button
+  {
+    JoystickData.killSwitch = 0; // Deactivate kill switch
+    Serial.println("Kill switch deactivated, motors can run.");
+    u8g2.clearBuffer();
+    u8g2.drawStr(25, 15, "Kill Switch: OFF");
+  }
+  u8g2.sendBuffer();
+}
+
+void OLEDSetup()
+{
+  u8g2.begin();
+  u8g2.clearBuffer();
+  u8g2.sendBuffer();
+  u8g2.setFont(u8g2_font_6x10_tf); // Use a different, simpler font
+  u8g2.clearBuffer();
+  u8g2.drawStr(25, 15, "Kill Switch: ON");
+}
+
 /////////////////////////////////////////////
 //           Debugging Functions           //
 /////////////////////////////////////////////
@@ -246,7 +373,19 @@ void readAllMultiPlexer()
   Serial.println(analogReadMultiPlexer(1, 1, 1, 1, A0));
 }
 
-void printPWMValues()
+void printFilteredPWMValues()
+{
+  Serial.print("PWMCH1: ");
+  Serial.print(filteredJoystick.PWMCH1);
+  Serial.print(" PWMCH2: ");
+  Serial.print(filteredJoystick.PWMCH2);
+  Serial.print(" PWMCH3: ");
+  Serial.print(filteredJoystick.PWMCH3);
+  Serial.print(" PWMCH4: ");
+  Serial.println(filteredJoystick.PWMCH4);
+}
+
+void printUnfilteredPWMValues()
 {
   Serial.print("PWMCH1: ");
   Serial.print(JoystickData.PWMCH1);
@@ -258,31 +397,73 @@ void printPWMValues()
   Serial.println(JoystickData.PWMCH4);
 }
 
-void killSwitch()
+void printOffsets()
 {
-  // Set the kill switch to 1 to stop the motors
-  if (abs(analogReadMultiPlexer(0, 0, 1, 0, A0) / 4095) == 1) // Right button
-  {
-    JoystickData.killSwitch = 1; // Activate kill switch
-    Serial.println("Kill switch activated, motors stopped.");
-    u8g2.clearBuffer();
-    u8g2.drawStr(25, 15, "Kill Switch: ON");
-  }
-  else if (abs(analogReadMultiPlexer(1, 0, 1, 0, A0) / 4095) == 1) // Left button
-  {
-    JoystickData.killSwitch = 0; // Deactivate kill switch
-    Serial.println("Kill switch deactivated, motors can run.");
-    u8g2.clearBuffer();
-    u8g2.drawStr(25, 15, "Kill Switch: OFF");
-  }
-  u8g2.sendBuffer();
+  Serial.print("offsetPWMCH1: ");
+  Serial.print(offsetPWMCH1);
+  Serial.print(" offsetPWMCH2: ");
+  Serial.print(offsetPWMCH2);
+  Serial.print(" offsetPWMCH3: ");
+  Serial.print(offsetPWMCH3);
+  Serial.print(" offsetPWMCH4: ");
+  Serial.println(offsetPWMCH4);
 }
 
-void OLEDSetup(){
-  u8g2.begin();
-  u8g2.clearBuffer();
-  u8g2.sendBuffer();
-  u8g2.setFont(u8g2_font_6x10_tf); // Use a different, simpler font
-  u8g2.clearBuffer();
-  u8g2.drawStr(25, 15, "Kill Switch: ON");
+void debugAllChannels() {
+  // Raw values
+  Serial.print("Raw: [");
+  Serial.print(JoystickData.PWMCH1); Serial.print(",");
+  Serial.print(JoystickData.PWMCH2); Serial.print(",");
+  Serial.print(JoystickData.PWMCH3); Serial.print(",");
+  Serial.print(JoystickData.PWMCH4); Serial.print("] ");
+  
+  // Offsets
+  Serial.print("Offsets: [");
+  Serial.print(offsetPWMCH1); Serial.print(",");
+  Serial.print(offsetPWMCH2); Serial.print(",");
+  Serial.print(offsetPWMCH3); Serial.print(",");
+  Serial.print(offsetPWMCH4); Serial.print("] ");
+  
+  // PWM converted values
+  int pwmCH1 = mapPot(JoystickData.PWMCH1);
+  int pwmCH2 = mapPot(JoystickData.PWMCH2);
+  int pwmCH3 = mapPot(JoystickData.PWMCH3);
+  int pwmCH4 = mapPot(JoystickData.PWMCH4);
+  
+  int pwmOffsetCH1 = mapPot(offsetPWMCH1);
+  int pwmOffsetCH2 = mapPot(offsetPWMCH2);
+  int pwmOffsetCH3 = mapPot(offsetPWMCH3);
+  int pwmOffsetCH4 = mapPot(offsetPWMCH4);
+  
+  Serial.print("PWM: [");
+  Serial.print(pwmCH1); Serial.print(",");
+  Serial.print(pwmCH2); Serial.print(",");
+  Serial.print(pwmCH3); Serial.print(",");
+  Serial.print(pwmCH4); Serial.print("] ");
+  
+  // Adjusted values
+  int adjustedPWMCH1 = pwmCH1 - pwmOffsetCH1;
+  int adjustedPWMCH2 = pwmCH2 - pwmOffsetCH2;
+  int adjustedPWMCH3 = pwmCH3 - pwmOffsetCH3;
+  int adjustedPWMCH4 = pwmCH4 - pwmOffsetCH4;
+  
+  Serial.print("Adj: [");
+  Serial.print(adjustedPWMCH1); Serial.print(",");
+  Serial.print(adjustedPWMCH2); Serial.print(",");
+  Serial.print(adjustedPWMCH3); Serial.print(",");
+  Serial.print(adjustedPWMCH4); Serial.print("] ");
+  
+  // Deadzone status
+  Serial.print("InDZ: [");
+  Serial.print(abs(adjustedPWMCH1) < pwmDeadzone ? "Y" : "N"); Serial.print(",");
+  Serial.print(abs(adjustedPWMCH2) < pwmDeadzone ? "Y" : "N"); Serial.print(",");
+  Serial.print(abs(adjustedPWMCH3) < pwmDeadzone ? "Y" : "N"); Serial.print(",");
+  Serial.print(abs(adjustedPWMCH4) < pwmDeadzone ? "Y" : "N"); Serial.print("] ");
+  
+  // Final filtered values
+  Serial.print("Final: [");
+  Serial.print(filteredJoystick.PWMCH1); Serial.print(",");
+  Serial.print(filteredJoystick.PWMCH2); Serial.print(",");
+  Serial.print(filteredJoystick.PWMCH3); Serial.print(",");
+  Serial.print(filteredJoystick.PWMCH4); Serial.println("]");
 }

src/drone/src/main.cpp

@@ -189,10 +189,10 @@ float MagScaleY = 1.0;
 float MagScaleZ = 1.0;
 
 // IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error()
-float AccErrorX = 0.05;
-float AccErrorY = 0.01;
+float AccErrorX = 0.07;
+float AccErrorY = -0.01;
 float AccErrorZ = -0.01;
-float GyroErrorX = 0.00;
+float GyroErrorX = -0.00;
 float GyroErrorY = 0.00;
 float GyroErrorZ = 0.00;
 
@@ -359,6 +359,7 @@ void setupMotorPWM();
 int pulseWidthToDutyCycle(int pulseWidth);
 void setupMatrixSerial();
 void printPitchInfo();
+void printAttitudeDebug();
 //========================================================================================================================//
 //                                                      VOID SETUP                                                        //
 //========================================================================================================================//
@@ -456,6 +457,7 @@ void loop()
   // printLoopRate();      //Prints the time between loops in microseconds (expected: microseconds between loop iterations)
   // printDebugInfo(); //PWM 1 and 5 + Armed conditio
   // printPitchInfo();
+  // printAttitudeDebug();
 
   // Get arming status
   armedStatus(); // Check if the throttle cut is off and throttle is low.
@@ -508,12 +510,13 @@ void controlMixer()
    *roll_passthru, pitch_passthru, yaw_passthru - direct unstabilized command passthrough
    *channel_6_pwm - free auxillary channel, can be used to toggle things with an 'if' statement
    */
-
+  
   // Quad mixer with pitch bias because the sensor isnt flat on the board
   m1_command_scaled = thro_des - pitch_PID + roll_PID - yaw_PID; // Front Left CCW
   m2_command_scaled = thro_des - pitch_PID - roll_PID + yaw_PID; // Front Right CW
   m3_command_scaled = thro_des + pitch_PID - roll_PID - yaw_PID; // Back Right CCW
   m4_command_scaled = thro_des + pitch_PID + roll_PID + yaw_PID; // Back Left CW
+  
   m5_command_scaled = 0;
   m6_command_scaled = 0;
   
@@ -624,7 +627,6 @@ void getIMUdata()
   mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
   #elif defined USE_BNO085_I2C
   // Keep waiting until data becomes available
-  // Wait for data with timeout
   unsigned long startTime = millis();
   while (!myIMU.dataAvailable()) {
     delay(1);
@@ -634,18 +636,16 @@ void getIMUdata()
     }
   }
   
-
-    // Get quaternion directly from BNO085
-    q0 = myIMU.getQuatReal();  // w component
-    q1 = myIMU.getQuatI();     // x component
-    q2 = myIMU.getQuatJ();     // y component
-    q3 = myIMU.getQuatK();     // z component
-    
-    // Calculate Euler angles from quaternion
-    roll_IMU = atan2(2.0f * (q0*q1 + q2*q3), 1.0f - 2.0f * (q1*q1 + q2*q2)) * 57.29577951f;
-    pitch_IMU = asin(2.0f * (q0*q2 - q3*q1)) * 57.29577951f;
-    yaw_IMU = atan2(2.0f * (q0*q3 + q1*q2), 1.0f - 2.0f * (q2*q2 + q3*q3)) * 57.29577951f;
-
+  // Get quaternion directly from BNO085
+  q0 = myIMU.getQuatReal();  // w component
+  q1 = myIMU.getQuatI();     // x component
+  q2 = myIMU.getQuatJ();     // y component
+  q3 = myIMU.getQuatK();     // z component
+  
+  // CORRECTED Euler angle calculation for BNO085
+  roll_IMU = atan2(2.0f * (q0*q1 + q2*q3), 1.0f - 2.0f * (q1*q1 + q2*q2)) * 57.29577951f;
+  pitch_IMU = asin(constrain(2.0f * (q0*q2 - q3*q1), -0.999999, 0.999999)) * 57.29577951f;
+  yaw_IMU = atan2(2.0f * (q0*q3 + q1*q2), 1.0f - 2.0f * (q2*q2 + q3*q3)) * 57.29577951f;
   
   // Get raw sensor data for PID controller
   GyroX = myIMU.getGyroX() * 57.29577951f; // Convert rad/s to deg/s
@@ -664,7 +664,7 @@ void getIMUdata()
   GyroY_prev = GyroY;
   GyroZ_prev = GyroZ;
   
-  // Get accelerometer data (still useful for PID control)
+  // Get accelerometer data (still useful for validation)
   AccX = myIMU.getAccelX() / 9.80665f; // Convert m/s² to g
   AccY = myIMU.getAccelY() / 9.80665f;
   AccZ = myIMU.getAccelZ() / 9.80665f;
@@ -681,7 +681,6 @@ void getIMUdata()
   AccY_prev = AccY;
   AccZ_prev = AccZ;
 
-
   // Return to avoid executing code for other IMUs
   return;
 #endif
@@ -1486,9 +1485,7 @@ void calibrateESCs()
    */
   while (true)
   {
-    prev_time = current_time;
-    current_time = micros();
-    dt = (current_time - prev_time) / 1000000.0;
+
 
     // digitalWrite(13, HIGH); // LED on to indicate we are not in main loop
 
@@ -1496,7 +1493,6 @@ void calibrateESCs()
     failSafe();                                                                // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup
     getDesState();                                                             // Convert raw commands to normalized values based on saturated control limits
     getIMUdata();                                                              // Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise
-    Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); // Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees)
     getDesState();                                                             // Convert raw commands to normalized values based on saturated control limits
 
     m1_command_scaled = thro_des;
@@ -1915,6 +1911,24 @@ void printPitchInfo() {
   }
 }
 
+void printAttitudeDebug() {
+  if (current_time - print_counter > 10000) {
+    print_counter = micros();
+    Serial.print("q0:");
+    Serial.print(q0, 4);
+    Serial.print(" q1:");
+    Serial.print(q1, 4);
+    Serial.print(" q2:");
+    Serial.print(q2, 4);
+    Serial.print(" q3:");
+    Serial.print(q3, 4);
+    Serial.print(" roll:");
+    Serial.print(roll_IMU);
+    Serial.print(" pitch:");
+    Serial.println(pitch_IMU);
+  }
+}
+
 //=========================================================================================//
 
 // HELPER FUNCTIONS