Fix motors not getting armed

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

@@ -5,14 +5,15 @@
 // declarations
 int normalizePot(int pin, int minValue);
 int mapPot(int normalizedValue);
+int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
 
-// constants
+const int MAXPWMVALUE = 1000;
-const int POTPIN1 = 2;
-const int MAXPWMVALUE = 1400;
 const int MINPWMVALUE = 2000;
 const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8E, 0x5C};
+
 // Define the struct that will be sent
-typedef struct struct_message {
+typedef struct struct_message
+{
   int PWMCH1;
   int PWMCH2;
   int PWMCH3;
@@ -28,7 +29,8 @@ void setup()
   WiFi.mode(WIFI_STA);
 
   // Init ESP-NOW
-  if (esp_now_init() != ESP_OK) {
+  if (esp_now_init() != ESP_OK)
+  {
     Serial.println("Error initializing ESP-NOW");
     return;
   }
@@ -39,38 +41,49 @@ void setup()
   peerInfo.encrypt = false;
 
   // Add peer
-  if (esp_now_add_peer(&peerInfo) != ESP_OK){
+  if (esp_now_add_peer(&peerInfo) != ESP_OK)
+  {
     Serial.println("Failed to add peer");
     return;
   }
 
   Serial.begin(9600);
-  pinMode(POTPIN1, INPUT);
+
+  pinMode(D3, OUTPUT);
+  pinMode(D6, OUTPUT);
+  pinMode(D7, OUTPUT);
+  pinMode(D8, OUTPUT);
+  pinMode(D9, OUTPUT);
+  pinMode(D0, INPUT);
+
 }
 
 void loop()
 {
-  Serial.println(mapPot(normalizePot(POTPIN1, 80))); //call normalizePot and put the output into mapPot then print it
+  Serial.println(analogReadMultiPlexer(0, 0, 0, 0, A0)); // call normalizePot and put the output into mapPot then print it
 
   // Set values to send
-  myData.PWMCH1 = mapPot(normalizePot(POTPIN1, 80));
+  myData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0));
   myData.PWMCH2 = 1000; // test values
   myData.PWMCH3 = 1000;
   myData.PWMCH4 = 1000;
 
   // Send message via ESP-NOW
   esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&myData, sizeof(myData));
-   
+  if (result == ESP_OK)
-  if (result == ESP_OK) {
+  {
-    Serial.println("Sent with success");
+    // Serial.println("Sent with success");
   }
-  else {
+  else
-    Serial.println("Error sending the data");
+  {
+    // Serial.println("Error sending the data");
   }
+  delay(10);
 }
 
-int mapPot(int normalizedValue){
+int mapPot(int normalizedValue)
-  return map(normalizedValue, 80, 4095, MINPWMVALUE, MAXPWMVALUE); //map the normalized value to the PWM range
+{
+  return map(normalizedValue, 400, 2500, MINPWMVALUE, MAXPWMVALUE); // map the normalized value to the PWM range
 }
 
 int normalizePot(int pin, int minValue) // normalize the pot value to a range of 80 to 4095 instead of 0 to 4095 because the potmeter is at lower values not accurate
@@ -86,3 +99,13 @@ int normalizePot(int pin, int minValue) //normalize the pot value to a range of
     return pot;
   }
 }
+
+int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin)
+{
+  digitalWrite(D3, LOW);
+  digitalWrite(D6, LOW);
+  digitalWrite(D7, LOW);
+  digitalWrite(D9, LOW);
+  digitalWrite(D8, LOW);
+  return analogRead(pin);
+}

src/drone/src/main.cpp

@@ -367,6 +367,7 @@ float invSqrt(float x);
 void controlMixer();
 void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len);
 void ESPNowSetup();
+void printDebugInfo();
 
 //========================================================================================================================//
 //                                                      VOID SETUP                                                        //
@@ -476,14 +477,15 @@ void loop()
   // Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE:
   // printRadioData();     //Prints radio pwm values (expected: 1000 to 2000)
   // printDesiredState();  //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle)
-  // printGyroData();      //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest)
+  printGyroData();      //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest)
-  // printAccelData(); // Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level)
+  printAccelData(); // Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level)
   // printMagData();       //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300)
   printRollPitchYaw();  //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level)
-  // printPIDoutput();     //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1)
+  printPIDoutput();     //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1)
   // printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250)
   // printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180)
   // printLoopRate();      //Prints the time between loops in microseconds (expected: microseconds between loop iterations)
+  // printDebugInfo(); //PWM 1 and 5 + Armed condition
 
   // Get arming status
   armedStatus(); // Check if the throttle cut is off and throttle is low.
@@ -886,16 +888,7 @@ void calculate_IMU_error()
 void calibrateAttitude()
 {
   // DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators
-  // Assuming vehicle is powered up on level surface!
+  // Assuming vehicle is powered up on level surface!throttle
-  /*
-   * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds
-   * to boot.
-   */
-  // Warm up IMU and madgwick filter in simulated main loop
-  for (int i = 0; i <= 10000; i++)
-  {
-    prev_time = current_time;
-    current_time = micros();
     dt = (current_time - prev_time) / 1000000.0;
     getIMUdata();
     Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt);
@@ -1434,7 +1427,7 @@ void getCommands()
   channel_2_pwm = myData.PWMCH2;
   channel_3_pwm = myData.PWMCH3;
   channel_4_pwm = myData.PWMCH4;
-  // channel_5_pwm = getRadioPWM(5); //commented out because ESPnow only sends 4 channels
+  channel_5_pwm = 1000; // Temporary always armed 
   // channel_6_pwm = getRadioPWM(6);
 
 #endif
@@ -2005,6 +1998,24 @@ void printLoopRate()
   }
 }
 
+void printDebugInfo() {
+  if (current_time - print_counter > 10000) {
+    print_counter = micros();
+    Serial.print("CH1(thro):");
+    Serial.print(channel_1_pwm);
+    Serial.print(" CH5:");
+    Serial.print(channel_5_pwm);
+    Serial.print(" Armed:");
+    Serial.print(armedFly);
+    Serial.print(" thro_des:");
+    Serial.print(thro_des);
+    Serial.print(" m1_scaled:");
+    Serial.print(m1_command_scaled);
+    Serial.print(" m1_PWM:");
+    Serial.println(m1_command_PWM);
+  }
+}
+
 //=========================================================================================//
 
 // HELPER FUNCTIONS