#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] = gy100000;
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;
}
