File content as of revision 0:b0f9c5ac0305:

#include "mbed.h"
#include "pwmdeg.h"
#include "degpwm.h"
#include "PID_roll.h"
#include "PID_pitch.h"
#include "PID_yaw.h"
#include "mpu_output.h"
#include "motor_mixer.h"

#define pitch_output

RawSerial pc_main(USBTX, USBRX);
mpu_output main_mpu_output;
pwmdeg pwmdeg_main;
Degpwm Degpwm_main;
motormixer motormixer_main;
Timer test;

//Interrupt Handler
InterruptIn roll_int(A0);       //Channel 1
InterruptIn pitch_int(A1);      //Channel 2
InterruptIn throttle_int(D11);  //Channel 3
InterruptIn yaw_int(D12);       //Channel 4

//Variablen ChannelIn
volatile uint16_t _pulsewidth_ch1;
volatile uint16_t _pulsewidth_ch2;
volatile uint16_t _pulsewidth_ch3;
volatile uint16_t _pulsewidth_ch4;

//Timer ChannelIn
Timer _t_ch1;
Timer _t_ch2;
Timer _t_ch3;
Timer _t_ch4;


//Init von PID Regler
//Beispiel:    PIDClass pid_roll(AngleKp, AngleKi, RateKp, RateKd);

//Roll
PID_ROLL_Class pid_roll(4, 0.0007, 0.020, 0);

//Pitch
PID_PITCH_Class pid_pitch(4, 0.0007, 0.020, 0);  //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35);  //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35);  //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35);

//Yaw
PID_YAW_Class pid_yaw(4, 0.000, 0.000, 0.0); //PID_YAW_Class pid_yaw(4, 0.0002, 0.015, 0.0); Stärker einstellen!!!!

float Rate = 0, dt = 10;

//Main Funktionen
void pwm_in_main();
void pitch_rc_main();
void roll_rc_main();
void yaw_rc_main();
void motors_ready_main();
void pwm_read_main();
void rise_ch1();
void fall_ch1();
void rise_ch2();
void fall_ch2();
void rise_ch3();
void fall_ch3();
void rise_ch4();
void fall_ch4();

//PwmIn Variablen
int ele_in_main = 0; //Channel 1
int ail_in_main = 0; //Channel 2
int thr_in_main = 0; //Channel 3
int rud_in_main = 0; //Channel 4


//Übergabevariablen
//Channel1
float roll_error = 0;
int roll_error_pwm_ready = 0;

//Channel2
float pitch_error = 0;
int pitch_error_pwm_ready = 0;

//Channel3
float yaw_error = 0;
int yaw_error_pwm_ready = 0;

//Channel4
int motor_all_pwm_ready = 0;



////////////////////////////////////MAIN///////////////////////////////////////////////////////////////////////
int main()
{
    NVIC_SetPriority(EXTI3_IRQn, 0x01); //Priorität von Interrupt des Sensors dekrementieren!!!

    main_mpu_output.Init();             //Sensor initalisieren
    main_mpu_output.dmpDataUpdate();
    
    //MBED_ASSERT(main_mpu_output.Init() == true);
    
    motormixer_main.motor_all_period(2.5);        //Einstellen Periode Motoren
    //motormixer_main.motor_calibration(2.5); //Kalibrieren der Motoren

    //ISR Funktionen RC ChannelIn
    roll_int.rise(&rise_ch1);
    pitch_int.rise(&rise_ch2);
    throttle_int.rise(&rise_ch3);
    yaw_int.rise(&rise_ch4);

    while(1) {
        test.start();

        //Alle Channel einlesen
        pwm_in_main();

        //PWM Signale des RC in Grad umrechnen, Roll-Error abspeichern (danach in PWM)
        roll_rc_main();

        //PWM Signale des RC in Grad umrechnen, Pitch-Error abspeichern (danach in PWM)
        pitch_rc_main();

        //PWM Signale des RC in Grad umrechnen, Yaw-Error abspeichern (danach in PWM)
        yaw_rc_main();

        //Werte in Mototrmixer schreiben --> ansteuern der Motoren
        motors_ready_main();

        //pc_main.printf("hier\n");
        test.stop();
        int zeit = test.read_ms();
        //pc_main.printf("zeit: %d\n", zeit);
        test.reset();

    }
}

///////////////////ISR von Rise Channel 1 Roll/////////////////////////////////////
void rise_ch1()
{
    _t_ch1.start();
    roll_int.fall(&fall_ch1);//Interrupt Fall
}

void fall_ch1()
{
    _t_ch1.stop();
    if(_t_ch1.read_us()>=1000&&_t_ch1.read_us()<=2000) {
        _pulsewidth_ch1 = _t_ch1.read_us();
        //pc_main.printf("ch1: %d\n", _pulsewidth_ch1);
    }
    _t_ch1.reset();

}

///////////////////ISR von Rise Channel 2 Pitch/////////////////////////////////////
void rise_ch2()
{
    _t_ch2.start();
    pitch_int.fall(&fall_ch2);//Interrupt Fall

}

