0,0 → 1,173 |
#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 |
} |