Ardupilot or No Ardupilot. Cant lift the Quadcopter off of the ground

Is it necessary to use Ardupilot to control Navio 2 in Drone making? I am a beginner at drone making and I am using my own code to make my quadcopter fly but it isn’t stabilizing. Can anyone please help?

Whenever I am running all the four motors at 1500 pulse or 2000 pulse the drone isn’t taking off, its sliding across extreme directions and it isn’t flying.

Hardware components and their weight in grams:

  1. Navio 2
  2. Raspberry PI 3 (Navio 2 + PI) - 100g
  3. Simonk 30A 4 pieces - 92g
  4. LiPo Battery 3000MAh 4S 25C - 290g
  5. Motors 1400KV 4 pieces - 288g
  6. Quadcopter Frame - 405g
  7. Propellers 10X4.5 4(2 CCW and 2 CW)- 76g

Below is the code to takeoff the drone off the ground:
void takeoff(){
auto pwm = get_rcout();

if (check_apm()) {
        return;
    }
 if( !(pwm->initialize(PWM_OUTPUT)) ) {
        return;
    }
 if( !(pwm->initialize(PWM_OUTPUT_1)) ) {
        return;
    }
 if( !(pwm->initialize(PWM_OUTPUT_2)) ) {
        return;
    }
 if( !(pwm->initialize(PWM_OUTPUT_3)) ) {
        return;
    }
    
pwm->set_frequency(PWM_OUTPUT, 50);
pwm->set_frequency(PWM_OUTPUT_1, 50);
pwm->set_frequency(PWM_OUTPUT_2, 50);
pwm->set_frequency(PWM_OUTPUT_3, 50);

if ( !(pwm->enable(PWM_OUTPUT)) ) {
    return;
}
if ( !(pwm->enable(PWM_OUTPUT_1)) ) {
    return;
}
if ( !(pwm->enable(PWM_OUTPUT_2)) ) {
    return;
}
if ( !(pwm->enable(PWM_OUTPUT_3)) ) {
    return;
}
cout<<"Taking off.......!\n";

 if(flag==1){
        pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MIN);
    pwm->set_duty_cycle(PWM_OUTPUT_1,SERVO_MIN);
    pwm->set_duty_cycle(PWM_OUTPUT_2,SERVO_MIN);
    pwm->set_duty_cycle(PWM_OUTPUT_3,SERVO_MIN);
        sleep(1);

        pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_1, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_2, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_3, SERVO_MAX);

    flag=0;
    
}
else{
     pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_1, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_2, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_3, SERVO_MAX);
    sleep(0.5);
}

}

You are setting PWM outputs directly, without using sensors or anything.
Quadcopters are not self stabilizing. It requires at least a 3 axis gyro and PID algorithms for each axis to make a quadcopter (or any multicopter) fly.
If you want to make your copter fly, it will be much easier to use Ardupilot to start with. If you want to learn about PID algorithms and control loops, use a software or even a game (without advertising anything, Stormworks is a cool game, where you can build all kind vehicles and control them with logic blocks) to start, then build a test rig with a prop on a lever, to test how control loops work in real life.

I have initialized the gyro and the PID I am sharing the code.

#include “Common/MPU9250.h”
#include “Navio2/LSM9DS1.h”
#include “Common/Util.h”
#include <unistd.h>
#include
#include
#include <unistd.h>
#include “Navio2/PWM.h”
#include “Navio+/RCOutput_Navio.h”
#include “Navio2/RCOutput_Navio2.h”

#include “PID_v1.h”
#include
#include

#define SERVO_MIN 1000 /mS/
#define SERVO_MAX 1750 /mS/

#define PWM_OUTPUT 0
#define PWM_OUTPUT_1 1
#define PWM_OUTPUT_2 2
#define PWM_OUTPUT_3 3

using namespace Navio;
using namespace std;

void takeoff();
long double millis();
void hover(double ax, double ay, double az);

int flag=1;

std::unique_ptr get_rcout()
{
if (get_navio_version() == NAVIO2)
{
auto ptr = std::unique_ptr { new RCOutput_Navio2() };
return ptr;
} else
{
auto ptr = std::unique_ptr { new RCOutput_Navio() };
return ptr;
}

}

std::unique_ptr get_inertial_sensor( std::string sensor_name)
{
if (sensor_name == “mpu”) {
printf(“Selected: MPU9250\n”);
auto ptr = std::unique_ptr { new MPU9250() };
return ptr;
}
}

long double millis(){
long double millisecond;
millisecond = std::clock();

return millisecond;
}

