Thorttle, Pitch and Roll

Projects Programming
1

Hi all :slight_smile:
I am doing my Final year Project in Quadcopter. the problem I am facing is understanding the piece of code I got from somewhere.

I have tried to explain some variables used in my code:

-> CH1_delta_th = throttle signal from RC receiver.
-> CH3_delta_rl = Roll signal from RC receiver.
-> CH2_delta_pt = Pitch signal from RC receiver.
-> dpitch/droll = desired pitch/roll position I want to set. it is input of PID function also, which give me error free output in "roll" or "pitch"
-> pitch/roll = as explained above, output of PID function and actual pitch / roll value to be sent to quadcopter.

throttle=map(CH1_delta_th,1004,1968,0,200);// here signal is mapped between 0-200
throttle=constrain(throttle,0,200);
      if(1480<CH3_delta_rl<1492)
 { droll=0;
}
    droll=map(CH3_delta_rl,1008,1968,-12,10); // why roll signal from Rx is mapped to range -12 to 10 ?
 
droll=constrain(droll,-16,7);
droll=droll;
          if(1470<CH2_delta_pt<1480)
 { dpitch=0;
}

 dpitch=map(CH2_delta_pt,996,1976,-10,10); // why pitch signal from Rx is mapped to range -10 to 10 ?


 dpitch=constrain(dpitch,-10,4);
dpitch=dpitch+3;


            if(roll<1 && roll>-1) // output of PID function.
 { roll=0;
}     

            if(pitch<1&&pitch>-1)
 { pitch=0;
}
//moreover i just could not figure out what was the purpose behind these addition and subtraction :-(
b=map(throttle+roll+pitch,0,170,0,200); // front left motor
a=map(throttle-roll+pitch,0,170,0,200);//front right motor
c=map(throttle-roll-pitch,0,170,0,200);//back right motor
d=map(throttle+roll-pitch,0,170,0,200);//back left motor

any kind of help is appreciated. please respond.

2

... the piece of code I got from somewhere.

... used in my code:

Well, is it your code or not? If not, do you intend to credit the author?

3 
 if(1480<CH3_delta_rl<1492)

Unless you really know your programming, you may want to rewrite this.

4

It would be easier for you to write your own code. Then you would better understand what's going on. Because at least then, you would know what it is supposed to do.

5 
// This maps the throttle control range to a smaller range.  In a transmitter this is called Dual Rate.  
// It helps keep a beginner from over-controlling the model.
throttle = map(CH1_delta_th, 1004, 1968, 0, 200);
throttle = constrain(throttle, 0, 200);

// This was intended to add a dead band in the middle of the control range but since droll is immediately
// recalculated this has NO effect and can be removed. Good thing because it is written wrong
if (1480 < CH3_delta_rl < 1492) {
  droll = 0;
}
// This maps the roll control range to a smaller range.  In a transmitter this is called Dual Rate.  
// It helps keep a beginner from over-controlling the model.
droll = map(CH3_delta_rl, 1008, 1968, -12, 10);
droll = constrain(droll, -16, 7);
droll = droll;  //  This statement does nothing

// This was intended to add a dead band in the middle of the control range but since dpitch is immediately
// recalculated this has NO effect and can be removed.  Good thing because it is written wrong
if (1470 < CH2_delta_pt < 1480) {
  dpitch = 0;
}
// This maps the pitch control range to a smaller range.  In a transmitter this is called Dual Rate.  
// It helps keep a beginner from over-controlling the model.

dpitch = map(CH2_delta_pt, 996, 1976, -10, 10); // why pitch signal from Rx is mapped to range -10 to 10 ?
dpitch = constrain(dpitch, -10, 4);
// This adds a pitch bias and is probably use in place of trimming the controls on the transmitter.
dpitch = dpitch + 3;

// Ignore small values of roll.  This will interfere with the PID operation.
if (roll < 1 && roll > -1) { // output of PID function.
  roll = 0;
}

// Ignore small values of pitch.  This will interfere with the PID operation.
if (pitch < 1 && pitch > -1) {
  pitch = 0;
}

// Positive Roll increases power to the left motors and reduces power to the right motors
// Positive Pitch increases power to the front motors and reduces power to the back motors
b = map(throttle + roll + pitch, 0, 170, 0, 200); // front left motor
a = map(throttle - roll + pitch, 0, 170, 0, 200); //front right motor
c = map(throttle - roll - pitch, 0, 170, 0, 200); //back right motor
d = map(throttle + roll - pitch, 0, 170, 0, 200); //back left motor
6

aarg:
Well, is it your code or not? If not, do you intend to credit the author?

