diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 17079bc7459ef337f068f8f3727b3b1333bdff90..a838f2f2b2a8b8ca5ea03ede4742ae9f56b9c5f3 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 ade107f9434fed42dd7acf161534bbded3bdbecd..61b6d33059caac567b9d635401da2ecb23fd3a4f 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 985e50d78718701ec2f558b245751fb0ea705567..3e6cd6d9bec1c49c03596b4d92bda2d4195cd8e3 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 e06138303ac1b149b65502d52b1233b3a47588d5..63445d1be767f07bddde21d22fa206d891c92277 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 5a901340b55b623911c3a332197fdd3374b421a8..4266a77b9b69f81534e15062e2555bf9d534f4b2 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 ff3dead3b29c12a4f325a86729d99ff6bac7adf6..979abc5c4bd047133bd520771cb712443331c16e 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
---