Commit d1e38f43 authored by maruggv's avatar maruggv
Browse files

trajectory velocity generation changed

parent 5d62babe
......@@ -780,17 +780,27 @@ void calculateTrajectory(Controller::Request &request){
total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
trajectory_setpoint[0] = trajectory_to_ms(request, total_time_since_start)[0];
trajectory_setpoint[1] = trajectory_to_ms(request, total_time_since_start)[1];
trajectory_setpoint[2] = trajectory_to_ms(request, total_time_since_start)[2];
float trajectory_temp_setpoint[3] = trajectory_to_ms(request, total_time_since_start);
trajectory_setpoint[0] = trajectory_temp_setpoint[0];
trajectory_setpoint[1] = trajectory_temp_setpoint[1];
trajectory_setpoint[2] = trajectory_temp_setpoint[2];
trajectory_setpoint[3] = 0;
// just some deltaT to look ahead
float deltaT = 0.2;
trajectory_velocity[0] = (trajectory_to_ms(request, total_time_since_start + deltaT)[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory_to_ms(request, total_time_since_start + deltaT)[1]-request.ownCrazyflie.y)/deltaT;
float trajectory_temp_setpoint_future[3] = trajectory_to_ms(request, total_time_since_start + deltaT);
trajectory_velocity[0] = (trajectory_temp_setpoint_future[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory_temp_setpoint_future[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0;
float trajectory_real_velocity[3];
trajectory_real_velocity[0] = (trajectory_temp_setpoint_future[0]-trajectory_temp_setpoint[0])/deltaT;
trajectory_real_velocity[1] = (trajectory_temp_setpoint_future[1]-trajectory_temp_setpoint[1])/deltaT;
trajectory_real_velocity[2] = (trajectory_temp_setpoint_future[2]-trajectory_temp_setpoint[2])/deltaT;
// clip if velocity is too big
float tot_velocity_xy = sqrt(pow(trajectory_velocity[0],2)+pow(trajectory_velocity[1],2));
if(tot_velocity_xy > 1.0){
......@@ -810,6 +820,13 @@ void calculateTrajectory(Controller::Request &request){
trajectory_velocity[i] = -0.8 + current_stateInertialEstimate[i+3];
}
}*/
// TO BE TESTED:
/*
float tot_real_velocity_xy = sqrt(pow(trajectory_real_velocity[0],2)+pow(trajectory_real_velocity[1],2));
trajectory_velocity[0] = trajectory_velocity[0]*tot_real_velocity_xy/tot_velocity_xy;
trajectory_velocity[1] = trajectory_velocity[1]*tot_real_velocity_xy/tot_velocity_xy;
*/
}
......
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