Arduino Forum

Topics => Robotics => Topic started by: vortix2950 on Oct 13, 2019, 10:22 pm

Title: Rate Controller for a QuadCopter.
Post by: vortix2950 on Oct 13, 2019, 10:22 pm
im designing a flight controller and im still having issue with my quadcopter's Rate controller. the first stage towards a self leveling quad.
after fixing multiple issues here and there, im still left one problem, the PID loop doesn't balance like its suppose to for a rate controller. From my understanding, a Rate controller would accelerate toward's the direction of the stick until the stick is released. That angle is maintained.  What happens in my Rate controller however, is that it follows the stick at all times. so if i release the stick, the quad copter will return back it its initial state.   the quad cant fly yet but thats just from testing it on the floor.

Here is the PID :

Code: [Select]


  PidTime=micros();

   accelgyro.setDLPFMode(MPU6050_DLPF_BW_20);
   accelgyro.getRotation(&gx, &gy, &gz);

   gx=map(gx,-5000,5000,-250,250);
   gz=map(gz,-5000,5000,-250,250);
   gy=map(gy,-5000,5000,-250,250);

   if(gx<minInput){gx=minInput;}
   if(gy<minInput){gy=minInput;}
   if(gz<minInput){gz=minInput;}

   if(gx>maxInput){gx=maxInput;}
   if(gy>maxInput){gy=maxInput;}
   if(gz>maxInput){gz=maxInput;}

    TimeError=(PidTime-LastPidTime)/1000000;
    CurrentReading_Yaw=gz;;
    CurrentReading_Roll=gx;
    CurrentReading_Pitch=gy;

    Desired_Roll=RCx;
    Desired_Pitch=RCy;
    Desired_Yaw=RCz;
   
    Error_Pitch=Desired_Pitch-CurrentReading_Pitch;
    Error_Roll=Desired_Roll-CurrentReading_Roll;
    Error_Yaw=Desired_Yaw-CurrentReading_Yaw;
   
    Input_Pitch=(Porpotional_gain*Error_Pitch);
    Input_Roll=(Porpotional_gain*Error_Roll);
    Input_Yaw=(Porpotional_gain*Error_Yaw);
   
    Motor_1_Speed=(MotorPower_Throttle)+Input_Roll+Input_Pitch-Input_Yaw;
    Motor_3_Speed=(MotorPower_Throttle)-Input_Roll+Input_Pitch+Input_Yaw;
    Motor_2_Speed=(MotorPower_Throttle)-Input_Roll-Input_Pitch-Input_Yaw;
    Motor_4_Speed=(MotorPower_Throttle) +Input_Roll-Input_Pitch+Input_Yaw;

    ESC2.writeMicroseconds(Motor_2_Speed);
    ESC3.writeMicroseconds(Motor_3_Speed);
    ESC4.writeMicroseconds(Motor_4_Speed); 
    ESC.writeMicroseconds(Motor_1_Speed);

    Prev_Error_Pitch=Error_Pitch;
    Prev_Error_Roll=Error_Roll;
    Prev_Error_Yaw=Error_Yaw;
    LastPidTime=PidTime;
 



is something missing here?
Title: Re: Rate Controller for a QuadCopter.
Post by: MartinL on Oct 15, 2019, 09:12 am
Hi vortix2950,

In rate mode the sticks will act in the same way as an aeroplane joystick, in that the further from centre that the stick is moved on a given axis, the faster the aircraft rotates. Centring the stick will cause the aircraft to stop rotating on that axis.

Imagine a jet fighter flying straight and level, to fly inverted the pilot pulls hard right on the joystick to roll the aircraft, then immediately centres the stick once he/she has reached the inverted position. With the stick centred the fighter continues to fly up-side-down in the inverted position. It's the same in rate mode.

In your program you need to first calibrate your gyroscope in the setup() portion of your code. This is achieved successively adding say a hundred or so samples for each of the the gyroscope's axes then dividing by the number of samples, to calculate the mean average:

Code: [Select]
// Acquire the aircraft's current rate over a number of samples
  // This information is used to calibrate the gyro
  for (int i = 0; i < SAMPLE_NO; i++)
  {
    accelgyro.getRotation(&gx, &gy, &gz);
    baseGx += gx;
    baseGy += gy;
    baseGz += gz;
  }

  // Divide by the number of samples to find the starting base point for the gyro
  baseGx /= SAMPLE_NO;
  baseGy /= SAMPLE_NO;
  baseGz /= SAMPLE_NO;