int main(int argc, char *argv[])
{

if (check_apm()) {
    return 1;
}

auto sensor_name = "mpu";

auto sensor = get_inertial_sensor(sensor_name);



if (!sensor->probe()) {
    printf("Sensor not enabled\n");
    return EXIT_FAILURE;
}
sensor->initialize();

float ax, ay, az;
float gx, gy, gz;
float mx, my, mz;

//-------------------------------------------------------------------------

while(1) {
sensor->update();
sensor->read_accelerometer(&ax, &ay, &az);
sensor->read_gyroscope(&gx, &gy, &gz);
sensor->read_magnetometer(&mx, &my, &mz);
printf("Acc: %+7.3f %+7.3f %+7.3f\n ", ax, ay, az);
printf("Gyr: %+8.3f %+8.3f %+8.3f\n ", gx, gy, gz);
printf(“Mag: %+7.3f %+7.3f %+7.3f\n”, mx, my, mz);

    usleep(500000);
    


if(true){
    takeoff();
}
if(az <9.3){
    cout<<"Unstable drone......\n";
    hover(ax,ay,az);
}

}
return 0;
}

void takeoff(){
auto pwm = get_rcout();

if (check_apm()) {
        return;
    }
 if( !(pwm->initialize(PWM_OUTPUT)) ) {
        return;
    }
 if( !(pwm->initialize(PWM_OUTPUT_1)) ) {
        return;
    }
 if( !(pwm->initialize(PWM_OUTPUT_2)) ) {
        return;
    }
 if( !(pwm->initialize(PWM_OUTPUT_3)) ) {
        return;
    }
    
pwm->set_frequency(PWM_OUTPUT, 50);
pwm->set_frequency(PWM_OUTPUT_1, 50);
pwm->set_frequency(PWM_OUTPUT_2, 50);
pwm->set_frequency(PWM_OUTPUT_3, 50);

if ( !(pwm->enable(PWM_OUTPUT)) ) {
    return;
}
if ( !(pwm->enable(PWM_OUTPUT_1)) ) {
    return;
}
if ( !(pwm->enable(PWM_OUTPUT_2)) ) {
    return;
}
if ( !(pwm->enable(PWM_OUTPUT_3)) ) {
    return;
}
cout<<"Taking off.......!\n";

// while (true) {
//sleep(0.5);
if(flag==1){
pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MIN);
pwm->set_duty_cycle(PWM_OUTPUT_1,SERVO_MIN);
pwm->set_duty_cycle(PWM_OUTPUT_2,SERVO_MIN);
pwm->set_duty_cycle(PWM_OUTPUT_3,SERVO_MIN);
sleep(1);
// while(true){
pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MAX);
pwm->set_duty_cycle(PWM_OUTPUT_1, SERVO_MAX);
pwm->set_duty_cycle(PWM_OUTPUT_2, SERVO_MAX);
pwm->set_duty_cycle(PWM_OUTPUT_3, SERVO_MAX);
// sleep(1);
flag=0;

}
else{
     pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_1, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_2, SERVO_MAX);
    pwm->set_duty_cycle(PWM_OUTPUT_3, SERVO_MAX);
    sleep(0.5);
}

// sleep(1);
// }
//}
}
//PID class

