From 7bbe103099e30b21b1104d2b2f06e352e4567109 Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Fri, 10 Nov 2017 07:59:13 +0100
Subject: [PATCH] added mass to safe controller
---
pps_ws/src/d_fall_pps/param/Crazyflie.db | 2 +-
pps_ws/src/d_fall_pps/param/SafeController.yaml | 3 +--
.../d_fall_pps/src/SafeControllerService.cpp | 17 ++++++++---------
3 files changed, 10 insertions(+), 12 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index ac310142..d42bf657 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -1 +1 @@
-5,PPS_CF05,0/32/2M/E7E7E7E705,0,-0.868587,-0.673046,-0.2,0.686782,0.686782,2
+1,PPS_CF01,0/0/2M/E7E7E7E701,0,0.108447,-0.0602481,-0.2,1.85966,1.11258,1.2
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index dd2714c7..43a4ca86 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/pps_ws/src/d_fall_pps/param/SafeController.yaml
@@ -1,6 +1,5 @@
#equlibrium offset
-feedforwardMotor: [41000, 41000, 41000, 41000]
-
+mass = 29
#quadratic motor regression equation (a0, a1, a2)
motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index b2b97da3..9c3cef92 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -53,6 +53,7 @@ using namespace d_fall_pps;
std::vector<float> ffThrust(4);
std::vector<float> feedforwardMotor(4);
+float cf_mass;
std::vector<float> motorPoly(3);
std::vector<float> gainMatrixRoll(9);
@@ -85,12 +86,10 @@ void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std
}
void loadSafeParameters(ros::NodeHandle& nodeHandle) {
- loadParameterFloatVector(nodeHandle, "feedforwardMotor", feedforwardMotor, 4);
loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
-
- for(int i = 0; i < 4; ++i) {
- ffThrust[i] = motorPoly[2] * feedforwardMotor[i] * feedforwardMotor[i] + motorPoly[1] * feedforwardMotor[i] + motorPoly[0];
- }
+ cf_mass = getFloatParameter(nodeHandle, "mass");
+ // force that we need to counteract gravity (mg)
+ gravity_force = cf_mass * 9.81/(1000*4); // in N
saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
loadParameterFloatVector(nodeHandle, "gainMatrixRoll", gainMatrixRoll, 9);
@@ -261,10 +260,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
else if(thrustIntermediate < -saturationThrust)
thrustIntermediate = -saturationThrust;
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + ffThrust[0]);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + ffThrust[1]);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + ffThrust[2]);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + ffThrust[3]);
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
// ROS_INFO_STREAM("ffThrust" << ffThrust[0]);
// ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
--
GitLab