So I'm making a quadcopter using NRF24L01 reciver, 4 ESC 30A, 4 brushless motors, arduino nano. My drone weight is about 700-750 grams. I'm using 8045 propellers. I have around 1-1.1kg thrust per motor. This is my code.
#include <Servo.h>
#include <Wire.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;
int throttle1, throttle2;
int roll1;
int pitch1;
int yaw1;
float RateRoll, RatePitch, RateYaw;
float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
int RateCalibrationNumber;
uint32_t LoopTimer;
float DesiredRateRoll, DesiredRatePitch,DesiredRateYaw;
float ErrorRateRoll, ErrorRatePitch, ErrorRateYaw;
float InputRoll, InputThrottle, InputPitch, InputYaw;
float PrevErrorRateRoll, PrevErrorRatePitch, PrevErrorRateYaw;
float PrevItermRateRoll, PrevItermRatePitch, PrevItermRateYaw;
float PIDReturn[]={0, 0, 0};
float PRateRoll=0.6; float PRatePitch=PRateRoll; float PRateYaw=2;
float IRateRoll=3.5; float IRatePitch=IRateRoll; float IRateYaw=12;
float DRateRoll=0.03; float DRatePitch=DRateRoll; float DRateYaw=0;
float MotorInput1, MotorInput2, MotorInput3, MotorInput4;
float AccX, AccY, AccZ;
float AngleRoll, AnglePitch;
float KalmanAngleRoll=0, KalmanUncertaintyAngleRoll=2*2;
float KalmanAnglePitch=0, KalmanUncertaintyAnglePitch=2*2;
float Kalman1DOutput[]={0,0};
float DesiredAngleRoll, DesiredAnglePitch;
float ErrorAngleRoll, ErrorAnglePitch;
float PrevErrorAngleRoll, PrevErrorAnglePitch;
float PrevItermAngleRoll, PrevItermAnglePitch;
float PAngleRoll=2; float PAnglePitch=PAngleRoll;
float IAngleRoll=0; float IAnglePitch=IAngleRoll;
float DAngleRoll=0; float DAnglePitch=DAngleRoll;
void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) {
KalmanState=KalmanState+0.004*KalmanInput;
KalmanUncertainty=KalmanUncertainty + 0.004 * 0.004 * 4 * 4;
float KalmanGain=KalmanUncertainty * 1/(1*KalmanUncertainty + 3 * 3);
KalmanState=KalmanState+KalmanGain * (KalmanMeasurement-KalmanState);
KalmanUncertainty=(1-KalmanGain) * KalmanUncertainty;
Kalman1DOutput[0]=KalmanState;
Kalman1DOutput[1]=KalmanUncertainty;
}
RF24 radio(7, 8); // CE, CSN
const byte address[6] = "00001";
struct Data_Package{
byte roll;
byte pitch;
byte throttle;
byte yaw;
};
Data_Package data;
unsigned long lastReceiveTime = 0;
unsigned long currentTime = 0;
void gyro_signals(void) {
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
int16_t AccXLSB = Wire.read() << 8 | Wire.read();
int16_t AccYLSB = Wire.read() << 8 | Wire.read();
int16_t AccZLSB = Wire.read() << 8 | Wire.read();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x8);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
int16_t GyroX=Wire.read()<<8 | Wire.read();
int16_t GyroY=Wire.read()<<8 | Wire.read();
int16_t GyroZ=Wire.read()<<8 | Wire.read();
RateRoll=(float)GyroX/65.5;
RatePitch=(float)GyroY/65.5;
RateYaw=(float)GyroZ/65.5;
AccX=(float)AccXLSB/4096 - 0.03;
AccY=(float)AccYLSB/4096 + 0.01;
AccZ=(float)AccZLSB/4096 - 0.03;
AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*AccZ))*1/(3.142/180);
AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*AccZ))*1/(3.142/180);
}
void pid_equation(float Error, float P , float I, float D, float PrevError, float PrevIterm) {
float Pterm=P*Error;
float Iterm=PrevIterm+I*(Error+PrevError)*0.004/2;
if (Iterm > 400) Iterm=400;
else if (Iterm <-400) Iterm=-400;
float Dterm=D*(Error-PrevError)/0.004;
float PIDOutput= Pterm+Iterm+Dterm;
if (PIDOutput>400) PIDOutput=400;
else if (PIDOutput <-400) PIDOutput=-400;
PIDReturn[0]=PIDOutput;
PIDReturn[1]=Error;
PIDReturn[2]=Iterm;
}
void reset_pid(void) {
PrevErrorRateRoll=0; PrevErrorRatePitch=0; PrevErrorRateYaw=0;
PrevItermRateRoll=0; PrevItermRatePitch=0; PrevItermRateYaw=0;
PrevErrorAngleRoll=0; PrevErrorAnglePitch=0;
PrevItermAngleRoll=0; PrevItermAnglePitch=0;
}
void setup() {
Serial.begin(57600);
esc1.attach(6);
esc2.attach(5);
esc3.attach(3);
esc4.attach(4);
radio.begin();
radio.openReadingPipe(0, address);
radio.setAutoAck(false);
radio.setDataRate(RF24_2MBPS);
radio.setPALevel(RF24_PA_LOW);
radio.startListening();
Wire.setClock(400000);
Wire.begin();
delay(250);
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
for (RateCalibrationNumber=0;
RateCalibrationNumber<2000;
RateCalibrationNumber ++) {
gyro_signals();
RateCalibrationRoll+=RateRoll;
RateCalibrationPitch+=RatePitch;
RateCalibrationYaw+=RateYaw;
delay(1);
}
RateCalibrationRoll/=2000;
RateCalibrationPitch/=2000;
RateCalibrationYaw/=2000;
LoopTimer=micros();
}
void loop() {
if (radio.available()) {
radio.read(&data, sizeof(Data_Package));
lastReceiveTime = millis();
}
currentTime = millis();
if ( currentTime - lastReceiveTime > 1000 ) {
};
gyro_signals();
RateRoll-=RateCalibrationRoll;
RatePitch-=RateCalibrationPitch;
RateYaw-=RateCalibrationYaw;
kalman_1d(KalmanAngleRoll, KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);
KalmanAngleRoll=Kalman1DOutput[0]; KalmanUncertaintyAngleRoll=Kalman1DOutput[1];
kalman_1d(KalmanAnglePitch, KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
KalmanAnglePitch=Kalman1DOutput[0]; KalmanUncertaintyAnglePitch=Kalman1DOutput[1];
roll1 = map(data.roll,0,180,1000,2000);
pitch1 = map(data.pitch,0,180,1000,2000);
yaw1 = map(data.yaw,0,180,1000,2000);
if(data.throttle > 80){
throttle1 = map(data.throttle,90,180,0,180);
}
throttle2 = map(throttle1,0,180,1000,2000);
DesiredAngleRoll=0.10*(roll1-1500);
DesiredAnglePitch=0.10*(pitch1-1500);
InputThrottle=throttle2;
DesiredRateYaw=0.15*(yaw1-1500);
ErrorAngleRoll=DesiredAngleRoll-KalmanAngleRoll;
ErrorAnglePitch=DesiredAnglePitch-KalmanAnglePitch;
pid_equation(ErrorAngleRoll, PAngleRoll, IAngleRoll, DAngleRoll, PrevErrorAngleRoll, PrevItermAngleRoll);
DesiredRateRoll=PIDReturn[0];
PrevErrorAngleRoll=PIDReturn[1];
PrevItermAngleRoll=PIDReturn[2];
pid_equation(ErrorAnglePitch, PAnglePitch, IAnglePitch, DAnglePitch, PrevErrorAnglePitch, PrevItermAnglePitch);
DesiredRatePitch=PIDReturn[0];
PrevErrorAnglePitch=PIDReturn[1];
PrevItermAnglePitch=PIDReturn[2];
ErrorRateRoll=DesiredRateRoll-RateRoll;
ErrorRatePitch=DesiredRatePitch-RatePitch;
ErrorRateYaw=DesiredRateYaw-RateYaw;
pid_equation(ErrorRateRoll, PRateRoll, IRateRoll, DRateRoll, PrevErrorRateRoll, PrevItermRateRoll);
InputRoll=PIDReturn[0];
PrevErrorRateRoll=PIDReturn[1];
PrevItermRateRoll=PIDReturn[2];
pid_equation(ErrorRatePitch, PRatePitch,IRatePitch, DRatePitch, PrevErrorRatePitch, PrevItermRatePitch);
InputPitch=PIDReturn[0];
PrevErrorRatePitch=PIDReturn[1];
PrevItermRatePitch=PIDReturn[2];
pid_equation(ErrorRateYaw, PRateYaw,IRateYaw, DRateYaw, PrevErrorRateYaw, PrevItermRateYaw);
InputYaw=PIDReturn[0];
PrevErrorRateYaw=PIDReturn[1];
PrevItermRateYaw=PIDReturn[2];
if (InputThrottle > 1800) InputThrottle = 1800;
MotorInput1= 1.024*(InputThrottle-InputRoll-InputPitch-InputYaw);
MotorInput2= 1.024*(InputThrottle-InputRoll+InputPitch+InputYaw);
MotorInput3= 1.024*(InputThrottle+InputRoll+InputPitch-InputYaw);
MotorInput4= 1.024*(InputThrottle+InputRoll-InputPitch+InputYaw);
if (MotorInput1 > 2000)MotorInput1 = 1999;
if (MotorInput2 > 2000)MotorInput2 = 1999;
if (MotorInput3 > 2000)MotorInput3 = 1999;
if (MotorInput4 > 2000)MotorInput4 = 1999;
int ThrottleIdle=1180;
if (MotorInput1 < ThrottleIdle) MotorInput1 = ThrottleIdle;
if (MotorInput2 < ThrottleIdle) MotorInput2 = ThrottleIdle;
if (MotorInput3 < ThrottleIdle) MotorInput3 = ThrottleIdle;
if (MotorInput4 < ThrottleIdle) MotorInput4 = ThrottleIdle;
int ThrottleCutOff=1000;
if (throttle2<1050) {
MotorInput1=ThrottleCutOff;
MotorInput2=ThrottleCutOff;
MotorInput3=ThrottleCutOff;
MotorInput4=ThrottleCutOff;
reset_pid();
}
esc1.writeMicroseconds(MotorInput1);
esc2.writeMicroseconds(MotorInput2);
esc3.writeMicroseconds(MotorInput3);
esc4.writeMicroseconds(MotorInput4);
}
When using this code, I'm having problems with flying up, it looks like motors are not going the same speed from the start. The esc is calibrated so that isnt the issue. I feel like one of my motors is not rotating properly since they are from aliexpress. It looks like it has some deviation from its axis.
This is the controllers code.
#define roll A7
#define pitch A6
#define throttle A3
#define yaw A5
int roll1,pitch1,throttle1,yaw1;
int roll2,pitch2,throttle2,yaw2;
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
RF24 radio(7, 8); // CE, CSN
const byte address[6] = "00001";
int led = 5;
struct Data_Package{
byte roll;
byte pitch;
byte throttle;
byte yaw;
};
Data_Package data;
void setup() {
Serial.begin(9600);
pinMode(led, OUTPUT);
digitalWrite(led, HIGH);
radio.begin();
radio.openWritingPipe(address);
radio.setAutoAck(false);
radio.setDataRate(RF24_2MBPS);
radio.setPALevel(RF24_PA_LOW);
}
void loop(){
roll1 = analogRead(roll);
pitch1 = analogRead(pitch);
throttle1 = analogRead(throttle);
yaw1 = analogRead(yaw);
if(roll1 > 512){
roll2 = map(roll1, 524,1023,512,1023);
}
else if(roll1 < 512){
roll2 = map(roll1, 0,499,0,512);
}
if(pitch1 < 512){
pitch2 = map(pitch1,0,502,0,512);
}
if(pitch1 > 512){
pitch2 = map(pitch1,521,1023,0,1023);
}
if(throttle1 < 512){
throttle2 = map(throttle1,0,507,0,512);
}
if(throttle1 > 512){
throttle2 = map(throttle1,516,1023,512,1023);
}
if(yaw1 < 512){
yaw2 = map(yaw1,0,506,0,512);
}
if(yaw1 > 512){
yaw2 = map(yaw1,517,1023,512,1023);
}
data.roll = map(roll2,0,1023,0,180);
data.pitch = map(pitch2,0,1023,0,180);
data.throttle = map(throttle2,0,1023,0,180);
data.yaw = map(yaw2,0,1023,0,180);
radio.write(&data, sizeof(Data_Package));
}
I had to add some extra code since when the joysticks were in the middle they werent showing 512 but 506-518. That kinda messes up with miliseconds sent to the motors.