To calculate the gyroscope output in terms of degrees per second, (assuming that's the units you're using?), it's first necessary to determine the full scale rotation (±250°/s, ±500°/s, ±1000°/s, ±°2000/s), again during setup():

Code: [Select]
// #define MPU6050_GYRO_FS_250         0x00
  // #define MPU6050_GYRO_FS_500         0x01
  // #define MPU6050_GYRO_FS_1000        0x02
  // #define MPU6050_GYRO_FS_2000        0x03
  accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_500);

Then use this information to convert the raw gyroscope output to degrees per second in the loop() portion of your sketch:

Code: [Select]
// Convert raw gyro output to degress per second
  //const float FS_SEL = 133.0;  // Gyro +-250 degrees/s
  const float FS_SEL = 66.0;     // Gyro +-500 degrees/s
  //const float FS_SEL = 33.0;   // Gyro +- 1000 degrees/s
  //const float FS_SEL = 16.0;   // Gyro +- 2000 degrees/s
  float gyroRollRate = (float)((gy - baseGy) / FS_SEL);
  float gyroPitchRate = (float)((gx - baseGx) / FS_SEL);
  float gyroYawRate = (float)((gz - baseGz) / FS_SEL);

Your axes assignment may differ depending on the orientation of the gyroscope on your flight controller board.

The gyroscope output in degrees per second can be used as the "input" to your roll, pitch and yaw PID control loops, while the pilot stick inputs, also converted to degrees per second, is the "setpoint".
Title: Re: Rate Controller for a QuadCopter.
Post by: vortix2950 on Oct 20, 2019, 07:10 am
Hi vortix2950,

In rate mode the sticks will act in the same way as an aeroplane joystick, in that the further from centre that the stick is moved on a given axis, the faster the aircraft rotates. Centring the stick will cause the aircraft to stop rotating on that axis.

Imagine a jet fighter flying straight and level, to fly inverted the pilot pulls hard right on the joystick to roll the aircraft, then immediately centres the stick once he/she has reached the inverted position. With the stick centred the fighter continues to fly up-side-down in the inverted position. It's the same in rate mode.

In your program you need to first calibrate your gyroscope in the setup() portion of your code. This is achieved successively adding say a hundred or so samples for each of the the gyroscope's axes then dividing by the number of samples, to calculate the mean average:

Code: [Select]
// Acquire the aircraft's current rate over a number of samples
  // This information is used to calibrate the gyro
  for (int i = 0; i < SAMPLE_NO; i++)
  {
    accelgyro.getRotation(&gx, &gy, &gz);
    baseGx += gx;
    baseGy += gy;
    baseGz += gz;
  }

  // Divide by the number of samples to find the starting base point for the gyro
  baseGx /= SAMPLE_NO;
  baseGy /= SAMPLE_NO;
  baseGz /= SAMPLE_NO;

To calculate the gyroscope output in terms of degrees per second, (assuming that's the units you're using?), it's first necessary to determine the full scale rotation (±250°/s, ±500°/s, ±1000°/s, ±°2000/s), again during setup():

Code: [Select]
// #define MPU6050_GYRO_FS_250         0x00
  // #define MPU6050_GYRO_FS_500         0x01
  // #define MPU6050_GYRO_FS_1000        0x02
  // #define MPU6050_GYRO_FS_2000        0x03
  accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_500);

Then use this information to convert the raw gyroscope output to degrees per second in the loop() portion of your sketch:

Code: [Select]
// Convert raw gyro output to degress per second
  //const float FS_SEL = 133.0;  // Gyro +-250 degrees/s
  const float FS_SEL = 66.0;     // Gyro +-500 degrees/s
  //const float FS_SEL = 33.0;   // Gyro +- 1000 degrees/s
  //const float FS_SEL = 16.0;   // Gyro +- 2000 degrees/s
  float gyroRollRate = (float)((gy - baseGy) / FS_SEL);
  float gyroPitchRate = (float)((gx - baseGx) / FS_SEL);
  float gyroYawRate = (float)((gz - baseGz) / FS_SEL);

Your axes assignment may differ depending on the orientation of the gyroscope on your flight controller board.

The gyroscope output in degrees per second can be used as the "input" to your roll, pitch and yaw PID control loops, while the pilot stick inputs, also converted to degrees per second, is the "setpoint".

I had my Gyro Set to 250 and is divided by 131. switched to the 500/s but no difference. it looks fine as you  increase throttle, but it just topples once it starts taking off.

my Pid does what you described but doesnt seem to work. i pretty much followed along with  this video for Pid


