diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 7ba8f80a77af7ab83ad81b7ac45fc51b87b3cf23..b415d9875e153e1327001631918c73e44edda140 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -179,19 +179,22 @@ float m_time_in_seconds = 0.0;
// Max setpoint change per second
float yaml_max_setpoint_change_per_second_horizontal = 0.60;
-float yaml_max_setpoint_change_per_second_vertical = 0.30;
+float yaml_max_setpoint_change_per_second_vertical = 0.40;
// Max error for z
float yaml_max_setpoint_error_z = 0.4;
-// Max error for yaw angle
-float yaml_max_setpoint_error_yaw_degrees = 60.0;
-float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;
+// Max error for xy
+float yaml_max_setpoint_error_xy = 0.3;
// Max {roll,pitch} angle request
float yaml_max_roll_pitch_request_degrees = 30.0;
float yaml_max_roll_pitch_request_radians = 30.0 * DEG2RAD;
+// Max error for yaw angle
+float yaml_max_setpoint_error_yaw_degrees = 60.0;
+float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;
+
// Theshold for {roll,pitch} angle beyond
// which the motors are turned off
float yaml_threshold_roll_pitch_for_turn_off_degrees = 70.0;
@@ -225,7 +228,7 @@ float yaml_takoff_integrator_on_time = 2.0;
float yaml_landing_move_down_end_height_setpoint = 0.05;
float yaml_landing_move_down_end_height_threshold = 0.10;
// The time for: landing move-down
-float yaml_landing_move_down_time_max = 2.0;
+float yaml_landing_move_down_time_max = 5.0;
// The thrust for landing spin motors
float yaml_landing_spin_motors_thrust = 10000;
@@ -302,7 +305,7 @@ bool yaml_shouldDisplayDebugInfo = false;
// VARIABLES FOR THE CONTROLLER
// > A flag for which controller to use:
-int yaml_controller_method = CONTROLLER_METHOD_RATES;
+int yaml_controller_method = CONTROLLER_METHOD_RATE_ANGLE_NESTED;
// The LQR Controller parameters for z-height
std::vector<float> yaml_gainMatrixThrust_2StateVector = { 0.98, 0.25};
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
index 38e728fca6de9bd6adf60d82de2ec764a326e8d6..64d77b13533f071c327fe852406f07169db9ad59 100644
--- a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -3,17 +3,20 @@
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.60 # [meters]
-max_setpoint_change_per_second_vertical : 0.30 # [meters]
+max_setpoint_change_per_second_vertical : 0.40 # [meters]
# Max error for z
max_setpoint_error_z: 0.4
-# Max error for yaw angle
-max_setpoint_error_yaw_degrees: 60
+# Max error for xy
+max_setpoint_error_xy: 0.3
# Max {roll,pitch} angle request
max_roll_pitch_request_degrees: 30
+# Max error for yaw angle
+max_setpoint_error_yaw_degrees: 60
+
# Theshold for {roll,pitch} angle beyond
# which the motors are turned off
threshold_roll_pitch_for_turn_off_degrees: 70
@@ -46,7 +49,7 @@ takoff_integrator_on_time: 2.0
landing_move_down_end_height_setpoint: 0.05
landing_move_down_end_height_threshold: 0.10
# The time for: landing move-down
-landing_move_down_time_max: 4.0
+landing_move_down_time_max: 5.0
# The thrust for landing spin motors
landing_spin_motors_thrust: 10000
@@ -88,7 +91,7 @@ shouldDisplayDebugInfo : false
# A flag for which controller to use, defined as:
# 1 - Rate controller
# 2 - Angle-Rate nested controller
-controller_method : 1
+controller_method : 2
# The LQR Controller parameters for z-height
gainMatrixThrust_2StateVector : [ 0.98, 0.25]
@@ -103,9 +106,9 @@ gainPitchRate_fromAngle : 4.00
# The LQR Controller parameters for yaw
gainYawRate_fromAngle : 2.30
# Integrator gains
-integratorGain_forThrust : 0.01
-integratorGain_forTauXY : 0.01
-integratorGain_forTauYaw : 0.01
+integratorGain_forThrust : 0.00
+integratorGain_forTauXY : 0.00
+integratorGain_forTauYaw : 0.00
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index b7d42b07a5b86bc0fe4fa621818f22562aec79a3..d19321f1cfb08ad5876d75dcb1393779a94f9b9b 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -487,8 +487,6 @@ void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response)
box_reached = true;
}
- ROS_INFO_STREAM("[DEFAULT CONTROLLER] time = " << m_time_in_seconds << ", switch at = " << time_to_switch << " abs error (x,y,z) = ( " << abs_error_x << ", " << abs_error_y << ", " << abs_error_z << ")" );
-
if (m_time_in_seconds > time_to_switch)
{
// Inform the user
@@ -585,8 +583,6 @@ void computeResponse_for_landing_move_down(Controller::Response &response)
// Call the LQR control function
calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
- ROS_INFO_STREAM("[DEFAULT CONTROLLER] m_current_stateInertialEstimate[2] = " << m_current_stateInertialEstimate[2] );
-
// Check if within the threshold of zero
if (m_current_stateInertialEstimate[2] < yaml_landing_move_down_end_height_threshold)
{
@@ -699,6 +695,10 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)[4] )
{
+ // NO SMOOTHING IS APPLIED TO THE YAW-COORDINATE
+ // > Hence copy it across directly
+ current_setpoint[3] = target_setpoint[3];
+
// SMOOTH THE Z-COORIDINATE
// > Compute the max allowed change
float max_for_z = yaml_max_setpoint_change_per_second_vertical / yaml_control_frequency;
@@ -892,16 +892,41 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI
// Store the current YAW in a local variable
float temp_stateInertial_yaw = stateInertial[8];
+ // Initialise an array for the state error in the inertial frame
+ float stateInertialError[9];
+
// Adjust the INERTIAL (x,y,z) position for the setpoint
- stateInertial[0] = stateInertial[0] - setpoint[0];
- stateInertial[1] = stateInertial[1] - setpoint[1];
- stateInertial[2] = stateInertial[2] - setpoint[2];
+ stateInertialError[0] = stateInertial[0] - setpoint[0];
+ stateInertialError[1] = stateInertial[1] - setpoint[1];
+ stateInertialError[2] = stateInertial[2] - setpoint[2];
+
+ // The linear velocities can be directly copied across
+ stateInertialError[3] = stateInertial[3];
+ stateInertialError[4] = stateInertial[4];
+ stateInertialError[5] = stateInertial[5];
+
+ // The angular velocities for {roll,pitch} can be directly
+ // copied across
+ stateInertialError[6] = stateInertial[6];
+ stateInertialError[7] = stateInertial[7];
+
+ // Clip the x-coordination to within the specified bounds
+ if (stateInertialError[0] > yaml_max_setpoint_error_xy)
+ stateInertialError[0] = yaml_max_setpoint_error_xy;
+ else if (stateInertialError[0] < -yaml_max_setpoint_error_xy)
+ stateInertialError[0] = -yaml_max_setpoint_error_xy;
+
+ // Clip the y-coordination to within the specified bounds
+ if (stateInertialError[1] > yaml_max_setpoint_error_xy)
+ stateInertialError[1] = yaml_max_setpoint_error_xy;
+ else if (stateInertialError[1] < -yaml_max_setpoint_error_xy)
+ stateInertialError[1] = -yaml_max_setpoint_error_xy;
// Clip the z-coordination to within the specified bounds
- if (stateInertial[2] > yaml_max_setpoint_error_z)
- stateInertial[2] = yaml_max_setpoint_error_z;
- else if (stateInertial[2] < -yaml_max_setpoint_error_z)
- stateInertial[2] = -yaml_max_setpoint_error_z;
+ if (stateInertialError[2] > yaml_max_setpoint_error_z)
+ stateInertialError[2] = yaml_max_setpoint_error_z;
+ else if (stateInertialError[2] < -yaml_max_setpoint_error_z)
+ stateInertialError[2] = -yaml_max_setpoint_error_z;
// Fill in the yaw angle error
// > This error should be "unwrapped" to be in the range
@@ -918,14 +943,14 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI
yawError = -yaml_max_setpoint_error_yaw_radians;
// > Finally, put the "yawError" into the "stateError" variable
- stateInertial[8] = yawError;
+ stateInertialError[8] = yawError;
// CONVERSION INTO BODY FRAME
// Initialise a variable for the body frame error
float bodyFrameError[9];
// Call the function to convert the state erorr from
// the Inertial frame into the Body frame
- convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
+ convertIntoBodyFrame(stateInertialError, bodyFrameError, temp_stateInertial_yaw);
calculateControlOutput_viaLQR_givenError(bodyFrameError, response, integrator_flag);
}
@@ -1538,12 +1563,15 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// Max error for z
yaml_max_setpoint_error_z = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_z");
- // Max error for yaw angle
- yaml_max_setpoint_error_yaw_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_yaw_degrees");
-
+ // Max error for xy
+ yaml_max_setpoint_error_xy = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_xy");
+
// Max {roll,pitch} angle request
yaml_max_roll_pitch_request_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_roll_pitch_request_degrees");
+ // Max error for yaw angle
+ yaml_max_setpoint_error_yaw_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_yaw_degrees");
+
// Theshold for {roll,pitch} angle beyond
// which the motors are turned off
yaml_threshold_roll_pitch_for_turn_off_degrees = getParameterFloat(nodeHandle_for_paramaters , "threshold_roll_pitch_for_turn_off_degrees");