this is my code I just took help from the guy of other campus who is working on same project. I just don't get the logic of the code he sent me. I cannot pay him visit otherwise i could understand the code by his help.

7

ahsanraza:
this is my code I just took help from the guy of other campus who is working on same project. I just don't get the logic of the code he sent me. I cannot pay him visit otherwise i could understand the code by his help.

As I see it, that leaves you with two choices:

  1. try to understand it
  2. forget it and write your own
8

johnwasser:

// This maps the throttle control range to a smaller range.  In a transmitter this is called Dual Rate.  

// It helps keep a beginner from over-controlling the model.
throttle = map(CH1_delta_th, 1004, 1968, 0, 200);
throttle = constrain(throttle, 0, 200);

// This was intended to add a dead band in the middle of the control range but since droll is immediately
// recalculated this has NO effect and can be removed. Good thing because it is written wrong
if (1480 < CH3_delta_rl < 1492) {
 droll = 0;
}
// This maps the roll control range to a smaller range.  In a transmitter this is called Dual Rate.  
// It helps keep a beginner from over-controlling the model.
droll = map(CH3_delta_rl, 1008, 1968, -12, 10);
droll = constrain(droll, -16, 7);
droll = droll;  //  This statement does nothing

// This was intended to add a dead band in the middle of the control range but since dpitch is immediately
// recalculated this has NO effect and can be removed.  Good thing because it is written wrong
if (1470 < CH2_delta_pt < 1480) {
 dpitch = 0;
}
// This maps the pitch control range to a smaller range.  In a transmitter this is called Dual Rate.  
// It helps keep a beginner from over-controlling the model.

dpitch = map(CH2_delta_pt, 996, 1976, -10, 10); // why pitch signal from Rx is mapped to range -10 to 10 ?
dpitch = constrain(dpitch, -10, 4);
// This adds a pitch bias and is probably use in place of trimming the controls on the transmitter.
dpitch = dpitch + 3;

// Ignore small values of roll.  This will interfere with the PID operation.
if (roll < 1 && roll > -1) { // output of PID function.
 roll = 0;
}

// Ignore small values of pitch.  This will interfere with the PID operation.
if (pitch < 1 && pitch > -1) {
 pitch = 0;
}

// Positive Roll increases power to the left motors and reduces power to the right motors
// Positive Pitch increases power to the front motors and reduces power to the back motors
b = map(throttle + roll + pitch, 0, 170, 0, 200); // front left motor
a = map(throttle - roll + pitch, 0, 170, 0, 200); //front right motor
c = map(throttle - roll - pitch, 0, 170, 0, 200); //back right motor
d = map(throttle + roll - pitch, 0, 170, 0, 200); //back left motor

Thanks alot jhonwasser ! it was very much helping for me. I am much obliged.
There is something more I forgot to ask, what if we constrain the values of b,a,c,d between 0 - 200? what is it for?

b=constrain(b,0,200); // fl
a=constrain(a,0,200);//fr
c=constrain(c,0,200);//br
d=constrain(d,0,200);
9

if we constrain the values of b,a,c,d between 0 - 200? what is it for?

To constrain the values so they didn't step outside of acceptable ranges.

10

aarg:
As I see it, that leaves you with two choices:

  1. try to understand it
  2. forget it and write your own

I am on a tough deadline. I have understand it more or less. I just need to understand some of the key points. may be after that I could be able to write a new and better code as fellas are saying it has mistake in it. I am very much new to quadrotors. :frowning:

11

Here is a similar block of code that I wrote for my own purposes. Perhaps you will see how this is similar to your friend's code and the comments will help:

#include <PID_v1.h>
#include <TinyGPS.h>

#define USING_PWM

enum MotorIndex {MOTOR_FL_CW, MOTOR_FR_CCW, MOTOR_BL_CCW, MOTOR_BR_CW};
void MotorSpeed(MotorIndex motor, int throttle);

#ifdef USING_PWM
const int THROTTLE_MIN = 0;
const int THROTTLE_MAX = 255;
const int MotorPins[4] = {3, 5, 6, 9};
#else  // USING_ESC
const int THROTTLE_MIN = 1000;
const int THROTTLE_MAX = 2000;
Servo MotorFL, MotorFR, MotorBL, MotorBR;
Servo *ESCs[MotorIndex] = {MotorFL, MotorFR, MotorBL, MotorBR};
const int ESCPins[4] = {4, 5, 6, 7};
#endif

