From 5e4abb5353842853f6fefa2b6bdc69e63006c720 Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Fri, 8 Sep 2017 17:43:34 +0200
Subject: [PATCH] fixed custom controller
---
.../src/CustomControllerService.cpp | 28 +++++++++++++++----
.../d_fall_pps/src/SafeControllerService.cpp | 13 ++++++---
2 files changed, 32 insertions(+), 9 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index 0d656944..228d8c31 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -32,6 +32,8 @@ float gravity_force;
CrazyflieData previous_location;
+std::vector<float> setpoint(4);
+
const float gainMatrixRoll[9] = {0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0};
const float gainMatrixPitch[9] = {1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0};
const float gainMatrixYaw[9] = {0, 0, 0, 0, 0, 0, 0, 0, 2.843099534};
@@ -74,7 +76,7 @@ void loadCustomParameters(ros::NodeHandle& nodeHandle)
// compute things that we will need after from these parameters
// force that we need to counteract gravity (mg)
- gravity_force = cf_mass * 9.81/1000; // in N
+ gravity_force = cf_mass * 9.81/(1000*4); // in N
}
float computeMotorPolyBackward(float thrust) {
@@ -126,6 +128,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// calculate the velocity based in the derivative of the position
+ //move coordinate system to make setpoint origin
+ request.ownCrazyflie.x -= setpoint[0];
+ request.ownCrazyflie.y -= setpoint[1];
+ request.ownCrazyflie.z -= setpoint[2];
+ float yaw = request.ownCrazyflie.yaw - setpoint[3];
+
float est[9];
est[0] = request.ownCrazyflie.x;
@@ -156,6 +164,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
thrustIntermediate -= gainMatrixThrust[i] * state[i];
}
+ ROS_INFO_STREAM("thrustIntermediate = " << thrustIntermediate);
+
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
@@ -169,6 +179,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
/*choosing the Crazyflie onBoard controller type.
@@ -189,10 +206,10 @@ void customYAMLloadedCallback(const std_msgs::Int32& msg)
}
void setpointCallback(const Setpoint& newSetpoint) {
- // setpoint[0] = newSetpoint.x;
- // setpoint[1] = newSetpoint.y;
- // setpoint[2] = newSetpoint.z;
- // setpoint[3] = newSetpoint.yaw;
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
}
@@ -200,6 +217,7 @@ int main(int argc, char* argv[]) {
//starting the ROS-node
ros::init(argc, argv, "CustomControllerService");
ros::NodeHandle nodeHandle("~");
+ loadCustomParameters(nodeHandle);
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index c9afacc4..efe421bf 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -166,9 +166,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ros::NodeHandle nodeHandle("~");
// loadSafeParameters(nodeHandle); // do not put this here, cannot control anymore
- CrazyflieData vicon = request.ownCrazyflie;
- float yaw_measured = request.ownCrazyflie.yaw;
+ float yaw_measured = request.ownCrazyflie.yaw;
//move coordinate system to make setpoint origin
request.ownCrazyflie.x -= setpoint[0];
@@ -176,7 +175,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
request.ownCrazyflie.z -= setpoint[2];
float yaw = request.ownCrazyflie.yaw - setpoint[3];
- //bag.write("Offset", ros::Time::now(), request.ownCrazyflie);
+ //bag.write("Offset", ros::Time::now(), request.ownCrazyflie);
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
@@ -232,7 +231,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
outYaw -= gainMatrixYaw[i] * state[i];
thrustIntermediate -= gainMatrixThrust[i] * state[i];
}
-
+ ROS_INFO_STREAM("thrustIntermediate = " << thrustIntermediate);
//INFORMATION: this ugly fix was needed for the older firmware
//outYaw *= 0.5;
@@ -250,6 +249,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + ffThrust[2]);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + ffThrust[3]);
+ ROS_INFO_STREAM("ffThrust" << ffThrust[0]);
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+
response.controlOutput.onboardControllerType = RATE_CONTROLLER;
previousLocation = request.ownCrazyflie;
--
GitLab