To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 5d62babe authored by mastefan's avatar mastefan
Browse files

did some testing and param tuning

parent fed7aba7
......@@ -308,8 +308,8 @@ float m_setpoint[4] = {0.0,0.0,0.0,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0};
// > The velocities for (x,y,z) position, in that order
float m_velocity[3] = {0.0,0.0,0.0,0.0};
float m_velocity_for_controller[3] = {0.0,0.0,0.0,0.0};
float m_velocity[3] = {0.0,0.0,0.0};
float m_velocity_for_controller[3] = {0.0,0.0,0.0};
// > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f;
......@@ -546,7 +546,7 @@ float calculate_distance_in_xyz(std::vector<float> p1, std::vector<float> p2);
// CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response);
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response, float (&stateErrorBody)[12]);
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
......@@ -579,6 +579,7 @@ float computeMotorPolyBackward(float thrust);
// SETPOINT CHANGE CALLBACK
void setpointCallback(const Setpoint& newSetpoint);
void velocityCallback(const Setpoint& newVelocity);
void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint);
void publishCurrentSetpoint();
......
......@@ -13,9 +13,8 @@ vicon_frequency : 200
# controller_mode : 0: Basic Controller
# controller_mode : 1: Angle Controller
# controller_mode : 2: Trajectory Controller
controller_mode : 2
# controller_mode : 1: Angle Controller, Trajectory Controller
controller_mode : 1
......@@ -110,4 +109,5 @@ gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
# LQR: get baseline Angle from Velocity for feedforward (roll, pitch) [-20°,20°]=[-0.34906585, 0.34906585]
gainFeedforwardAnglefromVelocity: [-0.174532925, 0.174532925]
\ No newline at end of file
# [10°, 10°]=[-0.174532925, 0.174532925]
gainFeedforwardAnglefromVelocity: [-0.28, 0.28]
\ No newline at end of file
......@@ -182,25 +182,6 @@ void perControlCycleOperations()
m_setpoint_for_controller[3] = m_setpoint[3];
}
if (m_shouldClipVelocity)
{
// clip if velocity is too big
m_velocity_for_controller[0] = m_velocity[0];
m_velocity_for_controller[1] = m_velocity[1];
m_velocity_for_controller[2] = m_velocity[2];
float tot_velocity_xy = sqrt(abs(pow(m_velocity[0],2))+abs(pow(m_velocity[1],2)));
if(tot_velocity_xy > 1.0){
m_velocity_for_controller[0] = 1.0 * m_velocity[0]/tot_velocity_xy;
m_velocity_for_controller[1] = 1.0 * m_velocity[1]/tot_velocity_xy;
}
if(m_velocity[3]<-0.8){
m_velocity_for_controller[3] = -0.8;
}else if(m_velocity[3]>0.8){
m_velocity_for_controller[3] = 0.8;
}
}
}
......@@ -558,9 +539,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
break;
case DRONEX_STATE_FOLLOWING_TRAJECTORY:
if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_land[0] &&
abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_land[1] &&
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_land[2] ){
if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
}
......@@ -614,6 +595,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
*/
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
// "current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
calculateMSVelocity(request);
// THIS IS THE START OF THE "OUTER" CONTROL LOOP
......@@ -666,13 +654,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
}
setpointCallback(dronexSetpoint, dronexVelocity);
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
// "current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
setpointCallback(dronexSetpoint);
m_velocity_for_controller[0] = dronexVelocity.x;
m_velocity_for_controller[1] = dronexVelocity.y;
m_velocity_for_controller[2] = dronexVelocity.z;
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS: limits setpoint changes and velocity for controller
perControlCycleOperations();
......@@ -748,7 +733,7 @@ void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context){
// DroneX Controller
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response, float stateErrorBody[12]){
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response, float (&stateErrorBody)[12]){
if(controller_mode == 0){
......@@ -801,11 +786,30 @@ void calculateTrajectory(Controller::Request &request){
trajectory_setpoint[3] = 0;
// just some deltaT to look ahead
float deltaT = 1.0/m_vicon_frequency;
float deltaT = 0.2;
trajectory_velocity[0] = (trajectory_to_ms(request, total_time_since_start + deltaT)[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory_to_ms(request, total_time_since_start + deltaT)[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0;
// clip if velocity is too big
float tot_velocity_xy = sqrt(pow(trajectory_velocity[0],2)+pow(trajectory_velocity[1],2));
if(tot_velocity_xy > 1.0){
trajectory_velocity[0] = 1.0 * trajectory_velocity[0]/tot_velocity_xy;
trajectory_velocity[1] = 1.0 * trajectory_velocity[1]/tot_velocity_xy;
}
if(trajectory_velocity[2]<-0.8){
trajectory_velocity[2] = -0.8;
}else if(m_velocity[2]>0.8){
trajectory_velocity[2] = 0.8;
}
/*
for(int i=0; i<3; i++){
if(trajectory_velocity[i]-current_stateInertialEstimate[i+3] > 0.8 ){
trajectory_velocity[i] = 0.8 + current_stateInertialEstimate[i+3];
}else if(trajectory_velocity[i]-current_stateInertialEstimate[i+3] < -0.8 ){
trajectory_velocity[i] = -0.8 + current_stateInertialEstimate[i+3];
}
}*/
}
......@@ -837,7 +841,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// TODO possible change: choose xm1 based on relative position CF and mothership, CF velocity, mothership velocity
xm1[0] = xcf0[0] + xm1_x_distance; // xm1_x_distance hardcoded at the moment
xm1[1] = xcf0[1];
xm1[2] = request.otherCrazyflies[0].z;
xm1[2] = request.otherCrazyflies[0].z+0.6;
first_trajectory_calculation = false;
}
......@@ -854,7 +858,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
xms[0] = request.otherCrazyflies[0].x;
xms[1] = request.otherCrazyflies[0].y;
xms[2] = request.otherCrazyflies[0].z; // TODO add a certain height to not collide with mothership
xms[2] = request.otherCrazyflies[0].z+0.2; // TODO add a certain height to not collide with mothership
// Calculate the whole trajectory distance
......@@ -886,23 +890,23 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position on line between xcf0 and xm1
for(int i = 0; i < 3; i++){
trajectory_return[i] = xcf0[i] + t*trajectory_velocity_of_CF*(xm1[i] - xcf0[i]);
trajectory_return[i] = xcf0[i] + t*(xm1[i] - xcf0[i])/trajectory_t1;
}
}else if(trajectory_t1 < t && t <= trajectory_t2){ // Calculate position on line between xm1 and xm2
for(int i = 0; i < 3; i++){
trajectory_return[i] = xm1[i] + (t - trajectory_t1)*trajectory_velocity_of_CF*(xm2[i] - xm1[i]);
trajectory_return[i] = xm1[i] + (t - trajectory_t1)*(xm2[i] - xm1[i])/(trajectory_t2 - trajectory_t1);
}
}else if(trajectory_t2 < t && t <= trajectory_t3){ // Calculate position on line between xm2 and xms
for(int i = 0; i < 3; i++){
trajectory_return[i] = xm2[i] + (t - trajectory_t2)*trajectory_velocity_of_CF*(xms[i] - xm2[i]);
trajectory_return[i] = xm2[i] + (t - trajectory_t2)*(xms[i] - xm2[i])/(trajectory_t3 - trajectory_t2);
}
}else{
trajectory_return[0] = 0;
trajectory_return[1] = 0;
trajectory_return[2] = 0.3;
trajectory_return[0] = xms[0];
trajectory_return[1] = xms[1];
trajectory_return[2] = xms[2];
}
/*
trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4;
......@@ -910,8 +914,8 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
trajectory_return[0] = originX + cos(2*PI/trajectory_period * t)*(trajectory_x_radius);
trajectory_return[1] = originY + sin(2*PI/trajectory_period * t)*(trajectory_y_radius);
trajectory_return[2] = 0.5;
*/
return trajectory_return;
}
......@@ -1597,13 +1601,16 @@ float computeMotorPolyBackward(float thrust)
// ----------------------------------------------------------------------------------
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
void setpointCallback(const Setpoint& newSetpoint, const Setpoint& newVelocity)
void setpointCallback(const Setpoint& newSetpoint)
{
m_setpoint[0] = newSetpoint.x;
m_setpoint[1] = newSetpoint.y;
m_setpoint[2] = newSetpoint.z;
m_setpoint[3] = newSetpoint.yaw;
}
void velocityCallback(const Setpoint& newVelocity)
{
m_velocity[0] = newVelocity.x;
m_velocity[1] = newVelocity.y;
m_velocity[2] = newVelocity.z;
......
Markdown is supported
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