// Set Pitch/Roll/Yaw limits to 10% of throttle range.
const double LIMIT_MIN = -0.1 * (THROTTLE_MAX - THROTTLE_MIN);
const double LIMIT_MAX = 0.1 * (THROTTLE_MAX - THROTTLE_MIN);

TinyGPS GPS;

const double Kp = 1.0, Ki = 0.0, Kd = 0.0;

double AltitudeSetpoint = 0;  // Meters above start point
double AltitudeInput, ThrottleOutput;
PID AltitudePID(&AltitudeSetpoint, &AltitudeInput, &ThrottleOutput, Kp, Ki, Kd, DIRECT);

double PitchSetpoint = 0, PitchInput, PitchOutput;
PID PitchPID(&PitchSetpoint, &PitchInput, &PitchOutput, Kp, Ki, Kd, DIRECT);

double RollSetpoint = 0, RollInput, RollOutput;
PID RollPID(&RollSetpoint, &RollInput, &RollOutput, Kp, Ki, Kd, DIRECT);

double YawSetpoint = 0, YawInput, YawOutput;
PID YawPID(&YawSetpoint, &YawInput, &YawOutput, Kp, Ki, Kd, DIRECT);

struct Waypoint {
  double altitude;
  unsigned long latitude;
  unsigned long longitude;
} StartPoint, Destination, CurrentLocation;

void setup() {
#ifndef USING_PWM
  for (int i = 0; i < 4; i++)
    ESCs[i].attach(ESCPins[i]);
#endif

  AltitudePID.SetOutputLimits(THROTTLE_MIN, THROTTLE_MAX);
  AltitudePID.SetMode(AUTOMATIC);

  PitchPID.SetOutputLimits(LIMIT_MIN, LIMIT_MAX);
  PitchPID.SetMode(AUTOMATIC);

  RollPID.SetOutputLimits(LIMIT_MIN, LIMIT_MAX);
  RollPID.SetMode(AUTOMATIC);

  YawPID.SetOutputLimits(LIMIT_MIN, LIMIT_MAX);
  YawPID.SetMode(AUTOMATIC);


//  StartPoint.altitude = Barometer.altitude();
//  StartPoint.latitude = GPS.GetLatitude();
//  StartPoint.longitude = GPS.GetLongitude();
}

void loop() {
//  AltitudeInput = Barometer.altitude() - StartPoint.altitude;
  AltitudePID.Compute();

//  PitchInput = Accelerometer.pitch();
  PitchPID.Compute();

//  RollInput = Accelerometer.roll();
  RollPID.Compute();

//  YawInput = Magnetometer.heading();
  YawPID.Compute();

  MotorSpeed(MOTOR_FL_CW , ThrottleOutput + PitchOutput - RollOutput + YawOutput);
  MotorSpeed(MOTOR_FR_CCW, ThrottleOutput + PitchOutput + RollOutput - YawOutput);
  MotorSpeed(MOTOR_BL_CCW , ThrottleOutput - PitchOutput - RollOutput - YawOutput);
  MotorSpeed(MOTOR_BR_CW, ThrottleOutput - PitchOutput + RollOutput + YawOutput);
}

void MotorSpeed(MotorIndex motor, int throttle) {
  throttle = constrain(throttle, THROTTLE_MIN, THROTTLE_MAX);

#ifdef USING_PWM
  // Using PWM pins
  analogWrite(MotorPins[motor], throttle);
#else
  // Using ESC control
  ESCs[motor]->writeMicroseconds(throttle);
#endif
}

The USING_PWM option is for brushed DC motors with a simple transistor driver. These are common on very small and inexpensive toy quadcopters. For BRUSHLESS motors you would use and Electronic Speed Control (ESC) for each and those are driven like a hobby servo.

1 Like
12

AWOL:
To constrain the values so they didn't step outside of acceptable ranges.

though the values remain in range of 170 - 200 as they were mapped in a map function?

13

johnwasser:
Here is a similar block of code that I wrote for my own purposes. Perhaps you will see how this is similar to your friend's code and the comments will help:

#include <PID_v1.h>

#include <TinyGPS.h>

#define USING_PWM

enum MotorIndex {MOTOR_FL_CW, MOTOR_FR_CCW, MOTOR_BL_CCW, MOTOR_BR_CW};
void MotorSpeed(MotorIndex motor, int throttle);

#ifdef USING_PWM
const int THROTTLE_MIN = 0;
const int THROTTLE_MAX = 255;
const int MotorPins[4] = {3, 5, 6, 9};
#else  // USING_ESC
const int THROTTLE_MIN = 1000;
const int THROTTLE_MAX = 2000;
Servo MotorFL, MotorFR, MotorBL, MotorBR;
Servo *ESCs[MotorIndex] = {MotorFL, MotorFR, MotorBL, MotorBR};
const int ESCPins[4] = {4, 5, 6, 7};
#endif

