Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2105 - 1
#include "main.h"
2
#include "flightcontrol.h"
3
 
4
int reference_altitude;                                 //Regelgröße
5
int reference_accz;                                             //Regelgröße D-Anteil
6
int reference_flow_x;
7
int reference_flow_y;
8
 
9
int altitude_differenz;                                 //Regelabweichung
10
int altitude_differenz_sum = 0;                 //I-Glied
11
int xDirection_differenz_sum = 0;
12
int yDirection_differenz_sum = 0;
13
 
14
int altitude_old;
15
int altitude_velocity;
16
int xDirection_old;
17
int xDirection_velocity;
18
int yDirection_old;
19
int yDirection_velocity;
20
 
21
float gas_correction_dpart;
22
float gas_correction_ipart = 0.0;
23
float gas_correction_ppart;
24
float gas_correction_final;
25
float nick_correction_dpart;
26
float nick_correction_ipart = 0.0;
27
float nick_correction_ppart;
28
float nick_correction_final;
29
float roll_correction_dpart;
30
float roll_correction_ipart = 0.0;
31
float roll_correction_ppart;
32
float roll_correction_final;
33
 
34
float pFactorGas = 0.3;                                 //Verstärkungsfaktor P
35
float iFactorGas = 0.02;                                //Verstärkungsfaktor I
36
float dFactorGas = 0.3;                                 //Verstärkungsfaktor D
37
float pFactorNick = 0.2;                                //Verstärkungsfaktor P
38
float iFactorNick = 0.02;                               //Verstärkungsfaktor I
39
float dFactorNick = 0.3;                                //Verstärkungsfaktor D
40
float pFactorRoll = 0.2;                                //Verstärkungsfaktor P
41
float iFactorRoll = 0.02;                               //Verstärkungsfaktor I
42
float dFactorRoll = 0.3;                                //Verstärkungsfaktor D
43
 
44
//>> Performing Flight Correction
45
//------------------------------------------------------------------------------------------------------
46
void perform_flight_correction(){
47
        reference_altitude = (int)(optical_flow_values.ground_distance*1000);                   //Regelgröße in mm
48
        reference_flow_x += (int)(optical_flow_values.flow_comp_m_x*1000);
49
        reference_flow_y += (int)(optical_flow_values.flow_comp_m_y*1000);
50
        altitude_differenz = STARTING_ALTITUDE - reference_altitude;                                    //Regelabweichung in mm
51
 
52
        if(reference_altitude != altitude_old){
53
                altitude_velocity = (reference_altitude - altitude_old) / 0.1;
54
        }
55
        if(reference_flow_x != xDirection_old){
56
                xDirection_velocity = (reference_flow_x - xDirection_old) / 0.1;
57
        }
58
        if(reference_flow_y != yDirection_old){
59
                yDirection_velocity = (reference_flow_y - yDirection_old) / 0.1;
60
        }
61
 
62
        if(altitude_differenz_sum < 10000 && altitude_differenz_sum > -10000){
63
                altitude_differenz_sum += altitude_differenz * 0.1;
64
                if(altitude_differenz_sum > 9999){altitude_differenz_sum = 9999;}else
65
                        if(altitude_differenz_sum < -9999){altitude_differenz_sum = -9999;}
66
        }
67
        if(xDirection_differenz_sum < 1000 && xDirection_differenz_sum > -1000){
68
                xDirection_differenz_sum += reference_flow_x;
69
                if(xDirection_differenz_sum > 9999){xDirection_differenz_sum = 9999;}else
70
                        if(xDirection_differenz_sum < -9999){xDirection_differenz_sum = -9999;}
71
        }
72
        if(yDirection_differenz_sum < 1000 && yDirection_differenz_sum > -1000){
73
                yDirection_differenz_sum += reference_flow_y;
74
                if(yDirection_differenz_sum > 9999){yDirection_differenz_sum = 9999;}else
75
                        if(yDirection_differenz_sum < -9999){yDirection_differenz_sum = -9999;}
76
        }
77
 
78
        //////////////////////////////////////////////////////////////////////////
79
    // PID Regelung Gas
80
        gas_correction_ipart = iFactorGas * altitude_differenz_sum;
81
        gas_correction_dpart = dFactorGas * altitude_velocity *-1;
82
        gas_correction_ppart = pFactorGas * altitude_differenz;
83
 
84
        gas_correction_final = (gas_correction_ppart + gas_correction_dpart + gas_correction_ipart) / 10;
85
        //////////////////////////////////////////////////////////////////////////
86
        //////////////////////////////////////////////////////////////////////////
87
    // PID Regelung Nick
88
        nick_correction_ipart = iFactorNick * yDirection_differenz_sum;
89
        nick_correction_dpart = dFactorNick * yDirection_velocity;
90
        nick_correction_ppart = pFactorNick * reference_flow_y;
91
 
92
        nick_correction_final = (nick_correction_ppart + nick_correction_dpart + nick_correction_ipart) / -10;
93
        //////////////////////////////////////////////////////////////////////////
94
        //////////////////////////////////////////////////////////////////////////
95
    // PID Regelung Roll
96
        roll_correction_ipart = iFactorRoll * xDirection_differenz_sum;
97
        roll_correction_dpart = dFactorRoll * xDirection_velocity;
98
        roll_correction_ppart = pFactorRoll * reference_flow_x;
99
 
100
        roll_correction_final = (roll_correction_ppart + roll_correction_dpart + roll_correction_ipart) / -10;
101
        //////////////////////////////////////////////////////////////////////////
102
        //printf("Final: %f   PPART: %f   DPART: %f\n", nick_correction_final, nick_correction_ppart, nick_correction_dpart);
103
 
104
        //////////////////////////////////////////////////////////////////////////
105
    // Set Limits
106
        if(gas_correction_final >= 20){
107
                fc_correction_data.Gas = 20;
108
        }else if(gas_correction_final <= -20){
109
                fc_correction_data.Gas = -20;
110
        }else{
111
                fc_correction_data.Gas = gas_correction_final;
112
        }
113
        if(nick_correction_final >= 20){
114
                fc_correction_data.Nick = 20;
115
        }else if(nick_correction_final <= -20){
116
                fc_correction_data.Nick = -20;
117
        }else{
118
                fc_correction_data.Nick = nick_correction_final;
119
        }
120
        if(roll_correction_final >= 20){
121
                fc_correction_data.Roll = 20;
122
        }else if(roll_correction_final <= -20){
123
                fc_correction_data.Roll = -20;
124
        }else{
125
                fc_correction_data.Roll = roll_correction_final;
126
        }
127
 
128
        //printf("%d\n", reference_altitude/10);
129
        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 );
