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;
int flying_state = DRONEX_STATE_GROUND;
int integratorFlag = DRONEX_INTEGRATOR_OFF;
float integrator_var[3] = {0,0,0};
float integrator_sum_XYZ[3] = {0,0,0};
// Controller Mode
......@@ -283,7 +283,10 @@ std::vector<float> gainMatrixPitchAngle (9,0.0);
// 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
float cmd_sixteenbit_min;
......
......@@ -81,8 +81,9 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# 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
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(){
}
void integratorCallback (const Setpoint& integrParams ) {
integrator_var[0] = integrParams.x;
integrator_var[1] = integrParams.y;
integrator_var[2] = integrParams.z;
integrator_sum_XYZ[0] = integrParams.x;
integrator_sum_XYZ[1] = integrParams.y;
integrator_sum_XYZ[2] = integrParams.z;
}
void WeightParamCallback (const Setpoint& weightParam ) {
......@@ -718,6 +718,14 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
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:
......@@ -1101,7 +1109,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
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:
{
for(int i=0; i < 3; i++)
{
integrator_var[i] = 0;
integrator_sum_XYZ[i] = 0;
}
}
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:
for(int i = 0; i < 9; ++i)
{
......@@ -1678,7 +1698,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// 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, "gainIntegratorRate", gainIntegratorRate, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegratorAngle", gainIntegratorAngle, 3);
// DEBUGGING: Print out one of the parameters that was loaded
......@@ -1795,7 +1818,7 @@ void integrator_XYZ(float (&stateErrorBody)[12])
ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
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])
{
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++)
{
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