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

https://www.youtube.com/watch?v=2pHdO8m6T7c&t=1s

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;

}