void chatterCallback_LocalPosition(const geometry_msgs::PoseStamped &msg)
{
if(near_bool(setpoint.x, msg.pose.position.x)&&near_bool(setpoint.y, msg.pose.position.y)&&
near_bool(setpoint.z, msg.pose.position.z))
close_counter += 1;
else close_counter = 0;
if(close_counter >= 3)
ready_for_next = true;
}
void chatterCallback_local_position(const geometry_msgs::PoseStamped &msg)
{
if(offboard_ready)
{
if(send_counter == 0) //initial point
{
setpoint.px = -1000.0;
setpoint.py = -1000.0;
setpoint.ph = route_point[send_counter][2];
setpoint.yaw = -120; //<-100, mark the first take off point,wont get into trajactory generate in process_setpoints.cpp
if(near_bool(setpoint.ph, standard_height))
close_counter += 1;
else {
close_counter = 0;
}
}
else
{
if(near_bool(setpoint.px, msg.pose.position.x)&&near_bool(setpoint.py, msg.pose.position.y))
close_counter += 1;
else {
close_counter = 0;
}
}
if(close_counter >= 1){
close_counter = 0;
//set new route point
send_counter += 1;
setpoint.px = route_point[send_counter][0];
setpoint.py = route_point[send_counter][1];
setpoint.ph = route_point[send_counter][2];
setpoint.yaw = route_yaw;
}
new_setpoint_writed = true;
}
else new_setpoint_writed = false;
current_px = msg.pose.position.x;
current_py = msg.pose.position.y;
float q2=msg.pose.orientation.x;
float q1=msg.pose.orientation.y;
float q0=msg.pose.orientation.z;
float q3=msg.pose.orientation.w;
current_yaw = atan2(2*q1*q2 - 2*q0*q3, -2*q1*q1 - 2*q3*q3 + 1) + Pi;//North:0, south:Pi, East:Pi/2, West: Pi*3/2
}
void chatterCallback_local_position(const geometry_msgs::PoseStamped &msg)
{
if(offboard_ready)
{
if(near_bool(setpoint.px, msg.pose.position.x)&&near_bool(setpoint.py, msg.pose.position.y))
close_counter += 1;
else {
close_counter = 0;
}
if(send_counter == 0) //initial point
{
setpoint.px = route_point[send_counter][0] - start_px;
setpoint.py = route_point[send_counter][1] - start_py;
setpoint.ph = route_point[send_counter][2];
setpoint.yaw = current_yaw;;
}
if(close_counter >= 1){
close_counter = 0;
//set new route point
send_counter += 1;
setpoint.px = route_point[send_counter][0] - start_px;
setpoint.py = route_point[send_counter][1] - start_py;
setpoint.ph = route_point[send_counter][2];
setpoint.yaw = route_yaw;
}
}
current_px = msg.pose.position.x;
current_py = msg.pose.position.y;
float q2=msg.pose.orientation.x;
float q1=msg.pose.orientation.y;
float q0=msg.pose.orientation.z;
float q3=msg.pose.orientation.w;
current_yaw = (-atan2(2*q1*q2 - 2*q0*q3, -2*q1*q1 - 2*q3*q3 + 1))+Pi;//North:0, south:Pi, East:Pi/2, West: Pi*3/2
}
