From 759c4da33ebdedbcb21c5961f1052ce580d50c3e Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Fri, 3 Apr 2020 22:44:26 +0200
Subject: [PATCH] Updated also the Student Controller to be compatible with the
new type of control command package
---
.../src/nodes/StudentControllerService.cpp | 130 ++++++++++--------
1 file changed, 72 insertions(+), 58 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index 742c5484..d9983fb2 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -100,7 +100,8 @@ float yaml_ki_z = 1.0
//
// The arument "request" is a structure provided to this service with the following two
// properties:
-// request.ownCrazyflie
+//
+// >> request.ownCrazyflie
// This property is itself a structure of type "CrazyflieData", which is defined in the
// file "CrazyflieData.msg", and has the following properties
// string crazyflieName
@@ -115,49 +116,62 @@ float yaml_ki_z = 1.0
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
-// request.otherCrazyflies
+// >> request.otherCrazyflies
// This property is an array of "CrazyflieData" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
// service by this function, it has only the following property
-// response.ControlCommand
-// This property is iteself a structure of type "ControlCommand", which is defined in
-// the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// >> response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is
+// defined in the file "ControlCommand.msg", and has the following properties:
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 ... same for motor 2
+// uint16 motorCmd3 ... same for motor 3
+// uint16 motorCmd4 ... same for motor 4
+// uint8 xControllerMode The mode sent to the Crazyflie for what controller to run for the body frame x-axis
+// uint8 yControllerMode ... same body frame y-axis
+// uint8 zControllerMode ... same body frame z-axis
+// uint8 yawControllerMode ... same body frame yaw
+// float32 xControllerSetpoint The setpoint sent to the Crazyflie for the body frame x-axis controller
+// float32 yControllerSetpoint ... same body frame y-axis
+// float32 zControllerSetpoint ... same body frame z-axis
+// float32 yawControllerSetpoint ... same body frame yaw
//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines"
-// section in the header file, they are:
-// - CF_COMMAND_TYPE_MOTORS
-// - CF_COMMAND_TYPE_RATE
-// - CF_COMMAND_TYPE_ANGLE
+// IMPORTANT NOTES FOR "{x,y,z,yaw}ControllerMode" AND AXIS CONVENTIONS
+// > The allowed values for "{x,y,z,yaw}ControllerMode" are in the
+// "Constants.h" header file, they are:
+// - CF_ONBOARD_CONTROLLER_MODE_OFF
+// - CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE
+// - CF_ONBOARD_CONTROLLER_MODE_ANGLE
+// - CF_ONBOARD_CONTROLLER_MODE_VELOCITY
+// - CF_ONBOARD_CONTROLLER_MODE_POSITION
//
-// > For completeing the class exercises it is only required to use
-// the CF_COMMAND_TYPE_RATE option.
+// > The most common option to use for the {x,y,yaw} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE option.
//
-// > For the CF_COMMAND_TYPE_RATE optoin:
-// 1) the ".roll", ".ptich", and ".yaw" properties of
-// "response.ControlCommand" specify the angular rate in
-// [radians/second] that will be requested from the PID controllers
-// running in the Crazyflie 2.0 firmware.
-// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// > The most common option to use for the {z} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_OFF option, and thus the
+// body frame z-axis is controlled by the motorCmd{1,2,3,4}
+// values that you set.
+//
+// > When the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE is selected, then:
+// 1) the ".xControllerSetpoint", ".yControllerSetpoint", and
+// ".yawControllerSetpoint" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested
+// from the PID controllers running in the Crazyflie 2.0 firmware.
+// 2) the axis convention for the roll, pitch, and yaw body rates,
+// i.e., as set in the {y,x,yaw}ControllerSetpoint properties of
+// the "response.ControlCommand" that you return, is a right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the ASCII art below depicts this convention.
+// 3) the ".motorCmd1" to ".motorCmd4" properties of
// "response.ControlCommand" are the baseline motor commands
// requested from the Crazyflie, with the adjustment for body rates
// being added on top of this in the firmware (i.e., as per the
// code of the "distribute_power" found in the firmware).
-// 3) the axis convention for the roll, pitch, and yaw body rates
-// returned in "response.ControlCommand" should use right-hand
-// coordinate axes with x-forward and z-upwards (i.e., the positive
-// z-axis is aligned with the direction of positive thrust). To
-// assist, the following is an ASCII art of this convention.
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -280,8 +294,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
// Put the computed yaw rate into the "response" variable
- response.controlOutput.yaw = yawRate_forResponse;
-
+ // > The "controller mode" specifies that it is an
+ // angular rate setpoint
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
@@ -304,8 +320,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
thrustAdjustment -= m_gainMatrixThrust[i] * stateErrorBody[i];
}
- // Put the computed thrust adjustment into the "response" variable,
- // as well as adding the feed-forward thrust to counter-act gravity.
+ // Put the computed thrust adjustment into the "response" variable.
+ // > On top of the thrust adjustment, we must add the feed-forward thrust
++ // to counter-act gravity.
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
@@ -320,6 +337,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.motorCmd3 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
+ // Set the onboard z-controller to be OFF
+ // > This is because we commands the motor thrusts and
+ // hence an "inner" control loop is NOT needed onboard
+ // to control the z-height
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+
// **************************************
@@ -342,7 +365,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
// Put the computed pitch rate into the "response" variable
- response.controlOutput.pitch = pitchRate_forResponse;
+ // > The "controller mode" specifies that it is an
+ // angular rate setpoint
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
@@ -367,17 +393,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
// Put the computed roll rate into the "response" variable
- response.controlOutput.roll = rollRate_forResponse;
-
-
-
- // PREPARE AND RETURN THE VARIABLE "response"
-
- // Choose the controller type use on-board the Crazyflie,
- // it can be either be Motor, Rate, or Angle based
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+ // > The "controller mode" specifies that it is an
+ // angular rate setpoint
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.xControllerSetpoint = rollRate_forResponse;
@@ -429,21 +448,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
// ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
// ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
- // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
+ // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
// ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
- // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
+ // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
// ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
// An example of "printing out" the control actions computed.
// ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- // ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- // ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- // ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
-
- // An example of "printing out" the "thrust-to-command" conversion parameters.
- // ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
- // ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
- // ROS_INFO_STREAM("motorPoly 2:" << yaml_motorPoly[2]);
+ // ROS_INFO_STREAM("controlOutput.xControllerSetpoint = " << response.controlOutput.xControllerSetpoint);
+ // ROS_INFO_STREAM("controlOutput.yControllerSetpoint = " << response.controlOutput.yControllerSetpoint);
+ // ROS_INFO_STREAM("controlOutput.yawControllerSetpoint = " << response.controlOutput.yawControllerSetpoint);
// An example of "printing out" the per motor 16-bit command computed.
// ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
--
GitLab