GPS position hold (Arduino Drone)

Other Hardware Robotics
1

Hi people. I devolve a drone following the instructions from Joop Brokking in this video...

the drone works very well but now I want to introduce into the code a hold position instructions.
And what I do doesn't work very well.

Could someone see any problem intro my code or give me a tip to solve this?

I'll put here the part of the code that I change to try hold position:

What I do in the code. I calculate the position error and then calculate PID values to the hold position of the drone.
then I add this implementation intro the error from the pid stability values.

#include <TinyGPS.h>
SoftwareSerial serial1(2, 3); // RX, TX
TinyGPS gps1;
if (start == 2) {                                                         //The motors are started.
    if (throttle > 1800) throttle = 1800;                                   //We need some room to keep full control at full throttle.

    if (Ch6 > 1800) { //here I define a new channel in radio, then if I change the channel the drone hold the position
      esc_1 = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW)
      esc_2 = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
      esc_3 = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW)
      esc_4 = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW)

      ii=0;      
    }
    else { // hold position control

if(ii==0){
  float lat_input = ((latitude) / 100000, 6); //it define the first lat position
  float lon_input = ((longitude) / 100000, 6);
  ii=ii+1;
  }
  else{
  float lat_new = ((latitude) / 100000, 6); //it defines the drone position after it deslocate
  float lon_new = ((longitude) / 100000, 6);
  }

      calculate_pid_hold_position();                                                            //PID inputs are known. So we can calculate the pid output.

      esc_1 = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW)
      esc_2 = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
      esc_3 = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW)
      esc_4 = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW)

    }
  }

//to calculate de hold position PID