//class PID{
//public:
// PID(double input, int setpoint){
// inAuto = true;
//
// SetOutputLimit(0,4096);
// lastTime = millis() - SampleTime;
// Kp = 2;
// Ki = 0.5;
// Kd = 0.05;
// Compute(input,Setpoint);
// }
//
// void SetOutputLimit(int min, int max){
// if(inAuto){
// if(Output > max) Output = max;
// else if (Output < min) Output = min;
// }
// }
//
// bool Compute(double input, double setpoint){
// unsigned long now;
//
// now = millis();
//
// unsigned long timeChange = now - lastTime;
// if(!inAuto) return false;
// if(timeChange >= SampleTime){
// double in = input;
// double error =setpoint - in;
// outputSum += (Ki * error);
//
// if(outputSum > 4096) outputSum = 4096;
// else if(outputSum < 0) outputSum =0;
// double dInput = (in - lastInput);
//
// double output = Kp * error + outputSum - Kd * dInput;
//
// if(output > 4096) output = 4096;
// else if(output < 0) output =0;
//
// Output = output;
//
// lastInput = in;
// lastTime = now;
// return true;
//
// }
// else return false;
// }
//};
void hover(double ax, double ay, double az){
flag=1;
double Out_x, Out_y, Out_z;

double Output,Input, Kp, Ki, Kd,Setpoint;
double Set,ControllerDirection=0;

Kp = 2;
Ki = 0.5;
Kd = 0.05;

cout<<"Hovering......!\n";
//---------------------------------
Set = 1;
Input = az;
Setpoint = Set;
PID PID_z(&Input,&Output,&Setpoint,Kp,Ki,Kd,ControllerDirection);
Output = ((Output/4096)*.35)+1.4;
Out_z = Output;
//---------------------------------
Set = 0;
Input = ax;
Setpoint = Set;
//PID PID_x(Input,Setpoint);
PID PID_x(&Input,&Output,&Setpoint,Kp,Ki,Kd,ControllerDirection);
Output = ((Output/4096)*.35)+1.4;
Out_x = Output;
//---------------------------------
Set = 0;
Input = ay;
Setpoint = Set;
//PID PID_y(Input,Setpoint);
PID PID_y(&Input,&Output,&Setpoint,Kp,Ki,Kd,ControllerDirection);
Output = ((Output/4096)*.35)+1.4;
Out_y = Output;

if(ax<-0.8 && ay>0.8){
    cout<<"Motor 2 is Activated!\n";
    cout<<Out_x<<" "<<Out_y<<" "<<Out_z<<endl;

// pwm->set_duty_cycle(PWM_OUTPUT, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_1, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_2, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_3, 1.4);
}
else if(ax>0.8 && ay<-0.8){
cout<<“Motor 4 is Activated!\n”;
cout<<Out_x<<" “<<Out_y<<” “<<Out_z<<endl;
// pwm->set_duty_cycle(PWM_OUTPUT, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_1, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_2, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_3, 1.4);
}
else if(ax<-0.8 && ay<-0.8){
cout<<“Motor 1 is Activated!\n”;
cout<<Out_x<<” “<<Out_y<<” “<<Out_z<<endl;
// pwm->set_duty_cycle(PWM_OUTPUT, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_1, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_2, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_3, 1.4);
}
else if(ax>0.8 && ay>0.8){
cout<<“Motor 3 is Activated!\n”;
cout<<Out_x<<” “<<Out_y<<” “<<Out_z<<endl;
// pwm->set_duty_cycle(PWM_OUTPUT, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_1, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_2, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_3, 1.4);
}
else if(ay>0.8){
cout<<“Motor 3 and 2 Activated!\n”;
cout<<Out_x<<” “<<Out_y<<” “<<Out_z<<endl;
// pwm->set_duty_cycle(PWM_OUTPUT, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_1, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_2, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_3, 1.4);
}
else if(ay<-0.8){
cout<<“Motor 4 and 1 Activated!\n”;
cout<<Out_x<<” “<<Out_y<<” “<<Out_z<<endl;
// pwm->set_duty_cycle(PWM_OUTPUT, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_1, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_2, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_3, 1.4);
}
else if(ax<-0.8){
cout<<“Motor 2 and 1 Activated!\n”;
cout<<Out_x<<” “<<Out_y<<” "<<Out_z<<endl;
// pwm->set_duty_cycle(PWM_OUTPUT, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_1, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_2, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_3, 1.4);

}
else if(ax>0.8){
    cout<<"Motor 3 and 4 Activated!\n";
      cout<<Out_x<<" "<<Out_y<<" "<<Out_z<<endl;

// pwm->set_duty_cycle(PWM_OUTPUT, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_1, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_2, 1.4);
// pwm->set_duty_cycle(PWM_OUTPUT_3, 1.4);
}
}

PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int ControllerDirection)
{

myOutput = Output;
myInput = Input;
mySetpoint = Setpoint;
inAuto = true;

PID::Initialize();
PID::SetOutputLimits(0, 4096);				//default output limit corresponds to 
											//the arduino pwm limits


SampleTime = 1;							//default Controller Sample Time is 0.1 seconds
lastTime = millis()-SampleTime;		
PID::SetControllerDirection(ControllerDirection);

PID::SetTunings(Kp, Ki, Kd);



PID::Compute();

}

void PID::SetOutputLimits(double Min, double Max)
{
if(Min >= Max) return;
outMin = Min;
outMax = Max;

if(inAuto)
{
if(*myOutput > outMax) *myOutput = outMax;
else if(*myOutput < outMin) *myOutput = outMin;

   if(ITerm > outMax) ITerm= outMax;
   else if(ITerm < outMin) ITerm= outMin;

}
}

void PID::SetControllerDirection(int Direction)
{
if(inAuto && Direction !=controllerDirection)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
controllerDirection = Direction;
}

