//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++ Begin PID-Pitch control function ++++++++++++++++
// Last Update: 29-09-2022
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void control_pid_pitch(float set_drone_altitude, float drone_altituderange)
{
float pid_p_pitch_gain = 1.25; // for p-pitchangle error gain
// float pid_i_pitch_gain = 0.0; // for i-pitchangle error gain
// int iP_switch = 0; // is 0 or 1 for testing only
// float pid_d_pitch_gain = 0.0; // for d-pitchangle error gain
float pid_p_pitch_angle_error = 0.0; //, pid_p_pitch_output;
// float pid_i_pitch_angle_error;
// float pid_d_pitch_angle_error = 0.0;
// float pid_pitch_error_sum;
// float last_pid_p_pitch_angle_error;
int p_max_pitch_angle_error = 30; // Maximum p-output of pitch PID-controller (+/-) deg
int p_min_pitch_angle_error = -30; // Minimum p-output of pitch PID-controller (+/-) deg
// int i_min_pitch_angle_error = -25; // Minimum i-output of pitch PID-controller (+/-) deg
// int i_max_pitch_angle_error = 25; // Maximum i-output of pitch PID-controller (+/-) deg
// 1600 is neutrale stand
int max_pid_pitch_output = 2000; //max is 2400;
int min_pid_pitch_output = 1200; //min is 800;
float set_baropressure = set_drone_altitude/8; //assume 1mbar => 8m
float baro_range = drone_altituderange/8; //mbar
float delta_baropressure = start_baropressure - actual_baropressure;
Serial.print("10: set_baropressure = ");
Serial.println(set_baropressure,1);
Serial.print("10: delta_baropressure = ");
Serial.println(delta_baropressure,1);
// P: Proportional PID error term in [degrees]
// drone_altitude < set_drone_altitude; //assume 1mbar => 8m
if (delta_baropressure < set_baropressure)
{
pid_pitch_output = max_pid_pitch_output;
if (GyroPitch>15) pid_pitch_output = //avoid looping nose-up
(max_pid_pitch_output + min_pid_pitch_output)/2;
}
// drone_altitude >= set_drone_altitude and
// drone_altitude <= set_drone_altitude + drone_altituderange
if ((delta_baropressure >= set_baropressure) &&
(delta_baropressure <= (set_baropressure + baro_range)))
{
pid_p_pitch_angle_error = GyroPitch - pitch_setpoint;
pid_p_pitch_angle_error = pid_p_pitch_angle_error * pid_p_pitch_gain;
pid_pitch_output = mapfloat(pid_p_pitch_angle_error,
p_min_pitch_angle_error, p_max_pitch_angle_error,
max_pid_pitch_output, min_pid_pitch_output);
// beep();beep();
}
// drone_altitude > set_drone_altitude + drone_altituderange
if (delta_baropressure > (set_baropressure + baro_range))
{
pid_pitch_output = min_pid_pitch_output;
if (GyroPitch<-15) pid_pitch_output = //avoid looping nose-down
(max_pid_pitch_output + min_pid_pitch_output)/2;
// beep();beep();beep();
}
/*
pid_pitch_output = mapfloat(pid_pitch_error_sum,
p_min_pitch_angle_error, p_max_pitch_angle_error,
min_pid_pitch_output, max_pid_pitch_output);
// if (pid_p_pitch_angle_error > p_max_pitch_angle_error)
// pid_p_pitch_angle_error = p_max_pitch_angle_error;
// if (pid_p_pitch_angle_error < p_min_pitch_angle_error)
// pid_p_pitch_angle_error = p_min_pitch_angle_error;
*/
//.........................................................
// I: Integral PID error term in [degrees/sec]
// pid_i_pitch_angle_error += pid_i_pitch_gain * pid_p_pitch_angle_error;
// if(pid_i_pitch_angle_error > i_max_pitch_angle_error)
// pid_i_pitch_angle_error = i_max_pitch_angle_error;
// if(pid_i_pitch_angle_error < i_min_pitch_angle_error)
// pid_i_pitch_angle_error = i_min_pitch_angle_error;
// Serial.print("19 pid_i_pitch_angle_error= ");
// Serial.println(pid_i_pitch_angle_error,1);
/*
//..........................................................
// D: Differential PID error term in [degrees/second]
pid_d_pitch_angle_error = GyroPitchdot; //Time derivative of GyroPitch;
// max.gemeten GyroY_rate +/-100 deg/s
// pid_d_pitch_angle_error = pid_p_pitch_angle_error - last_pid_p_pitch_angle_error;
// last_pid_p_pitch_angle_error = pid_p_pitch_angle_error;
// Serial.print("19 Pid_d_pitch_angle_error = ");
// Serial.println(pid_d_pitch_angle_error,1);
//...................................................................
pid_pitch_error_sum = pid_p_pitch_gain * pid_p_pitch_angle_error
+ pid_i_pitch_angle_error * iP_switch
+ pid_d_pitch_gain * pid_d_pitch_angle_error;
pid_pitch_output = mapfloat(pid_pitch_error_sum,
p_min_pitch_angle_error, p_max_pitch_angle_error,
min_pid_pitch_output, max_pid_pitch_output);
*/
// pid_pitch_output = mapfloat(pid_pitch_error_sum,
// p_min_pitch_angle_error, p_max_pitch_angle_error,
// max_pid_pitch_output, min_pid_pitch_output);
/*
if (pid_pitch_output > max_pid_pitch_output)
pid_pitch_output = max_pid_pitch_output;
if (pid_pitch_output < min_pid_pitch_output)
pid_pitch_output = min_pid_pitch_output;
*/
// Serial.print("19: Altitude [m]= ");
// Serial.println((start_baropressure-actual_baropressure)*8);
Serial.print("19: GyroPitch= ");
Serial.println(GyroPitch,1);
Serial.print("19: Pid_pitch_output, elevator = ");
Serial.print(pid_pitch_output,1);
Serial.println();
}
// *********************************************************
// *********** End of PID-Pitch control function ***********
// *********************************************************