void calculate_pid_hold_position() {

  //PID GPS////////////////////////////////////////////
  float Kp_gps = 0;
  float Ki_gps = 0;
  float Kd_gps =0;

#define m_quad 1.1 //kg
#define RAD_TO_DEG 57.295779513082320876798154814105
#define DEG_TO_RAD 0.017453292519943295769236907684886


  float lat_input = ((latitude) / 100000, 6); //it define the first lat position
  float lon_input = ((longitude) / 100000, 6);

  float lat_new = ((latitude) / 100000, 6); //it defines the drone position after it deslocate
  float lon_new = ((longitude) / 100000, 6);

  float error_rate_LAT = (lat_input - lat_new) * 6371000;
  float error_rate_LON = (lon_input - lon_new) * 6371000; //error in cm. 6371000 mean de radius of the earth
   
// in this part I calculate the PID to hold position)
  error_rate_LAT = constrain(error_rate_LAT, -300, 300); //+-200 cm/s
  error_rate_LON = constrain(error_rate_LON, -300, 300);
  GPSI_LAT = GPSI_LAT + (error_rate_LAT * Ki_gps * 0.02); //divide to 50 like you say me.
  GPSI_LON = GPSI_LON + (error_rate_LON * Ki_gps * 0.02);
  GPSI_LAT = constrain(GPSI_LAT, -250, 250); //win speed +-200 cm/s
  GPSI_LON = constrain(GPSI_LON, -250, 250);
  Control_XEf = error_LAT * Kp_gps + error_rate_LAT * Kd_gps + GPSI_LAT + Distance_Y * 0.5; //PID Control speed
  Control_YEf = error_LON * Kp_gps + error_rate_LON * Kd_gps + GPSI_LON + Distance_X * 0.5;
  Control_XEf = constrain(Control_XEf, -800, 800); //PWM 1000 - 1900
  Control_YEf = constrain(Control_YEf, -800, 800);
   float urolldesir = (Control_YEf * m_quad) / throttle; 
  float upitchdesir = (Control_XEf * m_quad * -1.0) / throttle; //*-1
  urolldesir = constrain(urolldesir, -0.7, 0.7); 
  upitchdesir = constrain(upitchdesir, -0.7, 0.7);
  float temp_YBf = asin(urolldesir) * RAD_TO_DEG;
  float temp_XBf = asin(upitchdesir) * RAD_TO_DEG;

  Control_XBf = constrain(temp_YBf, -20, 20);//+-20 deg
  Control_YBf = constrain(temp_XBf, -20, 20);//+-20 deg


//after this calculation I only add the Contro values intro the PID errors from roll and Pitch.
  //Roll calculations
  pid_error_temp = gyro_roll_input - pid_roll_setpoint + Control_XBf;
  pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
  if (pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
  else if (pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;

  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);
  if (pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
  else if (pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;

  pid_last_roll_d_error = pid_error_temp;


  //Pitch calculations
  pid_error_temp = gyro_pitch_input - pid_pitch_setpoint + Control_YBf;
  pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
  if (pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
  else if (pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;

  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);
  if (pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
  else if (pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;

  pid_last_pitch_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;
  if (pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw;
  else if (pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1;

  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);
  if (pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw;
  else if (pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1;

  pid_last_yaw_d_error = pid_error_temp;

}

the original code, to calculate the stability PID, I mean, the what happens when CH6>6 is composed like this:

void calculate_pid() {
  //Roll calculations
  pid_error_temp = gyro_roll_input - pid_roll_setpoint;
  pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
  if (pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
  else if (pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;

  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);
  if (pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
  else if (pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;

  pid_last_roll_d_error = 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;
  if (pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
  else if (pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;

  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);
  if (pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
  else if (pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;

  pid_last_pitch_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;
  if (pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw;
  else if (pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1;

  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);
  if (pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw;
  else if (pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1;

  pid_last_yaw_d_error = pid_error_temp;
}
2 
  float lon_input = ((longitude) / 100000, 6)

This line does not do what you might think it does. Look up the "comma operator".

In any case, float (double) variables on Arduino do not have enough precision to be useful for position holding. You MUST use long integer variables to store coordinates, and be extremely careful with distance calculations using them.

3

jremington,

I'm comprehending that have a big precision on hold position is difficult because of the errors of the gps, but, what I mean is to be possible to find a way to find the drone intro the position with the gps error.
I don't get any error using the comma operator.
my bigger difficulty is to know what i should do to modify the esc output to find a way to make the drone stay in one position.

4

hi,Brooking is going to talk firstly about altitude hold with new controller stm 32 ,arduino uno is slow for what you planing

5

I don't get any error using the comma operator.

Of course not. You just get an unexpected answer.

Try running this little program and see what you get!

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  long longitude = 12345678;
  float x = ((longitude) / 100000, 6);
  Serial.print(" x = ");
  Serial.println(x);
}

void loop() {
  // put your main code here, to run repeatedly:

}
6

You can use an accelerometer/magnetometer together with your GPS to correct coordinates.

The logic is, you update your X, Y, and Z coordinates acording with the following criteria:

  • If GPS coordinate X changed but accelerometer coordinate X did not changed, discard the change;

  • If GPS coordinate X changed and the accelerometer coordinate X changed too, accept the GPS change and update the coordinate X.

Do the same for Y and Z coordinates.

This way you "soften" or "average" the coordinate variation on your GPS, using the noisy accelerometer to discard noisy changes on your GPS. When you have a noisy/imprecise sensor, you need another sensor to correct it.

I think you can also use a Gyroscope to do this correction.

Of course you need some tolerance, I suggest you to use a percent-based parameter to decide if the change in parameter will be taken into account. Let's say, if the parameter had a 2% change from its previous value, you consider it changed, otherwise it's only noise and you can discard it.

7

If GPS coordinate X changed but accelerometer coordinate X did not changed, discard the change;

How do you imagine this to work? Accelerometers measure acceleration, not coordinates.

8

jremington:
How do you imagine this to work? Accelerometers measure acceleration, not coordinates.

You just have to detect movement to validate GPS coordinate change.

Any movement causes acceleration, even for a moment, and this sensor can detect it.

A Gyro detects movement, but around an axis, so maybe can be used too, but I'm not so sure.

9

In this page there's a lot of resources on the subject, a must-study for anyone programming drones:

Video Tutorials Outlining Inertial Measurement Unit (IMU) implementation and Sensor Fusion with GPS

10

Any movement causes acceleration,

Nonsense. Acceleration is defined as increase or decrease in velocity , and is the result of a net force.

For steady state motion, i.e. with no change in velocity, the acceleration is zero.

11

This way you "soften" or "average" the coordinate variation on your GPS, using the noisy accelerometer to discard noisy changes on your GPS. When you have a noisy/imprecise sensor, you need another sensor to correct it.

Of course you need some tolerance, I suggest you to use a percent-based parameter to decide if the change in parameter will be taken into account. Let's say, if the parameter had a 2% change from its previous value, you consider it changed, otherwise it's only noise and you can discard it.

What you're describing here is essentially a Kalman filter.

The Kalman filter is able to combine two sources of infomation, that is the predicted state generated from a mathematical model (kinematic equations) of your system and the measured state (including noise) from your sensors, to produce an optimal estimation of your system's current state.

This allows the Kalman filter to make accurate predictions of your current state, for example GPS position, velocity etc..., even in the presence of noisy and sometimes inaccurate measurement data. It even dynamically works out, whether it should put more trust in the information from the predicted state (mathematical model), or the measured state (sensors).

I found the excellent iLectureOnline tutorials by Michel van Biezen on the subject of the Kalman filter, (amongst all his other outstanding mathematics and engineering videos), a good starting point for learning how it all works. They're available on YouTube or on his iLectureOnline website.

The other issue with fusing sensor data with the GPS, is that your on-board sensors such as the gyroscope, acclerometer and magnetometer axes are aligned with the body of the aircraft, or body frame of reference, whereas your GPS is aligned with the inertial or earth frame of reference. This usually requires the use of either 3D Direction Cosine Matrix (DCM) or Quaternion mathematics to map your sensor vectors (measurements) between the two frames of reference.

It is possible however, to get reasonably good automated flight using the GPS data alone.

12

jremington:
Nonsense. Acceleration is defined as increase or decrease in velocity , and is the result of a net force.

For steady state motion, i.e. with no change in velocity, the acceleration is zero.

For a body to go from point A to point B, must have some speed involved. Without speed, there's no motion.

For a body to go from static state to moving state, I mean gaining speed, there's acceleration involved.

But what really matter is: the value read on the accelerometer axis changes when you move it? If yes, the sensor can be used. But it's possible to move the accelerometer without seeing a change in it's values? If so, an accelerometer can't be used the way I said. The whole question can be reduced to this. I'm not able to make this test right now, I'm away from my workbench.

Maybe for this case should be implemented a full Kallman filter combining accelerometer, gyroscope and magnetometer, making an "inertial measuring unit", to get desired data. MartinL gave a very good explanation on this, and the link I posted also have resources of interest.

13

MartinL:
The other issue with fusing sensor data with the GPS, is that your on-board sensors such as the gyroscope, acclerometer and magnetometer axes are aligned with the body of the aircraft, or body frame of reference, whereas your GPS is aligned with the inertial or earth frame of reference. This usually requires the use of either 3D Direction Cosine Matrix (DCM) or Quaternion mathematics to map your sensor vectors (measurements) between the two frames of reference.

Yes, you are right, I have not considered this possibility.

It is possible however, to get reasonably good automated flight using the GPS data alone.

Yes, you are right, but in the case of losing GPS signal, the better scenario would be having a backup navigation system, at least until GPS signal is back.

14

Without speed, there's no motion.

Speed relative to what origin?

Is the book I am reading, while riding in the train, not moving?

Back to your high school physics!