From 67970ddf7fff690279f5897952b6367828262448 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Fri, 27 Mar 2020 10:55:44 +0100
Subject: [PATCH] Added a flag in the controller call so that the flying agent
client node notifies the controller being call of whether this is the first
call or not. The default controller and student controller correctly respond
to this flag, while the other controllers need to be updated to take it into
account
---
.../include/nodes/DefaultControllerService.h | 6 +--
.../include/nodes/FlyingAgentClient.h | 9 ++--
.../src/nodes/DefaultControllerService.cpp | 47 ++++++++++++++-----
.../dfall_pkg/src/nodes/FlyingAgentClient.cpp | 19 ++++++++
.../src/nodes/StudentControllerService.cpp | 19 ++++++--
dfall_ws/src/dfall_pkg/srv/Controller.srv | 1 +
6 files changed, 80 insertions(+), 21 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 17079bc7..a838f2f2 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -426,7 +426,7 @@ bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Re
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
// > For the normal state
-void computeResponse_for_normal(Controller::Response &response);
+void computeResponse_for_normal(bool isFirstControllerCall, Controller::Response &response);
// > For the standby state (also used for unknown state)
void computeResponse_for_standby(Controller::Response &response);
// > For the take-off phases
@@ -449,8 +449,8 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate);
// PUBLISHING OF THE DEBUG MESSAGE
void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index ade107f9..61b6d330 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -175,15 +175,18 @@ ros::Timer m_timer_land_complete;
// VARIABLES RELATING TO CONTROLLER SELECTION
+// > Boolean indicating that the controller service clients
+// have been successfully created
+bool m_controllers_avialble = false;
// > Integer indicating the controller that is to be
// used in when motion capture data is received
int m_instant_controller;
// > Pointer to the controller service client that
// agrees with the "m_instant_controller" variable
ros::ServiceClient* m_instant_controller_service_client;
-// > Boolean indicating that the controller service clients
-// have been successfully created
-bool m_controllers_avialble = false;
+// > Boolean indicating that this is the first call to
+// the instant controller
+bool m_isFirstCall_to_instant_controller = false;
// > Timer for creating the controller service client after
// some delay
ros::Timer m_timer_for_createAllControllerServiceClients;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 985e50d7..3e6cd6d9 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -188,7 +188,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
switch (m_current_state)
{
case DEFAULT_CONTROLLER_STATE_NORMAL:
- computeResponse_for_normal(response);
+ computeResponse_for_normal(request.isFirstControllerCall, response);
break;
case DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS:
@@ -295,10 +295,10 @@ void computeResponse_for_standby(Controller::Response &response)
}
-void computeResponse_for_normal(Controller::Response &response)
+void computeResponse_for_normal(bool isFirstControllerCall,Controller::Response &response)
{
// Check if the state "just recently" changed
- if (m_current_state_changed)
+ if (m_current_state_changed || isFirstControllerCall)
{
// PERFORM "ONE-OFF" OPERATIONS HERE
// Set the "m_setpoint_for_controller" variable
@@ -794,11 +794,11 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// RUN THE FINITE DIFFERENCE ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
@@ -853,7 +853,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate)
{
// PUT IN THE CURRENT MEASUREMENT DIRECTLY
// > for (x,y,z) position
@@ -866,15 +866,26 @@ void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
m_stateInterialEstimate_viaFiniteDifference[8] = m_current_xzy_rpy_measurement[5];
// COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
- // > for (x,y,z) velocities
- m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
+ // > Only if this is NOT the first call to the controller
+ if (!isFirstUpdate)
+ {
+ // > for (x,y,z) velocities
+ m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
+ }
+ else
+ {
+ // Set the velocities to zero
+ m_stateInterialEstimate_viaFiniteDifference[3] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[4] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[5] = 0.0;
+ }
}
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate)
{
// PERFORM THE KALMAN FILTER UPDATE STEP
// > First take a copy of the estimator state
@@ -883,6 +894,18 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
{
temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
+ // If this is the "first update", then:
+ if (isFirstUpdate)
+ {
+ // Set the positions to the current measurement
+ temp_PMKFstate[0] = m_current_xzy_rpy_measurement[0];
+ temp_PMKFstate[1] = m_current_xzy_rpy_measurement[1];
+ temp_PMKFstate[2] = m_current_xzy_rpy_measurement[2];
+ // Set the velocities to zero
+ temp_PMKFstate[3] = 0.0;
+ temp_PMKFstate[4] = 0.0;
+ temp_PMKFstate[5] = 0.0;
+ }
// > Now perform update for:
// > x position and velocity:
m_stateInterialEstimate_viaPointMassKalmanFilter[0] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[0];
@@ -1358,6 +1381,8 @@ void setNewSetpoint(float x, float y, float z, float yaw)
msg.y = m_setpoint[1];
msg.z = m_setpoint[2];
msg.yaw = m_setpoint[3];
+ // Put the current state into the "buttonID" field
+ msg.buttonID = m_current_state;
// Publish the message
m_setpointChangedPublisher.publish(msg);
}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index e0613830..63445d1b 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -190,6 +190,17 @@ void viconCallback(const ViconData& viconData)
{
controllerCall.request.otherCrazyflies.push_back(poseDataForOtherAgent);
}
+
+ // Fill in the "isFirstControllerCall" field
+ if (m_isFirstCall_to_instant_controller)
+ {
+ controllerCall.request.isFirstControllerCall = true;
+ m_isFirstCall_to_instant_controller = false;
+ }
+ else
+ {
+ controllerCall.request.isFirstControllerCall = false;
+ }
// PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
@@ -220,6 +231,9 @@ void viconCallback(const ViconData& viconData)
{
// Set the DEFAULT controller to be active
setInstantController(DEFAULT_CONTROLLER);
+ // Update the "isFirstControllerCall" field
+ controllerCall.request.isFirstControllerCall = true;
+ m_isFirstCall_to_instant_controller = false;
// Try the controller call
isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
// Inform the user is not successful
@@ -823,6 +837,11 @@ void setInstantController(int controller)
// Update the class variable
m_instant_controller = controller;
+ // Set the class variable indicating that the next
+ // controller call should be flagged as the first
+ // call for this instant controller
+ m_isFirstCall_to_instant_controller = true;
+
// Point to the correct service client
switch(controller)
{
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index 5a901340..4266a77b 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -188,10 +188,21 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
// Compute an estimate of the velocity
- // > As simply the derivative between the current and previous position
- stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
- stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
- stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+ // > But only if this is NOT the first call to the controller
+ if (!request.isFirstControllerCall)
+ {
+ // > Compute as simply the derivative between the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+ }
+ else
+ {
+ // Set the velocities to zero
+ stateErrorInertial[3] = 0.0;
+ stateErrorInertial[4] = 0.0;
+ stateErrorInertial[5] = 0.0;
+ }
// Fill in the roll and pitch angle measurements directly
stateErrorInertial[6] = request.ownCrazyflie.roll;
diff --git a/dfall_ws/src/dfall_pkg/srv/Controller.srv b/dfall_ws/src/dfall_pkg/srv/Controller.srv
index ff3dead3..979abc5c 100644
--- a/dfall_ws/src/dfall_pkg/srv/Controller.srv
+++ b/dfall_ws/src/dfall_pkg/srv/Controller.srv
@@ -1,3 +1,4 @@
+bool isFirstControllerCall
CrazyflieData ownCrazyflie
CrazyflieData[] otherCrazyflies
---
--
GitLab