void fall_ch2()
{
    _t_ch2.stop();
    if(_t_ch2.read_us()>=1000&&_t_ch2.read_us()<=2000) {
        _pulsewidth_ch2 = _t_ch2.read_us();
        //pc_main.printf("ch1: %d\n", _pulsewidth_ch2);
    }
    _t_ch2.reset();

}

///////////////////ISR von Rise Channel 3 Throttle/////////////////////////////////////
void rise_ch3()
{
    _t_ch3.start();
    throttle_int.fall(&fall_ch3);//Interrupt Fall
}

void fall_ch3()
{
    _t_ch3.stop();
    if(_t_ch3.read_us()>=1000&&_t_ch3.read_us()<=2000) {
        _pulsewidth_ch3 = _t_ch3.read_us();
        //pc_main.printf("ch1: %d\n", _pulsewidth_ch1);
    }
    _t_ch3.reset();

}

///////////////////ISR von Rise Channel 4 YAW/////////////////////////////////////
void rise_ch4()
{
    _t_ch4.start();
    yaw_int.fall(&fall_ch4);//Interrupt Fall

}

void fall_ch4()
{
    _t_ch4.stop();
    if(_t_ch4.read_us()>=1000&&_t_ch4.read_us()<=2000) {
        _pulsewidth_ch4 = _t_ch4.read_us();
        //pc_main.printf("ch4: %d\n", _pulsewidth_ch4);
    }
    _t_ch4.reset();

}


///////PWM In abspeichern///////////////////////////////

void pwm_in_main()
{
    ail_in_main = _pulsewidth_ch1;
    ele_in_main = _pulsewidth_ch2;
    thr_in_main = _pulsewidth_ch3;
    rud_in_main = _pulsewidth_ch4;

    //pc_main.printf("throttle: %d|pitch: %d|roll: %d|rud: %d \n", thr_in_main, ele_in_main, ail_in_main, rud_in_main);

}

///////////Roll Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
void roll_rc_main()
{
    //Roll-Error abspeichern (in PWM)
    float Setpoint_Roll = pwmdeg_main.roll(ail_in_main), CurrentPosition_Roll = main_mpu_output.sensor_roll();
    roll_error = pid_roll.update_roll(Setpoint_Roll, CurrentPosition_Roll, Rate, dt);
    //pc_main.printf("Output pitch error: %d\n", pitch_error);
    roll_error_pwm_ready = Degpwm_main.roll_pwm(roll_error);
    //pc_main.printf("Output roll error pwm: %d\n", roll_error_pwm_ready);
}

///////////Pitch Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
void pitch_rc_main()
{
    float Setpoint_Pitch = pwmdeg_main.pitch_forward(ele_in_main), CurrentPosition_Pitch = main_mpu_output.sensor_pitch();
    pitch_error = pid_pitch.update_pitch(Setpoint_Pitch, CurrentPosition_Pitch, Rate, dt);
    //pc_main.printf("Output pitch error: %f\n", pitch_error);
    pitch_error_pwm_ready = Degpwm_main.pitch_pwm(pitch_error);
    //pc_main.printf("Output pitch error pwm: %d\n", pitch_error_pwm_ready);
    #ifdef pitch_output
        pc_main.printf("SetAccel: %f| CurrentAccel: %f| Output pitch error pwm: %d|pitch error: %f\n", Setpoint_Pitch, CurrentPosition_Pitch, pitch_error_pwm_ready, pitch_error);
    #endif

}


///////////Yaw Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
void yaw_rc_main()
{
    //Yaw Error abspeichern (in PWM)
    float SetAccel_Yaw = pwmdeg_main.yaw(rud_in_main), CurrentAccel_Yaw = main_mpu_output.sensor_yaw();
    yaw_error = pid_yaw.update_yaw(SetAccel_Yaw, CurrentAccel_Yaw, Rate, dt);
    yaw_error_pwm_ready = Degpwm_main.yaw_pwm(yaw_error);
    //pc_main.printf("Output yaw error pwm: %d\n", yaw_error_pwm_ready);
    //pc_main.printf("SetAccel: %f| CurrentAccel: %f| Output yaw error pwm: %d|yaw error: %f\n", SetAccel_Yaw, CurrentAccel_Yaw, yaw_error_pwm_ready, yaw_error);

}

///////////Übergabe an Motormixer///////////////////////////////////////////////////////////////////////////////////
void motors_ready_main()
{
    //yaw_error_pwm_ready = 0;
    //Throttle abspeichern (in PWM)
    if(thr_in_main > 1111) {

        motor_all_pwm_ready = thr_in_main;

        //PWM Werte in Motormixer und in fertige Motoren in Variablen speichern
        //Motor1
        motormixer_main.motor1_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
        //Motor2
        motormixer_main.motor2_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
        //Motor3
        motormixer_main.motor3_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
        //Motor4
        motormixer_main.motor4_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
    }

    else {
        //pc_main.printf("throttle: %d\n", throttle_in.pulsewidth());
        motormixer_main.motor_all_off();
    }

}