Integrating gyro stabilization into arduino RC- receiver

Hello,

About my project
I am trying to build an arduino+nrf24l01 based rc fixed wing UAV. With transmitter and receiver made from arduino nano and nrf24l01+ module and have so far been successful in making them work.

What i am trying to achieve
I need to integrate a MPU6050 module into my receiver so that the aircraft is gyro stablilized (like using a flight controller) for smooth flying experience.

Issues
After testing and confiriming that both transmitter and receiver work, i soldered the mpu6050 unit into the receiver. But i cannot figure out how to code it to do the desired behaviour.

receiver code

//  6 Channel Receiver | 6 Kanal Al─▒c─▒
//  PWM output on pins D2, D3, D4, D5, D6, D7 (├ç─▒k─▒┼č pinleri)

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>

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;

Servo ch1;
Servo ch2;
Servo ch3;
Servo ch4;
Servo ch5;
Servo ch6;

struct Signal {
byte throttle;      
byte pitch;
byte roll;
byte yaw;
byte aux1;
byte aux2;
};

Signal data;

const uint64_t pipeIn = 0xE9E8F0F0E1LL;
RF24 radio(9, 10); 

void ResetData()
{
// Define the inicial value of each data input. | Veri giri┼člerinin ba┼člang─▒├ž de─čerleri
// The middle position for Potenciometers. (254/2=127) | Potansiyometreler i├žin orta konum
data.roll = 127;   // Center | Merkez
data.pitch = 127;  // Center | Merkez
data.throttle = 127; // Motor Stop | Motor Kapal─▒
data.yaw = 127;   // Center | Merkez
data.aux1 = 127;   // Center | Merkez
data.aux2 = 12;   // Center | Merkez
}

void setup()
{
  //Set the pins for each PWM signal | Her bir PWM sinyal i├žin pinler belirleniyor.
  ch1.attach(2);
  ch2.attach(3);
  ch3.attach(4);
  ch4.attach(5);
  ch5.attach(6);
  ch6.attach(7);

  //Configure the NRF24 module
  ResetData();
  radio.begin();
  radio.openReadingPipe(1,pipeIn);
  radio.setAutoAck(false);
  radio.setDataRate(RF24_250KBPS);
  radio.setPALevel(RF24_PA_HIGH);
  radio.startListening(); //start the radio comunication for receiver | Al─▒c─▒ olarak sinyal ileti┼čimi ba┼člat─▒l─▒yor

  pinMode(6,OUTPUT);

}

unsigned long lastRecvTime = 0;

void recvData()
{
while ( radio.available() ) {
radio.read(&data, sizeof(Signal));
lastRecvTime = millis();   // receive the data | data al─▒n─▒yor
}
}

void loop()
{
recvData();
unsigned long now = millis();
if ( now - lastRecvTime > 1000 ) {
ResetData(); // Signal lost.. Reset data | Sinyal kay─▒psa data resetleniyor
}

ch_width_4 = map(data.yaw,      0, 255, 1000, 2000);     // pin D5 (PWM signal)
ch_width_2 = map(data.pitch,    0, 255, 1000, 2000);     // pin D3 (PWM signal)
ch_width_3 = map(data.throttle, 0, 255, 1000, 2000);     // pin D4 (PWM signal)
ch_width_1 = map(data.roll,     0, 255, 1000, 2000);     // pin D2 (PWM signal)

ch_width_5 = map(data.aux1,     0, 255, 1000, 2000);     // pin D6 (PWM signal)
ch_width_6 = map(data.aux2,     0, 255, 1000, 2000);     // pin D7 (PWM signal)

// Write the PWM signal | PWM sinyaller ├ž─▒k─▒┼člara g├Ânderiliyor
ch1.writeMicroseconds(ch_width_1);
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);
}

receiver diagram and mpu6050 connection side by side

Credits:
The receiver code is taken from "KendinYap" on youtube.
His video : video

i hope i have provided enough info and broken any forum rules. If i did i am sorry this is my first time posting here. Please help me with this project. Thank you.

No, your first post is 10 times better than most. Thanks for putting the time and consideration into it.

1 Like

