| 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 |
| } |