Commit 71770f86 authored by maruggv's avatar maruggv
Browse files

Merge branch 'valentin' into mothership: several yaml params added

parents a08a8564 fdf671d7
......@@ -207,11 +207,10 @@ int integratorFlag = DRONEX_INTEGRATOR_OFF;
float integrator_sum_XYZ[3] = {0,0,0};
// Controller Mode
// Controller Mode: fetched from .yaml
// 0:
// 1: Nested Controller
// 2: trajectory Follower
int controller_mode = 2;
// 1: Nested Controller, trajectory Follower
int controller_mode;
// Variable for choosing flight sequence
int flightSequence = 0;
......@@ -227,10 +226,10 @@ float startCoordinateX;
float startCoordinateY;
float startCoordinateZ;
// tolerances (x,y,z)
float tol_takeoff[3] = {0.07, 0.07, 0.07}; //{0.03, 0.03, 0.03};
float tol_approach[3] = {0.3, 0.3, 0.07}; // added to differ between approach and land *
float tol_land[3] = {0.03, 0.03, 0.03}; // *
// tolerances (x,y,z), fetched from .yaml
float tol_takeoff[3] = {0.0, 0.0, 0.0};
float tol_approach[3] = {0.0, 0.0, 0.0};
float tol_land[3] = {0.0, 0.0, 0.0};
......@@ -240,6 +239,10 @@ double trajectory_start_time = 0;
double total_time_since_start = 0;
bool first_trajectory_calculation = true;
// lookahead for trajectory: fetched from .yaml
float trajectory_deltaT_position;
float trajectory_deltaT_velocity;
// Trajectory: xcf0 -> xm1 -> xm2 -> xms
// xcf0: inital(Button pressed) position of Crazyflie
......@@ -260,32 +263,28 @@ float trajectory_t1 = 0; // Time at xm1
float trajectory_t2 = 0; // Time at xm2
float trajectory_t3 = 0; // Time at xms
float xm1_x_distance = 0.5;
float xm2_distance_to_ms = 0.6;
// fetched from .yaml
float xm1_x_distance;
float xm2_distance_to_ms;
// Calculate from approximate distance and velocity the total duration of the trajectory
float trajectory_velocity_of_CF = 2.0; // Fly at 1 m/s
float trajectory_velocity_of_CF; // fetched from yaml
float trajectory_total_distance = 0;
float trajectory_duration = 0;
// Origin of the Flying zone
// returned trajectory setpoint & velocity
float trajectory_setpoint[4]; // x,y,z,yaw
float trajectory_velocity[3]; // x,y,z
// Origin of the Flying zone
float originX = 0;
float originY = 0;
d_fall_pps::AreaBounds area;
float trajectory_x_radius = 0;
float trajectory_y_radius = 0;
float trajectory_period = 5; // 1 round in 5 sec
// thrust factor to do a smooth landing
float thrust_factor = 1;
float pitchAngle_baseline = 1;
float rollAngle_baseline = 1;
......@@ -320,7 +319,6 @@ float m_yAdjustment = 0.0f;
// Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = false;
bool m_shouldClipVelocity = true;
// Max setpoint change per second
float m_max_setpoint_change_per_second_horizontal;
......
......@@ -17,6 +17,25 @@ vicon_frequency : 200
controller_mode : 1
# parameters for trajectory:
# lookahead times
trajectory_deltaT_position: 0.0
trajectory_deltaT_velocity: 0.1
# distance to first point on trajectory
xm1_x_distance: 0.5
# distance from MS to second point on trajectory
xm2_distance_to_ms: 0.6
# velocity of trajctory
trajectory_velocity_of_CF: 2.0
# tolerances
tol_takeoff: [0.07, 0.07, 0.07]
tol_approach: [0.3, 0.3, 0.07]
tol_land: [0.03, 0.03, 0.03]
......@@ -110,4 +129,4 @@ gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
# LQR: get baseline Angle from Velocity for feedforward (roll, pitch) [-20°,20°]=[-0.34906585, 0.34906585]
# [10°, 10°]=[-0.174532925, 0.174532925]
gainFeedforwardAnglefromVelocity: [-0.28, 0.28]
\ No newline at end of file
gainFeedforwardAnglefromVelocity: [-0.174532925, 0.174532925]
\ No newline at end of file
......@@ -408,14 +408,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.4;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
/*
ROS_INFO_STREAM("APPROACH: (x,y,z) Difference: ("
<< request.ownCrazyflie.x-dronexSetpoint.x << ", "
<< request.ownCrazyflie.y-dronexSetpoint.y << ", "
<< request.ownCrazyflie.z-dronexSetpoint.z << ")");
*/
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_approach[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_approach[2] ){
approachedFlag = true;
......@@ -462,10 +462,20 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_LAND_ON_MOTHERSHIP:
{
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] ){
//ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.03;
}else {
flying_state = DRONEX_STATE_APPROACH;
ROS_INFO_STREAM("Entering from DRONEX_STATE_LAND_ON_MOTHERSHIP: DRONEX_STATE_APPROACH");
}
}
break;
......@@ -613,12 +623,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if(controller_mode == 0){ // lqr controller
m_shouldSmoothSetpointChanges = true;
// do not change, use setpoint defined in states
}else if(controller_mode == 1){ // nested lqr controller
if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){ // Trajectory Follower
// to implement the trajectory tracking
m_shouldSmoothSetpointChanges = false;
calculateTrajectory(request);
......@@ -635,25 +648,37 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
/*
m_setpoint_for_controller_2[0] = request.owncraazyflie.x;
m_setpoint_for_controller_2[1] = request.owncraazyflie.y;
m_setpoint_for_controller_2[2] = request.owncraazyflie.z;
m_setpoint_for_controller_2[0] = request.owncrazyflie.x;
m_setpoint_for_controller_2[1] = request.owncrazyflie.y;
m_setpoint_for_controller_2[2] = request.owncrazyflie.z;
m_velocity_for_controller_2[0] = 1;
m_velocity_for_controller_2[1] = 0;
m_velocity_for_controller_2[2] = 0;
*/
}else{ // standard: if not following trajectory
}else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP){ // landing (close to mothership)
m_shouldSmoothSetpointChanges = true;
dronexSetpoint.x = request.otherCrazyflies[0].x; // setpoint on mothership
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.03;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;
dronexVelocity.x = mothership_vel[0]; // velocity of mothership
dronexVelocity.y = mothership_vel[1];
dronexVelocity.z = mothership_vel[2];
}else { // standard: if not following trajectory
m_shouldSmoothSetpointChanges = true;
//dronexSetpoint.x = dronexSetpoint.x;
//dronexSetpoint.y = dronexSetpoint.y;
//dronexSetpoint.z = dronexSetpoint.z;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
dronexVelocity.x = mothership_vel[0]*0;
dronexVelocity.y = mothership_vel[1]*0;
dronexVelocity.z = mothership_vel[2]*0;
dronexVelocity.x = 0;
dronexVelocity.y = 0;
dronexVelocity.z = 0;
}
}
......@@ -784,27 +809,35 @@ void calculateTrajectory(Controller::Request &request){
total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
std::vector<float> trajectory_temp_setpoint(3);
trajectory_temp_setpoint = trajectory_to_ms(request, total_time_since_start);
trajectory_setpoint[0] = trajectory_temp_setpoint[0];
trajectory_setpoint[1] = trajectory_temp_setpoint[1];
trajectory_setpoint[2] = trajectory_temp_setpoint[2];
trajectory_temp_present = trajectory_to_ms(request, total_time_since_start + trajectory_deltaT_position);
trajectory_setpoint[0] = trajectory_temp_present[0];
trajectory_setpoint[1] = trajectory_temp_present[1];
trajectory_setpoint[2] = trajectory_temp_present[2];
trajectory_setpoint[3] = 0;
trajectory_temp_future = trajectory_to_ms(request, total_time_since_start + trajectory_deltaT_velocity);
trajectory_velocity[0] = trajectory_temp_future[3];
trajectory_velocity[1] = trajectory_temp_future[4];
trajectory_velocity[2] = trajectory_temp_future[5];
// first try to follow trajectory velocity (keep to maybe put in comparison later)
/*
// just some deltaT to look ahead
float deltaT = 0.2;
std::vector<float> trajectory_temp_setpoint_future(3);
trajectory_temp_setpoint_future = trajectory_to_ms(request, total_time_since_start + deltaT);
trajectory_velocity[0] = (trajectory_temp_setpoint_future[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory_temp_setpoint_future[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0;
float trajectory_real_velocity[3];
trajectory_real_velocity[0] = (trajectory_temp_setpoint_future[0]-trajectory_temp_setpoint[0])/deltaT;
trajectory_real_velocity[1] = (trajectory_temp_setpoint_future[1]-trajectory_temp_setpoint[1])/deltaT;
trajectory_real_velocity[2] = (trajectory_temp_setpoint_future[2]-trajectory_temp_setpoint[2])/deltaT;
// clip if velocity is too big
float tot_velocity_xy = sqrt(pow(trajectory_velocity[0],2)+pow(trajectory_velocity[1],2));
......@@ -817,23 +850,19 @@ void calculateTrajectory(Controller::Request &request){
}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];
}
}*/
// TO BE TESTED:
/*
// TO BE TESTED: (different concept for clipping velocity)
float trajectory_real_velocity[3];
trajectory_real_velocity[0] = (trajectory_temp_setpoint_future[0]-trajectory_temp_setpoint[0])/deltaT;
trajectory_real_velocity[1] = (trajectory_temp_setpoint_future[1]-trajectory_temp_setpoint[1])/deltaT;
trajectory_real_velocity[2] = (trajectory_temp_setpoint_future[2]-trajectory_temp_setpoint[2])/deltaT;
float tot_real_velocity_xy = sqrt(pow(trajectory_real_velocity[0],2)+pow(trajectory_real_velocity[1],2));
trajectory_velocity[0] = trajectory_velocity[0]*tot_real_velocity_xy/tot_velocity_xy;
trajectory_velocity[1] = trajectory_velocity[1]*tot_real_velocity_xy/tot_velocity_xy;
*/
trajectory_velocity[0] = 0;
trajectory_velocity[1] = 0;
}
......@@ -852,7 +881,8 @@ void calculateTrajectory(Controller::Request &request){
// If t > trajectory_t3 then OUTPUT: (-1,-1,-1) -> exception handling
std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
std::vector<float> trajectory_return(3);
// (x,y,z,vx,vy,vz) where abs(vx,vy,vz) = trajectory_velocity_of_CF
std::vector<float> trajectory_return(6);
// If this method is called the first time after follow trajectory button
......@@ -913,28 +943,41 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// Calculate the x,y,z position depending on where the CF should be at a certain time t
if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position on line between xcf0 and xm1
if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position & velocity on line between xcf0 and xm1
for(int i = 0; i < 3; i++){
trajectory_return[i] = xcf0[i] + t*(xm1[i] - xcf0[i])/trajectory_t1;
trajectory_return[i+3] = (xm1[i]-xcf0[i])/calculate_distance_in_xyz(xcf0, xm1) * trajectory_velocity_of_CF;
}
}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)*(xm2[i] - xm1[i])/(trajectory_t2 - trajectory_t1);
trajectory_return[i+3] = (xm2[i]-xm1[i])/calculate_distance_in_xyz(xm1, xm2) * trajectory_velocity_of_CF;
}
}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)*(xms[i] - xm2[i])/(trajectory_t3 - trajectory_t2);
trajectory_return[i+3] = (xms[i]-xm2[i])/calculate_distance_in_xyz(xms, xm2) * trajectory_velocity_of_CF;
}
}else{
trajectory_return[0] = xms[0];
trajectory_return[1] = xms[1];
trajectory_return[2] = xms[2];
trajectory_return[3] = mothership_vel[0];
trajectory_return[4] = mothership_vel[1];
trajectory_return[5] = mothership_vel[2];
}
// test trajectory: Oval
/*
trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4;
float trajectory_x_radius = 1.0;
float trajectory_y_radius = 0.4;
float trajectory_period = 5; // 1 round in 5 sec
float originX = 0;
float originY = 0;
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);
......@@ -1598,13 +1641,13 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
stateInertial[1] = stateInertial[1] - setpoint[1] - m_yAdjustment;
stateInertial[2] = stateInertial[2] - setpoint[2];
if (stateInertial[2] > 30.0f)
if (stateInertial[2] > 1.0f)
{
stateInertial[2] = 30.0f;
stateInertial[2] = 1.0f;
}
else if (stateInertial[2] < -30.0f)
else if (stateInertial[2] < -1.0f)
{
stateInertial[2] = 30.0f;
stateInertial[2] = 1.0f;
}
// Fill in the yaw angle error
......@@ -1971,13 +2014,25 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_dronexController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// for our integrator
// > For the tolerances
getParameterFloatVector(nodeHandle_for_dronexController, "tol_takeoff", tol_takeoff, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "tol_approach", tol_approach, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "tol_land", tol_land, 3);
// > For the integrator
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegratorRate", gainIntegratorRate, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegratorAngle", gainIntegratorAngle, 3);
// > For the feedforward gain (velocity -> angle)
getParameterFloatVector(nodeHandle_for_dronexController, "gainFeedforwardAnglefromVelocity", gainFeedforwardAnglefromVelocity, 3);
// > For the trajectory lookahead params
trajectory_deltaT_position = getParameterFloat(nodeHandle_for_dronexController, "trajectory_deltaT_position");
trajectory_deltaT_velocity = getParameterFloat(nodeHandle_for_dronexController, "trajectory_deltaT_velocity");
xm1_x_distance = getParameterFloat(nodeHandle_for_dronexController, "xm1_x_distance");
xm2_distance_to_ms = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms");
trajectory_velocity_of_CF = getParameterFloat(nodeHandle_for_dronexController, "trajectory_velocity_of_CF");
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[DRONEX CONTROLLER] DEBUGGING: the fetched DroneXController/mass_CF = " << m_mass_CF_grams);
......
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