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
}