schematic and calculator
12.4*330/(1000+330) povides the output I want, it's also the formula on the calculator'sr website.
Good to know. It's just two applications of Ohms law.
Which way around are your resistors? Are you measuring with the resistor between AnalogInputPin and Ground as a 330K or the 1M?
Because the wrong-way-around will give you the higher voltage.
I decided to use 300K instead 330K, because I had it at home. 330K is the one between AnalogInputPin and Ground.
For some reason I still have a different output than on the multimeter (esp32 voltage divider measures less voltage).
My code:
const int VOLTAGE_PIN = 35;
const float R1 = 1007000;
const float R2 = 303000;
const float V_REF = 2.883;
void setup() {
Serial.begin(9600);
}
void loop() {
float voltage = analogRead(VOLTAGE_PIN) * V_REF / 4095.0;
float batteryVoltage = voltage * (R1 + R2) / R2;
Serial.print("Napeti baterie: ");
Serial.print(batteryVoltage, 2);
Serial.println(" V");
delay(1000);
}
Any ideas? Couldn't be the problem own resistance of ESP32?
At this point, I would run some experiments. I would write a small sketch that reads A0 and print the raw value to Serial. Then use a potentiometer to make a variable voltage source. Use a meter to read the voltage on the A0 pin and use the pot to get 1/2 full sale (512 or 2096) on Serial Monitor. The measured voltage is 1/2 the "V_REF".
Hi, I've run into a few problems recently. I also upgraded the drone frame. I am now using a dji f450 frame. My code is better now, but I'm having issues with the PID loop response. The drone responds to errors late regardless of the values of the pid constants. Changing them changes the drone's behavior, but the late response is same for all, so I think I have a bug in my code(in ReadSensor() or double CalculatePID()). I've seen dozens of videos on youtube about PID controllers, but I still can't solve this problem. I am attaching a video (Kp = 0.3, Ki = 0.04, Kd = 28.5 , the graph that moves is the PID pitch output) of the drone behavior and the code.
Full code for pitch axis tunning:
#include <Wire.h>
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>
#include <BME280I2C.h>
#include <ESP32Servo.h>
#include <Adafruit_Sensor.h>
#include <BluetoothSerial.h>
#include <EEPROM.h>
#include <Adafruit_HMC5883_U.h>
//library objects
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
BluetoothSerial SerialBT;
Servo escLF;
Servo escRF;
Servo escRB;
Servo escLB;
BME280I2C bme;
//motor pins
#define esc1 33 // right back
#define esc2 32 // right front
#define esc3 26 // left back
#define esc4 25 // left front
//radio input
RF24 radio(5, 4);
const uint64_t address[] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
short data[6];
short altitude;
double seaLevelPres = 1013.25;
bool camera = false;
//IMU
const int MPU_addr = 0x68;
double alpha = 1;
double xa, ya, za, xg, yg, zg;
double xAcc, yAcc, zAcc, xGyro, yGyro, zGyro;
double currentGyroMillis, previousGyroMillis, deltaGyroTime;
double pitchInputAcc, rollInputAcc, yawInputAcc;
double pitchInputGyro, rollInputGyro, yawInputGyro;
double pitchInput, rollInput, yawInput, altitudeInput;
double pitchInput2, rollInput2, lastYawInput;
int counter = 0;
double rollGyroOffset, pitchGyroOffset, yawGyroOffset, rollAccOffset, pitchAccOffset, yawAccOffset;
float declinationAngle = 0.0986;
double heading, headingOffset, lastHeadingOffset, totalHeadingOffset;
double altitudeInputOffset;
//kalman's pitch & roll filter
const double QRoll = 0.25; // Procesní šum
const double RRoll = 6.9; // Měřící šum
double xRoll = 0; // Stavová proměnná
double pRoll = 0.9; // Kovariance stavu
double kRoll; // Kalmanovo zesílení
const double QPitch = 0.25; // Procesní šum
const double RPitch = 6.9; // Měřící šum
double xPitch = 0; // Stavová proměnná
double pPitch = 0.9; // Kovariance stavu
double kPitch; // Kalmanovo zesílení
//kalman's altitude filter
const double QTemp = 0.91; // Procesní šum
const double RTemp = 0.125; // Měřící šum
double xTemp = 0; // Stavová proměnná
double pTemp = 1; // Kovariance stavu
double kTemp; // Kalmanovo zesílení
const double QPres = 0.043; // Procesní šum
const double RPres = 0.007; // Měřící šum
double xPres = 0; // Stavová proměnná
double pPres = 1; // Kovariance stavu
double kPres; // Kalmanovo zesílení
bool tunning = false;
//pid
double pitchKp = 0, pitchKi = 0, pitchKd = 0;
double rollKp = pitchKp, rollKi = pitchKi, rollKd = pitchKd;
double yawKp = 2, yawKi = 0.02, yawKd = 0;
double altitudeKp = 1, altitudeKi = 0, altitudeKd = 0;
double pitchSetpoint = 0, rollSetpoint = 0, yawSetpoint = 0, altitudeSetpoint = 0; //radio transmitter values
double pitchLastError, rollLastError, yawLastError, altitudeLastError;
double pitchIntegral, rollIntegral, yawIntegral, altitudeIntegral;
//integral windup comp
double pitchOutputMax = 30, rollOutputMax = 30, yawOutputMax = 180, altitudeOutputMax = 180;
double pitchOutputMin = -30, rollOutputMin = -30, yawOutputMin = -180, altitudeOutputMin = 0;
//pid outputs
double pitchOutput, rollOutput, yawOutput, altitudeOutput;
//eeprom
byte bytes[8];
//time
unsigned long previousMillis;
unsigned long previousMillisRadio;
unsigned long previousMillisRadioError;
unsigned long previousMillisBluetooth;
unsigned long currentMillis;
unsigned long currentMillisRadioError;
const unsigned long interval = 20;
const unsigned long intervalRadio = 40;
//motor speeds
double sp1 = 0, sp2 = 0, sp3 = 0, sp4 = 0;
//functions initialization
void InitInput();
void ReadSensors(double& pitchInput, double& rollInput, double& yawInput, double& altitudeInput);
double CalculatePID(double Input, double Kp, double Ki, double Kd, double Setpoint, double& LastError, double& Integral, double OutputMin, double OutputMax);
void GetRadio(double& yawSetpoint, double& rollSetpoint, double& pitchSetpoint, double& altitudeSetpoint, double& seaLevelPres, bool& camera);
void Bluetooth(double& pitchKp, double& pitchKi, double& pitchKd);
void ControlMotors();
void setup() {
delay(3000);
InitInput();
}
void loop() {
currentMillis = millis();
if ((currentMillis - previousMillis) >= interval) {
previousMillis = currentMillis;
ReadSensors(pitchInput, rollInput, yawInput, altitudeInput);
tunning = true;
pitchOutput = CalculatePID(pitchInput, pitchKp, pitchKi, pitchKd, pitchSetpoint, pitchLastError, pitchIntegral, pitchOutputMin, pitchOutputMax);
//rollOutput = CalculatePID(rollInput, rollKp, rollKi, rollKd, rollSetpoint, rollLastError, rollIntegral, rollOutputMin, rollOutputMax);
tunning = false;
//yawOutput = CalculatePID(yawInput, yawKp, yawKi, yawKd, yawSetpoint, yawLastError, yawIntegral, yawOutputMin, yawOutputMax);
altitudeOutput = CalculatePID(altitudeInput, altitudeKp, altitudeKi, altitudeKd, altitudeSetpoint, altitudeLastError, altitudeIntegral, altitudeOutputMin, altitudeOutputMax);
}
if ((currentMillis - previousMillisRadio) >= intervalRadio) {
GetRadio(yawSetpoint, rollSetpoint, pitchSetpoint, altitudeSetpoint, seaLevelPres, camera);
previousMillisRadio = currentMillis;
}
if (SerialBT.connected()) Bluetooth(pitchKp, pitchKi, pitchKd);
/*Serial.print("altitudeSetpoint ");
Serial.print(altitudeSetpoint);
Serial.print(" | altitudeOutput ");
Serial.println(altitudeOutput);*/
/*Serial.println(pitchOutput);
Serial.println(rollOutput);
Serial.println(yawOutput);
Serial.println(altitudeOutput);
Serial.print("Altitude sea level pressure");
Serial.println(seaLevelPres);
Serial.print("altitudeSetpoint");
Serial.println(altitudeSetpoint);
Serial.print("pitchSetpoint");
Serial.println(pitchSetpoint);
Serial.print("rollSetpoint");
Serial.println(rollSetpoint);
Serial.print("yawSetpoint");
Serial.println(yawSetpoint);
Serial.print("Pitch");
Serial.println(pitchInput);
Serial.print("Roll");
Serial.println(rollInput);
Serial.print("Yaw");
Serial.println(yawInput);
Serial.print("Altitude");
Serial.println(altitudeInput);
Serial.print("Altitude Offset");
Serial.println(altitudeInputOffset);*/
if (camera) ControlMotors();
}
static String LastConsPID;
int CounterBluetooth = 0;
void Bluetooth(double& pitchKp, double& pitchKi, double& pitchKd) {
CounterBluetooth += 1;
if (SerialBT.available()) {
switch (SerialBT.read()) {
case 'A':
pitchKp += 0.05;
break;
case 'B':
pitchKp -= 0.05;
break;
case 'C':
memcpy(bytes, &pitchKp, sizeof(pitchKp));
EEPROM.put(0, bytes);
break;
case 'D':
pitchKi += 0.01;
break;
case 'E':
pitchKi -= 0.01;
break;
case 'F':
memcpy(bytes, &pitchKi, sizeof(pitchKi));
EEPROM.put(8, bytes);
break;
case 'G':
pitchKd += 0.5;
break;
case 'H':
pitchKd -= 0.5;
break;
case 'I':
memcpy(bytes, &pitchKd, sizeof(pitchKd));
EEPROM.put(16, bytes);
break;
}
EEPROM.commit();
rollKp = pitchKp;
rollKi = pitchKi;
rollKd = pitchKd;
}
String ConsPID = "(" + String(pitchKp) + "|" + String(pitchKi) + "|" + String(pitchKd) + ")";
if (LastConsPID != ConsPID || CounterBluetooth == 1) {
SerialBT.println(ConsPID);
//Serial.print("BLUETOOTH PID CONSTANTS - ");
//Serial.println(ConsPID);
}
LastConsPID = ConsPID;
}
double CalculatePID(double Input, double Kp, double Ki, double Kd, double Setpoint, double &LastError, double &Integral, double OutputMin, double OutputMax) {
double error = Setpoint - Input;
// Anti-windup: back-calculation
if (Integral > OutputMax) {
Integral = OutputMax;
} else if (Integral < OutputMin) {
Integral = OutputMin;
}
double derivative = (error - LastError) / interval;
double Output = Kp * error + Ki * Integral + Kd * derivative;
// Anti-windup: back-calculation
if (Output > OutputMax) {
Integral -= (Output - OutputMax) / Ki;
Output = OutputMax;
} else if (Output < OutputMin) {
Integral -= (Output - OutputMin) / Ki;
Output = OutputMin;
}
LastError = error;
Integral += error * interval;
/*Serial.print("error | ");
Serial.print(error);
Serial.print(" | Setpoint | ");
Serial.print(Setpoint);
Serial.print(" | Input ");
Serial.println(Input);*/
// Plot DATA
/*Serial.print("PTerm:");
Serial.print(Kp * error);
Serial.print(",");
Serial.print("ITerm:");
Serial.print(Ki * Integral);
Serial.print(",");
Serial.print("DTerm:");
Serial.print(Kd * derivative);
Serial.print(",");*/
if (tunning) {
//Serial.print("Output:");
Serial.print(Output);
Serial.print(",");
//Serial.print("Setpoint:");
Serial.print(Setpoint);
Serial.println(" ");
}
return Output;
}
void ControlMotors() {
sp1 = -rollOutput - pitchOutput + altitudeOutput;
sp2 = -rollOutput + pitchOutput + altitudeOutput;
sp3 = rollOutput - pitchOutput + altitudeOutput;
sp4 = rollOutput + pitchOutput + altitudeOutput;
if (altitudeSetpoint > 40) {
sp1 += yawOutput;
sp2 -= yawOutput;
sp3 -= yawOutput;
sp4 += yawOutput;
}
sp1 = constrain(sp1, 0, 180);
sp2 = constrain(sp2, 0, 180);
sp3 = constrain(sp3, 0, 180);
sp4 = constrain(sp4, 0, 180);
/*Serial.print("M1 | ");
Serial.print(sp1);
Serial.print(" | M2 | ");
Serial.print(sp2);
Serial.print(" | M3 | ");
Serial.print(sp3);
Serial.print(" | M4 | ");
Serial.println(sp4);*/
if (altitudeSetpoint > 0) {
escLB.write(sp1);
escLF.write(sp2);
escRB.write(sp3);
escRF.write(sp4);
} else {
escLB.write(0);
escLF.write(0);
escRB.write(0);
escRF.write(0);
}
}
void GetRadio(double& yawSetpoint, double& rollSetpoint, double& pitchSetpoint, double& altitudeSetpoint, double& seaLevelPres, bool& camera) {
if (radio.available()) {
radio.read(&data, sizeof(data));
yawSetpoint = data[0];
altitudeSetpoint = data[1];
altitudeSetpoint = map(altitudeSetpoint, 0, 360, 0, 180);
rollSetpoint = data[2];
rollSetpoint = map(rollSetpoint, -45, 45, -30, 30);
pitchSetpoint = data[3];
pitchSetpoint = map(pitchSetpoint, -45, 45, -30, 30);
seaLevelPres = data[4];
camera = data[5];
previousMillisRadioError = millis();
} else {
currentMillisRadioError = millis();
if (currentMillisRadioError - previousMillisRadioError > 500) altitudeSetpoint = 0;
}
radio.stopListening();
radio.openWritingPipe(address[1]);
altitude = int(altitudeInput);
radio.write(&altitude, sizeof(altitude));
radio.openReadingPipe(1, address[0]);
radio.startListening();
}
void InitInput() {
Serial.begin(115200);
SerialBT.begin("DRON MK1");
radio.begin();
Wire.begin();
bme.begin();
mag.begin();
escRB.attach(esc1, 1000, 2000);
escRF.attach(esc2, 1000, 2000);
escLB.attach(esc3, 1000, 2000);
escLF.attach(esc4, 1000, 2000);
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
//gyro config
Wire.beginTransmission(MPU_addr);
Wire.write(0x1B);
Wire.write(0x10);
Wire.endTransmission(true);
//accelometer config
Wire.beginTransmission(MPU_addr);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission(true);
currentGyroMillis = millis();
if (rollAccOffset == 0) {
for (int i = 0; i < 500; i++) {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 6, true);
xAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
yAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
zAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
pitchAccOffset += (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);
rollAccOffset += (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);
if (i == 499) {
rollAccOffset = rollAccOffset / 500;
pitchAccOffset = pitchAccOffset / 500;
}
}
}
if (rollGyroOffset == 0) {
for (int i = 0; i < 500; i++) {
Wire.beginTransmission(MPU_addr);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 6, true);
xGyro = (int16_t(Wire.read() << 8 | Wire.read()));
yGyro = (int16_t(Wire.read() << 8 | Wire.read()));
zGyro = (int16_t(Wire.read() << 8 | Wire.read()));
rollGyroOffset += yGyro / 32.8;
pitchGyroOffset += xGyro / 32.8;
yawGyroOffset += zGyro / 32.8;
if (i == 499) {
rollGyroOffset = rollGyroOffset / 500;
pitchGyroOffset = pitchGyroOffset / 500;
yawGyroOffset = yawGyroOffset / 500;
}
}
}
EEPROM.get(0, pitchKp);
EEPROM.get(8, pitchKi);
EEPROM.get(16, pitchKd);
Serial.println("");
Serial.println("|---------------------------------------|");
Serial.println("| |");
Serial.println("| STG |");
Serial.println("| FLIGHT |");
Serial.println("| CONTROLLER |");
Serial.println("| |");
Serial.println("|---------------------------------------|");
Serial.println("");
Serial.println("PID EEPROM VALUES");
Serial.println("");
Serial.println("-----------------");
Serial.println("");
Serial.print("pitchKp | ");
Serial.print(pitchKi);
Serial.print(" | pitchKp | ");
Serial.print(pitchKi);
Serial.print(" | pitchKd | ");
Serial.println(pitchKd);
Serial.flush();
}
void ReadSensors(double& pitchInput, double& rollInput, double& yawInput, double& altitudeInput) {
//IMU
previousGyroMillis = currentGyroMillis;
currentGyroMillis = millis();
deltaGyroTime = (currentGyroMillis - previousGyroMillis) / 1000;
//gyro
Wire.beginTransmission(MPU_addr);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 6, true);
xg = (int16_t(Wire.read() << 8 | Wire.read()));
yg = (int16_t(Wire.read() << 8 | Wire.read()));
zg = (int16_t(Wire.read() << 8 | Wire.read()));
xGyro = alpha * xg + (1 - alpha) * xGyro;
yGyro = alpha * yg + (1 - alpha) * yGyro;
zGyro = alpha * zg + (1 - alpha) * zGyro;
xGyro = (xGyro / 32.8) - pitchGyroOffset;
yGyro = (yGyro / 32.8) - rollGyroOffset;
zGyro = (zGyro / 32.8) - yawGyroOffset;
pitchInputGyro = xGyro * deltaGyroTime;
rollInputGyro = yGyro * deltaGyroTime;
yawInputGyro = zGyro * deltaGyroTime;
//accelometer
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 6, true);
xa = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
ya = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
za = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
xAcc = alpha * xa + (1 - alpha) * xAcc;
yAcc = alpha * ya + (1 - alpha) * yAcc;
zAcc = alpha * za + (1 - alpha) * zAcc;
pitchInputAcc = (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - pitchAccOffset;
rollInputAcc = (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - rollAccOffset;
//complementary filter
rollInput2 = 0.9 * (rollInput2 + rollInputGyro) + 0.1 * (rollInputAcc);
pitchInput2 = 0.9 * (pitchInput2 + pitchInputGyro) + 0.1 * (pitchInputAcc);
//kalman's pitch & roll filter
float xPredRoll = xRoll;
float pPredRoll = pRoll + QRoll;
float xPredPitch = xPitch;
float pPredPitch = pPitch + QPitch;
// Výpočet Kalmanova zesílení
kRoll = pPredRoll / (pPredRoll + RRoll);
kPitch = pPredPitch / (pPredPitch + RPitch);
// Korekce stavu
xRoll = xPredRoll + kRoll * (rollInput2 - xPredRoll);
xPitch = xPredPitch + kPitch * (pitchInput2 - xPredPitch);
// Aktualizace kovariance stavu
pRoll = (1 - kRoll) * pPredRoll;
pPitch = (1 - kPitch) * pPredPitch;
pitchInput = xRoll;
rollInput = xPitch;
// Plot DATA
/*//Serial.print("pitchInputKalman:");
Serial.print(xRoll);
Serial.print(",");
//Serial.print("rollInput:");
Serial.print(xPitch);
Serial.println(" ");*/
//compass
sensors_event_t event;
mag.getEvent(&event);
heading = atan2(event.magnetic.y, event.magnetic.x);
heading -= declinationAngle;
if (heading > 0) heading -= 2 * PI;
if (heading < 0) heading += 2 * PI;
heading *= 180 / PI;
//compass heading reset algorithm
if (heading < headingOffset) {
yawInput = 360 - (headingOffset - heading);
} else {
yawInput = heading - headingOffset;
}
yawInput = (int(yawInput + 180) % 360) - 180;
if (yawSetpoint != 0 || counter == 1) {
if (yawSetpoint >= (yawInput - 30) && yawSetpoint <= (yawInput + 30) || counter == 1) headingOffset = heading;
}
yawInput = lastYawInput * 0.8 + yawInput * 0.2;
lastYawInput = yawInput;
//Heading calculation
/*Serial.print("Offset | ");
Serial.print(headingOffset);
Serial.print(" | Setpoint | ");
Serial.print(yawSetpoint);
Serial.print(" | Input ");
Serial.println(yawInput); */
//altitudemeter
//bmp280
float temp(NAN), hum(NAN), pres(NAN);
BME280::TempUnit tempUnit(BME280::TempUnit_Celsius);
BME280::PresUnit presUnit(BME280::PresUnit_Pa);
bme.read(pres, temp, hum, tempUnit, presUnit);
pres = pres / 100;
counter += 1;
//kalman's altitude filter
float xPredTemp = xTemp;
float pPredTemp = pTemp + QTemp;
float xPredPres = xPres;
float pPredPres = pPres + QPres;
// Výpočet Kalmanova zesílení
kTemp = pPredTemp / (pPredTemp + RTemp);
kPres = pPredPres / (pPredPres + RPres);
// Korekce stavu
xTemp = xPredTemp + kTemp * (temp - xPredTemp);
xPres = xPredPres + kPres * (pres - xPredPres);
// Aktualizace kovariance stavu
pTemp = (1 - kTemp) * pPredTemp;
pPres = (1 - kPres) * pPredPres;
//barometric equation
altitudeInput = (((pow((1013.15 / xPres), 1 / 5.257) - 1) * (xTemp + 273.15)) / 0.0065);
if (altitudeInputOffset == 0 && counter == 100) {
altitudeInputOffset = altitudeInput;
}
altitudeInput -= altitudeInputOffset;
altitudeInput = constrain(altitudeInput, 0, 1000);
//IMU output
/*Serial.print("Pitch | ");
Serial.print(pitchInput);
Serial.print(" | Roll | ");
Serial.print(rollInput);
Serial.print(" | Yaw | ");
Serial.print(yawInput);
Serial.print(" | Altitude ");
Serial.println(altitudeInput);*/
}
PID function code only:
double CalculatePID(double Input, double Kp, double Ki, double Kd, double Setpoint, double &LastError, double &Integral, double OutputMin, double OutputMax) {
double error = Setpoint - Input;
// Anti-windup: back-calculation
if (Integral > OutputMax) {
Integral = OutputMax;
} else if (Integral < OutputMin) {
Integral = OutputMin;
}
double derivative = (error - LastError) / interval;
double Output = Kp * error + Ki * Integral + Kd * derivative;
// Anti-windup: back-calculation
if (Output > OutputMax) {
Integral -= (Output - OutputMax) / Ki;
Output = OutputMax;
} else if (Output < OutputMin) {
Integral -= (Output - OutputMin) / Ki;
Output = OutputMin;
}
LastError = error;
Integral += error * interval;
/*Serial.print("error | ");
Serial.print(error);
Serial.print(" | Setpoint | ");
Serial.print(Setpoint);
Serial.print(" | Input ");
Serial.println(Input);*/
// Plot DATA
/*Serial.print("PTerm:");
Serial.print(Kp * error);
Serial.print(",");
Serial.print("ITerm:");
Serial.print(Ki * Integral);
Serial.print(",");
Serial.print("DTerm:");
Serial.print(Kd * derivative);
Serial.print(",");*/
if (tunning) {
//Serial.print("Output:");
Serial.print(Output);
Serial.print(",");
//Serial.print("Setpoint:");
Serial.print(Setpoint);
Serial.println(" ");
}
return Output;
}
ReadSensors function only:
void ReadSensors(double& pitchInput, double& rollInput, double& yawInput, double& altitudeInput) {
//IMU
previousGyroMillis = currentGyroMillis;
currentGyroMillis = millis();
deltaGyroTime = (currentGyroMillis - previousGyroMillis) / 1000;
//gyro
Wire.beginTransmission(MPU_addr);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 6, true);
xg = (int16_t(Wire.read() << 8 | Wire.read()));
yg = (int16_t(Wire.read() << 8 | Wire.read()));
zg = (int16_t(Wire.read() << 8 | Wire.read()));
xGyro = alpha * xg + (1 - alpha) * xGyro;
yGyro = alpha * yg + (1 - alpha) * yGyro;
zGyro = alpha * zg + (1 - alpha) * zGyro;
xGyro = (xGyro / 32.8) - pitchGyroOffset;
yGyro = (yGyro / 32.8) - rollGyroOffset;
zGyro = (zGyro / 32.8) - yawGyroOffset;
pitchInputGyro = xGyro * deltaGyroTime;
rollInputGyro = yGyro * deltaGyroTime;
yawInputGyro = zGyro * deltaGyroTime;
//accelometer
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 6, true);
xa = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
ya = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
za = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0;
xAcc = alpha * xa + (1 - alpha) * xAcc;
yAcc = alpha * ya + (1 - alpha) * yAcc;
zAcc = alpha * za + (1 - alpha) * zAcc;
pitchInputAcc = (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - pitchAccOffset;
rollInputAcc = (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - rollAccOffset;
//complementary filter
rollInput2 = 0.9 * (rollInput2 + rollInputGyro) + 0.1 * (rollInputAcc);
pitchInput2 = 0.9 * (pitchInput2 + pitchInputGyro) + 0.1 * (pitchInputAcc);
//kalman's pitch & roll filter
float xPredRoll = xRoll;
float pPredRoll = pRoll + QRoll;
float xPredPitch = xPitch;
float pPredPitch = pPitch + QPitch;
// Výpočet Kalmanova zesílení
kRoll = pPredRoll / (pPredRoll + RRoll);
kPitch = pPredPitch / (pPredPitch + RPitch);
// Korekce stavu
xRoll = xPredRoll + kRoll * (rollInput2 - xPredRoll);
xPitch = xPredPitch + kPitch * (pitchInput2 - xPredPitch);
// Aktualizace kovariance stavu
pRoll = (1 - kRoll) * pPredRoll;
pPitch = (1 - kPitch) * pPredPitch;
pitchInput = xRoll;
rollInput = xPitch;
// Plot DATA
/*//Serial.print("pitchInputKalman:");
Serial.print(xRoll);
Serial.print(",");
//Serial.print("rollInput:");
Serial.print(xPitch);
Serial.println(" ");*/
//compass
sensors_event_t event;
mag.getEvent(&event);
heading = atan2(event.magnetic.y, event.magnetic.x);
heading -= declinationAngle;
if (heading > 0) heading -= 2 * PI;
if (heading < 0) heading += 2 * PI;
heading *= 180 / PI;
//compass heading reset algorithm
if (heading < headingOffset) {
yawInput = 360 - (headingOffset - heading);
} else {
yawInput = heading - headingOffset;
}
yawInput = (int(yawInput + 180) % 360) - 180;
if (yawSetpoint != 0 || counter == 1) {
if (yawSetpoint >= (yawInput - 30) && yawSetpoint <= (yawInput + 30) || counter == 1) headingOffset = heading;
}
yawInput = lastYawInput * 0.8 + yawInput * 0.2;
lastYawInput = yawInput;
//Heading calculation
/*Serial.print("Offset | ");
Serial.print(headingOffset);
Serial.print(" | Setpoint | ");
Serial.print(yawSetpoint);
Serial.print(" | Input ");
Serial.println(yawInput); */
//altitudemeter
//bmp280
float temp(NAN), hum(NAN), pres(NAN);
BME280::TempUnit tempUnit(BME280::TempUnit_Celsius);
BME280::PresUnit presUnit(BME280::PresUnit_Pa);
bme.read(pres, temp, hum, tempUnit, presUnit);
pres = pres / 100;
counter += 1;
//kalman's altitude filter
float xPredTemp = xTemp;
float pPredTemp = pTemp + QTemp;
float xPredPres = xPres;
float pPredPres = pPres + QPres;
// Výpočet Kalmanova zesílení
kTemp = pPredTemp / (pPredTemp + RTemp);
kPres = pPredPres / (pPredPres + RPres);
// Korekce stavu
xTemp = xPredTemp + kTemp * (temp - xPredTemp);
xPres = xPredPres + kPres * (pres - xPredPres);
// Aktualizace kovariance stavu
pTemp = (1 - kTemp) * pPredTemp;
pPres = (1 - kPres) * pPredPres;
//barometric equation
altitudeInput = (((pow((1013.15 / xPres), 1 / 5.257) - 1) * (xTemp + 273.15)) / 0.0065);
if (altitudeInputOffset == 0 && counter == 100) {
altitudeInputOffset = altitudeInput;
}
altitudeInput -= altitudeInputOffset;
altitudeInput = constrain(altitudeInput, 0, 1000);
//IMU output
/*Serial.print("Pitch | ");
Serial.print(pitchInput);
Serial.print(" | Roll | ");
Serial.print(rollInput);
Serial.print(" | Yaw | ");
Serial.print(yawInput);
Serial.print(" | Altitude ");
Serial.println(altitudeInput);*/
}
did you ever measure how many microseconds requesting the sensor-values need ?
add assigning time-stamps of function micros() to your code and if one cycle has run through calulate and print the differencies
for each request
example
and at the botton calculate and print all differencies
Serial.print("Time1=");
Serial.println(EndTime1 - StartTime1);
do this for all requests
additionally calculate how long a complete cycle needs time
best regards Stefan
you may be interested in the lecture series which include PID control and use in drones.
Anti-windup for PID control | Understanding PID Control, Part 2, but looks at others
Thanks, but I've already seen the whole series, it's great stuff!
I followed your instructions and found out that it takes 920 to 940 microseconds (together about 1.8 ms) to read and store the data from the accelerometer and gyroscope sensors and 5 ms for the whole ReadSensors function.
I'm in a state where I don't know what to do. Do you have any ideas?
So with the code in #107, you've got an interval=20ms control loop, sensing takes you maybe 7ms per #111, and response is slow.
Can you tune a P-only PID to be quicker?
I see that this bit of anti-windup isn't robust against Ki=0:
Maybe wrap it in some if (Ki > 0){...} protection.
Looking at the video, it seems like you should hit max speed quicker. With Kp = 0.3, it takes 100° of pitch error to deliver the full thrust output of 30.
Maybe try something like:
Kp = 3; Ki = 0; Kd = 0;
and see if 10° of pitch error delivers the +/-30 pitchOutputMin/Max and it corrects (overcorrects) quicker?
Is the full +/-30 thrust/correction torque at -/+10° error too aggressive?
For faster response I changed the interval ( now interval = 10 ms).
I did as you advised:
if (Ki > 0) {
if (Output > OutputMax) {
Integral -= (Output - OutputMax) / Ki;
Output = OutputMax;
} else if (Output < OutputMin) {
Integral -= (Output - OutputMin) / Ki;
Output = OutputMin;
}
}
The result was that the drone behaved extremely aggressively due to the combination of the high Kp and the unlimited Output value.
Oh, I thought you were limiting it elsewhere since you'd commented out the limits within the function. Maybe this:
if (Output > OutputMax) {
if( Ki > 0 ) { // undo integration
Integral -= (Output - OutputMax) / Ki;
}
Output = OutputMax;
} else if (Output < OutputMin) {
if( Ki > 0 ) { // undo integration
Integral -= (Output - OutputMax) / Ki;
}
Output = OutputMin;
}
What I was aiming for with Kp=3, Ki =0, Kd=0 was saturation at the limits of the control with a tighter proportional band around the setpoint than +/-100°. If +/-10° is too aggressive/stiff, maybe Kp=1.0 for +/-30°? Or some other Kp between "extremely aggressive" and "late response" that might behave more to your liking?
I limited the Output and went to test it. The result was again not good. I recorded a video of drone's behaviour.
That's not the problem, I've tried multiple values even when the ropes were fine and it resulted in overcorrections just like in the video.
sorry to chime in so late, haven't read much of the thread, but have you tried simulating the code to see it's response?
of course you'll need to simulate the aerodynamic characteristics and it may be more helpful to use for better understanding them
If you are twisting the right knob, there ought to be an optimal point somewhere between too aggressive and too slow.
Does this mean that my control function is written correctly and now it's just a matter of debugging? I was also wondering if the error is caused by oscillations in the IMU measurements. I am using a kalman filter, but still the results are not perfect during motors vibration. I have used a kitchen sponge to dampen them, which has helped, but I would like a better result.