void PID::SetTunings(double Kp, double Ki, double Kd)
{
if (Kp<0 || Ki<0 || Kd<0) return;

dispKp = Kp; dispKi = Ki; dispKd = Kd;

double SampleTimeInSec = ((double)SampleTime)/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;

if(controllerDirection ==REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}

void PID::Initialize()
{
ITerm = *myOutput;
lastInput = *myInput;
if(ITerm > outMax) ITerm = outMax;
else if(ITerm < outMin) ITerm = outMin;
}

bool PID::Compute()
{
if(!inAuto) return false;
unsigned long now = millis();
unsigned long timeChange = (now - lastTime);
if(timeChange>=SampleTime)
{
/Compute all the working error variables/
double input = *myInput;
double error = *mySetpoint - input;
ITerm+= (ki * error);
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
double dInput = (input - lastInput);

  /*Compute PID Output*/
  double output = kp * error + ITerm- kd * dInput;
  
  if(output > outMax) output = outMax;
  else if(output < outMin) output = outMin;
  *myOutput = output;
  
  /*Remember some variables for next time*/
  lastInput = input;
  lastTime = now;
  return true;

}
else return false;
}

I was using Ardupilot and calibrated my drone’s acceleration and gyroscope using mission planner, after that I was trying to give input through running the code that i shared while the Ardupilot still on. Can you suggest something? I can share the video of my drone trying to take off.

I only had a brief look, but I did not find where you do the actual mixing for the motor outputs. The only thing I found, are the “if” statements near the end
with “motor X activated” etc. Are you trying to do the “mixing” with “if” statements?

Yes I am sending the current Output pulse to the pid and getting the value for x, y and z axis every time there is a change along the z axis of the quadcopter it will try to lift it up.

This is not going to work. Stabilizing a multicopter is a task which requires very fast calculations. The first sensors you should use are the gyros, because the accelerometers are to slow and noisy. The gyro stabilizition itself should only take a few lines of code. Mixing is done by adding and subtracting the PID outputs for every axis, so each motor gets the correct output.
Sleep and print commands slow down the calculations too much, you have to avoid them completly.
I think my first post is still the way to go, to learn the basics.
And I hope you are not doing your experiments with a free flying copter. This would be very dangerous. Tether the copter or you will hurt yourself or others. Props cut deep!

1 Like

Thank you so much. Just one last thing, I don’t have a Transmitter and Receiver and I want to give PWM signal to my drone through running codes. I will do all the calibration and through Mission Planner and while Ardupilot still running, I want to give them the PWM signal just like the Transmitter would have? does that make any difference? Not having a Transmitter.

As far as I know, the calibration values/offsets are stored in parameters. They do not get stored in the sensors and in case of the gyros the calibration is done by Ardupilot every time the copter is powered up. (Anybody, please correct me, if this is wrong). So, if it is like I said, you will not gain anything by doing a calibration with Ardupilot. You will have to do the calibration in your code.
Even if you are using Ardupilot/Arducopter, it is not easy to setup and tune a copter without a transmitter. Many countries and/or insurance companies require you to be able to take control of your vehicle at any time, via a RC transmitter.
I really think you are trying to do too much at once. Break down your project into several smaller goals and projects and if you complete them, add them together to achieve the end goal.

1 Like

I shall do that.But if the Quad needs calibration every-time I power it, in my code then why are we configuring it using Arducopter/Ardupilot in the first place? Sorry it’s just I am unable to understand the difference between Ardupilot and manually trying to fly the quad. I have this image of calibrating the quad using any GCS, reboot it and then while the Ardupilot still running, try execute my flight codes.

In Mission Planner we do:

  1. Acceleration Calibration
  2. Gyro Calibration
  3. ESCs Calibration

after doing all this, I reboot my Navio 2, then I try to run my Flight algorithm. If nothing is saved then why are we doing the Calibrations? Sorry I just have this major doubts(what are the things which are already available for me i.e the open source softwares which I can use and the things that I have to write codes for every-time I have to fly the quad).

The calibration gets saved, just not in a way you can use in your code. In case of the Navio Ardupilot/copter saves a .stm file on the SD-card were all parameters are saved. Every time the Arducopter executable is executed, it loads the parameters from that file, performs a gyro calibration and saves the new gyro offsets. You also can not access the sensors with your code, while Arducopter is running. It does not work at all or Arducopter or your code crashes.
There is no “manual” flying a copter. It always needs some kind of stabilization on all three axes of rotation. The most “manual” way to fly a copter is called “Acro” mode for most flight softwares. It only uses the gyros. The flightcontroller only controls rotation rates, given by the pilot through transmitter input. There is no self leveling, position hold or other fanciness.
The next step would be “Stabilized” mode, adding the accelerometers to the calculations. Here the pilot controls pitch and roll angles, not rotation rates with the transmitter sticks. Yaw is still rate controlled. This is normally done with an outer and inner PID loop, the inner loop still controls the rotation rates, getting its input from the outer loop, which controls the roll/pitch angles.
This is the basic flight control, everything after that (GPS, optical flow, waypoint missions etc.) generates input for the basic flight control.

I made a program that sets the gyro offsets everytime the quad is turned on, now whenever I am throttling the motors the quad isn’t going up, rather it’s dragging from left to right or right to left, PID isn’t working in a very fast rate. I don’t know what problem it’s facing now. PID is supposed to keep my drone at fixed position

Without looking at the code, I can not say what might be wrong.

#include “Common/MPU9250.h”
#include “Navio2/LSM9DS1.h”
#include “Common/Util.h”
#include <unistd.h>
#include
#include
#include <unistd.h>
#include “Navio2/PWM.h”
#include “Navio+/RCOutput_Navio.h”
#include “Navio2/RCOutput_Navio2.h”

#include “PID_v1.h”
#include
#include
#include

//#define SERVO_MIN 1000 /mS/
//#define SERVO_MAX 1070 /mS/

/----- THIS IS FROM THE PAPER ------/
// DEFINING THE THROTTLE, PITCH, ROLL AND YAW
#define YAW 0
#define PITCH 1
#define ROLL 2
#define THROTTLE 3

//DEFINING THE AXIS FROM THE PAPER
#define X 0
#define Y 1
#define Z 2

#define PI 3.14

//Sampling Frequency and Sensitivity Scale factor of the MPU9250 from the datasheet
#define FREQ 250
#define SSF_GYRO 65.5

//-------MPU variables-----------
//degree per second refresh rate
int gyro_raw[3] = {0,0,0};

//average gyro offsets of the each axis in order X,Y and Z
long gyro_offset[3] = {0,0,0};

//calculate the angles from the gyro’s axis in order X,Y and Z
float gyro_angle[3] = {0,0,0};

//the raw values from the accelemeter in m/sec^2 in order X,Y and Z
int acc_raw[3] = {0,0,0};

//calculate angles fromt he accelometer’s values in that order X,Y and Z
float acc_angle[3] = {0,0,0};

//total 3D acceleration in m/sec^2
long acc_total_vector;

unsigned long pulse_length_esc1 = 1000, pulse_length_esc2 = 1000, pulse_length_esc3 = 1000, pulse_length_esc4 = 1000;

volatile unsigned int pulse_length[4] = {1500, 1500, 1000, 1500};

int mode_mapping[4];
//INIT flag set to true afte 1st loop
bool initialized = false;

float measures[3] = {0,0,0};
float errors[3];
float error_sum[3] = {0,0,0};
float previous_error[3] = {0,0,0};
/* from the papers Ends */

#define PWM_OUTPUT 0
#define PWM_OUTPUT_1 1
#define PWM_OUTPUT_2 2
#define PWM_OUTPUT_3 3

using namespace Navio;
using namespace std;

void takeoff();
long double millis();
void hover(double ax, double ay, double az);

void calibrateMpu9250();
void readSensor();
void configureChannelMapping();
void calculateErrors();

void calculateAngles();
void calculateGyroAngles();
void calculateAccAngles();

void getFlightInstructions();

void resetGyroAngles();
void pidController();

long map(long x, long in_min, long in_max, long out_min, long out_max);
float minMax(float value, float min_value, float max_value);

bool isStarted();
void resetPID();
void applyMotorSpeed();
//used to calculate pulse duration on each channel
volatile unsigned long current_time;
volatile unsigned long timer[4];
unsigned int period;
unsigned long loop_timer;
unsigned long now,difference;

int flag=1;
float instructions[4];
//double Output,Input, Kp, Ki, Kd,outputSum, lastInput=0;
//int Setpoint=0, ControllerDirection=0;
//long lastTime;
//long SampleTime=1;
//bool inAuto;

std::unique_ptr get_rcout()
{
if (get_navio_version() == NAVIO2)
{
auto ptr = std::unique_ptr { new RCOutput_Navio2() };
return ptr;
} else
{
auto ptr = std::unique_ptr { new RCOutput_Navio() };
return ptr;
}

}

std::unique_ptr get_inertial_sensor( std::string sensor_name)
{
if (sensor_name == “mpu”) {
// printf(“Selected: MPU9250\n”);
auto ptr = std::unique_ptr { new MPU9250() };
return ptr;
}
}

long double millis(){
long double millisecond;
millisecond = std::clock();

return millisecond;
}
auto sensor_name = “mpu”;
auto sensor = get_inertial_sensor(sensor_name);

float ax, ay, az;
float gx, gy, gz;
auto pwm = get_rcout();

int main(int argc, char *argv[])
{

if (check_apm()) {
    return 1;
}
if( !(pwm->initialize(PWM_OUTPUT)) ) {
        return 1;
    }
 if( !(pwm->initialize(PWM_OUTPUT_1)) ) {
        return 1;
    }
 if( !(pwm->initialize(PWM_OUTPUT_2)) ) {
        return 1;
    }
 if( !(pwm->initialize(PWM_OUTPUT_3)) ) {
        return 1;
    }

pwm->set_frequency(PWM_OUTPUT, FREQ);
pwm->set_frequency(PWM_OUTPUT_1, FREQ);
pwm->set_frequency(PWM_OUTPUT_2, FREQ);
pwm->set_frequency(PWM_OUTPUT_3, FREQ);

if ( !(pwm->enable(PWM_OUTPUT)) ) {
    return 1;
}
if ( !(pwm->enable(PWM_OUTPUT_1)) ) {
    return 1;
}
if ( !(pwm->enable(PWM_OUTPUT_2)) ) {
    return 1;
}
if ( !(pwm->enable(PWM_OUTPUT_3)) ) {
    return 1;
}

cout<<“THROTTLE down!\n”;
pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MIN);
pwm->set_duty_cycle(PWM_OUTPUT_1,SERVO_MIN);
pwm->set_duty_cycle(PWM_OUTPUT_2,SERVO_MIN);
pwm->set_duty_cycle(PWM_OUTPUT_3,SERVO_MIN);
sleep(1);

if (!sensor->probe()) {
    printf("Sensor not enabled\n");
    return EXIT_FAILURE;
}
sensor->initialize();// initializing the sensor


calibrateMpu9250();
// cout<<"gyro_offset: "<<gyro_offset[X]<<" "<<gyro_offset[Y]<<" "<<gyro_offset[Z]<<endl;

// cout<<“X: “<<gyro_offset[0]<<”\n”<<“Y: “<<gyro_offset[1]<<”\n”<<"Z: "<<gyro_offset[2];

configureChannelMapping();
period = (1000000/FREQ);
loop_timer = millis();

// cout<<"Gyro Angle: "<<endll;

while(1){
  readSensor(); //calling function to get the raw acc and gyro data

  calculateAngles();


  getFlightInstructions();
  calculateErrors();
  // cout<<acc_angle[X]<<" "<<acc_angle[Y]<<" "<<acc_angle[Z]<<endl;
  // cout<<"Errors:\n";
  // cout<<errors[YAW]<<" "<<errors[PITCH]<<" "<<errors[ROLL]<<endl;
//  if(isStarted()){
   if(acc_angle[X] == 0 && acc_angle[Y]==0 && acc_angle[Z]==0 && gyro_angle[X]==0 && gyro_angle[Y]==0 && gyro_angle[Z]==0 ){
     resetGyroAngles();
     resetPID();
   }
   cout<<"Gyro angles: "<<gyro_angle[X]<<" "<<gyro_angle[Y]<<" "<<gyro_angle[Z]<<endl;
  pidController();

// }

applyMotorSpeed();

}

//-------------------------------------------------------------------------

// while(1) {
// sensor->update();
// sensor->read_accelerometer(&ax, &ay, &az);
// sensor->read_gyroscope(&gx, &gy, &gz);
// sensor->read_magnetometer(&mx, &my, &mz);
// printf("Acc: %+7.3f %+7.3f %+7.3f\n ", ax, ay, az);
// // printf("Gyr: %+8.3f %+8.3f %+8.3f\n ", gx, gy, gz);
// // printf(“Mag: %+7.3f %+7.3f %+7.3f\n”, mx, my, mz);
//
// usleep(500000);
//
// // cout<<“testing 1\n”;
// // cout<<millis()<<endl;
// // cout<<“testing 2\n”;
//
//
// if(true){
// takeoff();
// }
// if(az <9.3){
// cout<<“Unstable drone…\n”;
// hover(ax,ay,az);
// }
// }
return 0;
}

