From eae19076f94b0f402187b3542bd1f2ff7ca515ac Mon Sep 17 00:00:00 2001
From: Jeremiah the Great <jcoulson@ethz.ch>
Date: Thu, 28 Nov 2019 18:17:55 +0100
Subject: [PATCH] CS1 integrator working on real system
---
.../include/nodes/DefaultControllerService.h | 6 ++---
.../src/nodes/CsoneControllerService.cpp | 6 +++++
.../src/nodes/DefaultControllerService.cpp | 22 +++++++++----------
3 files changed, 20 insertions(+), 14 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 0bd861de..17079bc7 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -146,9 +146,9 @@ using namespace dfall_pkg;
// These constants deine the behaviour of the intergrator
-#define INTEGRATOR_FLAG_ON 1
-#define INTEGRATOR_FLAG_OFF 2
-#define INTEGRATOR_FLAG_RESET 3
+#define DEFAULT_INTEGRATOR_FLAG_ON 1
+#define DEFAULT_INTEGRATOR_FLAG_OFF 2
+#define DEFAULT_INTEGRATOR_FLAG_RESET 3
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index 0e94e7c6..cbf31549 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -933,6 +933,12 @@ void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+ // Integrator gains
+ yaml_integratorGain_forThrust = getParameterFloat(nodeHandle_for_paramaters, "integratorGain_forThrust");
+ yaml_integratorGain_forTauXY = getParameterFloat(nodeHandle_for_paramaters, "integratorGain_forTauXY");
+ yaml_integratorGain_forTauYaw = getParameterFloat(nodeHandle_for_paramaters, "integratorGain_forTauYaw");
+
+
// > DEBUGGING: Print out one of the parameters that was loaded to
// debug if the fetching of parameters worked correctly
ROS_INFO_STREAM("[CSONE CONTROLLER] DEBUGGING: the fetched CsoneController/mass = " << yaml_cf_mass_in_grams);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 05b1a91c..985e50d7 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -317,7 +317,7 @@ void computeResponse_for_normal(Controller::Response &response)
smoothSetpointChanges( m_setpoint , m_setpoint_for_controller );
// Call the LQR control function
- calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
}
@@ -415,7 +415,7 @@ void computeResponse_for_takeoff_move_up(Controller::Response &response)
m_setpoint_for_controller[3] = initial_yaw + time_elapsed_proportion * yaw_start_to_end_diff;
// Call the LQR control function
- calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
// Change to next state after specified time
if (m_time_in_seconds > yaml_takoff_move_up_time)
@@ -459,7 +459,7 @@ void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response)
smoothSetpointChanges( m_setpoint , m_setpoint_for_controller );
// Call the LQR control function
- calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
// Check if within the "integrator on" box of the setpoint
// > First, compute the current errors
@@ -520,7 +520,7 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
// accordingly
static bool static_integrator_complete_once = false;
static float static_integrator_on_time = yaml_takoff_integrator_on_time;
- static int static_integrator_flag = INTEGRATOR_FLAG_ON;
+ static int static_integrator_flag = DEFAULT_INTEGRATOR_FLAG_ON;
// Check if the state "just recently" changed
if (m_current_state_changed)
@@ -535,12 +535,12 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
// Adjust the integrator on/off and time
if (static_integrator_complete_once)
{
- static_integrator_flag = INTEGRATOR_FLAG_OFF;
+ static_integrator_flag = DEFAULT_INTEGRATOR_FLAG_OFF;
static_integrator_on_time = 0.5;
}
else
{
- static_integrator_flag = INTEGRATOR_FLAG_ON;
+ static_integrator_flag = DEFAULT_INTEGRATOR_FLAG_ON;
static_integrator_on_time = yaml_takoff_integrator_on_time;
}
// Set the change flag back to false
@@ -605,7 +605,7 @@ void computeResponse_for_landing_move_down(Controller::Response &response)
smoothSetpointChanges( target_landing_setpoint , m_setpoint_for_controller );
// Call the LQR control function
- calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
// Check if within the threshold of zero
if (m_current_stateInertialEstimate[2] < yaml_landing_move_down_end_height_threshold)
@@ -657,7 +657,7 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
if (time_elapsed_proportion<0.5)
{
// Call the LQR control function
- calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
// Compute the desired "spinning" thrust
float thrust_for_spinning =
(1.0-time_elapsed_proportion)
@@ -1084,14 +1084,14 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
// PERFORM THE INTEGRATOR COMPUTATIONS
- if (integrator_flag == INTEGRATOR_FLAG_ON)
+ if (integrator_flag == DEFAULT_INTEGRATOR_FLAG_ON)
{
newtons_int -= yaml_integratorGain_forThrust * stateErrorBody[2] / yaml_control_frequency;
tau_x += yaml_integratorGain_forTauXY * stateErrorBody[1] / yaml_control_frequency;
tau_y -= yaml_integratorGain_forTauXY * stateErrorBody[0] / yaml_control_frequency;
tau_z -= yaml_integratorGain_forTauYaw * stateErrorBody[8] / yaml_control_frequency;
}
- else if (integrator_flag == INTEGRATOR_FLAG_RESET)
+ else if (integrator_flag == DEFAULT_INTEGRATOR_FLAG_RESET)
{
newtons_int = 0.0;
tau_x = 0.0;
@@ -1459,7 +1459,7 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// Call the controller function with the reset flag
float tempStateBodyError[9];
Controller::Response temp_response;
- calculateControlOutput_viaLQR_givenError(tempStateBodyError, temp_response, INTEGRATOR_FLAG_RESET);
+ calculateControlOutput_viaLQR_givenError(tempStateBodyError, temp_response, DEFAULT_INTEGRATOR_FLAG_RESET);
break;
}
--
GitLab