output finally works

forgot to set standard values for controller when it's not connected

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

@@ -5,22 +5,23 @@
 // 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 {
+// Define the struct that will be sent
+typedef struct struct_message
+{
   int PWMCH1;
   int PWMCH2;
   int PWMCH3;
   int PWMCH4;
 } struct_message;
-struct_message myData; //declare the struct as myData
+struct_message myData; // declare the struct as myData
 
-esp_now_peer_info_t peerInfo; //create a class object of the ESPNow class
+esp_now_peer_info_t peerInfo; // create a class object of the ESPNow class
 
 void setup()
 {
@@ -28,52 +29,64 @@ 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;
   }
 
   // Register peer
   memcpy(peerInfo.peer_addr, broadcastAddress, 6);
-  peerInfo.channel = 0;  
+  peerInfo.channel = 0;
   peerInfo.encrypt = false;
-  
+
-  // Add peer        
+  // 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.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));
+  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
+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
 {
   int pot = analogRead(pin);
 
@@ -85,4 +98,14 @@ 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

@@ -283,10 +283,10 @@ DSM1024 DSM;
 // Must match the sender structure
 typedef struct struct_message
 {
-  int PWMCH1;
+  int PWMCH1 = 1500;
-  int PWMCH2;
+  int PWMCH2 = 1500;
-  int PWMCH3;
+  int PWMCH3 = 1500;
-  int PWMCH4;
+  int PWMCH4 = 1500;
 } struct_message;
 
 // Create a struct_message called myData
@@ -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                                                        //
@@ -479,11 +480,12 @@ void loop()
   // 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)
   // 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)
+  // 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)
-  // printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250)
+  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,22 +888,13 @@ 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);
     loopRate(2000); // do not exceed 2000Hz
   }
-}
+
 
 void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float invSampleFreq)
 {
@@ -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