Commit 23f25e3b authored by bucyril's avatar bucyril
Browse files

moved parameter files, cleaned up safe controller

parent 243214f6
......@@ -4,18 +4,19 @@
</node>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/launch/studentParams.yaml" />
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/launch/studentParams.yaml" />
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
......
#equlibrium offset
feedforwardMotor: [37000, 37000, 37000, 37000]
#quadratic motor regression equation (a0, a1, a2)
motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
#feedback gain matrix
gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
......@@ -66,8 +66,8 @@ void ppsClientToController(ViconData data, bool autocontrolOn){
Setpoint goalLocation;
goalLocation.x = 0; //testvalue
goalLocation.y = 0; //testvalue
goalLocation.z = 0.3; //testvalue
goalLocation.y = 0.5; //testvalue
goalLocation.z = 0.5; //testvalue
goalLocation.yaw = 0;
srvRate.request.crazyflieLocation = data;
......
......@@ -15,135 +15,139 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
using namespace d_fall_pps;
#define PI 3.1415926535
#define RATE_CONTROLLER 0
using namespace d_fall_pps;
ViconData previousLocation;
ControlCommand previousCommand;
float ffThrust1; float ffThrust2; float ffThrust3; float ffThrust4;
const float a2=2.130295e-11;
const float a1=1.032633e-6;
const float a0=5.484560e-4;
const float saturationThrust = a2*12000*12000 + a1*12000 + a0;
const float feedforwardm1 = 37000;
const float feedforwardm2 = 37000;
const float feedforwardm3 = 37000;
const float feedforwardm4 = 37000;
void ffSetup()
{
ffThrust1 = a2*feedforwardm1*feedforwardm1+a1*feedforwardm1+a0;
ffThrust2 = a2*feedforwardm2*feedforwardm2+a1*feedforwardm2+a0;
ffThrust3 = a2*feedforwardm3*feedforwardm3+a1*feedforwardm3+a0;
ffThrust4 = a2*feedforwardm4*feedforwardm4+a1*feedforwardm4+a0;
}
std::vector<float> ffThrust(4);
std::vector<float> feedforwardMotor(4);
std::vector<float> motorPoly(3);
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
ROS_INFO("calculate control output");
ffSetup(); //to change that it only does this once at intialization
std::vector<float> gainMatrixRoll(9);
std::vector<float> gainMatrixPitch(9);
std::vector<float> gainMatrixYaw(9);
std::vector<float> gainMatrixThrust(9);
float saturationThrust;
ViconData vicon = request.crazyflieLocation;
Setpoint goal = request.setpoint;
ViconData previousLocation;
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
if(!nodeHandle.getParam(name, val)){
ROS_ERROR_STREAM("missing parameter '" << name << "'");
}
if(val.size() != length) {
ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
}
}
ROS_INFO("request received with following ViconData");
ROS_INFO_STREAM(vicon);
ROS_INFO("the goal setpoint is:");
ROS_INFO_STREAM(request.setpoint);
void loadParameters(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];
}
saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
//add/calculate safeController
//K matrix for kLqrOuter
const float k[] = {
0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0,
1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0,
0, 0, 0, 0, 0, 0, 0, 0, 2.843099534,
0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0
};
loadParameterFloatVector(nodeHandle, "gainMatrixRoll", gainMatrixRoll, 9);
loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
}
float px = request.crazyflieLocation.x - request.setpoint.x;
float py = request.crazyflieLocation.y - request.setpoint.y;
float pz = request.crazyflieLocation.z - request.setpoint.z;
float computeMotorPolyBackward(float thrust) {
return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
}
void estimateState(Controller::Request &request, float (&est)[9]) {
est[0] = request.crazyflieLocation.x - request.setpoint.x;
est[1] = request.crazyflieLocation.y - request.setpoint.y;
est[2] = request.crazyflieLocation.z - request.setpoint.z;
//linear approximation of derivative of position (no Estimator implemented...)
float vx = (request.crazyflieLocation.x - previousLocation.x) / request.crazyflieLocation.acquiringTime;
float vy = (request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime;
float vz = (request.crazyflieLocation.z - previousLocation.z) / request.crazyflieLocation.acquiringTime;
ROS_INFO_STREAM("px: " << px);
ROS_INFO_STREAM("py: " << py);
ROS_INFO_STREAM("pz: " << pz);
ROS_INFO_STREAM("vx: " << vx);
ROS_INFO_STREAM("vy: " << vy);
ROS_INFO_STREAM("vz: " << vz);
float roll = request.crazyflieLocation.roll;
float pitch = request.crazyflieLocation.pitch;
est[3] = (request.crazyflieLocation.x - previousLocation.x) / request.crazyflieLocation.acquiringTime;
est[4] = (request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime;
est[5] = (request.crazyflieLocation.z - previousLocation.z) / request.crazyflieLocation.acquiringTime;
est[6] = request.crazyflieLocation.roll;
est[7] = request.crazyflieLocation.pitch;
float yaw = request.crazyflieLocation.yaw - request.setpoint.yaw;
const float PI = 3.141592535f;
while(yaw > PI) yaw -= 2 * PI;
while(yaw < -PI) yaw += 2 * PI;
est[8] = yaw;
}
ROS_INFO_STREAM("yaw: " << yaw);
//convert into body frame
void convertIntoBodyFrame(Controller::Request &request, float est[9], float (&state)[9]) {
float sinYaw = sin(request.crazyflieLocation.yaw);
float cosYaw = cos(request.crazyflieLocation.yaw);
state[0] = est[0] * cosYaw + est[1] * sinYaw;
state[1] = -est[0] * sinYaw + est[1] * cosYaw;
state[2] = est[2];
state[3] = est[3] * cosYaw + est[4] * sinYaw;
state[4] = -est[3] * sinYaw + est[4] * cosYaw;
state[5] = est[5];
state[6] = est[6];
state[7] = est[7];
state[8] = est[8];
}
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
ROS_INFO("calculate control output");
ViconData vicon = request.crazyflieLocation;
Setpoint goal = request.setpoint;
float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
estimateState(request, est);
float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
state[0] = px * cosYaw + py * sinYaw;
state[1] = -px * sinYaw + py * cosYaw;
state[2] = pz;
state[3] = vx * cosYaw + vy * sinYaw;
state[4] = -vx * sinYaw + vy * cosYaw;
state[5] = vz;
state[6] = roll;
state[7] = pitch;
state[8] = yaw;
//roll,pitch,yaw calculations
float resRoll = 0;
float resPitch = 0;
float resYaw = 0;
convertIntoBodyFrame(request, est, state);
//calculate feedback
float outRoll = 0;
float outPitch = 0;
float outYaw = 0;
float thrustIntermediate = 0;
for(int i = 0; i < 9; ++i) {
resRoll -= k[i + 0] * state[i];
resPitch -= k[i + 9] * state[i];
resYaw -= k[i + 18] * state[i];
thrustIntermediate -= k[i + 27] * state[i];
outRoll -= gainMatrixRoll[i] * state[i];
outPitch -= gainMatrixPitch[i] * state[i];
outYaw -= gainMatrixYaw[i] * state[i];
thrustIntermediate -= gainMatrixThrust[i] * state[i];
}
response.controlOutput.roll = resRoll;
response.controlOutput.pitch = resPitch;
response.controlOutput.yaw = resYaw;
ROS_INFO_STREAM("x: " << state[0]);
ROS_INFO_STREAM("vx: " << state[3]);
ROS_INFO_STREAM("pitch: " << state[7]);
ROS_INFO_STREAM("roll: " << outRoll);
ROS_INFO_STREAM("pitch: " << outPitch);
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
ROS_INFO_STREAM("thrustIntermediate before: " << thrustIntermediate);
if(thrustIntermediate > saturationThrust)
thrustIntermediate = saturationThrust;
else if(thrustIntermediate < -saturationThrust)
thrustIntermediate = -saturationThrust;
response.controlOutput.motorCmd1 = (-a1+sqrt(a1*a1-4*a2*(a0-(thrustIntermediate+ffThrust1))))/(2*a2);
response.controlOutput.motorCmd2 = (-a1+sqrt(a1*a1-4*a2*(a0-(thrustIntermediate+ffThrust2))))/(2*a2);
response.controlOutput.motorCmd3 = (-a1+sqrt(a1*a1-4*a2*(a0-(thrustIntermediate+ffThrust3))))/(2*a2);
response.controlOutput.motorCmd4 = (-a1+sqrt(a1*a1-4*a2*(a0-(thrustIntermediate+ffThrust4))))/(2*a2);
previousCommand = response.controlOutput;
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.onboardControllerType = 0; //RATE
response.controlOutput.onboardControllerType = RATE_CONTROLLER;
previousLocation = request.crazyflieLocation;
......@@ -155,9 +159,10 @@ int main(int argc, char* argv[]) {
ros::init(argc, argv, "SafeControllerService");
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready to send");
ROS_INFO("SafeControllerService ready");
ros::spin();
return 0;
......
......@@ -73,7 +73,7 @@ int main(int argc, char* argv[]) {
}
//set data stream parameters
client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull); //phfriedl: maybe ServerPush instead of ClientPull for less latency?
client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush); //phfriedl: maybe ServerPush instead of ClientPull for less latency?
client.EnableSegmentData();
client.EnableMarkerData();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment