Commit f75442d6 authored by bucyril's avatar bucyril
Browse files

removed setpoint to be a parameter for controller, added an option for setpoint in the param file

parent 7be82218
......@@ -89,7 +89,6 @@ add_message_files(
FILES
ViconData.msg
ControlCommand.msg
Setpoint.msg
CrazyflieContext.msg
AreaBounds.msg
)
......
float64 x
float64 y
float64 z
float64 yaw
......@@ -10,3 +10,6 @@ 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]
#setpoint in meters (x, y, z, yaw)
setpoint: [0.2, 0.2, 0.6, 0]
......@@ -75,15 +75,7 @@ void ppsClientToController(ViconData data, bool autocontrolOn){
//for the moment use safecontroller for TESTING
Controller srvRate;
Setpoint goalLocation;
goalLocation.x = 0; //testvalue
goalLocation.y = 0.5; //testvalue
goalLocation.z = 0.4; //testvalue
goalLocation.yaw = 0;
srvRate.request.crazyflieLocation = data;
srvRate.request.setpoint = goalLocation;
//TODO:
//return control commands
......@@ -132,7 +124,7 @@ void viconCallback(const ViconData& data){
if(data.crazyflieName == cflie){
ROS_INFO_STREAM(data);
//forward data to safety check
bool autocontrolOn = safetyCheck(data);
bool autocontrolOn = false;//safetyCheck(data);
ppsClientToController(data, autocontrolOn);
}
else {
......
......@@ -25,6 +25,7 @@
#define PI 3.1415926535
#define RATE_CONTROLLER 0
#define RAD2DEG 57.3
#define VEL_AVERAGE_SIZE 10
using namespace d_fall_pps;
......@@ -36,8 +37,14 @@ std::vector<float> gainMatrixRoll(9);
std::vector<float> gainMatrixPitch(9);
std::vector<float> gainMatrixYaw(9);
std::vector<float> gainMatrixThrust(9);
std::vector<float> setpoint(4);
float saturationThrust;
//averaging test, will probably be replaced by Kalman filter
//float prevVelocities[VEL_AVERAGE_SIZE * 3];
//int velocityIndex = 0;
ViconData previousLocation;
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
......@@ -62,6 +69,7 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
loadParameterFloatVector(nodeHandle, "setpoint", setpoint, 4);
}
float computeMotorPolyBackward(float thrust) {
......@@ -69,20 +77,39 @@ float computeMotorPolyBackward(float thrust) {
}
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;
est[0] = request.crazyflieLocation.x - setpoint[0];
est[1] = request.crazyflieLocation.y - setpoint[1];
est[2] = request.crazyflieLocation.z - setpoint[2];
//linear approximation of derivative of position (no Estimator implemented...)
/*prevVelocities[3 * velocityIndex + 0] = (request.crazyflieLocation.x - previousLocation.x) / request.crazyflieLocation.acquiringTime;
prevVelocities[3 * velocityIndex + 1] = (request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime;
prevVelocities[3 * velocityIndex + 2] = (request.crazyflieLocation.z - previousLocation.z) / request.crazyflieLocation.acquiringTime;
velocityIndex = (velocityIndex + 1) % VEL_AVERAGE_SIZE;
est[3] = 0;
est[4] = 0;
est[5] = 0;
for(int i = 0; i < VEL_AVERAGE_SIZE; ++i) {
est[3] += prevVelocities[3 * i + 0] / ((float) VEL_AVERAGE_SIZE);
est[4] += prevVelocities[3 * i + 1] / ((float) VEL_AVERAGE_SIZE);
est[5] += prevVelocities[3 * i + 2] / ((float) VEL_AVERAGE_SIZE);
}*/
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;
//ROS_INFO_STREAM("velocityIndex: " << velocityIndex);
ROS_INFO_STREAM("vx: " << est[3]);
ROS_INFO_STREAM("vy: " << est[4]);
ROS_INFO_STREAM("vz: " << est[5]);
est[6] = request.crazyflieLocation.roll;
est[7] = request.crazyflieLocation.pitch;
//ROS_INFO_STREAM("crazyflieyaw: " << request.crazyflieLocation.yaw);
//ROS_INFO_STREAM("setpointyaw: " << request.setpoint.yaw);
float yaw = request.crazyflieLocation.yaw - request.setpoint.yaw;
float yaw = request.crazyflieLocation.yaw - setpoint[4];
//ROS_INFO_STREAM("differenceyaw: " << yaw);
while(yaw > PI) yaw -= 2 * PI;
......@@ -109,7 +136,6 @@ void convertIntoBodyFrame(Controller::Request &request, float est[9], float (&st
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
ViconData vicon = request.crazyflieLocation;
Setpoint goal = request.setpoint;
float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
estimateState(request, est);
......
ViconData crazyflieLocation
Setpoint setpoint
---
ControlCommand controlOutput
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