Commit cf49cbd7 authored by mastefan's avatar mastefan
Browse files

debugging, max for integrator with yaml param, velocity "future" tracking

parent 71770f86
......@@ -227,9 +227,9 @@ float startCoordinateY;
float startCoordinateZ;
// tolerances (x,y,z), fetched from .yaml
float tol_takeoff[3] = {0.0, 0.0, 0.0};
float tol_approach[3] = {0.0, 0.0, 0.0};
float tol_land[3] = {0.0, 0.0, 0.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);
......@@ -240,8 +240,8 @@ double total_time_since_start = 0;
bool first_trajectory_calculation = true;
// lookahead for trajectory: fetched from .yaml
float trajectory_deltaT_position;
float trajectory_deltaT_velocity;
float trajectory_deltaT_position = 0;
float trajectory_deltaT_velocity = 0;
// Trajectory: xcf0 -> xm1 -> xm2 -> xms
......@@ -249,10 +249,10 @@ float trajectory_deltaT_velocity;
// xm1: next Point after the Crazyflie: the Drone should reach the final height by xm1
// xm2: Point behind the mothership
// xms: Point on mothership where we will land
std::vector<float> xcf0(3);
std::vector<float> xm1(3);
std::vector<float> xm2(3);
std::vector<float> xms(3);
std::vector<float> xcf0(3, 0.0);
std::vector<float> xm1(3, 0.0);
std::vector<float> xm2(3, 0.0);
std::vector<float> xms(3, 0.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
......@@ -363,6 +363,7 @@ std::vector<float> gainMatrixPitchAngle (9,0.0);
// Integrator parameters
std::vector<float> gainIntegratorRate (3,0.0);
std::vector<float> gainIntegratorAngle (3,0.0);
std::vector<float> integrator_max (3,0.0);
// Feedforward gain (velocity -> angle)
std::vector<float> gainFeedforwardAnglefromVelocity (3,0.0);
......@@ -460,6 +461,7 @@ ros::Publisher dronexSetpointToGUIPublisher;
ros::Publisher flyingStatePublisher;
visualization_msgs::Marker drone_position_list;
visualization_msgs::Marker setpoint_position_list;
ros::Publisher rviz_points_publisher;
......
......@@ -7,6 +7,9 @@ max_setpoint_change_per_second_horizontal : 2.00 # [meters]
max_setpoint_change_per_second_vertical : 0.4 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# max absolute value for integrator
integrator_max : [1.0,1.0,1.0]
# Frequency of the controller, in hertz
vicon_frequency : 200
......
......@@ -240,8 +240,8 @@ void buttonPressed_follow_trajectory(){
flightSequence = SEQUENCE_NONE;
flying_state = DRONEX_STATE_FOLLOWING_TRAJECTORY;
total_time_since_start = 0;
trajectory_x_radius = 0;
trajectory_y_radius = 0;
//trajectory_x_radius = 0;
//trajectory_y_radius = 0;
first_trajectory_calculation = true;
trajectory_start_time = ros::Time::now().toSec();
trajectory_t0 = 0.0;
......@@ -276,11 +276,6 @@ void WeightParamCallback (const Setpoint& weightParam ) {
}
// Add a factor to the Pitchbaseline (default factor is 1)
void PitchBaselineParamCallback(const Setpoint& pitchAngleParam ) {
pitchAngle_baseline = pitchAngleParam.x;
}
......@@ -808,13 +803,14 @@ void calculateTrajectory(Controller::Request &request){
total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
std::vector<float> trajectory_temp_setpoint(3);
std::vector<float> trajectory_temp_present(3);
trajectory_temp_present = trajectory_to_ms(request, total_time_since_start + trajectory_deltaT_position);
trajectory_setpoint[0] = trajectory_temp_present[0];
trajectory_setpoint[1] = trajectory_temp_present[1];
trajectory_setpoint[2] = trajectory_temp_present[2];
trajectory_setpoint[3] = 0;
std::vector<float> trajectory_temp_future(3);
trajectory_temp_future = trajectory_to_ms(request, total_time_since_start + trajectory_deltaT_velocity);
trajectory_velocity[0] = trajectory_temp_future[3];
trajectory_velocity[1] = trajectory_temp_future[4];
......@@ -1523,8 +1519,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
drone_position_list.color.a = 1.0;
drone_position_list.points.push_back(p);
rviz_points_publisher.publish(drone_position_list);
......@@ -1534,28 +1529,28 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
p.y = dronexSetpoint.y;
p.z = dronexSetpoint.z;
drone_position_list.header.frame_id = "/frame_Trajectory";
drone_position_list.header.stamp = ros::Time::now();
drone_position_list.ns = "spheres";
drone_position_list.action = visualization_msgs::Marker::ADD;
drone_position_list.pose.orientation.w = 1.0;
setpoint_position_list.header.frame_id = "/frame_Trajectory";
setpoint_position_list.header.stamp = ros::Time::now();
setpoint_position_list.ns = "spheres";
setpoint_position_list.action = visualization_msgs::Marker::ADD;
setpoint_position_list.pose.orientation.w = 1.0;
drone_position_list.id = 0;
setpoint_position_list.id = 0;
drone_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
setpoint_position_list.type = visualization_msgs::Marker::SPHERE_LIST;
drone_position_list.scale.x = 0.01;
drone_position_list.scale.y = 0.01;
drone_position_list.scale.z = 0.01;
setpoint_position_list.scale.x = 0.01;
setpoint_position_list.scale.y = 0.01;
setpoint_position_list.scale.z = 0.01;
drone_position_list.color.r = 0.0f;
drone_position_list.color.g = 1.0f;
drone_position_list.color.a = 1.0;
setpoint_position_list.color.r = 0.0f;
setpoint_position_list.color.g = 1.0f;
setpoint_position_list.color.a = 1.0;
drone_position_list.points.push_back(p);
setpoint_position_list.points.push_back(p);
rviz_points_publisher.publish(drone_position_list);
rviz_points_publisher.publish(setpoint_position_list);
}
......@@ -2022,6 +2017,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > For the integrator
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegratorRate", gainIntegratorRate, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegratorAngle", gainIntegratorAngle, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "integrator_max", integrator_max, 3);
// > For the feedforward gain (velocity -> angle)
getParameterFloatVector(nodeHandle_for_dronexController, "gainFeedforwardAnglefromVelocity", gainFeedforwardAnglefromVelocity, 3);
......@@ -2149,6 +2145,10 @@ void integrator_XYZ(float (&stateErrorBody)[12])
for(int i=0; i < 3; i++)
{
integrator_sum_XYZ[i] += stateErrorBody[i]*(1.0/control_frequency);
if(integrator_sum_XYZ[i] > integrator_max[i])
integrator_sum_XYZ[i] = integrator_max[i];
else if(integrator_sum_XYZ[i] < -integrator_max[i])
integrator_sum_XYZ[i] = -integrator_max[i];
}
}
......@@ -2326,8 +2326,6 @@ int main(int argc, char* argv[]) {
// get weight params from GUI
ros::Subscriber droneXWeightParamSubscriber = nodeHandle.subscribe("WeightParam", 1, WeightParamCallback);
ros::Subscriber droneXPitchBaselineParamSubscriber = nodeHandle.subscribe("PitchBaselineParam", 1, PitchBaselineParamCallback);
......
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