Skip to content

GitLab

Commit 64b81823 authored by mastefan's avatar mastefan
Browse files

VM: trajectory working, tuning needed, yaw problem solved

parent 1a5f1c23
Hide whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/include/nodes/DroneXControllerService.h
View file @ 64b81823
......@@ -268,6 +268,7 @@ float trajectory_t3 = 0; // Time at xms
// fetched from .yaml
float xm1_x_distance;
float xm2_distance_to_ms;
float xm2_height_over_ms;
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
......@@ -471,10 +472,22 @@ ros::Publisher flyingStatePublisher;
visualization_msgs::Marker drone_position_list;
visualization_msgs::Marker setpoint_position_list;
visualization_msgs::Marker rviz_mothership;
ros::Publisher rviz_points_trajectory_publisher;
ros::Publisher rviz_points_trajectory_generated_publisher;
bool reset_rviz = false;
float red_1 = 1.0f;
float green_1 = 0.0f;
float blue_1 = 0.0f;
float red_2 = 0.0f;
float green_2 = 1.0f;
float blue_2 = 0.0f;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
......
pps_ws/src/d_fall_pps/param/DroneXController.yaml
View file @ 64b81823
......@@ -3,8 +3,8 @@ mass_CF : 28 #32
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 2.00 # [meters]
max_setpoint_change_per_second_vertical : 0.4 # [meters]
max_setpoint_change_per_second_horizontal : 3.00 # [meters]
max_setpoint_change_per_second_vertical : 0.8 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# max absolute value for integrator
......@@ -24,11 +24,12 @@ controller_mode : 1
# lookahead times
trajectory_deltaT_position: 0.0
trajectory_deltaT_velocity: 1
trajectory_deltaT_velocity: 0.35
# 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
xm2_height_over_ms: 0.3
# velocity of trajctory
trajectory_velocity_of_CF : 2.0
xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms
......@@ -39,8 +40,8 @@ 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.3, 0.3, 0.07]
tol_land: [0.03, 0.03, 0.03]
tol_approach: [0.2, 0.2, 0.1]
tol_land: [0.07, 0.07, 0.07]
......@@ -134,6 +135,8 @@ gainMatrixPitchRatefromAngle : [ 0.00, 4.00, 0.00]
gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
# LQR: get baseline Angle from Velocity for feedforward (roll, pitch) [-20°,20°]=[-0.34906585, 0.34906585]
# LQR: get baseline Angle from Velocity for feedforward (roll, pitch)
# [-20°,20°]=[-0.34906585, 0.34906585]
# [15°, 15°]=[-0.261799388, 0.261799388]
# [10°, 10°]=[-0.174532925, 0.174532925]
gainFeedforwardAnglefromVelocity: [-0.174532925, 0.174532925]
gainFeedforwardAnglefromVelocity: [-0.261799388, 0.261799388]
pps_ws/src/d_fall_pps/src/nodes/DroneXControllerService.cpp
View file @ 64b81823
......@@ -132,7 +132,7 @@ void perControlCycleOperations()
{
if (m_shouldSmoothSetpointChanges)
{
for(int i = 0; i < 3; ++i)
for(int i = 0; i < 4; ++i)
{
float max_for_this_coordinate;
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
......@@ -159,6 +159,13 @@ void perControlCycleOperations()
// Compute the difference in setpoint
float setpoint_difference = m_setpoint[i] - m_setpoint_for_controller[i];
// anti windup for yaw
if (i==3){
if (setpoint_difference > PI)
setpoint_difference -= 2*PI;
if (setpoint_difference < -PI)
setpoint_difference += 2*PI;
}
// Clip the difference to the maximum
if (setpoint_difference > max_for_this_coordinate)
{
......@@ -172,7 +179,7 @@ void perControlCycleOperations()
// Update the setpoint of the controller
m_setpoint_for_controller[i] += setpoint_difference;
}
m_setpoint_for_controller[3] = m_setpoint[3];
//m_setpoint_for_controller[3] = m_setpoint[3];
}
else
......@@ -431,6 +438,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
approachedFlag = false;
//bool landedFlag = true;
integratorFlag == DRONEX_INTEGRATOR_OFF;
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
......@@ -458,20 +467,21 @@ 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] ){
integratorFlag == DRONEX_INTEGRATOR_ON;
//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.03;
}else {
flying_state = DRONEX_STATE_APPROACH;
ROS_INFO_STREAM("Entering from DRONEX_STATE_LAND_ON_MOTHERSHIP: DRONEX_STATE_APPROACH");
}
//}else {
//
// flying_state = DRONEX_STATE_APPROACH;
// ROS_INFO_STREAM("Entering from DRONEX_STATE_LAND_ON_MOTHERSHIP: DRONEX_STATE_APPROACH");
//}
}
break;
......@@ -525,6 +535,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
flying_state = DRONEX_STATE_HOVER;
reset_rviz = true;
}
}
break;
......@@ -548,12 +560,17 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
/*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] )*/
if(abs(request.ownCrazyflie.x-xm2[0]) < tol_approach[0] &&
abs(request.ownCrazyflie.y-xm2[1]) < tol_approach[1] &&
abs(request.ownCrazyflie.z-xm2[2]) < tol_approach[2] ){
if( (abs(request.ownCrazyflie.x-xm2[0]) < tol_approach[0] &&
abs(request.ownCrazyflie.y-xm2[1]) < tol_approach[1] &&
abs(request.ownCrazyflie.z-xm2[2]) < tol_approach[2] ) ||
(abs(request.ownCrazyflie.x-xms[0]) < tol_approach[0] &&
abs(request.ownCrazyflie.y-xms[1]) < tol_approach[1] &&
abs(request.ownCrazyflie.z-xms[2]) < tol_approach[2]) ){
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
ROS_INFO_STREAM("Entering from Trajectory: DRONEX_STATE_LAND_ON_MOTHERSHIP");
} else{
ROS_INFO_STREAM("distance to xm2: "<<request.ownCrazyflie.x-xm2[0]<<", "<<request.ownCrazyflie.y-xm2[1]<<", "<<request.ownCrazyflie.z-xm2[2]);
}
break;
......@@ -675,7 +692,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_shouldSmoothSetpointChanges = true;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
ROS_INFO_STREAM(request.otherCrazyflies[0].yaw);
dronexVelocity.x = 0;
dronexVelocity.y = 0;
......@@ -934,7 +950,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
xm2[0] = request.otherCrazyflies[0].x - cosYaw*xm2_distance_to_ms;
xm2[1] = request.otherCrazyflies[0].y - sinYaw*xm2_distance_to_ms;
xm2[2] = xm1[2];
xm2[2] = request.otherCrazyflies[0].z + xm2_height_over_ms;
xms[0] = request.otherCrazyflies[0].x;
xms[1] = request.otherCrazyflies[0].y;
......@@ -1521,68 +1537,103 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
debugPublisher.publish(debugMsg);
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.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.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.color.r = 0.0f;
rviz_mothership.color.g = 0.0f;
rviz_mothership.color.b = 1.0f;
rviz_mothership.color.a = 1.0f;
rviz_points_trajectory_publisher.publish(rviz_mothership);
//if (ros::ok()){
//ROS_INFO("Init RVIZ STUFF");
geometry_msgs::Point p;
p.x = request.ownCrazyflie.x;
p.y = request.ownCrazyflie.y;
p.z = request.ownCrazyflie.z;
geometry_msgs::Point p;
p.x = request.ownCrazyflie.x;
p.y = request.ownCrazyflie.y;
p.z = request.ownCrazyflie.z;
drone_position_list.header.frame_id = "/frame_Trajectory";
drone_position_list.header.stamp = ros::Time::now();
drone_position_list.ns = "spheres";
drone_position_list.action = visualization_msgs::Marker::ADD;
drone_position_list.pose.orientation.w = 1.0;
drone_position_list.header.frame_id = "/frame_Trajectory";
drone_position_list.header.stamp = ros::Time::now();
drone_position_list.ns = "spheres";
drone_position_list.action = visualization_msgs::Marker::ADD;
drone_position_list.pose.orientation.w = 1.0;
drone_position_list.id = 0;
drone_position_list.id = 0;
drone_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
drone_position_list.scale.x = 0.01;
drone_position_list.scale.y = 0.01;
drone_position_list.scale.z = 0.01;
drone_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
drone_position_list.color.r = 1.0f;
drone_position_list.color.a = 1.0;
drone_position_list.color.g = 0;
drone_position_list.scale.x = 0.01;
drone_position_list.scale.y = 0.01;
drone_position_list.scale.z = 0.01;
drone_position_list.points.push_back(p);
// Only save first 5000 points
if(drone_position_list.points.size()>50)
drone_position_list.points.erase(drone_position_list.points.begin());
drone_position_list.color.r = red_1;
drone_position_list.color.g = green_1;
drone_position_list.color.b = blue_1;
drone_position_list.color.a = 1.0f;
rviz_points_trajectory_publisher.publish(drone_position_list);
drone_position_list.points.push_back(p);
rviz_points_trajectory_publisher.publish(drone_position_list);
p.x = dronexSetpoint.x;
p.y = dronexSetpoint.y;
p.z = dronexSetpoint.z;
p.x = dronexSetpoint.x;
p.y = dronexSetpoint.y;
p.z = dronexSetpoint.z;
setpoint_position_list.header.frame_id = "/frame_Trajectory";
setpoint_position_list.header.stamp = ros::Time::now();
setpoint_position_list.ns = "spheres";
setpoint_position_list.action = visualization_msgs::Marker::ADD;
setpoint_position_list.pose.orientation.w = 1.0;
setpoint_position_list.header.frame_id = "/frame_Trajectory";
setpoint_position_list.header.stamp = ros::Time::now();
setpoint_position_list.ns = "spheres";
setpoint_position_list.action = visualization_msgs::Marker::ADD;
setpoint_position_list.pose.orientation.w = 1.0;
setpoint_position_list.id = 0;
setpoint_position_list.id = 0;
setpoint_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
setpoint_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
setpoint_position_list.scale.x = 0.01;
setpoint_position_list.scale.y = 0.01;
setpoint_position_list.scale.z = 0.01;
setpoint_position_list.scale.x = 0.01;
setpoint_position_list.scale.y = 0.01;
setpoint_position_list.scale.z = 0.01;
setpoint_position_list.color.r = red_2;
setpoint_position_list.color.g = green_2;
setpoint_position_list.color.b = blue_2;
setpoint_position_list.color.a = 1.0f;
setpoint_position_list.color.r = 0.0f;
setpoint_position_list.color.g = 1.0f;
setpoint_position_list.color.a = 1.0;
setpoint_position_list.points.push_back(p);
setpoint_position_list.points.push_back(p);
// delete after each run for a clean rviz gui
if(reset_rviz){
while(drone_position_list.points.size() > 0){
drone_position_list.points.erase(drone_position_list.points.begin());
}
while(setpoint_position_list.points.size() > 0){
setpoint_position_list.points.erase(setpoint_position_list.points.begin());
}
reset_rviz = false;
}
rviz_points_trajectory_generated_publisher.publish(setpoint_position_list);
rviz_points_trajectory_generated_publisher.publish(setpoint_position_list);
//}
......@@ -1688,8 +1739,6 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
while(yawError < -PI) {yawError += 2 * PI;}
// > Third, put the "yawError" into the "stateError" variable
//ROS_INFO_STREAM("yaw(cf,ms,error): " << stateInertial[8]<< ", " << setpoint[3] << ", " << yawError);
ROS_INFO_STREAM("yaw(ms): "<<setpoint[3]);
stateInertial[8] = yawError;
......@@ -2063,6 +2112,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
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");
xm2_height_over_ms = 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");
......
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