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 
#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();
}