130
        //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 );
131
        fc_correction_data.Gas = 0;
132
        //fc_correction_data.Roll = 0;
133
        //fc_correction_data.Nick = 0;
134
 
135
 
136
        altitude_old = reference_altitude;
137
        xDirection_old = reference_flow_x;
138
        yDirection_old = reference_flow_y;
139
}
140
 
141
//>> Transmitting Correction Data
142
//------------------------------------------------------------------------------------------------------
143
u8 *flight_values_stream;
144
serial_data_struct data_package_flightcontrol;
145
void transmit_flight_data(){
146
        flight_values_stream = (unsigned char *) &fc_correction_data;
147
        //printf("Correction: %d\n", fc_correction_data.Gas);
148
        for (int i = 0; i < sizeof(struct str_fc_correction_data); ++i)
149
        {
150
                data_package_flightcontrol.data[i] = flight_values_stream[i];
151
        }
152
        create_serial_frame(1, 'b', sizeof(struct str_fc_correction_data), &data_package_flightcontrol);
153
        //printf("%s\n", data_package_flightcontrol.txrxdata);
154
        transmit_data(TO_KOPTER, &data_package_flightcontrol);
155
}
156
 
157
//>> Resetting Flow and Altitude Values
158
//------------------------------------------------------------------------------------------------------
159
void reset_reference_values(){
160
        reference_flow_x = 0.0;
161
        reference_flow_y = 0.0;
162
        reference_altitude = 0.0;
163
}
164
 
165
//>> Initializing flightcontrol
166
//------------------------------------------------------------------------------------------------------
167
void flightcontrol_init(){
168
        fc_correction_data.Nick = 0;
169
        fc_correction_data.Roll = 0;
170
        fc_correction_data.Gier = 0;
171
        fc_correction_data.Gas = 0;
172
        fc_correction_data.Config = EC_VALID | EC_GAS_ADD | EC_IGNORE_RC;               //Ignore Remote Control
173
}