From 60e963960e3ef9094c659176f8cf06e48d9c2407 Mon Sep 17 00:00:00 2001
From: beuchatp <beuchatp@control.ee.ethz.ch>
Date: Thu, 14 Feb 2019 00:19:52 +0100
Subject: [PATCH] Adjusted landing move down function of default controller to
make the landing smoother
---
.../include/nodes/DefaultControllerService.h | 2 +-
.../dfall_pkg/param/DefaultController.yaml | 4 +-
.../src/nodes/DefaultControllerService.cpp | 46 ++++++++++---------
3 files changed, 27 insertions(+), 25 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index c20633f2..7ba8f80a 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -178,7 +178,7 @@ float m_time_in_seconds = 0.0;
// PARAMETERS FROM THE YAML FILE
// Max setpoint change per second
-float yaml_max_setpoint_change_per_second_horizontal = 0.80;
+float yaml_max_setpoint_change_per_second_horizontal = 0.60;
float yaml_max_setpoint_change_per_second_vertical = 0.30;
// Max error for z
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
index 3a5105d6..38e728fc 100644
--- a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -2,7 +2,7 @@
# PARAMTERS FOR THE TAKE-OFF AND LANDING MANOEUVRES
# Max setpoint change per second
-max_setpoint_change_per_second_horizontal : 0.80 # [meters]
+max_setpoint_change_per_second_horizontal : 0.60 # [meters]
max_setpoint_change_per_second_vertical : 0.30 # [meters]
# Max error for z
@@ -46,7 +46,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: 2.0
+landing_move_down_time_max: 4.0
# The thrust for landing spin motors
landing_spin_motors_thrust: 10000
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 47463df7..b7d42b07 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -556,46 +556,48 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
void computeResponse_for_landing_move_down(Controller::Response &response)
{
// Initialise a static variable for the starting height and yaw
- static float initial_height = 0.4;
+ static float target_landing_setpoint[4] = {0.0,0.0,0.4,0.0};
// Check if the state "just recently" changed
if (m_current_state_changed)
{
// PERFORM "ONE-OFF" OPERATIONS HERE
- // Set the current (x,y,yaw) location as the setpoint
+ // Set the current (x,y,z,yaw) location as the setpoint
m_setpoint_for_controller[0] = m_current_stateInertialEstimate[0];
m_setpoint_for_controller[1] = m_current_stateInertialEstimate[1];
+ m_setpoint_for_controller[2] = m_current_stateInertialEstimate[2];
m_setpoint_for_controller[3] = m_current_stateInertialEstimate[8];
- // Store the current z
- initial_height = m_current_stateInertialEstimate[2];
+ // Make the target setpoint the same for (x,y,yaw)
+ target_landing_setpoint[0] = m_setpoint_for_controller[0];
+ target_landing_setpoint[1] = m_setpoint_for_controller[1];
+ target_landing_setpoint[3] = m_setpoint_for_controller[3];
+ // Set the target height
+ target_landing_setpoint[2] = yaml_landing_move_down_end_height_setpoint;
// Set the change flag back to false
m_current_state_changed = false;
// Inform the user
ROS_INFO_STREAM("[DEFAULT CONTROLLER] state \"landing move-down\" started with \"m_setpoint_for_controller\" (x,y,z,yaw) = ( " << m_setpoint_for_controller[0] << ", " << m_setpoint_for_controller[1] << ", " << m_setpoint_for_controller[2] << ", " << m_setpoint_for_controller[3] << ")");
}
- // Compute the time elapsed as a proportion
- float time_elapsed_proportion = m_time_in_seconds / (0.8*yaml_landing_move_down_time_max);
- if (time_elapsed_proportion > 1.0)
- time_elapsed_proportion = 1.0;
-
- // Compute the z-height setpoint
- m_setpoint_for_controller[2] = initial_height + time_elapsed_proportion * (yaml_landing_move_down_end_height_setpoint-initial_height);
+ // Smooth out any setpoint changes
+ 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);
- // // Check if within the threshold of zero
- // if (m_current_stateInertialEstimate[2] < yaml_landing_move_down_end_height_threshold)
- // {
- // // Inform the user
- // ROS_INFO("[DEFAULT CONTROLLER] Switch to state: landing spin motors");
- // // Reset the time variable
- // m_time_in_seconds = 0.0;
- // // Update the state accordingly
- // m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS;
- // m_current_state_changed = true;
- // }
+ 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)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: landing spin motors");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS;
+ m_current_state_changed = true;
+ }
// Change to landing spin motors if the timeout is reached
if ( m_time_in_seconds > yaml_landing_move_down_time_max )
--
GitLab