Hi,
A while ago I cobbled this sketch together to take the input from a CPPM receiver (so that only one wire between RC receiver and Arduino was required) and then added MPU6050 based stabilization.

Some explanation:
The sketch has now 2 channel stabilization and has elevon mixing. You can comment that out at the top of the sketch, when you want to use a conventional plane with a rudder and 3-channel stabilization.

Keep the defined AUX_GAIN if you want to be able to set the gyro gain setting via channels 5 and 6. comment out if you need these channels for something else

DYNAMIC_GAIN means that the stabilization is max at center stick position and zero when sticks at the end points. I am using this on a combat wing, and when I move the stick to full up or down, I want no stabilization, but maximum maneuvering capability.

I am not using Servo library, but a few libraries from the DigisparkArduino. suite. I seem to get less servo jitter from them, compared to the servo library.
Maybe you can re-use some of this code. Good Luck.

Be sure to add an extra capacitor to your Arduino. I had some mid-air freezes, before I added one, resulting in the application of more hotmelt glue to my wing :slight_smile:


#define ELEVON_MIXING        // comment out if elevon mixing routine is NOT required.
#define AUX_GAIN             // comment out if no gain setting required via sliders
#define DYNAMIC_GAIN         // comment out for constant gyro gain

#include <Wire.h>

#define CALIBRATION_COUNT      200
#define MPU6050_I2C_ADDRESS    0x68
#define MPU6050_PWR_MGMT_1     0x6B   // R/W
#define GYRO_ADDRESS           0x68
#define GYRO_VALUES_REGISTER   0x43
#define GYRO_HIGH_BYTE_FIRST

// Set the order in which the gyro values are read
#define GYRO_AXIS_1    gyro_pitch
#define GYRO_AXIS_2    gyro_roll
#define GYRO_AXIS_3    gyro_yaw

// Set a value of -1 for gyro axes that should be reversed
#define GYRO_INVERT_PITCH   1
#define GYRO_INVERT_ROLL    1
#define GYRO_INVERT_YAW     1

// Set a value of -1 for RC input axes that should be reversed
#define RC_INVERT_PITCH      1
#define RC_INVERT_ROLL       1
#define RC_INVERT_YAW        1

#define PITCH_RATE           100   // adjust to make servos move max range with tx control
#define ROLL_RATE            100   // adjust to make servos move max range with tx control
#define YAW_RATE             100   // adjust to make servos move max range with tx control

#define MIN_SERVO_PULSE      800
#define MAX_SERVO_PULSE      2200

#define RC_CHANS            6

#define ROLL_CHANNEL        1
#define PITCH_CHANNEL       2
#define THROTTLE_CHANNEL    3
#define YAW_CHANNEL         4
#define AUX1_CHANNEL        5
#define AUX2_CHANNEL        6

#include <SoftRcPulseOut.h>
#define SERVO1_PIN              11
#define SERVO2_PIN              12
#define SERVO3_PIN              13
#define SERVO4_PIN              14
#define NEUTRAL_US              1500 /* Default position in case of no pulse at startup */
#define NOW                     1
enum {AILERON = 0, ELEVATOR, THROTTLE, RUDDER, SERVO_NB}; /* Trick: use an enumeration to declare the index of the servos AND the amount of servos */
SoftRcPulseOut ServoMotor[SERVO_NB]; /* Table Creation for SERVO_NB objects of SoftRcPulseOut type */

int servoVal3; // only throttle needs defined here (compilation error if defined in loop)

#include <TinyCppmReader.h>
#define PPM_INPUT_PIN  10
#define CHANNEL_NB     6 // does not work with 8 on stormRC receiver

float pid_p_gain_roll = 1.0;               //Gain setting for the roll P-controller (1.3)
float pid_i_gain_roll = 0.0;               //Gain setting for the roll I-controller (0.05)
float pid_d_gain_roll = 1.0;               //Gain setting for the roll D-controller (15)
int   pid_max_roll = 400;                  //Maximum output of the PID-controller (+/-)

float pid_p_gain_pitch = 1.0;              //Gain setting for the pitch P-controller.
float pid_i_gain_pitch = 0.0;              //Gain setting for the pitch I-controller.
float pid_d_gain_pitch = 1.0;              //Gain setting for the pitch D-controller.
int pid_max_pitch = 400;                   //Maximum output of the PID-controller (+/-)