void applyMotorSpeed(){
while ((now = millis())-loop_timer<period);
pwm->set_duty_cycle(PWM_OUTPUT, pulse_length_esc1);
pwm->set_duty_cycle(PWM_OUTPUT_1, pulse_length_esc2);
pwm->set_duty_cycle(PWM_OUTPUT_2, pulse_length_esc3);
pwm->set_duty_cycle(PWM_OUTPUT_3, pulse_length_esc4);

 // cout<<"Pulse PID: \n";
 // cout<<pulse_length_esc1<<endl;
 // cout<<pulse_length_esc2<<endl;
 // cout<<pulse_length_esc3<<endl;
 // cout<<pulse_length_esc4<<endl;

}

void calibrateMpu9250(){

int max_samples = 2000;

for(int i=0;i<max_samples;++i){
readSensor();

gyro_offset[X] += gyro_raw[X];
gyro_offset[Y] += gyro_raw[Y];
gyro_offset[Z] += gyro_raw[Z];

// cout<<"Gyro:Present “<<gyro_offset[X]<<” “<<gyro_offset[Y]<<” "<<gyro_offset[Z]<<endl;

//generate low throttle and high throttle to stop the esc to stop beeping annoyingly
usleep(3);

}

//Calculate average offsets
cout<<"Gyro:before “<<gyro_offset[X]<<” “<<gyro_offset[Y]<<” "<<gyro_offset[Z]<<endl;
gyro_offset[X] /= max_samples;
gyro_offset[Y] /= max_samples;
gyro_offset[Z] /= max_samples;

cout<<"Gyro:after “<<gyro_offset[X]<<” “<<gyro_offset[Y]<<” "<<gyro_offset[Z]<<endl;

}

