// 13/8/2024 // Abdelrazzak Merheb // Using the Red Nano board as an RC airplane receiver // Very similar to the code "KendinYap_ReceiveNRF24Servomotors" but it includes trimming and stabilization capabilities // 6 Channel Receiver | 6 Kanal Alıcı // IMPORTANT NOTE: Works with the two chip NRF (the one with amplifier) // VERY IMPORTANT 13/8/2024: // When I first run the code I had: "FIFO Overflow" problem in the mpu data (Find altitudes) // This is because the mpu6050 is very fast, but the code is slow; the mpu is floading the FIFO register // with data and nobody was reading them! to fix the problem, you need to slow down the mpu data rate // To do that, locate the "MPU6050_6Axis_MotionApps20.h" file in the "MPU6050" library. First, I changed line 261 // From "02, 0x16, 0x02, 0x00, 0x00 // D_0_22 inv_set_fifo_rate" to "02, 0x16, 0x02, 0x00, 0x04 // D_0_22 inv_set_fifo_rate" // The last byte (changed from 0x00 to 0x04) slows down data rate. Second, I added these lines at line 272: /* #ifndef MPU6050_DMP_FIFO_RATE_DIVISOR #define MPU6050_DMP_FIFO_RATE_DIVISOR 0x04 #endif * */ /* Pin configuration for the Red Nano Red Nano mpu connection: MPU GND --- Nano GND MPU Vcc --- Nano 3v3 MPU SDA --- Nano A4 MPU SCL --- Nano A5 ======================================= Red Nano NRFconnections: CE -> D7 SCK -> D13 MISO -> D12 CSN -> D8 MOSI -> D11 IRQ -> NC ========================================== Red Nano Servomotor connections (Connected to the sensors' pins): Throttle -> D3 (S1 Echo) Ailerons -> D5 (S3 Echo) Rudder -> D9 (S4 Trig) Elevator -> D10 (S4 Echo) ============================================= */ #include #include #include #include // For the stabilization///////////////////////////////////////////////////////////////// #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif // Attitude angles (Gyro readings) float mpuPitch = 0; float mpuRoll = 0; float mpuYaw = 0; // Actual attitude angles float PitchG = 0; float RollG = 0; float YawG = 0; // define MPU instance MPU6050 mpu; // class default I2C address is 0x68; specific I2C addresses may be passed as a parameter here // MPU control/status vars uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector // relative ypr[x] usage based on sensor orientation when mounted, e.g. ypr[PITCH] #define PITCH 1 // defines the position within ypr[x] variable for PITCH; may vary due to sensor orientation when mounted #define ROLL 2 // defines the position within ypr[x] variable for ROLL; may vary due to sensor orientation when mounted #define YAW 0 // defines the position within ypr[x] variable for YAW; may vary due to sensor orientation when mounted /////////////////////////////////////////////////////////////////////////////////////////////////// const uint64_t pipeIn = 0xABCDABCD71LL; RF24 radio(7, 8); // CE, CSN // Define servomotor channel values int ch_width_1 = 0; int ch_width_2 = 0; int ch_width_3 = 0; int ch_width_4 = 0; int ch_width_5 = 0; int ch_width_6 = 0; // Trim values (updated inside the trimming function) int RollTrim = 0; int PitchTrim = 0; int YawTrim = 0; // Name the servos Servo ch1; Servo ch2; Servo ch3; Servo ch4; Servo ch5; Servo ch6; // The packet transmitted between the transmitter and receiver struct Signal { byte throttle; byte pitch; byte roll; byte yaw; byte aux1; // Trimming: If true, the joystick readings are used to make small trimms for the moving parts byte aux2; // Stabilize yes or no }; Signal data; // Define the structure called "data" // Reset all the packet void ResetData() { data.throttle = 0; // Define the inicial value of each data input. | Veri girişlerinin başlangıç değerleri data.roll = 127 + RollTrim;; data.pitch = 127 + PitchTrim;; data.yaw = 127 + YawTrim;; data.aux1 = 0; data.aux2 = 0; } void setup() { Serial.begin(115200); // Serial communication speed // Set the pins for each PWM signal | Her bir PWM sinyal için pinler belirleniyor. ch1.attach(3); // Throttle ch2.attach(5); // Aileron: roll ch3.attach(10); // elevator: pitch ch4.attach(9); // Rudder: Yaw ch5.attach(6); // Not used ch6.attach(2); // Not used // Configure the NRF24 module | NRF24 Modül konfigürasyonu ResetData(); radio.begin(); radio.openReadingPipe(1,pipeIn); // radio.setChannel(100); radio.setAutoAck(false); radio.setDataRate(RF24_250KBPS); // The lowest data rate value for more stable communication | Daha kararlı iletişim için en düşük veri hızı. radio.setPALevel(RF24_PA_MAX); // Output power is set for maximum | Çıkış gücü maksimum için ayarlanıyor. radio.startListening(); // Start the radio comunication for receiver | Alıcı için sinyal iletişimini başlatır. // For the stabilization/////////////////////////////////////////////////////////////////////////////// // Initialize the I2C connection for the mpu6050 // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // load and configure the DMP Serial.println(F("Initializing DMP")); devStatus = mpu.dmpInitialize(); // INPUT CALIBRATED OFFSETS HERE; SPECIFIC FOR EACH UNIT AND EACH MOUNTING CONFIGURATION!!!! /* // Added by ABD // Vertical Nano board mpu.setXGyroOffset(40); mpu.setYGyroOffset(58); mpu.setZGyroOffset(-47); mpu.setXAccelOffset(-5403); mpu.setYAccelOffset(-4685); mpu.setZAccelOffset(1173); */ // Added by ABD // Horizontal Nano board mpu.setXGyroOffset(40); mpu.setYGyroOffset(58); mpu.setZGyroOffset(-47); mpu.setXAccelOffset(-7400); mpu.setYAccelOffset(-4704); mpu.setZAccelOffset(2740); // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP")); mpu.setDMPEnabled(true); // enable Arduino interrupt detection Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)")); mpuIntStatus = mpu.getIntStatus(); // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed, 2 = DMP configuration updates failed (if it's going to break, usually the code will be 1) Serial.print(F("DMP Initialization failed code = ")); Serial.println(devStatus); } //////////////////////////////////////////////////////////////// } unsigned long lastRecvTime = 0; // NRF24L receive function void recvData() { // Data is received while ( radio.available() ) { radio.read(&data, sizeof(Signal)); lastRecvTime = millis(); // Receive the data | Data alınıyor Serial.println("Packet Received successfully!"); // Choose between trimming or controlling the airplane // If the trimming switch is ON if(data.aux1 == true){ trimm(); // Call the function to trim the airplane } else { // If the trimming switch is OFF control(); // Control the airplane } // Map received data ch_width_1 = map(data.roll, 0, 255, 1000, 2000); ch_width_2 = map(data.pitch, 0, 255, 1000, 2000); ch_width_3 = map(data.throttle, 0, 255, 1000, 2000); ch_width_4 = map(data.yaw, 0, 255, 1000, 2000); ch_width_5 = map(data.aux1, 0, 1, 1000, 2000); ch_width_6 = map(data.aux2, 0, 1, 1000, 2000); // Apply the control ch1.writeMicroseconds(ch_width_1); // Write the PWM signal | PWM sinyaller çıkışlara gönderiliyor ch2.writeMicroseconds(ch_width_2); ch3.writeMicroseconds(ch_width_3); ch4.writeMicroseconds(ch_width_4); ch5.writeMicroseconds(ch_width_5); ch6.writeMicroseconds(ch_width_6); // Show the values on the serial port Serial.print("Throttle Data is: "); Serial.println(data.throttle); Serial.print("Roll Data is: "); Serial.println(data.roll); Serial.print("Pitch Data is: "); Serial.println(data.pitch); Serial.print("Yaw Data is: "); Serial.println(data.yaw); Serial.print("Button 1 is: "); Serial.println(data.aux1); Serial.print("Button 2 is: "); Serial.println(data.aux2); //////////////////////////////////////////////////////////////////// // delay(200); } } ////////////////////////////////////////// // The function to stabilize the airplane void stabilize() // stabilize the airplane by controlling the elevator and the ailerons { Serial.println("Airplane stabilization is Active"); ////////////////////////////////////////////////////////////////// // Read the actual angles from the gyroscope FindAttitude(); // Show the values // display tab-separated accel/gyro x/y/z values Serial.print("Attitude angles are:"); Serial.print("\n"); Serial.print("Roll angle is"); Serial.print(RollG); Serial.print("\n"); Serial.print("Pitch angle is"); Serial.print(PitchG); Serial.print("\n"); Serial.print("Yaw angle is"); Serial.print(YawG); Serial.print("\n"); ////////////////////////////////////////////////////////////////// // Apply motion according to the error values // Should be changed according to the set up // Roll compensation if(RollG > 0){ ch_width_1 = 1200; // Ailerons left Serial.println("Stabilize Roll left"); } else if(RollG < 0){ ch_width_1 = 1700; // Ailerons right Serial.println("Stabilize Roll right"); } else{ ch_width_1 = 1500; // Center the ailerons } ch1.writeMicroseconds(ch_width_1); // Write the PWM signal | PWM sinyaller çıkışlara gönderiliyor //////////////////////////////////////////////////////////////////////////// // Pitch compensation if(PitchG > 0){ ch_width_2 = 1200; // Elevator down Serial.println("Stabilize Pitch down"); } else if(PitchG < 0){ ch_width_2 = 1700; // Elevator up Serial.println("Stabilize Pitch up"); } else{ ch_width_2 = 1500; // Center the elevator } ch2.writeMicroseconds(ch_width_2); //////////////////////////////////////////////////////////////////////////// delay(50); // Just to be able to see the data } ////////////////////////////////////////// ////////////////////////////////////////// // The function to trim the airplane void trimm() // trim the airplane { Serial.println("Airplane Trimming is Active"); // Trimming the aileron servos if(data.roll > 150){ // If the roll joystick is to the far left RollTrim = RollTrim + 10; } if(data.roll < 100){ // If the roll joystick is to the far right RollTrim = RollTrim - 10; } // Trimming the elevator servos if(data.pitch > 150){ // If the elevator joystick is to the far up PitchTrim = PitchTrim + 10; } if(data.pitch < 100){ // If the elevator joystick is to the far down PitchTrim = PitchTrim - 10; } // Trimming the rudder servos if(data.yaw > 150){ // If the rudder joystick is to the far right YawTrim = YawTrim + 10; } if(data.yaw < 100){ // If the rudder joystick is to the far left YawTrim = YawTrim - 10; } // Apply the trimms ResetData(); // Now return and trim the servomotors } ////////////////////////////////////////// ////////////////////////////////////////// // The function to control the airplane void control() // control the airplane { // If the stabilization switch is ON, AND the aileron/elevator joystick is centered --> Stabilize the airplane if((115 Control the airplane regularly Serial.println("Airplane Regular Control is Active"); // Trimming the servomotors data.roll = data.roll + RollTrim; // Trim ailerons data.pitch = data.pitch + PitchTrim; // Trim elevator data.yaw = data.yaw + YawTrim; // Trim rudder // Now return and control the servomotors } } ////////////////////////////////////////// void loop() { // Receive data from the NRF24L recvData(); unsigned long now = millis(); if ( now - lastRecvTime > 1000 ) { ResetData(); // Signal lost.. Reset data | Sinyal kayıpsa data resetleniyor } } // ================================================================ // === Find the attitude angles IF AVAILABLE === // ================================================================ void FindAttitude() { // Get INT_STATUS byte mpuIntStatus = mpu.getIntStatus(); // get current FIFO count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); Serial.println(F("FIFO overflow!")); return; } if (mpuIntStatus & 0x02) // otherwise continue processing { // check for correct available data length if (fifoCount < packetSize) return; // fifoCount = mpu.getFIFOCount(); // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available fifoCount -= packetSize; // flush buffer to prevent overflow mpu.resetFIFO(); // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); mpuPitch = ypr[PITCH] * 180 / M_PI; mpuRoll = ypr[ROLL] * 180 / M_PI; mpuYaw = ypr[YAW] * 180 / M_PI; // flush buffer to prevent overflow mpu.resetFIFO(); // The actual attitude angles are PitchG = mpuPitch; RollG = mpuRoll; YawG = mpuYaw; /* // Show the values // display tab-separated accel/gyro x/y/z values Serial.print("Attitude angles are:"); Serial.print("\n"); Serial.print("Roll angle is"); Serial.print(mpuRoll); Serial.print("\n"); Serial.print("Pitch angle is"); Serial.print(mpuPitch); Serial.print("\n"); Serial.print("Yaw angle is"); Serial.print(mpuYaw); Serial.print("\n"); // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); // flush buffer to prevent overflow mpu.resetFIFO(); // Should be changed according to the set up // Note that because we will have four servos, the control will be different Servo1.write(-mpuPitch + 90); Servo2.write(mpuRoll + 90); Servo1.write(-mpuPitch + 90); Servo2.write(mpuRoll + 90); //delay(10); */ // flush buffer to prevent overflow mpu.resetFIFO(); } // if (mpuIntStatus & 0x02) } // processAccelGyro()