float pid_p_gain_yaw = 0.0;                //Gain setting for the pitch P-controller. //4.0
float pid_i_gain_yaw = 0.0;                //Gain setting for the pitch I-controller. //0.02
float pid_d_gain_yaw = 0.0;                //Gain setting for the pitch D-controller.
int pid_max_yaw = 400;                     //Maximum output of the PID-controller (+/-)
int pid_max_yaw_i = 100;                   //Maximum value of I term of the PID-controller (+/-)

float gyro_roll_cal, gyro_pitch_cal, gyro_yaw_cal;
float gyro_pitch, gyro_roll, gyro_yaw;
float gyro_roll_input, gyro_pitch_input, gyro_yaw_input;

int receiver_input_roll, receiver_input_pitch, receiver_input_throttle, receiver_input_yaw;
float pid_i_mem_roll, pid_roll_setpoint, pid_output_roll, pid_last_roll_d_error;
float pid_i_mem_pitch, pid_pitch_setpoint, pid_output_pitch, pid_last_pitch_d_error;
float pid_i_mem_yaw, pid_yaw_setpoint, pid_output_yaw, pid_last_yaw_d_error;

void setup() {
  delay(5); //wait for the capacitor to charge and vcc stabilization
  i2c_write_reg(GYRO_ADDRESS, 0x6B, 0x80);  //PWR_MGMT_1    -- DEVICE_RESET 1
  delay(5); // wait for the sensor to reset
  i2c_write_reg(GYRO_ADDRESS, 0x6B, 0x03);  //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
  i2c_write_reg(GYRO_ADDRESS, 0x1A, 0);     //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
  i2c_write_reg(GYRO_ADDRESS, 0x1B, 0x08);  //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 500 deg/sec

  // Clear the 'sleep' bit to start the sensor.
  MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
  delay(1); // wait for the sensor to startup

  gyro_pitch_cal = 0;
  gyro_roll_cal = 0;
  gyro_yaw_cal = 0;

  for (int i = 0; i < CALIBRATION_COUNT ; i++) {
    gyro_read_raw();                                           //Read the gyro output.
    gyro_pitch_cal += gyro_pitch;                              //Add pitch value to gyro_pitch_cal.
    gyro_roll_cal  += gyro_roll;                               //Add roll value to gyro_roll_cal.
    gyro_yaw_cal   += gyro_yaw;                                //Add yaw value to gyro_yaw_cal.
    delay(1);                                   //Wait one milliseconds before the next loop.
  }
  //Now that we have samples, we need to divide by the sample count to get the average gyro offset.
  gyro_pitch_cal /= CALIBRATION_COUNT;
  gyro_roll_cal  /= CALIBRATION_COUNT;
  gyro_yaw_cal   /= CALIBRATION_COUNT;
  //Configure servo pins as output.

  ServoMotor[AILERON].attach(SERVO1_PIN);   /* enumeration is used a index for the ServoMotor[] table */
  ServoMotor[ELEVATOR].attach(SERVO2_PIN);   /* enumeration is used a index for the ServoMotor[] table */
  ServoMotor[THROTTLE].attach(SERVO3_PIN);   /* enumeration is used a index for the ServoMotor[] table */
  ServoMotor[RUDDER].attach(SERVO4_PIN);   /* enumeration is used a index for the ServoMotor[] table */
  TinyCppmReader::attach(PPM_INPUT_PIN); /* Attach TinyPpmReader to PPM_INPUT_PIN pin */
}

