Commit ae0e2287 authored by mastefan's avatar mastefan
Browse files

Minor changes

parent 70bb2884
...@@ -261,9 +261,8 @@ private slots: ...@@ -261,9 +261,8 @@ private slots:
void on_integrate_on_dronex_button_clicked(); void on_integrate_on_dronex_button_clicked();
void on_integrate_off_dronex_button_clicked(); void on_integrate_off_dronex_button_clicked();
void on_integrate_reset_dronex_button_clicked(); void on_integrate_reset_dronex_button_clicked();
void on_set_weigth_param_clicked();
void on_set_pitch_baseline_param_clicked();
private: private:
Ui::MainWindow *ui; Ui::MainWindow *ui;
......
# Mass of the crazyflie # Mass of the crazyflie
mass_CF : 32 mass_CF : 28 #32
# Max setpoint change per second # Max setpoint change per second
...@@ -82,8 +82,8 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342] ...@@ -82,8 +82,8 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
# for our integrator (so far just random values) # for our integrator (so far just random values)
gainIntegrator : [ 0.50, 0.50, 0.50] # [x,y,z] (in case we want to go back to old integrator concept, else TODO: delete) gainIntegrator : [ 0.50, 0.50, 0.50] # [x,y,z] (in case we want to go back to old integrator concept, else TODO: delete)
gainIntegratorRate : [-1.00, 1.00, 1.00] # [roll, pitch, z] gainIntegratorRate : [-5.00, 5.00, 5.00] # [roll, pitch, z]
gainIntegratorAngle : [-1.00, 1.00, 1.00] # [roll, pitch, z] gainIntegratorAngle : [-0.20, 0.20, 0.20] # [roll, pitch, z]
# The LQR Controller # The LQR Controller
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00] gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
......
...@@ -731,19 +731,19 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -731,19 +731,19 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Perform the "-Kx" LQR computation for the rates and thrust: // Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 6; ++i){ for(int i = 0; i < 6; ++i){
// BODY FRAME Y CONTROLLER // BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i]; rollAngle_forResponse -= 1.1*gainMatrixRollAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER // BODY FRAME X CONTROLLER
pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i]; pitchAngle_forResponse -= 1.1*gainMatrixPitchAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER // BODY FRAME X CONTROLLER
//thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i]; //thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
} }
//float droneXAngle = ;
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership // Calculcate Roll and Pitch Baseline, which comes from the moving mothership
float rollAngle_baseline = 0; float rollAngle_baseline = 1;
float pitchAngle_baseline = 0; float pitchAngle_baseline = 1;
// TODO // TODO
rollAngle_forResponse += rollAngle_baseline; rollAngle_forResponse += rollAngle_baseline;
...@@ -774,10 +774,15 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -774,10 +774,15 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i]; yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i];
} }
for(int i = 0; i < 9; ++i){ /*for(int i = 0; i < 9; ++i){
thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i]; thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
}*/
for(int i = 0; i < 6; ++i){
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
} }
//thrustAdjustment -= F; //thrustAdjustment -= F;
...@@ -1130,13 +1135,13 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller: ...@@ -1130,13 +1135,13 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// integrator // integrator
// BODY FRAME Y CONTROLLER // BODY FRAME Y CONTROLLER
rollRate_forResponse -= gainIntegratorRate[0] * integrator_var[1]; rollRate_forResponse -= gainIntegratorRate[0] * integrator_sum_XYZ[1];//integrator_var[1];
// BODY FRAME X CONTROLLER // BODY FRAME X CONTROLLER
pitchRate_forResponse -= gainIntegratorRate[1] * integrator_var[0]; pitchRate_forResponse -= gainIntegratorRate[1] * integrator_sum_XYZ[0];//integrator_var[0];
// BODY FRAME YAW CONTROLLER // BODY FRAME YAW CONTROLLER
// yawRate_forResponse -= gainIntegratorYawRate[i] * integrator_var[i]; // yawRate_forResponse -= gainIntegratorYawRate[i] * integrator_var[i];
// > ALITUDE CONTROLLER (i.e., z-controller): // > ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment -= gainIntegratorRate[2] * integrator_var[2]; thrustAdjustment -= gainIntegratorRate[2] * integrator_sum_XYZ[2];//integrator_var[2];
// Perform the "-Kx" LQR computation for the rates and thrust: // Perform the "-Kx" LQR computation for the rates and thrust:
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment