Subversion Repositories Projects

Rev

Blame | Last modification | View Log | RSS feed

#include "main.h"
#include "flightcontrol.h"

int reference_altitude;                                 //Regelgröße
int reference_accz;                                             //Regelgröße D-Anteil
int reference_flow_x;
int reference_flow_y;

int altitude_differenz;                                 //Regelabweichung
int altitude_differenz_sum = 0;                 //I-Glied
int xDirection_differenz_sum = 0;
int yDirection_differenz_sum = 0;

int altitude_old;
int altitude_velocity;
int xDirection_old;
int xDirection_velocity;
int yDirection_old;
int yDirection_velocity;

float gas_correction_dpart;
float gas_correction_ipart = 0.0;
float gas_correction_ppart;
float gas_correction_final;
float nick_correction_dpart;
float nick_correction_ipart = 0.0;
float nick_correction_ppart;
float nick_correction_final;
float roll_correction_dpart;
float roll_correction_ipart = 0.0;
float roll_correction_ppart;
float roll_correction_final;

float pFactorGas = 0.3;                                 //Verstärkungsfaktor P
float iFactorGas = 0.02;                                //Verstärkungsfaktor I
float dFactorGas = 0.3;                                 //Verstärkungsfaktor D
float pFactorNick = 0.2;                                //Verstärkungsfaktor P
float iFactorNick = 0.02;                               //Verstärkungsfaktor I
float dFactorNick = 0.3;                                //Verstärkungsfaktor D
float pFactorRoll = 0.2;                                //Verstärkungsfaktor P
float iFactorRoll = 0.02;                               //Verstärkungsfaktor I
float dFactorRoll = 0.3;                                //Verstärkungsfaktor D