https://www.youtube.com/watch?v=JBvnB0279-Q (https://www.youtube.com/watch?v=JBvnB0279-Q)

moving the quad to any direction gives correct response when it comes to motor mixing. but still, no PID combination seems to give anything resembling flight.

here is the Code again :


Code: [Select]

Servo ESC;
Servo ESC2;
Servo ESC3;
Servo ESC4;


float Porpotional_gain=0.2;
float Integral_gain=0;
float Derivative_gain=0.0;

float Porpotional_gain_y=0.1;
float Integral_gain_y=0.0;
float Derivative_gain_y=0;

MPU6050 mpu;

MPU6050 accelgyro;


int16_t ax, ay, az;
int16_t gx, gy, gz;

setup(){
   
    mpu.setXGyroOffset(81);
    mpu.setYGyroOffset(13);
    mpu.setZGyroOffset(-10);
    mpu.setZAccelOffset(16394);


 ESC.attach(Motor1ESC , 1000 , 2000); //3
 ESC2.attach(Motor2ESC , 1000 , 2000);//
 ESC3.attach(Motor3ESC , 1000 , 2000);
  ESC4.attach(Motor4ESC , 1000 , 2000);




}

Loop(){

    accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_500);
    accelgyro.setDLPFMode(MPU6050_DLPF_BW_20);
    accelgyro.getRotation(&gx, &gy, &gz);

    gx=(gx/66.0);
    gy=(gy/66.0);
    gz=(gz/66.0);

    CurrentReading_Yaw=gz;
    CurrentReading_Roll=gx;
    CurrentReading_Pitch=gy;

   Desired_Roll=RCx;
   Desired_Pitch=RCy;
   Desired_Yaw=RCz;

    Error_Pitch=Desired_Pitch-CurrentReading_Pitch;
    Error_Roll=Desired_Roll-CurrentReading_Roll;
    Error_Yaw=Desired_Yaw-CurrentReading_Yaw;

    Integral_Pitch+=Integral_gain*Error_Pitch;
    Integral_Roll+=Integral_gain*Error_Roll;
    Integral_Yaw+=Integral_gain_y*Error_Yaw;
   
    Derivative_Roll=Derivative_gain* (Error_Roll-Prev_Error_Roll);
    Derivative_Pitch=Derivative_gain* (Error_Pitch-Prev_Error_Pitch);
    Derivative_Yaw=Derivative_gain_y* (Error_Yaw-Prev_Error_Yaw);
   
    Input_Pitch=(Porpotional_gain*Error_Pitch)+Integral_Pitch+Derivative_Pitch;
    Input_Roll=(Porpotional_gain*Error_Roll)+Integral_Roll+Derivative_Roll;
    Input_Yaw=(Porpotional_gain_y*Error_Yaw)+Integral_Yaw+Derivative_Yaw;

if(Input_Pitch>200){Input_Pitch=200;}
if(Input_Roll>200){Input_Roll=200;}
if(Input_Yaw>200){Input_Yaw=200;}

if(Input_Pitch<-200){Input_Pitch=-200;}
if(Input_Roll<-200){Input_Roll=-200;}
if(Input_Yaw<-200){Input_Yaw=-200;}


    Motor_1_Speed=(MotorPower_Throttle)+Input_Roll+Input_Pitch+Input_Yaw;//+
    Motor_3_Speed=(MotorPower_Throttle)-Input_Roll+Input_Pitch-Input_Yaw;//-
    Motor_2_Speed=(MotorPower_Throttle)-Input_Roll-Input_Pitch+Input_Yaw;//+
    Motor_4_Speed=(MotorPower_Throttle) +Input_Roll-Input_Pitch-Input_Yaw;//-


   ESC.writeMicroseconds(Motor_1_Speed);
   ESC2.writeMicroseconds(Motor_2_Speed);
   ESC3.writeMicroseconds(Motor_3_Speed);
   ESC4.writeMicroseconds(Motor_4_Speed); 




}




Loop is limited to 4ms by a timer so no need to use a time error in the Pid.  The "desired" values are the setpoint from the transmitter and they range from -45 to 45. this should work as a rate controller from all the sources i have seen including the video linked but it doesn't.

The quad im using is a small 90mm if that matters.

