To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit d37fef4e authored by mastefan's avatar mastefan
Browse files

Minor changes.

parent 3732144f
......@@ -227,6 +227,7 @@ float startCoordinateY;
float startCoordinateZ;
// tolerances (x,y,z), fetched from .yaml
std::vector<float> tol_takeoff (3, 0.0);
std::vector<float> tol_approach (3, 0.0);
std::vector<float> tol_land (3, 0.0);
......@@ -280,10 +281,13 @@ float xm2_distance_to_ms_scaling_factor = 1;
// Calculate from approximate distance and velocity the total duration of the trajectory
float trajectory_velocity_of_CF_init;
float trajectory_velocity_of_CF; // fetched from yaml
float trajectory_total_distance = 0;
float trajectory_duration = 0;
// returned trajectory setpoint & velocity
float trajectory_setpoint[4]; // x,y,z,yaw
float trajectory_velocity[3]; // x,y,z
......@@ -382,6 +386,7 @@ std::vector<float> integrator_max (3,0.0);
std::vector<float> gainFeedforwardAnglefromVelocity (3,0.0);
// The 16-bit command limits
float cmd_sixteenbit_min;
float cmd_sixteenbit_max;
......@@ -564,6 +569,8 @@ void buttonPressed_integrator_reset();
void buttonPressed_follow_trajectory();
void buttonPressed_reset();
void setarrayzero3(float arr[3], int n);
void setarrayzero4(float arr[4], int n);
......
......@@ -35,6 +35,7 @@ xm1_height: 0.6
# velocity of trajctory
trajectory_velocity_of_CF : 3.0
trajectory_velocity_of_CF_init : 1
xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms
xm1_scaling_factor : 0.6 # Scale in the direction of the the difference between xcf0 and xms
......
......@@ -235,22 +235,29 @@ void buttonPressed_integrator_off(){
integratorFlag = DRONEX_INTEGRATOR_OFF;
}
void buttonPressed_integrator_reset(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET integrator to zero");
integratorFlag = DRONEX_INTEGRATOR_RESET;
//New:
m_time_ticks = 0;
integrator_sum_XYZ = {0,0,0};
setarrayzero3(integrator_sum_XYZ, 3);
//integrator_sum_XYZ = {0,0,0};
tookOffFlag = false;
approachedFlag = false;
landedFlag = false;
savedStartCoordinates = false;
tol_takeoff = std::fill(tol_takeoff.begin(), tol_takeoff.end(), 0);
tol_approach = std::fill(tol_approach.begin(), tol_approach.end(), 0);
tol_land = std::fill(tol_land.begin(), tol_land.end(), 0);
std::fill(tol_takeoff.begin(),tol_takeoff.end(), 0);
std::fill(tol_approach.begin(),tol_approach.end(), 0);
std::fill(tol_land.begin(),tol_land.end(), 0);
/*std::vector<float> tol_takeoff (3, 0.0);
std::vector<float> tol_approach (3, 0.0);
std::vector<float> tol_land (3, 0.0);
......@@ -263,13 +270,13 @@ void buttonPressed_integrator_reset(){
trajectory_deltaT_position = 0;
trajectory_deltaT_velocity = 0;
xcf0 = std::fill(xcf0.begin(), xcf0.end(), 0);
xm1 = std::fill(xm1.begin(), xm1.end(), 0);
xm2 = std::fill(xm2.begin(), xm2.end(), 0);
xm3 = std::fill(xm3.begin(), xm3.end(), 0);
/*std::vector<float> xm1(3, 0.0);
std::vector<float> xm2(3, 0.0);
std::vector<float> xms(3, 0.0);
/*xcf0 = */std::fill(xcf0.begin(),xcf0.end(), 0);
std::fill(xm1.begin(),xm1.end(), 0);
std::fill(xm2.begin(),xm2.end(), 0);
std::fill(xms.begin(),xms.end(), 0);
/*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
......@@ -291,23 +298,27 @@ void buttonPressed_integrator_reset(){
trajectory_duration = 0;
// returned trajectory setpoint & velocity
trajectory_setpoint = {0,0,0,0}; // x,y,z,yaw
trajectory_velocity = {0,0,0}; // x,y,z
//trajectory_setpoint = {0,0,0,0}; // x,y,z,yaw
setarrayzero4(trajectory_setpoint, 4);
//trajectory_velocity = {0,0,0}; // x,y,z
setarrayzero3(trajectory_velocity, 3);
// Origin of the Flying zone
//float originX = 0;
//float originY = 0;
// > The setpoints for (x,y,z) position and yaw angle, in that order
m_setpoint = {0.0,0.0,0.0,0.0};
m_setpoint_for_controller = {0.0,0.0,0.0,0.0};
// > The velocities for (x,y,z) position, in that order
m_velocity = {0.0,0.0,0.0};
m_velocity_for_controller = {0.0,0.0,0.0};
// > The setpoints for (&x,y,z) position and yaw angle, in that order
//m_setpoint = {0.0,0.0,0.0,0.0};
setarrayzero4(m_setpoint, 4);
//m_setpoint_for_controller = {0.0,0.0,0.0,0.0};
setarrayzero4(m_setpoint_for_controller, 4);
// > The velocities for (&x,y,z) position, in that order
//m_velocity = {0.0,0.0,0.0};
setarrayzero3(m_velocity, 3);
//m_velocity_for_controller = {0.0,0.0,0.0};
setarrayzero3(m_velocity_for_controller, 3);
// > Small adjustments to the x-y setpoint
m_xAdjustment = 0.0f;
m_yAdjustment = 0.0f;
......@@ -315,9 +326,12 @@ void buttonPressed_integrator_reset(){
// Boolean for whether to limit rate of change of the setpoint
m_shouldSmoothSetpointChanges = false;
prev_MS_pos = {0,0,0};
current_MS_pos = {0,0,0};
mothership_vel = {0,0,0};
//prev_MS_pos = {0,0,0};
setarrayzero3(prev_MS_pos, 3);
//current_MS_pos = {0,0,0};
setarrayzero3(current_MS_pos, 3);
//mothership_vel = {0,0,0};
setarrayzero3(mothership_vel, 3);
// thrust factor to do a smooth landing
thrust_factor = 1;
......@@ -327,6 +341,18 @@ void buttonPressed_integrator_reset(){
}
void setarrayzero3(float arr[3], int n){
for(int i = 0; i<n; i++){
arr[i] = 0;
}
}
void setarrayzero4(float arr[4], int n){
for(int i = 0; i<n; i++){
arr[i] = 0;
}
}
void buttonPressed_follow_trajectory(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] FOLLOW trajectory");
......@@ -1054,7 +1080,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// Calculate each duration of the different segments of the trajectory
// Time between xcf0 and xm1
trajectory_duration_1 = calculate_distance_in_xyz(xcf0, xm1)/trajectory_velocity_of_CF;
trajectory_duration_1 = calculate_distance_in_xyz(xcf0, xm1)/trajectory_velocity_of_CF_init;
// Time between xm1 and xm2
trajectory_duration_2 = calculate_distance_in_xyz(xm1,xm2)/trajectory_velocity_of_CF;
// Time between xm2 and xms
......@@ -1074,7 +1100,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position & velocity on line between xcf0 and xm1
for(int i = 0; i < 3; i++){
trajectory_return[i] = xcf0[i] + t*(xm1[i] - xcf0[i])/trajectory_t1;
trajectory_return[i+3] = (xm1[i]-xcf0[i])/calculate_distance_in_xyz(xcf0, xm1) * trajectory_velocity_of_CF;
trajectory_return[i+3] = (xm1[i]-xcf0[i])/calculate_distance_in_xyz(xcf0, xm1) * trajectory_velocity_of_CF_init;
}
}else if(trajectory_t1 < t && t <= trajectory_t2){ // Calculate position on line between xm1 and xm2
for(int i = 0; i < 3; i++){
......@@ -2211,6 +2237,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
xm1_height = getParameterFloat(nodeHandle_for_dronexController, "xm2_height_over_ms");
trajectory_velocity_of_CF = getParameterFloat(nodeHandle_for_dronexController, "trajectory_velocity_of_CF");
trajectory_velocity_of_CF_init = getParameterFloat(nodeHandle_for_dronexController, "trajectory_velocity_of_CF_init");
xm1_normalizing_factor = getParameterFloat(nodeHandle_for_dronexController, "xm1_normalizing_factor");
xm1_scaling_factor = getParameterFloat(nodeHandle_for_dronexController, "xm1_scaling_factor");
......
Markdown is supported
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