//>> Performing Flight Correction
//------------------------------------------------------------------------------------------------------
void perform_flight_correction(){
        reference_altitude = (int)(optical_flow_values.ground_distance*1000);                   //Regelgröße in mm
        reference_flow_x += (int)(optical_flow_values.flow_comp_m_x*1000);
        reference_flow_y += (int)(optical_flow_values.flow_comp_m_y*1000);
        altitude_differenz = STARTING_ALTITUDE - reference_altitude;                                    //Regelabweichung in mm

        if(reference_altitude != altitude_old){
                altitude_velocity = (reference_altitude - altitude_old) / 0.1;
        }
        if(reference_flow_x != xDirection_old){
                xDirection_velocity = (reference_flow_x - xDirection_old) / 0.1;
        }
        if(reference_flow_y != yDirection_old){
                yDirection_velocity = (reference_flow_y - yDirection_old) / 0.1;
        }

        if(altitude_differenz_sum < 10000 && altitude_differenz_sum > -10000){
                altitude_differenz_sum += altitude_differenz * 0.1;
                if(altitude_differenz_sum > 9999){altitude_differenz_sum = 9999;}else
                        if(altitude_differenz_sum < -9999){altitude_differenz_sum = -9999;}
        }
        if(xDirection_differenz_sum < 1000 && xDirection_differenz_sum > -1000){
                xDirection_differenz_sum += reference_flow_x;
                if(xDirection_differenz_sum > 9999){xDirection_differenz_sum = 9999;}else
                        if(xDirection_differenz_sum < -9999){xDirection_differenz_sum = -9999;}
        }
        if(yDirection_differenz_sum < 1000 && yDirection_differenz_sum > -1000){
                yDirection_differenz_sum += reference_flow_y;
                if(yDirection_differenz_sum > 9999){yDirection_differenz_sum = 9999;}else
                        if(yDirection_differenz_sum < -9999){yDirection_differenz_sum = -9999;}
        }

        //////////////////////////////////////////////////////////////////////////
    // PID Regelung Gas
        gas_correction_ipart = iFactorGas * altitude_differenz_sum;
        gas_correction_dpart = dFactorGas * altitude_velocity *-1;
        gas_correction_ppart = pFactorGas * altitude_differenz;

        gas_correction_final = (gas_correction_ppart + gas_correction_dpart + gas_correction_ipart) / 10;
        //////////////////////////////////////////////////////////////////////////
        //////////////////////////////////////////////////////////////////////////
    // PID Regelung Nick
        nick_correction_ipart = iFactorNick * yDirection_differenz_sum;
        nick_correction_dpart = dFactorNick * yDirection_velocity;
        nick_correction_ppart = pFactorNick * reference_flow_y;

        nick_correction_final = (nick_correction_ppart + nick_correction_dpart + nick_correction_ipart) / -10;
        //////////////////////////////////////////////////////////////////////////
        //////////////////////////////////////////////////////////////////////////
    // PID Regelung Roll
        roll_correction_ipart = iFactorRoll * xDirection_differenz_sum;
        roll_correction_dpart = dFactorRoll * xDirection_velocity;
        roll_correction_ppart = pFactorRoll * reference_flow_x;

        roll_correction_final = (roll_correction_ppart + roll_correction_dpart + roll_correction_ipart) / -10;
        //////////////////////////////////////////////////////////////////////////
        //printf("Final: %f   PPART: %f   DPART: %f\n", nick_correction_final, nick_correction_ppart, nick_correction_dpart);

        //////////////////////////////////////////////////////////////////////////
    // Set Limits
        if(gas_correction_final >= 20){
                fc_correction_data.Gas = 20;
        }else if(gas_correction_final <= -20){
                fc_correction_data.Gas = -20;
        }else{
                fc_correction_data.Gas = gas_correction_final;
        }
        if(nick_correction_final >= 20){
                fc_correction_data.Nick = 20;
        }else if(nick_correction_final <= -20){
                fc_correction_data.Nick = -20;
        }else{
                fc_correction_data.Nick = nick_correction_final;
        }
        if(roll_correction_final >= 20){
                fc_correction_data.Roll = 20;
        }else if(roll_correction_final <= -20){
                fc_correction_data.Roll = -20;
        }else{
                fc_correction_data.Roll = roll_correction_final;
        }

        //printf("%d\n", reference_altitude/10);
        printf("Final: %f   PPART: %f   DPART: %f   IPART: %f    Gas Correction: %d\n", gas_correction_final, gas_correction_ppart, gas_correction_dpart, gas_correction_ipart, fc_correction_data.Gas );
        //printf("Final: %f   PPART: %f   DPART: %f   IPART: %f    Gas Correction: %d\n", nick_correction_final, nick_correction_ppart, nick_correction_dpart, nick_correction_ipart, fc_correction_data.Nick );
        fc_correction_data.Gas = 0;
        //fc_correction_data.Roll = 0;
        //fc_correction_data.Nick = 0;


        altitude_old = reference_altitude;
        xDirection_old = reference_flow_x;
        yDirection_old = reference_flow_y;
}

//>> Transmitting Correction Data
//------------------------------------------------------------------------------------------------------
u8 *flight_values_stream;
serial_data_struct data_package_flightcontrol;
void transmit_flight_data(){
        flight_values_stream = (unsigned char *) &fc_correction_data;
        //printf("Correction: %d\n", fc_correction_data.Gas);
        for (int i = 0; i < sizeof(struct str_fc_correction_data); ++i)
        {
                data_package_flightcontrol.data[i] = flight_values_stream[i];
        }
        create_serial_frame(1, 'b', sizeof(struct str_fc_correction_data), &data_package_flightcontrol);
        //printf("%s\n", data_package_flightcontrol.txrxdata);
        transmit_data(TO_KOPTER, &data_package_flightcontrol);
}

//>> Resetting Flow and Altitude Values
//------------------------------------------------------------------------------------------------------
void reset_reference_values(){
        reference_flow_x = 0.0;
        reference_flow_y = 0.0;
        reference_altitude = 0.0;
}

//>> Initializing flightcontrol
//------------------------------------------------------------------------------------------------------
void flightcontrol_init(){
        fc_correction_data.Nick = 0;
        fc_correction_data.Roll = 0;
        fc_correction_data.Gier = 0;
        fc_correction_data.Gas = 0;
        fc_correction_data.Config = EC_VALID | EC_GAS_ADD | EC_IGNORE_RC;               //Ignore Remote Control
}