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 e6b08425 authored by mastefan's avatar mastefan
Browse files

mothership2 added, param tuning

parent 64b81823
......@@ -270,6 +270,8 @@ float xm1_x_distance;
float xm2_distance_to_ms;
float xm2_height_over_ms;
float xm1_height;
float xm1_normalizing_factor = 3; // normalize the difference between xcf0 and xms
float xm1_scaling_factor = 0.6; // Scale in the direction of the the difference between xcf0 and xms
......@@ -286,6 +288,7 @@ float trajectory_duration = 0;
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;
......
......@@ -30,8 +30,11 @@ xm1_x_distance: 0.5
# distance from MS to second point on trajectory
xm2_distance_to_ms: 0.6
xm2_height_over_ms: 0.3
xm1_height: 0.6
# velocity of trajctory
trajectory_velocity_of_CF : 2.0
trajectory_velocity_of_CF : 3.0
xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms
xm1_scaling_factor : 0.6 # Scale in the direction of the the difference between xcf0 and xms
......@@ -41,7 +44,7 @@ xm2_distance_to_ms_scaling_factor : 1 # Scale in direction of the mothersh
# tolerances
tol_takeoff: [0.07, 0.07, 0.07]
tol_approach: [0.2, 0.2, 0.1]
tol_land: [0.07, 0.07, 0.07]
tol_land: [0.07, 0.07, 0.04]
......
......@@ -925,7 +925,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
xm1[0] = xcf0[0] + xm1_x_distance; // xm1_x_distance hardcoded at the moment
xm1[1] = xcf0[1];
xm1[2] = request.otherCrazyflies[0].z+0.6;
xm1[2] = request.otherCrazyflies[0].z + xm1_height;
first_trajectory_calculation = false;
}
......@@ -1536,26 +1536,34 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// Publish the "debugMsg"
debugPublisher.publish(debugMsg);
float ms_sinYaw = sin(0.5*request.otherCrazyflies[0].yaw);
float ms_cosYaw = cos(0.5*request.otherCrazyflies[0].yaw);
float ms_sinRoll = sin(0.5*request.otherCrazyflies[0].roll);
float ms_cosRoll = cos(0.5*request.otherCrazyflies[0].roll);
float ms_sinPitch = sin(0.5*request.otherCrazyflies[0].pitch);
float ms_cosPitch = cos(0.5*request.otherCrazyflies[0].pitch);
visualization_msgs::Marker rviz_mothership;
rviz_mothership.pose.position.x = request.otherCrazyflies[0].x;
rviz_mothership.pose.position.y = request.otherCrazyflies[0].y;
rviz_mothership.pose.position.z = request.otherCrazyflies[0].z;
rviz_mothership.pose.position.z = request.otherCrazyflies[0].z-0.05;
rviz_mothership.header.frame_id = "/frame_Trajectory";
rviz_mothership.header.stamp = ros::Time::now();
rviz_mothership.ns = "mothership";
rviz_mothership.action = visualization_msgs::Marker::ADD;
rviz_mothership.pose.orientation.z = request.otherCrazyflies[0].yaw;
rviz_mothership.pose.orientation.w = 1.0;//request.otherCrazyflies[0].yaw;
rviz_mothership.pose.orientation.w = ms_cosRoll * ms_cosPitch * ms_cosYaw + ms_sinRoll * ms_sinPitch * ms_sinYaw;
rviz_mothership.pose.orientation.x = ms_sinRoll * ms_cosPitch * ms_cosYaw - ms_cosRoll * ms_sinPitch * ms_sinYaw;
rviz_mothership.pose.orientation.y = ms_cosRoll * ms_sinPitch * ms_cosYaw + ms_sinRoll * ms_cosPitch * ms_sinYaw;
rviz_mothership.pose.orientation.z = ms_cosRoll * ms_cosPitch * ms_sinYaw - ms_sinRoll * ms_sinPitch * ms_cosYaw;
rviz_mothership.id = 0;
rviz_mothership.type = visualization_msgs::Marker::CUBE;
rviz_mothership.scale.x = 0.4;
rviz_mothership.scale.y = 0.25;
rviz_mothership.scale.z = 0.01;
rviz_mothership.scale.x = 0.58f;
rviz_mothership.scale.y = 0.30f;
rviz_mothership.scale.z = 0.01f;
rviz_mothership.color.r = 0.0f;
rviz_mothership.color.g = 0.0f;
......@@ -2113,6 +2121,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
xm1_x_distance = getParameterFloat(nodeHandle_for_dronexController, "xm1_x_distance");
xm2_distance_to_ms = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms");
xm2_height_over_ms = getParameterFloat(nodeHandle_for_dronexController, "xm2_height_over_ms");
xm1_height = getParameterFloat(nodeHandle_for_dronexController, "xm2_height_over_ms");
trajectory_velocity_of_CF = getParameterFloat(nodeHandle_for_dronexController, "trajectory_velocity_of_CF");
xm1_normalizing_factor = getParameterFloat(nodeHandle_for_dronexController, "xm1_normalizing_factor");
......
......@@ -209,7 +209,7 @@ void goToControllerSetpoint()
void viconCallback(const ViconData& viconData)
{
CrazyflieData droneX;
CrazyflieData droneX, droneX2;
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData global = *it;
......@@ -218,6 +218,10 @@ void viconCallback(const ViconData& viconData)
droneX = global;
coordinatesToLocal(droneX);
}
if(global.crazyflieName == "Mothership2"){
droneX2 = global;
coordinatesToLocal(droneX2);
}
}
......@@ -234,6 +238,7 @@ void viconCallback(const ViconData& viconData)
coordinatesToLocal(local);
controllerCall.request.ownCrazyflie = local;
controllerCall.request.otherCrazyflies.push_back(droneX);
controllerCall.request.otherCrazyflies.push_back(droneX2);
ros::NodeHandle nodeHandle("~");
......
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