Skip to content
GitLab
Commit aa97f2c2 authored by pragash1's avatar pragash1
Browse files

xm2 more dynamically chosen, take off from mothership

parent 3b7eb375
Hide whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/include/nodes/DroneXControllerService.h
View file @ aa97f2c2
......@@ -202,6 +202,7 @@ float m_mass_CF_grams;
Setpoint dronexSetpoint;
Setpoint dronexVelocity;
int flying_state = DRONEX_STATE_GROUND;
int previous_flying_state = DRONEX_STATE_GROUND;
int integratorFlag = DRONEX_INTEGRATOR_OFF;
float integrator_sum_XYZ[3] = {0,0,0};
......@@ -222,10 +223,15 @@ bool landedFlag = false;
// Flag for saving starting coordinates
bool savedStartCoordinates = false;
bool is_height_reached = false;
float startCoordinateX;
float startCoordinateY;
float startCoordinateZ;
std::vector<float> startCoordinateFromMS(3, 0.0);
bool savedStartCoordinatesFromMS = false;
bool taking_off_from_ms_height_gain = true;
// tolerances (x,y,z), fetched from .yaml
std::vector<float> tol_takeoff (3, 0.0);
......@@ -255,6 +261,11 @@ std::vector<float> xm1(3, 0.0);
std::vector<float> xm2(3, 0.0);
std::vector<float> xms(3, 0.0);
std::vector<float> direction_ms_to_cf(3, 0.0);
std::vector<float> direction_ms_to_cf_ms_bodyframe(3, 0.0);
float xm2_sign = 1.0;
float trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1
float trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
float trajectory_duration_3 = 0; // Duration of flight between xm2 and xms
......
pps_ws/src/d_fall_pps/param/DroneXController.yaml
View file @ aa97f2c2
......@@ -31,7 +31,7 @@ xm1_x_distance: 0.5
xm2_distance_to_ms: 0.6
xm2_height_over_ms: 0.3
xm1_height: 0.6
xm1_height: 0.6
# velocity of trajctory
trajectory_velocity_of_CF : 3.0
......
pps_ws/src/d_fall_pps/src/nodes/DroneXControllerService.cpp
View file @ aa97f2c2
......@@ -203,6 +203,7 @@ void buttonPressed_take_off(){
//if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Taking off...");
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_TAKING_OFF;
//}else{
// ROS_ERROR("Cannot change to DRONEX_STATE_TAKING_OFF");
......@@ -219,12 +220,15 @@ void buttonPressed_land(){
void buttonPressed_abort(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Abort Mission!");
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_LAND_ON_GROUND;
// reset start position
savedStartCoordinates = false;
savedStartCoordinatesFromMS = false;
}
void buttonPressed_integrator_on(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn ON integrator");
integratorFlag = DRONEX_INTEGRATOR_ON;
......@@ -239,7 +243,7 @@ void buttonPressed_integrator_off(){
void buttonPressed_integrator_reset(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET integrator to zero");
integratorFlag = DRONEX_INTEGRATOR_RESET;
//New:
m_time_ticks = 0;
setarrayzero3(integrator_sum_XYZ, 3);
......@@ -248,6 +252,7 @@ void buttonPressed_integrator_reset(){
approachedFlag = false;
landedFlag = false;
savedStartCoordinates = false;
savedStartCoordinatesFromMS = false;
......@@ -277,7 +282,7 @@ void buttonPressed_integrator_reset(){
/*std::vector<float> xm1(&3, 0.0);
std::vector<float> xm2(&3, 0.0);
std::vector<float> xms(&3, 0.0);
*/
trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1
trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
......@@ -288,7 +293,7 @@ void buttonPressed_integrator_reset(){
trajectory_t2 = 0; // Time at xm2
xm1_normalizing_factor = 3; // normalize the difference between xcf0 and xms
xm1_scaling_factor = 0.6;
xm1_scaling_factor = 0.6;
xm2_distance_to_ms_at_zero_velocity = 0.6;
xm2_distance_to_ms_scaling_factor = 1;
......@@ -359,6 +364,7 @@ void buttonPressed_follow_trajectory(){
// Initialize trajectory timers
flightSequence = SEQUENCE_NONE;
previous_flying_state = flying_state;
flying_state = DRONEX_STATE_FOLLOWING_TRAJECTORY;
total_time_since_start = 0;
//trajectory_x_radius = 0;
......@@ -591,7 +597,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
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");
//}
......@@ -617,6 +623,63 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
thrust_factor = 1;
//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[1] + 0.4;
// 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]) {
//tookOffFlag = true;
//ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
//previous_flying_state = flying_state;
//flying_state = DRONEX_STATE_HOVER;
//reset_rviz = true;
taking_off_from_ms_height_gain = false;
}
}else{ // previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP
if(!savedStartCoordinates)
{
startCoordinateX = request.ownCrazyflie.x;
......@@ -632,9 +695,17 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.4;
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.4;
// For debugging the integrator
// ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
......@@ -648,11 +719,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
tookOffFlag = true;
ROS_INFO_STREAM("Entering: 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:
......@@ -676,11 +749,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
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.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{
......@@ -697,15 +771,18 @@ 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;
//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;
}
......@@ -834,12 +911,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.05 )){
ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
flying_state = DRONEX_STATE_GROUND;
previous_flying_state = DRONEX_STATE_LAND_ON_GROUND;
}
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (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 - 0.03 - 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;
}
if(flying_state == DRONEX_STATE_ON_MOTHERSHIP){
......@@ -1054,7 +1133,25 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// xm2_distance_to_ms: hardcoded distance
// xm2_distance_to_ms should be chosen based on mothership velocity
// Calculate the relative postion of the Crazyflie in coordinates of the mothership coordinates
direction_ms_to_cf[0] = request.ownCrazyflie.x - request.otherCrazyflies[0].x;
direction_ms_to_cf[1] = request.ownCrazyflie.y - request.otherCrazyflies[0].y;
direction_ms_to_cf[2] = request.ownCrazyflie.z - request.otherCrazyflies[0].z;
//TODO Attention this conversion might be buggy
direction_ms_to_cf_ms_bodyframe[0] = cosYaw*direction_ms_to_cf[0];
direction_ms_to_cf_ms_bodyframe[1] = sinYaw*direction_ms_to_cf[1];
if(direction_ms_to_cf_ms_bodyframe[0] < 0.0){
xm2_sign = 1.0; // Crazyflie can land from behind
}else{
xm2_sign = -1.0; // Crazyflie can land from front
}
// Calculate xm2(the point behind the mothership)
float ms_abs_velocity = sqrt(mothership_vel[0] * mothership_vel[0] + mothership_vel[1] * mothership_vel[1] + mothership_vel[2] * mothership_vel[2]);
float normalized_ms_velocity = ms_abs_velocity/xm2_distance_to_ms_at_zero_velocity;
......@@ -1065,8 +1162,8 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
xm2_distance_to_ms = xm2_distance_to_ms_at_zero_velocity - xm2_distance_to_ms_scaling_factor * normalized_ms_velocity;
xm2[0] = request.otherCrazyflies[0].x - cosYaw*xm2_distance_to_ms;
xm2[1] = request.otherCrazyflies[0].y - sinYaw*xm2_distance_to_ms;
xm2[0] = request.otherCrazyflies[0].x - xm2_sign*cosYaw*xm2_distance_to_ms;
xm2[1] = request.otherCrazyflies[0].y - xm2_sign*sinYaw*xm2_distance_to_ms;
xm2[2] = request.otherCrazyflies[0].z + xm2_height_over_ms;
xms[0] = request.otherCrazyflies[0].x;
......@@ -1180,7 +1277,7 @@ void motorsOFFSmooth(Controller::Response &response){
response.controlOutput.motorCmd2 = 0.55* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = 0.55* computeMotorPolyBackward(feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = 0.55* computeMotorPolyBackward(feed_forward_thrust_per_motor);
}
}
// Specify that this controller is a rate controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
......@@ -1196,13 +1293,13 @@ void motorsOFF(Controller::Response &response){
response.controlOutput.motorCmd4 = 0;
// Specify that this controller is a rate controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
}
......@@ -1684,7 +1781,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
float ms_sinYaw = sin(0.5*request.otherCrazyflies[0].yaw);
......@@ -1720,10 +1817,10 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
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;
......@@ -1752,7 +1849,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
drone_position_list.points.push_back(p);
rviz_points_trajectory_publisher.publish(drone_position_list);
......@@ -1783,7 +1880,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
xm2_position_list.color.a = 1.0f;
xm2_position_list.points.push_back(q);
rviz_xm2_publisher.publish(xm2_position_list);
......@@ -1838,7 +1935,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
xm2_position_list.points.erase(xm2_position_list.points.begin());
}*/
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