void readSensor(){

sensor->update();
sensor->read_gyroscope(&gx, &gy, &gz);
sensor->read_accelerometer(&ax, &ay, &az);

acc_raw[X] = ax;
acc_raw[Y] = ay;
acc_raw[Z] = az;
// printf("Acc: %+7.3f %+7.3f %+7.3f\n ", ax, ay, az);
// printf("Gyr: %+8.3f %+8.3f %+8.3f\n ", gx, gy, gz);

gyro_raw[X] = gx100000;
gyro_raw[Y] = gy
100000;
gyro_raw[Z] = gz*100000;

//cout<<"Gyro:Present “<<gyro_raw[X]<<” “<<gyro_raw[Y]<<” "<<gyro_raw[Z]<<endl;

}

void configureChannelMapping(){
mode_mapping[YAW] = PWM_OUTPUT_3;
mode_mapping[PITCH] = PWM_OUTPUT_1;
mode_mapping[ROLL] = PWM_OUTPUT;
mode_mapping[THROTTLE] = PWM_OUTPUT_2;

}

void calculateAngles(){

calculateGyroAngles();
calculateAccAngles();

//after calculating the gyro and acc angles
if(initialized){
gyro_angle[X] = gyro_angle[X]*0.9996 + acc_angle[X] * 0.0004;
gyro_angle[Y] = gyro_angle[Y]*0.9996 + acc_angle[Y] * 0.0004;
}else{
resetGyroAngles();
initialized = true;
}

measures[ROLL] = measures[ROLL] *0.9 + gyro_angle[X] * 0.1;
measures[PITCH] = measures[PITCH] *0.9 + gyro_angle[Y] * 0.1;
measures[YAW] = -gyro_raw[Z]/SSF_GYRO;

}

