Commit d4e2a9d8 authored by beuchatp's avatar beuchatp
Browse files

Fixed small bugs in Default controller. Now (almost) fully tested and working smoothly.

parent 60e96396
...@@ -179,19 +179,22 @@ float m_time_in_seconds = 0.0; ...@@ -179,19 +179,22 @@ float m_time_in_seconds = 0.0;
// Max setpoint change per second // Max setpoint change per second
float yaml_max_setpoint_change_per_second_horizontal = 0.60; 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 // Max error for z
float yaml_max_setpoint_error_z = 0.4; float yaml_max_setpoint_error_z = 0.4;
// Max error for yaw angle // Max error for xy
float yaml_max_setpoint_error_yaw_degrees = 60.0; float yaml_max_setpoint_error_xy = 0.3;
float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;
// Max {roll,pitch} angle request // Max {roll,pitch} angle request
float yaml_max_roll_pitch_request_degrees = 30.0; float yaml_max_roll_pitch_request_degrees = 30.0;
float yaml_max_roll_pitch_request_radians = 30.0 * DEG2RAD; 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 // Theshold for {roll,pitch} angle beyond
// which the motors are turned off // which the motors are turned off
float yaml_threshold_roll_pitch_for_turn_off_degrees = 70.0; float yaml_threshold_roll_pitch_for_turn_off_degrees = 70.0;
...@@ -225,7 +228,7 @@ float yaml_takoff_integrator_on_time = 2.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_setpoint = 0.05;
float yaml_landing_move_down_end_height_threshold = 0.10; float yaml_landing_move_down_end_height_threshold = 0.10;
// The time for: landing move-down // 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 // The thrust for landing spin motors
float yaml_landing_spin_motors_thrust = 10000; float yaml_landing_spin_motors_thrust = 10000;
...@@ -302,7 +305,7 @@ bool yaml_shouldDisplayDebugInfo = false; ...@@ -302,7 +305,7 @@ bool yaml_shouldDisplayDebugInfo = false;
// VARIABLES FOR THE CONTROLLER // VARIABLES FOR THE CONTROLLER
// > A flag for which controller to use: // > 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 // The LQR Controller parameters for z-height
std::vector<float> yaml_gainMatrixThrust_2StateVector = { 0.98, 0.25}; std::vector<float> yaml_gainMatrixThrust_2StateVector = { 0.98, 0.25};
......
...@@ -3,17 +3,20 @@ ...@@ -3,17 +3,20 @@
# Max setpoint change per second # Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.60 # [meters] 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 error for z
max_setpoint_error_z: 0.4 max_setpoint_error_z: 0.4
# Max error for yaw angle # Max error for xy
max_setpoint_error_yaw_degrees: 60 max_setpoint_error_xy: 0.3
# Max {roll,pitch} angle request # Max {roll,pitch} angle request
max_roll_pitch_request_degrees: 30 max_roll_pitch_request_degrees: 30
# Max error for yaw angle
max_setpoint_error_yaw_degrees: 60
# Theshold for {roll,pitch} angle beyond # Theshold for {roll,pitch} angle beyond
# which the motors are turned off # which the motors are turned off
threshold_roll_pitch_for_turn_off_degrees: 70 threshold_roll_pitch_for_turn_off_degrees: 70
...@@ -46,7 +49,7 @@ takoff_integrator_on_time: 2.0 ...@@ -46,7 +49,7 @@ takoff_integrator_on_time: 2.0
landing_move_down_end_height_setpoint: 0.05 landing_move_down_end_height_setpoint: 0.05
landing_move_down_end_height_threshold: 0.10 landing_move_down_end_height_threshold: 0.10
# The time for: landing move-down # 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 # The thrust for landing spin motors
landing_spin_motors_thrust: 10000 landing_spin_motors_thrust: 10000
...@@ -88,7 +91,7 @@ shouldDisplayDebugInfo : false ...@@ -88,7 +91,7 @@ shouldDisplayDebugInfo : false
# A flag for which controller to use, defined as: # A flag for which controller to use, defined as:
# 1 - Rate controller # 1 - Rate controller
# 2 - Angle-Rate nested controller # 2 - Angle-Rate nested controller
controller_method : 1 controller_method : 2
# The LQR Controller parameters for z-height # The LQR Controller parameters for z-height
gainMatrixThrust_2StateVector : [ 0.98, 0.25] gainMatrixThrust_2StateVector : [ 0.98, 0.25]
...@@ -103,9 +106,9 @@ gainPitchRate_fromAngle : 4.00 ...@@ -103,9 +106,9 @@ gainPitchRate_fromAngle : 4.00
# The LQR Controller parameters for yaw # The LQR Controller parameters for yaw
gainYawRate_fromAngle : 2.30 gainYawRate_fromAngle : 2.30
# Integrator gains # Integrator gains
integratorGain_forThrust : 0.01 integratorGain_forThrust : 0.00
integratorGain_forTauXY : 0.01 integratorGain_forTauXY : 0.00
integratorGain_forTauYaw : 0.01 integratorGain_forTauYaw : 0.00
......
...@@ -487,8 +487,6 @@ void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response) ...@@ -487,8 +487,6 @@ void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response)
box_reached = true; 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) if (m_time_in_seconds > time_to_switch)
{ {
// Inform the user // Inform the user
...@@ -585,8 +583,6 @@ void computeResponse_for_landing_move_down(Controller::Response &response) ...@@ -585,8 +583,6 @@ void computeResponse_for_landing_move_down(Controller::Response &response)
// Call the LQR control function // 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, INTEGRATOR_FLAG_OFF);
ROS_INFO_STREAM("[DEFAULT CONTROLLER] m_current_stateInertialEstimate[2] = " << m_current_stateInertialEstimate[2] );
// Check if within the threshold of zero // Check if within the threshold of zero
if (m_current_stateInertialEstimate[2] < yaml_landing_move_down_end_height_threshold) 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) ...@@ -699,6 +695,10 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
void smoothSetpointChanges( float target_setpoint[4] , float (&current_setpoint)[4] ) void smoothSetpointChanges( float target_setpoint[4] , float (&current_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 // SMOOTH THE Z-COORIDINATE
// > Compute the max allowed change // > Compute the max allowed change
float max_for_z = yaml_max_setpoint_change_per_second_vertical / yaml_control_frequency; 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 ...@@ -892,16 +892,41 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI
// Store the current YAW in a local variable // Store the current YAW in a local variable
float temp_stateInertial_yaw = stateInertial[8]; 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 // Adjust the INERTIAL (x,y,z) position for the setpoint
stateInertial[0] = stateInertial[0] - setpoint[0]; stateInertialError[0] = stateInertial[0] - setpoint[0];
stateInertial[1] = stateInertial[1] - setpoint[1]; stateInertialError[1] = stateInertial[1] - setpoint[1];
stateInertial[2] = stateInertial[2] - setpoint[2]; 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 // Clip the z-coordination to within the specified bounds
if (stateInertial[2] > yaml_max_setpoint_error_z) if (stateInertialError[2] > yaml_max_setpoint_error_z)
stateInertial[2] = yaml_max_setpoint_error_z; stateInertialError[2] = yaml_max_setpoint_error_z;
else if (stateInertial[2] < -yaml_max_setpoint_error_z) else if (stateInertialError[2] < -yaml_max_setpoint_error_z)
stateInertial[2] = -yaml_max_setpoint_error_z; stateInertialError[2] = -yaml_max_setpoint_error_z;
// Fill in the yaw angle error // Fill in the yaw angle error
// > This error should be "unwrapped" to be in the range // > This error should be "unwrapped" to be in the range
...@@ -918,14 +943,14 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI ...@@ -918,14 +943,14 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI
yawError = -yaml_max_setpoint_error_yaw_radians; yawError = -yaml_max_setpoint_error_yaw_radians;
// > Finally, put the "yawError" into the "stateError" variable // > Finally, put the "yawError" into the "stateError" variable
stateInertial[8] = yawError; stateInertialError[8] = yawError;
// CONVERSION INTO BODY FRAME // CONVERSION INTO BODY FRAME
// Initialise a variable for the body frame error // Initialise a variable for the body frame error
float bodyFrameError[9]; float bodyFrameError[9];
// Call the function to convert the state erorr from // Call the function to convert the state erorr from
// the Inertial frame into the Body frame // 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); calculateControlOutput_viaLQR_givenError(bodyFrameError, response, integrator_flag);
} }
...@@ -1538,12 +1563,15 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle) ...@@ -1538,12 +1563,15 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// Max error for z // Max error for z
yaml_max_setpoint_error_z = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_z"); yaml_max_setpoint_error_z = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_z");
// Max error for yaw angle // Max error for xy
yaml_max_setpoint_error_yaw_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_yaw_degrees"); yaml_max_setpoint_error_xy = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_xy");
// Max {roll,pitch} angle request // Max {roll,pitch} angle request
yaml_max_roll_pitch_request_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_roll_pitch_request_degrees"); 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 // Theshold for {roll,pitch} angle beyond
// which the motors are turned off // 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"); yaml_threshold_roll_pitch_for_turn_off_degrees = getParameterFloat(nodeHandle_for_paramaters , "threshold_roll_pitch_for_turn_off_degrees");
......
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