Commit 1a5f1c23 authored by mastefan's avatar mastefan
Browse files

VM: yaw issue fixed, rviz deleted ros:ok() -> works now without issues

parent 59bcde6e
...@@ -24,7 +24,7 @@ controller_mode : 1 ...@@ -24,7 +24,7 @@ controller_mode : 1
# lookahead times # lookahead times
trajectory_deltaT_position: 0.0 trajectory_deltaT_position: 0.0
trajectory_deltaT_velocity: 0.1 trajectory_deltaT_velocity: 1
# distance to first point on trajectory # distance to first point on trajectory
xm1_x_distance: 0.5 xm1_x_distance: 0.5
# distance from MS to second point on trajectory # distance from MS to second point on trajectory
......
...@@ -132,7 +132,7 @@ void perControlCycleOperations() ...@@ -132,7 +132,7 @@ void perControlCycleOperations()
{ {
if (m_shouldSmoothSetpointChanges) if (m_shouldSmoothSetpointChanges)
{ {
for(int i = 0; i < 4; ++i) for(int i = 0; i < 3; ++i)
{ {
float max_for_this_coordinate; float max_for_this_coordinate;
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
...@@ -172,6 +172,7 @@ void perControlCycleOperations() ...@@ -172,6 +172,7 @@ void perControlCycleOperations()
// Update the setpoint of the controller // Update the setpoint of the controller
m_setpoint_for_controller[i] += setpoint_difference; m_setpoint_for_controller[i] += setpoint_difference;
} }
m_setpoint_for_controller[3] = m_setpoint[3];
} }
else else
...@@ -622,6 +623,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -622,6 +623,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if(controller_mode == 0){ // lqr controller if(controller_mode == 0){ // lqr controller
m_shouldSmoothSetpointChanges = true; m_shouldSmoothSetpointChanges = true;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;
// do not change, use setpoint defined in states // do not change, use setpoint defined in states
...@@ -673,6 +675,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -673,6 +675,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_shouldSmoothSetpointChanges = true; m_shouldSmoothSetpointChanges = true;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
ROS_INFO_STREAM(request.otherCrazyflies[0].yaw);
dronexVelocity.x = 0; dronexVelocity.x = 0;
dronexVelocity.y = 0; dronexVelocity.y = 0;
...@@ -911,8 +914,8 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ ...@@ -911,8 +914,8 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
first_trajectory_calculation = false; first_trajectory_calculation = false;
} }
float sinYaw = sin(request.ownCrazyflie.yaw); float sinYaw = sin(request.otherCrazyflies[0].yaw);
float cosYaw = cos(request.ownCrazyflie.yaw); float cosYaw = cos(request.otherCrazyflies[0].yaw);
// TODO cosYaw and sinYaw maybe otherway round // TODO cosYaw and sinYaw maybe otherway round
// xm2_distance_to_ms: hardcoded distance // xm2_distance_to_ms: hardcoded distance
...@@ -1519,8 +1522,8 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle ...@@ -1519,8 +1522,8 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
if (ros::ok()){ //if (ros::ok()){
//ROS_INFO("Init RVIZ STUFF"); //ROS_INFO("Init RVIZ STUFF");
geometry_msgs::Point p; geometry_msgs::Point p;
p.x = request.ownCrazyflie.x; p.x = request.ownCrazyflie.x;
...@@ -1545,10 +1548,10 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle ...@@ -1545,10 +1548,10 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
drone_position_list.color.a = 1.0; drone_position_list.color.a = 1.0;
drone_position_list.color.g = 0; drone_position_list.color.g = 0;
drone_position_list.points.push_back(p);
// Only save first 5000 points // Only save first 5000 points
if(drone_position_list.points.size()<5000) if(drone_position_list.points.size()>50)
//drone_position_list.points.erase(drone_position_list.points.begin()); drone_position_list.points.erase(drone_position_list.points.begin());
drone_position_list.points.push_back(p);
rviz_points_trajectory_publisher.publish(drone_position_list); rviz_points_trajectory_publisher.publish(drone_position_list);
...@@ -1582,7 +1585,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle ...@@ -1582,7 +1585,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
rviz_points_trajectory_generated_publisher.publish(setpoint_position_list); rviz_points_trajectory_generated_publisher.publish(setpoint_position_list);
} //}
} }
...@@ -1681,9 +1684,12 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp ...@@ -1681,9 +1684,12 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
// > First, get the yaw error into a local variable // > First, get the yaw error into a local variable
float yawError = stateInertial[8] - setpoint[3]; float yawError = stateInertial[8] - setpoint[3];
// > Second, "unwrap" the yaw error to the interval ( -pi , pi ) // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
while(yawError > PI) {yawError -= 2 * PI;} while(yawError >= PI) {yawError -= 2 * PI;}
while(yawError < -PI) {yawError += 2 * PI;} while(yawError < -PI) {yawError += 2 * PI;}
// > Third, put the "yawError" into the "stateError" variable // > Third, put the "yawError" into the "stateError" variable
//ROS_INFO_STREAM("yaw(cf,ms,error): " << stateInertial[8]<< ", " << setpoint[3] << ", " << yawError);
ROS_INFO_STREAM("yaw(ms): "<<setpoint[3]);
stateInertial[8] = yawError; stateInertial[8] = yawError;
......
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