void calculateGyroAngles(){
//substracitng the offsets

gyro_raw[X] -= gyro_offset[X];
gyro_raw[Y] -= gyro_offset[Y];
gyro_raw[Z] -= gyro_offset[Z];

//angle calculation using integration
gyro_angle[X] += (gyro_raw[X] / (FREQ * SSF_GYRO));
gyro_angle[Y] += (-gyro_raw[X] / (FREQ * SSF_GYRO));

//Transfer roll to pitch if MPU has yawed
gyro_angle[Y] += gyro_angle[X] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
gyro_angle[X] -= gyro_angle[Y] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));

}

void calculateAccAngles(){

acc_total_vector = sqrt(pow(acc_raw[X],2)+ pow(acc_raw[Y],2) + pow(acc_raw[Z],2));

if(abs(acc_raw[X]) < acc_total_vector){
acc_angle[X] = asin((float) acc_raw[Y] / acc_total_vector) * (180 / PI);
}

if(abs(acc_raw[Y]) < acc_total_vector){
acc_angle[Y] = asin((float) acc_raw[X] / acc_total_vector) * (180 / PI);
}
}

void resetGyroAngles(){

gyro_angle[X] = acc_angle[X];
gyro_angle[Y] = acc_angle[Y];
}

void getFlightInstructions(){

instructions[YAW] = map(pulse_length[mode_mapping[YAW]],1000,2000,-180,180);
instructions[PITCH] = map(pulse_length[mode_mapping[PITCH]],1000,2000,-180,180);
instructions[ROLL] = map(pulse_length[mode_mapping[ROLL]],1000,2000,-180,180);
//instructions[THROTTLE] = map(pulse_length[mode_mapping[THROTTLE]],1000,2000,-180,180);
instructions[THROTTLE] = 1350;
}

