Commit c16f6e85 authored by maruggv's avatar maruggv
Browse files

VM: changed some names to more appropriate name, prework for trajectory generation and following

parent 0f49931a
......@@ -290,9 +290,9 @@ float m_max_setpoint_change_per_second_yaw_radians;
float m_vicon_frequency;
float prev_DroneX_pos[3] = {0,0,0};
float current_DroneX_pos[3];
float drone_X_vel[3];
float prev_MS_pos[3] = {0,0,0};
float current_MS_pos[3];
float mothership_vel[3];
// THE FOLLOWING PARAMETERS ARE USED
......@@ -513,7 +513,7 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12]
void calculateControlOutput_viaLQRforRates_forLanding( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// Velocity estimator
void calculateDroneXVelocity(Controller::Request &request);
void calculateMSVelocity(Controller::Request &request);
// Integrator
void integrator_XYZ(float (&stateErrorBody)[12]);
......
......@@ -582,7 +582,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
*/
calculateDroneXVelocity(request);
calculateMSVelocity(request);
setpointCallback(dronexSetpoint);
......@@ -717,9 +717,9 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Load Mothership velocity
// x dot, y dot, z dot
float m_velocity_for_controller_1[3];
m_velocity_for_controller_1[0] = drone_X_vel[0];
m_velocity_for_controller_1[1] = drone_X_vel[1];
m_velocity_for_controller_1[2] = drone_X_vel[2];
m_velocity_for_controller_1[0] = mothership_vel[0];
m_velocity_for_controller_1[1] = mothership_vel[1];
m_velocity_for_controller_1[2] = mothership_vel[2];
......@@ -892,17 +892,19 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
m_setpoint_for_controller_2[0] = trajectory_setpoint[0];
m_setpoint_for_controller_2[1] = trajectory_setpoint[1];
m_setpoint_for_controller_2[2] = trajectory_setpoint[2];
m_setpoint_for_controller_2[3] = trajectory_setpoint[3];
//m_setpoint_for_controller_2[3] = trajectory_setpoint[3];
m_setpoint_for_controller_2[3] = request.otherCrazyflies[0].yaw;
/*
m_velocity_for_controller_2[0] = trajectory_velocity[0];
m_velocity_for_controller_2[1] = trajectory_velocity[1];
m_velocity_for_controller_2[2] = trajectory_velocity[2];
*/
/*
m_velocity_for_controller_2[0] = 0;
m_velocity_for_controller_2[1] = 0;
m_velocity_for_controller_2[2] = 0;
*/
}else{
m_setpoint_for_controller_2[0] = m_setpoint_for_controller[0];
......@@ -911,9 +913,9 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
m_setpoint_for_controller_2[3] = request.otherCrazyflies[0].yaw;
m_velocity_for_controller_2[0] = drone_X_vel[0];
m_velocity_for_controller_2[1] = drone_X_vel[1];
m_velocity_for_controller_2[2] = drone_X_vel[2];
m_velocity_for_controller_2[0] = mothership_vel[0];
m_velocity_for_controller_2[1] = mothership_vel[1];
m_velocity_for_controller_2[2] = mothership_vel[2];
}
......@@ -1093,28 +1095,59 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Calculate an elliptic trajectory
// Center is the Crazyfliezone center
// Calculate a trajectory
void calculateTrajectory(){
// Gives back computes trajectory_setpoint and trajectory_velocity
void calculateTrajectory(Controller::Response &response){
trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4;
total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
trajectory_setpoint[0] = trajectory(total_time_since_start)[0];
trajectory_setpoint[1] = trajectory(total_time_since_start)[1];
trajectory_setpoint[2] = trajectory(total_time_since_start)[2];
trajectory_setpoint[3] = 0;
// just some deltaT to look ahead
float deltaT = 0.01;
trajectory_velocity[0] = (trajectory(total_time_since_start+deltaT)[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory(total_time_since_start+deltaT)[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0;
/*
trajectory_setpoint[0] = originX + cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius);
trajectory_setpoint[1] = originY + sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius);
trajectory_setpoint[2] = 0.5;
trajectory_setpoint[3] = 0;
*/
/*
trajectory_velocity[0] = -2*PI/trajectory_period * sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius);
trajectory_velocity[1] = 2*PI/trajectory_period * cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius);
trajectory_velocity[2] = 0;
*/
}
// returns (x,y,z) for a given time t
std::vector<float> trajectory(double t){
std::vector<float> trajectory_return(3);
trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4;
//total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
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);
trajectory_return[2] = 0.5;
return trajectory_return;
}
......@@ -1138,21 +1171,21 @@ void motorsOFF(Controller::Response &response){
// Estimate mothership velocity
void calculateDroneXVelocity(Controller::Request &request){
void calculateMSVelocity(Controller::Request &request){
prev_DroneX_pos[0] = current_DroneX_pos[0];
prev_DroneX_pos[1] = current_DroneX_pos[1];
prev_DroneX_pos[2] = current_DroneX_pos[2];
prev_MS_pos[0] = current_MS_pos[0];
prev_MS_pos[1] = current_MS_pos[1];
prev_MS_pos[2] = current_MS_pos[2];
current_DroneX_pos[0] = request.otherCrazyflies[0].x;
current_DroneX_pos[1] = request.otherCrazyflies[0].y;
current_DroneX_pos[2] = request.otherCrazyflies[0].z;
current_MS_pos[0] = request.otherCrazyflies[0].x;
current_MS_pos[1] = request.otherCrazyflies[0].y;
current_MS_pos[2] = request.otherCrazyflies[0].z;
drone_X_vel[0] = (current_DroneX_pos[0] - prev_DroneX_pos[0])*m_vicon_frequency;
drone_X_vel[1] = (current_DroneX_pos[1] - prev_DroneX_pos[1])*m_vicon_frequency;
drone_X_vel[2] = (current_DroneX_pos[2] - prev_DroneX_pos[2])*m_vicon_frequency;
mothership_vel[0] = (current_MS_pos[0] - prev_MS_pos[0])*m_vicon_frequency;
mothership_vel[1] = (current_MS_pos[1] - prev_MS_pos[1])*m_vicon_frequency;
mothership_vel[2] = (current_MS_pos[2] - prev_MS_pos[2])*m_vicon_frequency;
//ROS_INFO_STREAM("Velocity: vx " << drone_X_vel[0] << ", vy " << drone_X_vel[1] << ", vz " << drone_X_vel[2]);
//ROS_INFO_STREAM("Velocity: vx " << mothership_vel[0] << ", vy " << mothership_vel[1] << ", vz " << mothership_vel[2]);
}
......@@ -1470,11 +1503,13 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
// type "float64" that you can fill in with data you would like to plot in
// real-time.
debugMsg.value_1 = drone_X_vel[0];
debugMsg.value_2 = drone_X_vel[1];
debugMsg.value_3 = drone_X_vel[2];
debugMsg.value_1 = mothership_vel[0];
debugMsg.value_2 = mothership_vel[1];
debugMsg.value_3 = mothership_vel[2];
debugMsg.value_4 = thrust_factor;
debugMsg.value_5 = set
debugMsg.value_5 = dronexSetpoint.x;
debugMsg.value_6 = dronexSetpoint.y;
debugMsg.value_7 = dronexSetpoint.z;
// ......................
// debugMsg.value_10 = your_variable_name;
......
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