// Set Pitch/Roll/Yaw limits to 10% of throttle range.
const double LIMIT_MIN = -0.1 * (THROTTLE_MAX - THROTTLE_MIN);
const double LIMIT_MAX = 0.1 * (THROTTLE_MAX - THROTTLE_MIN);

TinyGPS GPS;

const double Kp = 1.0, Ki = 0.0, Kd = 0.0;

double AltitudeSetpoint = 0;  // Meters above start point
double AltitudeInput, ThrottleOutput;
PID AltitudePID(&AltitudeSetpoint, &AltitudeInput, &ThrottleOutput, Kp, Ki, Kd, DIRECT);

double PitchSetpoint = 0, PitchInput, PitchOutput;
PID PitchPID(&PitchSetpoint, &PitchInput, &PitchOutput, Kp, Ki, Kd, DIRECT);

double RollSetpoint = 0, RollInput, RollOutput;
PID RollPID(&RollSetpoint, &RollInput, &RollOutput, Kp, Ki, Kd, DIRECT);

double YawSetpoint = 0, YawInput, YawOutput;
PID YawPID(&YawSetpoint, &YawInput, &YawOutput, Kp, Ki, Kd, DIRECT);

struct Waypoint {
  double altitude;
  unsigned long latitude;
  unsigned long longitude;
} StartPoint, Destination, CurrentLocation;

void setup() {
#ifndef USING_PWM
  for (int i = 0; i < 4; i++)
    ESCs[i].attach(ESCPins[i]);
#endif

AltitudePID.SetOutputLimits(THROTTLE_MIN, THROTTLE_MAX);
  AltitudePID.SetMode(AUTOMATIC);

PitchPID.SetOutputLimits(LIMIT_MIN, LIMIT_MAX);
  PitchPID.SetMode(AUTOMATIC);

RollPID.SetOutputLimits(LIMIT_MIN, LIMIT_MAX);
  RollPID.SetMode(AUTOMATIC);

YawPID.SetOutputLimits(LIMIT_MIN, LIMIT_MAX);
  YawPID.SetMode(AUTOMATIC);

//  StartPoint.altitude = Barometer.altitude();
//  StartPoint.latitude = GPS.GetLatitude();
//  StartPoint.longitude = GPS.GetLongitude();
}

void loop() {
//  AltitudeInput = Barometer.altitude() - StartPoint.altitude;
  AltitudePID.Compute();

//  PitchInput = Accelerometer.pitch();
  PitchPID.Compute();

//  RollInput = Accelerometer.roll();
  RollPID.Compute();

//  YawInput = Magnetometer.heading();
  YawPID.Compute();

MotorSpeed(MOTOR_FL_CW , ThrottleOutput + PitchOutput - RollOutput + YawOutput);
  MotorSpeed(MOTOR_FR_CCW, ThrottleOutput + PitchOutput + RollOutput - YawOutput);
  MotorSpeed(MOTOR_BL_CCW , ThrottleOutput - PitchOutput - RollOutput - YawOutput);
  MotorSpeed(MOTOR_BR_CW, ThrottleOutput - PitchOutput + RollOutput + YawOutput);
}

void MotorSpeed(MotorIndex motor, int throttle) {
  throttle = constrain(throttle, THROTTLE_MIN, THROTTLE_MAX);

#ifdef USING_PWM
  // Using PWM pins
  analogWrite(MotorPins[motor], throttle);
#else
  // Using ESC control
  ESCs[motor]->writeMicroseconds(throttle);
#endif
}




The USING_PWM option is for brushed DC motors with a simple transistor driver. These are common on very small and inexpensive toy quadcopters. For BRUSHLESS motors you would use and Electronic Speed Control (ESC) for each and those are driven like a hobby servo.

Thanks alot jhonwasser. You are very helping to me :slight_smile: I would give your code a look and try to write something more useful from it. once again a bundle of thanks.

14

AWOL:

 if(1480<CH3_delta_rl<1492)

Unless you really know your programming, you may want to rewrite this.

can you please point out what is the mistake or how it should be? so that i will keep it in mind for programming my own code

15 
if(1480<CH3_delta_rl<1492)

If CH3_delta_rl is greater than 1480 and CH3_delta_rl is less than 1492.

16

ahsanraza:
though the values remain in range of 170 - 200 as they were mapped in a map function?

map() does not constrain the result value. If the input value is outside the input range, the result will be outside the target range.