long map(long x, long in_min, long in_max, long out_min, long out_max) {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void calculateErrors(){
errors[YAW] = instructions[YAW] - measures [YAW];
errors[PITCH] = instructions[PITCH] - measures [PITCH];
errors[ROLL] = instructions[ROLL] - measures [ROLL];

}

void pidController(){
float Kp[3] = {4.0,1.3,1.3};//P coeffiecient in order Yaw, Pitch, Roll
float Ki[3] = {0.02,0.04,0.04};//i
float Kd[3] = {0,18,18};//d

float delta_err[3] = {0,0,0};//error deltas
float yaw_pid = 0;
float pitch_pid = 0;
float roll_pid = 0;

//initialize motor commands with throttle
// pulse_length_esc1 = instructions[THROTTLE];
// pulse_length_esc2 = instructions[THROTTLE];
// pulse_length_esc3 = instructions[THROTTLE];
// pulse_length_esc4 = instructions[THROTTLE];

//donot calculate anything if the throttle is 0
if(instructions[THROTTLE] >= 1012){
//Calculate sum of errors: integral coeffiecients
error_sum[YAW] +=errors[YAW];
error_sum[PITCH] +=errors[PITCH];
error_sum[ROLL] +=errors[ROLL];

delta_err[YAW] = errors[YAW] - previous_error[YAW];
delta_err[PITCH] = errors[PITCH] - previous_error[PITCH];
delta_err[ROLL] = errors[ROLL] - previous_error[ROLL];

previous_error[YAW] = errors[YAW];
previous_error[PITCH] = errors[PITCH];
previous_error[ROLL] = errors[ROLL];

yaw_pid = (errors[YAW] * Kp[YAW]) +(error_sum[YAW] * Ki[YAW]) + (delta_err[YAW] * Kd[YAW]);
pitch_pid = (errors[PITCH] * Kp[PITCH]) +(error_sum[PITCH] * Ki[PITCH]) + (delta_err[PITCH] * Kd[PITCH]);
roll_pid = (errors[ROLL] * Kp[ROLL]) +(error_sum[ROLL] * Ki[ROLL]) + (delta_err[ROLL] * Kd[ROLL]);

pulse_length_esc1 = instructions[THROTTLE] + roll_pid + pitch_pid - yaw_pid;
pulse_length_esc2 = instructions[THROTTLE] - roll_pid + pitch_pid + yaw_pid;
pulse_length_esc3 = instructions[THROTTLE] + roll_pid - pitch_pid + yaw_pid;
pulse_length_esc4 = instructions[THROTTLE] - roll_pid - pitch_pid - yaw_pid;

}
pulse_length_esc1 = minMax(pulse_length_esc1,1100,1700);
pulse_length_esc2 = minMax(pulse_length_esc2,1100,1700);
pulse_length_esc3 = minMax(pulse_length_esc3,1100,1700);
pulse_length_esc4 = minMax(pulse_length_esc4,1100,1700);

// cout<<“Pulse PID: \n”;
// cout<<pulse_length_esc1<<endl;
// cout<<pulse_length_esc2<<endl;
// cout<<pulse_length_esc3<<endl;
// cout<<pulse_length_esc4<<endl;

}

float minMax(float value, float min_value, float max_value){
if(value>max_value){
value = max_value;
}else if(value<min_value){
value = min_value;
}
return value;
}

bool isStarted(){
resetPID();
resetGyroAngles();

return true;
}

void resetPID(){
errors[YAW] =0;
errors[PITCH] =0;
errors[ROLL] =0;

error_sum[YAW] =0;
error_sum[PITCH] =0;
error_sum[ROLL] =0;

previous_error[YAW] =0;
previous_error[PITCH] =0;
previous_error[ROLL] =0;

}

Every-time I run my program the first thing that happens is the calibration of MPU9250 then setting up the Gyro OFFSETS after that continuous calculation of angles along the X,Y and Z axes. If one side gets down the pluse is calculated higher for that side and lower to the opposite side vice-versa, I checked it the error calculation is working golden.

Is my calculation right? or maybe I am sending the calculated motor speed wrongly?

Please don’t mind the comments.