Title: Re: Rate Controller for a QuadCopter.
Post by: vortix2950 on Oct 20, 2019, 10:04 pm
After a few adjustments, it can go up but its impossible to control because the way its going up is erratic. i can only describe it as a coin that is a second away from stopping its spin on a table. it goes up with alot of yaw and starts moving  to a random direction. it looks more like a spinning ball than a quad copter.
Title: Re: Rate Controller for a QuadCopter.
Post by: MartinL on Oct 21, 2019, 04:00 pm
Hi vortix2950,

Your pilot setpoint for each of the axis should be converted to degrees per second, I suggest initially mapping the signals to a full scale range of: ±180°/s for roll/pitch and around ±270°/s for yaw.

(A full scale range of ±45° is used for auto-level, but its first necessary to get the quadcopter flying in rate mode).

The proportional gain depends on the size of your quadcopter, for a small quadcopter I'd initially use around 0.6 for roll/pitch and 1.6 for yaw.

I'd also recommend using the 490Hz analogWrite() function mapped from 0 to 255 on digital pin's 3, 9, 10 and 11, rather than the 50Hz servo library.

The gyroscope's full scale range and digital filter only needs to be set once and therefore can be placed in the setup() portion of your code.
Title: Re: Rate Controller for a QuadCopter.
Post by: vortix2950 on Oct 22, 2019, 10:38 pm
Hi vortix2950,

Your pilot setpoint for each of the axis should be converted to degrees per second, I suggest initially mapping the signals to a full scale range of: ±180°/s for roll/pitch and around ±270°/s for yaw.

(A full scale range of ±45° is used for auto-level, but its first necessary to get the quadcopter flying in rate mode).

The proportional gain depends on the size of your quadcopter, for a small quadcopter I'd initially use around 0.6 for roll/pitch and 1.6 for yaw.

I'd also recommend using the 490Hz analogWrite() function mapped from 0 to 255 on digital pin's 3, 9, 10 and 11, rather than the 50Hz servo library.

The gyroscope's full scale range and digital filter only needs to be set once and therefore can be placed in the setup() portion of your code.
if  i map the values to 0 255 and use analogwrite, the esc doesnt like that and motors just keeps beeping and never turn on.
Title: Re: Rate Controller for a QuadCopter.
Post by: MartinL on Oct 23, 2019, 08:34 am
Hi vortix2950,

Please could you show how you're mapping the signal for analogWrite().
Title: Re: Rate Controller for a QuadCopter.
Post by: vortix2950 on Oct 23, 2019, 03:44 pm
Hi vortix2950,

Please could you show how you're mapping the signal for analogWrite().
Code: [Select]


Motor_1_Speed=map(Motor_1_Speed,1000,2000,0,255);
Motor_2_Speed=map(Motor_2_Speed,1000,2000,0,255);
Motor_3_Speed=map(Motor_3_Speed,1000,2000,0,255);
Motor_4_Speed=map(Motor_4_Speed,1000,2000,0,255);

analogWrite(Motor1ESC,Motor_1_Speed);
analogWrite(Motor2ESC,Motor_2_Speed);
analogWrite(Motor3ESC,Motor_3_Speed);
analogWrite(Motor4ESC,Motor_4_Speed); 


this is in the main loop and runs as fast as possible. moto1Esc are pin numbers.
Title: Re: Rate Controller for a QuadCopter.
Post by: MartinL on Oct 23, 2019, 06:38 pm
You need to map across the same range, therefore 0 to 2000 maps from 0 to 255:

Code: [Select]
Motor_1_Speed=map(Motor_1_Speed,0,2000,0,255);
Motor_2_Speed=map(Motor_2_Speed,0,2000,0,255);
Motor_3_Speed=map(Motor_3_Speed,0,2000,0,255);
Motor_4_Speed=map(Motor_4_Speed,0,2000,0,255);
Title: Re: Rate Controller for a QuadCopter.
Post by: vortix2950 on Oct 24, 2019, 03:49 am
You need to map across the same range, therefore 0 to 2000 maps from 0 to 255:

Code: [Select]
Motor_1_Speed=map(Motor_1_Speed,0,2000,0,255);
Motor_2_Speed=map(Motor_2_Speed,0,2000,0,255);
Motor_3_Speed=map(Motor_3_Speed,0,2000,0,255);
Motor_4_Speed=map(Motor_4_Speed,0,2000,0,255);

tried. mapping it to 0 2000 didn't change much. they still beep and do nothing. one of them turned on  but it started at its middle speed instead of minimum while the rest didn't turn on  and kept beeping. tried to calibrate the Esc but that didn't work either. they seem more suitable for Servo, what do i need to do to make it work for analogwrite?
Title: Re: Rate Controller for a QuadCopter.
Post by: MartinL on Oct 24, 2019, 09:08 am
Are you using digtial pins 3, 9, 10 and 11 on you Arduino for analogWrite()? They're the only ones that work at 490Hz.

