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