void loop() {
 float rollStabMagnitude;
 float pitchStabMagnitude;
  static uint16_t Width_us = NEUTRAL_US; /* Static to keep the value at the next loop */
  //Let's get the current gyro data and scale it to degrees per second for the pid calculations.
  gyro_read_raw();
  gyro_pitch -= gyro_pitch_cal; // apply calibration
  gyro_roll  -= gyro_roll_cal;
  gyro_yaw   -= gyro_yaw_cal;
  gyro_pitch *= GYRO_INVERT_PITCH / 57.14286;   //Gyro pid input is deg/sec. gyro_apply_inversion_and_scale
  gyro_roll  *= GYRO_INVERT_ROLL / 57.14286;    //Gyro pid input is deg/sec.
  gyro_yaw   *= GYRO_INVERT_YAW / 57.14286;     //Gyro pid input is deg/sec.

  float uptake = 0.2;  //0.2
  float oneMinusUptake = 1 - uptake;

  gyro_pitch_input = (gyro_pitch_input * oneMinusUptake) + (gyro_pitch * uptake);
  gyro_roll_input  = (gyro_roll_input * oneMinusUptake)  + (gyro_roll * uptake);
  gyro_yaw_input   = (gyro_yaw_input * oneMinusUptake)   + (gyro_yaw * uptake);

#ifdef AUX_GAIN
  // gain setting via AUX channels
  rollStabMagnitude  = (constrain((float)TinyCppmReader::width_us(AUX1_CHANNEL) - 1000, 0, 1000)) / 1000.0f;
  pitchStabMagnitude = (constrain((float)TinyCppmReader::width_us(AUX2_CHANNEL) - 1000, 0, 1000)) / 1000.0f;
  gyro_roll_input *= rollStabMagnitude;
  gyro_pitch_input *= pitchStabMagnitude;
#endif

#ifdef DYNAMIC_GAIN
  // Dynamic gain setting
  // stick center is 1 and at both end-points = 0 , like this curve /\
  // so stabilization magnitude is max at centerstick and zero at full stick
  rollStabMagnitude  = (500 - fabs(constrain((float)TinyCppmReader::width_us(ROLL_CHANNEL) - 1500, -500, 500))) / 500.0f  ;
  gyro_roll_input *= rollStabMagnitude;
  pitchStabMagnitude  = (500 - fabs(constrain((float)TinyCppmReader::width_us(PITCH_CHANNEL) - 1500, -500, 500))) / 500.0f  ;
  gyro_pitch_input *= pitchStabMagnitude;
#endif // end dynamic gain

  if ((TinyCppmReader::detectedChannelNb() >= CHANNEL_NB) && TinyCppmReader::isSynchro()) {
    pid_roll_setpoint  = RC_INVERT_ROLL  * ((float)TinyCppmReader::width_us(ROLL_CHANNEL) - 1500.0f);
    pid_pitch_setpoint = RC_INVERT_PITCH * ((float)TinyCppmReader::width_us(PITCH_CHANNEL) - 1500.0f);
    servoVal3 = TinyCppmReader::width_us(THROTTLE_CHANNEL);
    pid_yaw_setpoint   = RC_INVERT_YAW   * ((float)TinyCppmReader::width_us(YAW_CHANNEL) - 1500.0f);
  }
  //The PID set point in degrees per second is determined by the receiver input.
  pid_pitch_setpoint *= PITCH_RATE / 100;
  pid_roll_setpoint  *= ROLL_RATE / 100;
  pid_yaw_setpoint   *= YAW_RATE / 100;

  //PID inputs are known. So we can calculate the pid output.
  float pid_error_temp;

  
  //Pitch calculations
  pid_error_temp = gyro_pitch_input - pid_pitch_setpoint;
  pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
  pid_i_mem_pitch = constrain(pid_i_mem_pitch, -pid_max_pitch, pid_max_pitch);
  
  pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error);
  pid_output_pitch = constrain(pid_output_pitch, -pid_max_pitch, pid_max_pitch);
    
  pid_last_pitch_d_error = pid_error_temp;

  //Roll calculations
  pid_error_temp = gyro_roll_input - pid_roll_setpoint;
  pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
  pid_i_mem_roll = constrain(pid_i_mem_roll, -pid_max_roll, pid_max_roll);
  
  pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
  pid_output_roll = constrain(pid_output_roll, -pid_max_roll, pid_max_roll);
  
  pid_last_roll_d_error = pid_error_temp;
 
  //Yaw calculations
  pid_error_temp = gyro_yaw_input - pid_yaw_setpoint;
  pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp;
  pid_i_mem_yaw = constrain(pid_i_mem_yaw, -pid_max_yaw_i, pid_max_yaw_i);
  
  pid_output_yaw = pid_p_gain_yaw * pid_error_temp + pid_i_mem_yaw + pid_d_gain_yaw * (pid_error_temp - pid_last_yaw_d_error);
  pid_output_yaw = constrain(pid_output_yaw, -pid_max_yaw, pid_max_yaw);
    
  pid_last_yaw_d_error = pid_error_temp;

  int servoVal1 = pid_output_roll;
  int servoVal2 = pid_output_pitch;
  int servoVal4 = pid_output_yaw;

