Skip to content
GitLab
Commit 37b6c1dd authored by mastefan's avatar mastefan
Browse files

Flightsequence bugfix, recorded y-pos with rqt for both controller modes.

parent 2353692a
Show whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/param/DroneXController.yaml
View file @ 37b6c1dd
......@@ -3,7 +3,7 @@ mass_CF : 31 #28 #32
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 3.00 # [meters]
max_setpoint_change_per_second_horizontal : 1.00 # [meters]
max_setpoint_change_per_second_vertical : 0.6 #0.8 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
......@@ -47,7 +47,7 @@ tol_takeoff: [0.07, 0.07, 0.07] #[0.07, 0.07, 0.07]
tol_approach: [0.3, 0.3, 0.1]
tol_land: [0.05, 0.05, 0.05] #[0.01, 0.01, 0.05]
land_height: 0.03 #0.3
start_height: 1.2
start_height: 0.6
pickupmode: 0 # 0=land directly; 1=hover to pickup chocolate
......@@ -115,7 +115,7 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# for our integrator (so far just random values)
gainIntegratorRate : [-5.00, 5.00, 5.00] # [roll, pitch, z]
gainIntegratorRate : [-0.20, 0.20, 0.20] # [roll, pitch, z]
gainIntegratorAngle : [-0.20, 0.20, 0.20] # [roll, pitch, z]
# The LQR Controller
......
pps_ws/src/d_fall_pps/src/nodes/DroneXControllerService.cpp
View file @ 37b6c1dd
......@@ -214,8 +214,9 @@ void buttonPressed_land(){
//if(flying_state == DRONEX_STATE_HOVER){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Start flight-sequence LA...");
// OLD:flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
// NEW:
flightSequence = SEQUENCE_LAND_ON_MOTHERSHIP;
//flightSequence = SEQUENCE_LAND_ON_MOTHERSHIP;
}
void buttonPressed_abort(){
......@@ -525,262 +526,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
calculateMSVelocity(request);
switch(flying_state){
// case DRONEX_STATE_APPROACH:
// {
// //ROS_INFO("DRONEX_STATE_APPROACH");
// dronexSetpoint.x = request.otherCrazyflies[0].x;
// dronexSetpoint.y = request.otherCrazyflies[0].y;
// 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;
// ROS_INFO("approached");
// }
// }
// break;
// case DRONEX_STATE_GROUND:
// {
// //ROS_INFO("DRONEX_STATE_GROUND");
// // Variable for choosing flight sequence off
// flightSequence = SEQUENCE_NONE;
// // Flags of landing sequence reset
// tookOffFlag = false;
// approachedFlag = false;
// //bool landedFlag = true;
// taking_off_from_ms_height_gain = false;
// integratorFlag = DRONEX_INTEGRATOR_OFF;
// dronexSetpoint.x = request.ownCrazyflie.x;
// dronexSetpoint.y = request.ownCrazyflie.y;
// dronexSetpoint.z = request.ownCrazyflie.z;
// // Uncomment this if this part of the FSM should not be below together with the modes.
// //motorsOFF(response);
// }
// break;
// case DRONEX_STATE_ON_MOTHERSHIP:
// {
// //ROS_INFO("DRONEX_STATE_ON_MOTHERSHIP");
// // Variable for choosing flight sequence off
// flightSequence = SEQUENCE_NONE;
// // Flags of landing sequence reset
// tookOffFlag = false;
// approachedFlag = false;
// //bool landedFlag = true;
// taking_off_from_ms_height_gain = true;
// integratorFlag = DRONEX_INTEGRATOR_OFF;
// dronexSetpoint.x = request.ownCrazyflie.x;
// dronexSetpoint.y = request.ownCrazyflie.y;
// dronexSetpoint.z = request.ownCrazyflie.z;
// // Uncomment this if this part of the FSM should not be below together with the modes.
// // if(pickupmode == 0){ // land directly/normally
// // motorsOFF(response);
// // }else{ // hover over mothership to pickup chocolate
// // flying_state = DRONEX_STATE_HOVER;
// // }
// }
// break;
// case DRONEX_STATE_LAND_ON_MOTHERSHIP:
// {
// 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 + land_height;//+ 0.05;
// //}else {
// //
// // flying_state = DRONEX_STATE_APPROACH;
// // ROS_INFO_STREAM("Entering from DRONEX_STATE_LAND_ON_MOTHERSHIP: DRONEX_STATE_APPROACH");
// //}
// // Uncomment this if this part of the FSM should not be below together with the modes.
// // 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 - land_height - request.otherCrazyflies[0].z) < tol_land[2]) ){
// // ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
// // flying_state = DRONEX_STATE_ON_MOTHERSHIP;
// // previous_flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
// // }
// }
// break;
// case DRONEX_STATE_LAND_ON_GROUND:
// {
// if(tookOffFlag){
// //ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
// dronexSetpoint.x = request.ownCrazyflie.x;
// dronexSetpoint.y = request.ownCrazyflie.y;
// dronexSetpoint.z = 0.0;
// tookOffFlag = false;
// }
// // Uncomment this if this part of the FSM should not be below together with the modes.
// // if(request.ownCrazyflie.z < 0.06 ){
// // ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
// // flying_state = DRONEX_STATE_GROUND;
// // previous_flying_state = DRONEX_STATE_LAND_ON_GROUND;
// // }
// }
// break;
// case DRONEX_STATE_TAKING_OFF:
// {
// //ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
// thrust_factor = 1;
// integratorFlag = DRONEX_INTEGRATOR_ON;
// m_shouldSmoothSetpointChanges = true;
// //Check where we start from, if from mothership first go straight up to a certain height
// // before aiming the startCoordinates.
// if(/*previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP && */taking_off_from_ms_height_gain){
// if(!savedStartCoordinatesFromMS)
// {
// startCoordinateFromMS[0] = request.otherCrazyflies[0].x;
// startCoordinateFromMS[1] = request.otherCrazyflies[0].y;
// startCoordinateFromMS[2] = request.otherCrazyflies[0].z;
// savedStartCoordinatesFromMS = true;
// ROS_INFO_STREAM("DRONEX: saved start Coordinates");
// ROS_INFO_STREAM("x = " << startCoordinateX);
// ROS_INFO_STREAM("y = " << startCoordinateY);
// ROS_INFO_STREAM("z = " << startCoordinateZ);
// }
// dronexSetpoint.x = startCoordinateFromMS[0];
// dronexSetpoint.y = startCoordinateFromMS[1];
// dronexSetpoint.z = startCoordinateFromMS[2] + start_height;
// if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
// abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
// //tookOffFlag = true;
// //ROS_INFO("took off");
// ROS_INFO_STREAM("Entering from MS-takeoff: ground-takeoff");
// //previous_flying_state = flying_state;
// //flying_state = DRONEX_STATE_HOVER;
// //reset_rviz = true;
// taking_off_from_ms_height_gain = false;
// savedStartCoordinatesFromMS = false;
// }
// }else{ // previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP
// if(!savedStartCoordinates)
// {
// startCoordinateX = request.ownCrazyflie.x;
// startCoordinateY = request.ownCrazyflie.y;
// startCoordinateZ = request.ownCrazyflie.z;
// savedStartCoordinates = true;
// ROS_INFO_STREAM("DRONEX: saved start Coordinates");
// ROS_INFO_STREAM("x = " << startCoordinateX);
// ROS_INFO_STREAM("y = " << startCoordinateY);
// ROS_INFO_STREAM("z = " << startCoordinateZ);
// }
// dronexSetpoint.x = startCoordinateX;
// dronexSetpoint.y = startCoordinateY;
// dronexSetpoint.z = startCoordinateZ + start_height;
// // For debugging the integrator
// // ROS_INFO_STREAM("TO: (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_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
// abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
// ROS_INFO("took off");
// tookOffFlag = true;
// ROS_INFO_STREAM("Entering from ground-takeoff: DRONEX_STATE_HOVER");
// previous_flying_state = flying_state;
// flying_state = DRONEX_STATE_HOVER;
// taking_off_from_ms_height_gain = false;
// reset_rviz = true;
// }
// }
// }
// break;
// case DRONEX_STATE_HOVER:
// {
// //integratorFlag = DRONEX_INTEGRATOR_ON;
// //ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// // keep setpoint constant
// // for testing hover over mothership
// /*
// dronexSetpoint.x = request.otherCrazyflies[0].x;
// dronexSetpoint.y = request.otherCrazyflies[0].y;
// dronexSetpoint.z = request.otherCrazyflies[0].z+0.3;
// */
// }
// break;
// case DRONEX_STATE_FOLLOWING_TRAJECTORY:
// {
// integratorFlag = DRONEX_INTEGRATOR_OFF;
// /*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] ) ||
// (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]) ){
// previous_flying_state = flying_state;
// 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;
case DRONEX_STATE_APPROACH:
{
......@@ -868,7 +613,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexVelocity.z = 0;
}
// Uncomment this if this part of the FSM should not be below together with the modes.
// // Uncomment this if this part of the FSM should not be below together with the modes.
// if(pickupmode == 0){ // land directly/normally
// motorsOFF(response);
// }else{ // hover over mothership to pickup chocolate
......@@ -1118,26 +863,26 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// flightSeqeunce 1: simple approaching and landing on static mothership
if (flightSequence == SEQUENCE_LAND_ON_MOTHERSHIP){
//ROS_INFO_STREAM("Entering: DRONEX_STATE_TAKING_OFF");
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_TAKING_OFF;
// if (flightSequence == SEQUENCE_LAND_ON_MOTHERSHIP){
// //ROS_INFO_STREAM("Entering: DRONEX_STATE_TAKING_OFF");
// previous_flying_state = flying_state;
// flying_state = DRONEX_STATE_TAKING_OFF;
//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
if(tookOffFlag){
//ROS_INFO_STREAM("Entering: DRONEX_STATE_APPROACH");
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_APPROACH;
// //ROS_INFO_STREAM("Flight sequence: Landing on mothership");
// if(tookOffFlag){
// //ROS_INFO_STREAM("Entering: DRONEX_STATE_APPROACH");
// previous_flying_state = flying_state;
// flying_state = DRONEX_STATE_APPROACH;
if(approachedFlag){
ROS_INFO_STREAM("Entering: DRONEX_STATE_LAND_ON_MOTHERSHIP");
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
}
// if(approachedFlag){
// ROS_INFO_STREAM("Entering: DRONEX_STATE_LAND_ON_MOTHERSHIP");
// previous_flying_state = flying_state;
// flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
// }
}
// }
}
// }
/*
// flightSequence 2: approach and land with velocity optimized controller
......@@ -2179,6 +1924,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
// Fill in the debugging message with any other data you would like to display
// in real time. For example, it might be useful to display the thrust
// adjustment computed by the z-altitude controller.
......@@ -2200,7 +1946,50 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// rviz publishers
// 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_cf_velocity;
// rviz_cf_velocity.pose.position.x = request.otherCrazyflies[0].x;
// rviz_cf_velocity.pose.position.y = request.otherCrazyflies[0].y;
// rviz_cf_velocity.pose.position.z = request.otherCrazyflies[0].z-0.05;
// rviz_cf_velocity.header.frame_id = "/frame_Trajectory";
// rviz_cf_velocity.header.stamp = ros::Time::now();
// rviz_cf_velocity.ns = "cf_velocity";
// rviz_cf_velocity.action = visualization_msgs::Marker::ADD;
// rviz_cf_velocity.pose.orientation.w = ms_cosRoll * ms_cosPitch * ms_cosYaw + ms_sinRoll * ms_sinPitch * ms_sinYaw;
// rviz_cf_velocity.pose.orientation.x = ms_sinRoll * ms_cosPitch * ms_cosYaw - ms_cosRoll * ms_sinPitch * ms_sinYaw;
// rviz_cf_velocity.pose.orientation.y = ms_cosRoll * ms_sinPitch * ms_cosYaw + ms_sinRoll * ms_cosPitch * ms_sinYaw;
// rviz_cf_velocity.pose.orientation.z = ms_cosRoll * ms_cosPitch * ms_sinYaw - ms_sinRoll * ms_sinPitch * ms_cosYaw;
// rviz_cf_velocity.id = 0;
// rviz_cf_velocity.type = visualization_msgs::Marker::CUBE;
// rviz_cf_velocity.scale.x = 0.58f;
// rviz_cf_velocity.scale.y = 0.30f;
// rviz_cf_velocity.scale.z = 0.01f;
// rviz_cf_velocity.color.r = 0.0f;
// rviz_cf_velocity.color.g = 0.0f;
// rviz_cf_velocity.color.b = 1.0f;
// rviz_cf_velocity.color.a = 1.0f;
// rviz_points_trajectory_publisher.publish(rviz_cf_velocity);
// CF model part 1
float cf_sinYaw_1 = sin(0.5*(request.ownCrazyflie.yaw+0.785398163));
float cf_cosYaw_1 = cos(0.5*(request.ownCrazyflie.yaw+0.785398163));
float cf_sinYaw_2 = sin(0.5*(request.ownCrazyflie.yaw-0.785398163));
......@@ -2240,6 +2029,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
rviz_cf_publisher_1.publish(rviz_cf_1);
// CF model part 2
visualization_msgs::Marker rviz_cf_2;
rviz_cf_2.pose.position.x = request.ownCrazyflie.x;
rviz_cf_2.pose.position.y = request.ownCrazyflie.y;
......@@ -2271,6 +2061,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// MS model
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);
......@@ -2300,17 +2091,22 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
rviz_mothership.scale.y = 0.30f;
rviz_mothership.scale.z = 0.01f;
if(flying_state == DRONEX_STATE_ON_MOTHERSHIP){
rviz_mothership.color.r = 0.0f;
rviz_mothership.color.g = 0.5f;
rviz_mothership.color.b = 0.0f;
} else{
rviz_mothership.color.r = 0.0f;
rviz_mothership.color.g = 0.0f;
rviz_mothership.color.g = 0.5f;
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");
// CF trace
geometry_msgs::Point p;
p.x = request.ownCrazyflie.x;
p.y = request.ownCrazyflie.y;
......@@ -2341,8 +2137,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
rviz_points_trajectory_publisher.publish(drone_position_list);
// trajectory
geometry_msgs::Point q;
q.x = xm2[0];
q.y = xm2[1];
......@@ -2372,10 +2167,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
rviz_xm2_publisher.publish(xm2_position_list);
// xm2
p.x = dronexSetpoint.x;
p.y = dronexSetpoint.y;
p.z = dronexSetpoint.z;
......@@ -2428,8 +2220,6 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
rviz_points_trajectory_generated_publisher.publish(setpoint_position_list);
//}
}
......
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