How are you calibrating the ESCs?

Is your receiver outputting a throttle value roughly between 1000 (standby) and 2000 (maximum throttle)? At standby you should be expecting a 490Hz PWM signal with a 50% duty-cycle. A multimeter will average the signal to measure 2.5V, (half of 5V).

I suggest testing by feeding the throttle directly from the receiver the ESCs without the PID control loops. Needless to say this should be done with the propellers removed from the motors. Also, it's best to keep the revs low when using the motors without props, to avoid damaging them by overheating.

BLHeli ESCs can accept a number of ESC protocols in addition to 490Hz PWM, this means they need to receive the standby signal for around 1.5 seconds or so before they'll ARM. They'll beep once they've established which protocol they're receiving. This is a safety feature to prevent them from misinterpreting which protocol is being used and inadvertently arming the motors.
Title: Re: Rate Controller for a QuadCopter.
Post by: vortix2950 on Oct 24, 2019, 07:47 pm
Are you using digtial pins 3, 9, 10 and 11 on you Arduino for analogWrite()? They're the only ones that work at 490Hz.

How are you calibrating the ESCs?

Is your receiver outputting a throttle value roughly between 1000 (standby) and 2000 (maximum throttle)? At standby you should be expecting a 490Hz PWM signal with a 50% duty-cycle. A multimeter will average the signal to measure 2.5V, (half of 5V).

I suggest testing by feeding the throttle directly from the receiver the ESCs without the PID control loops. Needless to say this should be done with the propellers removed from the motors. Also, it's best to keep the revs low when using the motors without props, to avoid damaging them by overheating.

BLHeli ESCs can accept a number of ESC protocols in addition to 490Hz PWM, this means they need to receive the standby signal for around 1.5 seconds or so before they'll ARM. They'll beep once they've established which protocol they're receiving. This is a safety feature to prevent them from misinterpreting which protocol is being used and inadvertently arming the motors.

alright it takes analogwrite now. My mistake is that i forgot to comment out the servo attach in the setup.

 But still feels the same. whats the benefit for the switch from servo? i did some   adjustments  before switching to the analogwrite and it feels like it can take off but it wonders quite abit. it feels like its flying but not up. more like forward. my room is too small to see if it can maintain that "forward" flight. is that normal for a rate controller?
Title: Re: Rate Controller for a QuadCopter.
Post by: MartinL on Oct 25, 2019, 09:38 am
Hi vortix2950,

I'm not sure what method you're using to control your quadcopter, but if feels like it's trying to get airborne then gradually increase the proportional gain.

If the proportional gain is too low then the quad will feel sloppy and unresponsive, on the other hand if the P gain is to high the aircraft will begin to oscillate. The aim is to find that sweet spot between the two.

In rate mode the aircraft won't self level, as has no notion of which way is up, therefore it's left you as the pilot control maintain level flight with your transmitter.

Quote
But still feels the same. whats the benefit for the switch from servo?
The reason is somewhat historical. Traditionally ESCs were used for aeroplanes and helicopters. These aircraft require a smooth throttle response, therefore for these aircraft the ESCs software filter the raw the 50Hz PWM input direct from the receiver. (Most RC aeroplanes don't require a flight controller).

Back in the day, we're talking up to around 2013, quadcopter specific ESCs didn't exist like they do now. Unlike an aeroplane, a quadcopter or for that matter any multi-rotor requires a fast throttle response, in order to maintain flight stability and improve reaction time.

To improve the throttle response with aeroplane ESCs, flight controllers at the time attempted to essentially override the ESCs software filter, by increasing the update rate 10 fold, (50Hz to almost 500Hz). This causes the ESC's software filter to converge on the desired value more quickly, ultimately making the motor output more responsive.

Later, multi-rotor specific firmware became available and it was possible to flash certain ESCs to make them multi-rotor specific, but the 490Hz PWM standard remained. Nowadays there's a wide range of drone specific ESCs with a myriad of faster protocols such as: Oneshot125, Oneshot42, Multishot and DShot.

490Hz PWM was chosen because it's the maximum update rate that can be achived with 2000us pulses produced by a traditional RC receiver, (1/2000us = 500Hz). Also, much of the flight controller development at that time was being done using 8-bit Arduino boards, such as the Uno, Nano and Mega, and analogWrite() coveniently produced this 490Hz PWM frequency.