#ifdef ELEVON_MIXING // elevon mixing required 
  int leftServo =   servoVal1 - servoVal2;
  int rightServo =  servoVal1 + servoVal2;
  servoVal1 = leftServo;
  servoVal2 = rightServo;
#endif // end elevon mixing

  servoVal1 += 1500;
  servoVal2 += 1500;
  servoVal4 += 1500;
  servoVal1 = constrain(servoVal1, MIN_SERVO_PULSE, MAX_SERVO_PULSE);
  servoVal2 = constrain(servoVal2, MIN_SERVO_PULSE, MAX_SERVO_PULSE);
  servoVal4 = constrain(servoVal4, MIN_SERVO_PULSE, MAX_SERVO_PULSE);

  ServoMotor[AILERON].write_us(servoVal1);
  ServoMotor[ELEVATOR].write_us(servoVal2);
  ServoMotor[THROTTLE].write_us(servoVal3);
  ServoMotor[RUDDER].write_us(servoVal4);
  SoftRcPulseOut::refresh();
} //end loop


// --------------------------------------------------------
// MPU6050_write
//
// This is a common function to write multiple bytes to an I2C device.
//
// If only a single register is written,
// use the function MPU_6050_write_reg().
//
// Parameters:
//   start : Start address, use a define for the register
//   pData : A pointer to the data to write.
//   size  : The number of bytes to write.
//
// If only a single register is written, a pointer
// to the data has to be used, and the size is
// a single byte:
//   int data = 0;        // the data to write
//   MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1);
//
int MPU6050_write(int start, const uint8_t *pData, int size)
{
  int n, error;

  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);        // write the start address
  if (n != 1)
    return (-20);

  n = Wire.write(pData, size);  // write data bytes
  if (n != size)
    return (-21);

  error = Wire.endTransmission(true); // release the I2C-bus
  if (error != 0)
    return (error);

  return (0);         // return : no error
}

// --------------------------------------------------------
// MPU6050_write_reg
//
// An extra function to write a single register.
// It is just a wrapper around the MPU_6050_write()
// function, and it is only a convenient function
// to make it easier to write a single register.
//
int MPU6050_write_reg(int reg, uint8_t data)
{
  int error;
  error = MPU6050_write(reg, &data, 1);
  return (error);
}

void i2c_write_reg(int address, byte reg, byte val) {
  Wire.beginTransmission(address);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission(true);
}

// Requires open I2C transmission with two bytes available
int gyro_read_int() {
  byte highByte = Wire.read();
  byte lowByte = Wire.read();
  return (highByte << 8) | lowByte;
}

void gyro_read_raw() {
  Wire.beginTransmission(GYRO_ADDRESS);                        //Start communication with the gyro
  Wire.write(GYRO_VALUES_REGISTER);                            //Start reading and auto increment with every read
  Wire.endTransmission();                                      //End the transmission
  Wire.requestFrom(GYRO_ADDRESS, 6);                           //Request 6 bytes from the gyro
  while (Wire.available() < 6);                                //Wait until the 6 bytes are received
  GYRO_AXIS_1 = gyro_read_int();
  GYRO_AXIS_2 = gyro_read_int();
  GYRO_AXIS_3 = gyro_read_int();
}
1 Like

Hello, Thank you for the code. It seems comlpex at first glance,but i shall use it and see how it works out. I will keep you updated here on how it turns out. Thank you so much again.

Hey, So i couldnt really do it. i have never coded a PID loop before so i cannot understand how to integrate your code into mine. Is there any way you could make it simpler like without a PID loop maybe?. Thanks again.

I would not know how to make a stabilization without a PID. Without one it would keep oscillating around the neutral position.

Start sensibly by reading about PID, and finding some elementary PID tutorials. Work through a couple of examples.

Once you understand how it works, you can start thinking about incorporating it into your existing code.