Hi, jimit here, I am working on my drone's rate controller, no matter how much I tune, my rate controller 's output are always below on the setpoint, I run rate controller at 1000hz, sampling rate are also 1000hz, I have question sampling rate are two times higher than control loop? Spacially for the rate controller? Or I am wrong? Why I get pid controller 's output below on setpoint? I increase p gain still not working, I try every possible way, still not work, please help me,
Most likely an error in the code.
1 Like
Print out the terms to see which one is dominating. I am no pid expert but i think the Integral term can work against the P term reducing the overall gain.
Code?
Are you saying the Controller Output or the Controller Input/ProcessValue is below the SetPoint?
The sampling/adjustment rate should be 2-20x times as fast as the system's time constant.
do you mean if I design a drone's rate controller if my control loop is 1000Hz so my input gyro's sampling rate should be 2Khz or more?
No, I mean the time constant that measures how long it takes for your process to substantially (63%) respond to a change:
If your process takes 100ms to substantially react, measuring and adjusting at 1000Hz-2000Hz is too fast for the D component of a PID to be acting on anything but noise, and too fast or the I component to do anything but overreact.
How fast does your system react?
And where's your code?
DaveX brother! I sent you my code please analyze it, note that the main loop run at 1000Hz which is for the rate controller (Here we take angular velocity from the gyro preprocess, calculate pid output then command motor, note that my Esc runs at 125us to 250us), inside the main loop I run nested loop, which runs once in four times, that way second loop run at 250Hz, which are for angle mode, I use EKF for estimate angle, Here are my all code.
DaveX sir, I don't understand properly what you want to say to me! can you please give me an example? let's say We want to build a drone's rate controller, and my ESC runs at 125us-250us if I want to run my drone's controller at 1000Hz what sampling rate my Gyro should be for angular velocity as input?
"void innerLoop()" run at 1000Hz"
ROLL_OUTPUT = rollController.updateInnerLoop(roll_input, ROLL_SETPOINT_INNER);
PITCH_OUTPUT = pitchController.updateInnerLoop(pitch_input, PITCH_SETPOINT_INNER);
YAW_OUTPUT = yawController.updateInnerLoop(YAW_INPUT, pid_yaw_setpoint);
"These are the rate controller"
void innerLoop()
{
prev_time = current_time;
current_time = millis();
dt = (current_time - prev_time) / 1000.0;
FlightMode.FLGIHT_MODE = 1;
if (radio.CHANNEL_5 >= 1200 && radio.CHANNEL_5 < 1600)
FlightMode.FLGIHT_MODE = 2;
if (radio.CHANNEL_5 >= 1600 && radio.CHANNEL_5 < 2100)
FlightMode.FLGIHT_MODE = 3;
imu.getBMI270GyroData();
omega = imu.mGyroBmi270Data;
for (int i = 0; i < filter_length; i++)
{
omega = filters[i].apply(omega);
omega = filters1[i].apply(omega);
}
ROLL_INPUT = omega(0);
PITCH_INPUT = omega(1);
YAW_INPUT = omega(2);
pid_yaw_setpoint = 0;
// We need a little dead band of 16us for better results.
if (radio.CHANNEL_3 > 1050)
{ // Do not yaw when turning off the motors.
if (radio.CHANNEL_4 > 1508)
pid_yaw_setpoint = ((float)radio.CHANNEL_4 - 1508) / 3.0;
else if (radio.CHANNEL_4 < 1492)
pid_yaw_setpoint = ((float)radio.CHANNEL_4 - 1492) / 3.0;
}
roll_input = 0.7 * roll_input + ROLL_INPUT * 0.3;
pitch_input = 0.7 * pitch_input + PITCH_INPUT * 0.3;
ROLL_OUTPUT = rollController.updateInnerLoop(roll_input, ROLL_SETPOINT_INNER);
PITCH_OUTPUT = pitchController.updateInnerLoop(pitch_input, PITCH_SETPOINT_INNER);
YAW_OUTPUT = yawController.updateInnerLoop(YAW_INPUT, pid_yaw_setpoint);
GetFlightStatus();
if (FlightMode.TAKEOFF_DETECTED == 1 && FlightMode.START == 2)
{
THROTTLE = (int16_t)radio.CHANNEL_3 + TAKEOFF_THROTTLE;
if (FlightMode.FLGIHT_MODE >= 2)
{
THROTTLE = 1500 + TAKEOFF_THROTTLE + (int16_t)ALTITUDE_OUTPUT + MANUAL_THROTTLE;
}
}
analogReadResolution(12);
BATTERY_VOLTAGE = BATTERY_VOLTAGE * 0.92 + ((float)analogRead(A10) / 1410.1);
// Turn on the led if battery voltage is to low. Default setting is 10.5V
if (BATTERY_VOLTAGE > 6.0 && BATTERY_VOLTAGE < LOW_BATTERY_WARNING && FlightMode.ERROR == 0){
FlightMode.ERROR = 2;
}
if (FlightMode.START == 2)
{
if (THROTTLE > 1800)
THROTTLE = 1800;
OMEGA1 = (float)THROTTLE - PITCH_OUTPUT + ROLL_OUTPUT - YAW_OUTPUT;
OMEGA2 = (float)THROTTLE + PITCH_OUTPUT + ROLL_OUTPUT + YAW_OUTPUT;
OMEGA3 = (float)THROTTLE + PITCH_OUTPUT - ROLL_OUTPUT - YAW_OUTPUT;
OMEGA4 = (float)THROTTLE - PITCH_OUTPUT - ROLL_OUTPUT + YAW_OUTPUT;
if (BATTERY_VOLTAGE < 12.50 && BATTERY_VOLTAGE > 6.0)
{ // Is the battery connected?
OMEGA1 += (12.60 - BATTERY_VOLTAGE) * battery_compensation; // Compensate the esc-1 pulse for voltage drop.
OMEGA2 += (12.60 - BATTERY_VOLTAGE) * battery_compensation; // Compensate the esc-2 pulse for voltage drop.
OMEGA3 += (12.60 - BATTERY_VOLTAGE) * battery_compensation; // Compensate the esc-3 pulse for voltage drop.
OMEGA4 += (12.60 - BATTERY_VOLTAGE) * battery_compensation; // Compensate the esc-4 pulse for voltage drop.
}
OMEGA1 = (OMEGA1 - 1000.0f) / 1000.0f;
OMEGA2 = (OMEGA2 - 1000.0f) / 1000.0f;
OMEGA3 = (OMEGA3 - 1000.0f) / 1000.0f;
OMEGA4 = (OMEGA4 - 1000.0f) / 1000.0f;
M1 = OMEGA1 * 125 + 125;
M2 = OMEGA2 * 125 + 125;
M3 = OMEGA3 * 125 + 125;
M4 = OMEGA4 * 125 + 125;
M1 = constrain(M1, 125, 250);
M2 = constrain(M2, 125, 250);
M3 = constrain(M3, 125, 250);
M4 = constrain(M4, 125, 250);
if (M1 < esc.MOTOR_IDEL_SPEED)
M1 = esc.MOTOR_IDEL_SPEED;
if (M2 < esc.MOTOR_IDEL_SPEED)
M2 = esc.MOTOR_IDEL_SPEED;
if (M3 < esc.MOTOR_IDEL_SPEED)
M3 = esc.MOTOR_IDEL_SPEED;
if (M4 < esc.MOTOR_IDEL_SPEED)
M4 = esc.MOTOR_IDEL_SPEED;
}
else
{
M1 = 125;
M2 = 125;
M3 = 125;
M4 = 125;
}
esc.commandMotors(M1, M2, M3, M4);
}
void loop()
{
OUTER_LOOP_COUNTER++;
innerLoop();
if (OUTER_LOOP_COUNTER == 1)
{
fuseAngularValocity();
fuseAccData();
}
if (OUTER_LOOP_COUNTER == 3)
{
fuseMagdata();
ekf.mMagDataAvailable = true;
ekf.fuseCompass(mMag);
}
if (OUTER_LOOP_COUNTER == 4)
{
OUTER_LOOP_COUNTER = 0;
getInputForAngleMode();
ROLL_SETPOINT_BASE = radio.CHANNEL_1;
PITCH_SETPOINT_BASE = radio.CHANNEL_2;
rollSetpointbase = ROLL_SETPOINT_BASE;
pitchSetpointbase = PITCH_SETPOINT_BASE;
led.errorSignal(FlightMode.ERROR);
getDesiredState();
pid_roll_angle = rollController.updateOuterLoop(ANGLE_ROLL, rollSetpoint);
pid_pitch_angle = pitchController.updateOuterLoop(ANGLE_PITCH, pitchSetpoint);
}
while (micros() - LoopTimer < 1000)
;
LoopTimer = micros();
}
Here is the main logic of the PID controller.
CascatedPid.cpp
#include <Arduino.h>
#include <average_filter.h>
class CascatedPid
{
private:
public:
AverageFilter filter = AverageFilter(10);
CascatedPid(float KP, float KD, float KI, float INNER_KP, float INNER_KD, float INNER_KI);
float KP, KD, KI;
float INNER_KP, INNER_KD, INNER_KI;
float error, prevError, prevInput, D;
float p_turm, d_turm, i_turm, outputInnerLoop;
float update(float input, float setpoint, float DT);
float updateInnerLoop(float input, float setpoint);
float pid_output, prevOutput;
float outerLoopError, outerLoopPrevError;
float outerLoopPturm, outerLoopIturm, outerLoopDturm, output;
float updateOuterLoop(float input, float setpoint);
};
CascatedPid::CascatedPid(float KP, float KD, float KI, float INNER_KP, float INNER_KD, float INNER_KI)
{
this->KP = KP;
this->KD = KD;
this->KI = KI;
this->INNER_KP = INNER_KP;
this->INNER_KD = INNER_KD;
this->INNER_KI = INNER_KI;
}
float CascatedPid::updateInnerLoop(float input, float setpoint)
{
error = input - setpoint;
p_turm = INNER_KP * error;
d_turm = (error - prevError) * INNER_KD;
i_turm += INNER_KI * error;
i_turm = constrain(i_turm, -25.0f, 25.0f);
outputInnerLoop = p_turm + i_turm + d_turm;
outputInnerLoop = constrain(outputInnerLoop, -400, 400);
prevError = error;
return outputInnerLoop;
}
float CascatedPid::updateOuterLoop(float input, float setpoint)
{
outerLoopError = setpoint - input;
outerLoopPturm = KP * outerLoopError;
outerLoopIturm += outerLoopError * KI;
outerLoopIturm = constrain(outerLoopIturm, -25.0f, 25.0f);
outerLoopDturm = (input - prevInput) * KD;
output = outerLoopPturm + outerLoopIturm - outerLoopDturm;
pid_output = output * 30.0f;
pid_output = constrain(pid_output, -400.0f, 400.0f);
pid_output = (1.0f - 0.7f) * prevOutput + 0.7f * pid_output;
prevInput = input;
prevOutput = pid_output;
return pid_output;
}
here we initialize pid-controller class and our loop gains.
float BATTERY_VOLTAGE;
float battery_compensation = 40.0;
float LOW_BATTERY_WARNING = 10.7;
int16_t MANUAL_TAKEOFF_THROTTLE = 1570;
float MAX_ROLL_PITCH = 30.0;
float MAX_YAW = 160.0;
int16_t M1, M2, M3, M4;
//today's best
float INNER_LOOP_KP = 1.3; // 1.3 best latest
float INNER_LOOP_KI = 0.0025; // 0.00090 best latest
float INNER_LOOP_KD = 20.0;
//OUTER LOOP GAINS.
float OUTER_LOOP_KP = 0.22f; //0.25
float OUTER_LOOP_KI = 0.00040f; //0.0040 max gain = 0.010f //best gain = 0.00050f
float OUTER_LOOP_KD = 2.25f; //2.25
// INNER LOOP YAW CONTROLLER GAINs.
float INNER_LOOP_YAW_KP = 3.0;
float INNER_LOOP_YAW_KD = 0.00;
float INNER_LOOP_YAW_KI = 0.004f;
CascatedPid rollController = CascatedPid(OUTER_LOOP_KP, OUTER_LOOP_KD,
OUTER_LOOP_KI, INNER_LOOP_KP,
INNER_LOOP_KD, INNER_LOOP_KI);
CascatedPid pitchController = CascatedPid(OUTER_LOOP_KP, OUTER_LOOP_KD,
OUTER_LOOP_KI, INNER_LOOP_KP,
INNER_LOOP_KD, INNER_LOOP_KI);
CascatedPid yawController = CascatedPid(0.0f, 0.0f,
0.0f, INNER_LOOP_YAW_KP,
INNER_LOOP_YAW_KD, INNER_LOOP_YAW_KI);
What are the units (degrees? ESC microseconds?) of the inputs, outputs and setpoints?
For example, if you were using a PID to control the temperature of a shower to 40°C by turning a temperature knob from 0 to 10, and system lags (suppose 30s to change) you shouldn't measure and adjust the temperature every 0.001s just because your controller can calculate that fast. The adjustments should be related to the lag of the physical system (30s in this example). If you measure and adjust much different than 10x the speed of the system, (3s) the PID will get confused--(dt = 300sec would make the controller dreadfully slow to respond; 0.03sec would makes the controller adjust based on trivialities.)
Although your incomplete code calculates a dt, it doesn't seem to use it anywhere. How did you write this?
My pid output are esc's microseconds, my esc run at125 to 250 us, and my control loop run at 1000hz, so, I think my system are fast enough, since my loop has constant time interval we should ignore dt, that's why I am not put dt,
Check the signs on the PID error terms for correctness. The sign is not standardized.
No bro, it is ok, I confirm twise, my drone fly like charm, but issue here output are always below then setpoint, I increase I gain and p gain, decrease d gain. I try too hard still not work, one last question in cascaded form of pid, I have angle controller and rate controller, outer controller are angle and inner are rate controller, where I can set system input (in my case esc's pulses), in controller or rate controller? Where I command motor inside1000hz rate controller inner loop or 250hz angle controller outer loop?
Show the input, setpoint, and output data.
Theres probably something doing something unexpected with the arithmetic somewhere.
Hi @jimit007
I'd suggest setting the Kp term to 1.0 and turning off the Ki and Kd terms by setting them to 0.0. Then fixing setpoint at 0.0, monitor the PID output while tilting the flight controller board or drone. The output should move back and forth both positive and negative around the 0 point.
1 Like
Sure, I post here my flight data,
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.