Commit 70bb2884 authored by maruggv's avatar maruggv
Browse files

VM: implemented new integrator concept, not tried to compile yet

parent edd95094
...@@ -196,7 +196,7 @@ Setpoint dronexSetpoint; ...@@ -196,7 +196,7 @@ Setpoint dronexSetpoint;
int flying_state = DRONEX_STATE_GROUND; int flying_state = DRONEX_STATE_GROUND;
int integratorFlag = DRONEX_INTEGRATOR_OFF; int integratorFlag = DRONEX_INTEGRATOR_OFF;
float integrator_var[3] = {0,0,0}; float integrator_sum_XYZ[3] = {0,0,0};
// Controller Mode // Controller Mode
...@@ -283,7 +283,10 @@ std::vector<float> gainMatrixPitchAngle (9,0.0); ...@@ -283,7 +283,10 @@ std::vector<float> gainMatrixPitchAngle (9,0.0);
// Integrator parameters // Integrator parameters
std::vector<float> gainIntegrator (3,0.0); std::vector<float> gainIntegrator (3,0.0); // in case we want to go back to old integrator concept, else TODO: delete
std::vector<float> gainIntegratorRate (3,0.0);
std::vector<float> gainIntegratorAngle (3,0.0);
// The 16-bit command limits // The 16-bit command limits
float cmd_sixteenbit_min; float cmd_sixteenbit_min;
......
...@@ -81,8 +81,9 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342] ...@@ -81,8 +81,9 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648] #PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# for our integrator (so far just random values) # for our integrator (so far just random values)
gainIntegrator : [0.2, 0.2, 0.2] 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]
gainIntegratorAngle : [-1.00, 1.00, 1.00] # [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]
......
...@@ -232,9 +232,9 @@ void buttonPressed_integrator_reset(){ ...@@ -232,9 +232,9 @@ void buttonPressed_integrator_reset(){
} }
void integratorCallback (const Setpoint& integrParams ) { void integratorCallback (const Setpoint& integrParams ) {
integrator_var[0] = integrParams.x; integrator_sum_XYZ[0] = integrParams.x;
integrator_var[1] = integrParams.y; integrator_sum_XYZ[1] = integrParams.y;
integrator_var[2] = integrParams.z; integrator_sum_XYZ[2] = integrParams.z;
} }
void WeightParamCallback (const Setpoint& weightParam ) { void WeightParamCallback (const Setpoint& weightParam ) {
...@@ -718,7 +718,15 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -718,7 +718,15 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
integrator_XYZ(stateErrorBody); integrator_XYZ(stateErrorBody);
// integrator
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainIntegratorAngle[0] * integrator_sum_XYZ[1];
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= gainIntegratorAngle[1] * integrator_sum_XYZ[0];
// > ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment -= gainIntegratorAngle[2] * integrator_sum_XYZ[2];
// 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){
...@@ -1101,7 +1109,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller: ...@@ -1101,7 +1109,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON"); ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
for(int i=0; i < 3; i++) for(int i=0; i < 3; i++)
{ {
integrator_var[i] += stateErrorBody[i]*(1.0/control_frequency); integrator_sum_XYZ[i] += stateErrorBody[i]*(1.0/control_frequency);
} }
} }
...@@ -1109,16 +1117,28 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller: ...@@ -1109,16 +1117,28 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
{ {
for(int i=0; i < 3; i++) for(int i=0; i < 3; i++)
{ {
integrator_var[i] = 0; integrator_sum_XYZ[i] = 0;
} }
} }
for(int i=0; i < 3; i++) for(int i=0; i < 3; i++)
{ {
stateErrorBody[i] += integrator_var[i] * gainIntegrator[i]; stateErrorBody[i] += integrator_sumXYZ[i] * gainIntegrator[i];
} }
*/ */
// integrator
// BODY FRAME Y CONTROLLER
rollRate_forResponse -= gainIntegratorRate[0] * integrator_var[1];
// BODY FRAME X CONTROLLER
pitchRate_forResponse -= gainIntegratorRate[1] * integrator_var[0];
// BODY FRAME YAW CONTROLLER
// yawRate_forResponse -= gainIntegratorYawRate[i] * integrator_var[i];
// > ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment -= gainIntegratorRate[2] * integrator_var[2];
// 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 < 9; ++i) for(int i = 0; i < 9; ++i)
{ {
...@@ -1678,7 +1698,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle) ...@@ -1678,7 +1698,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// for our integrator // for our integrator
// in case we want to go back to old integrator concept, else TODO: delete first line
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegrator", gainIntegrator, 3); getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegrator", gainIntegrator, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegratorRate", gainIntegratorRate, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegratorAngle", gainIntegratorAngle, 3);
// DEBUGGING: Print out one of the parameters that was loaded // DEBUGGING: Print out one of the parameters that was loaded
...@@ -1795,7 +1818,7 @@ void integrator_XYZ(float (&stateErrorBody)[12]) ...@@ -1795,7 +1818,7 @@ void integrator_XYZ(float (&stateErrorBody)[12])
ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON"); ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
for(int i=0; i < 3; i++) for(int i=0; i < 3; i++)
{ {
integrator_var[i] += stateErrorBody[i]*(1.0/control_frequency); integrator_sum_XYZ[i] += stateErrorBody[i]*(1.0/control_frequency);
} }
} }
...@@ -1803,14 +1826,17 @@ void integrator_XYZ(float (&stateErrorBody)[12]) ...@@ -1803,14 +1826,17 @@ void integrator_XYZ(float (&stateErrorBody)[12])
{ {
for(int i=0; i < 3; i++) for(int i=0; i < 3; i++)
{ {
integrator_var[i] = 0; integrator_sum_XYZ[i] = 0;
} }
} }
// in case we want to go back to old integrator concept, else TODO: delete
/*
for(int i=0; i < 3; i++) for(int i=0; i < 3; i++)
{ {
stateErrorBody[i] += integrator_var[i] * gainIntegrator[i]; stateErrorBody[i] += integrator_sum_XYZ[i] * gainIntegrator[i];
} }
*/
} }
......
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