void estimator_update_state_infrared( void ) {
float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON ?
estimator_rad_of_ir :
ir_rad_of_ir);
#ifdef IR_360
if (ir_360) { /* 360° estimation */
/* 250 us for the whole block */
float tmp_ir_roll = ir_roll * IR_360_LATERAL_CORRECTION;
float tmp_ir_pitch = ir_pitch * IR_360_LONGITUDINAL_CORRECTION;
float tmp_ir_top = ir_top * IR_360_VERTICAL_CORRECTION;
estimator_phi = atan2(tmp_ir_roll, tmp_ir_top) - ir_roll_neutral;
estimator_phi = correct_angle(estimator_phi, ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4);
estimator_theta = atan2(tmp_ir_pitch, tmp_ir_top) - ir_pitch_neutral;
estimator_theta = correct_angle(estimator_theta, ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4);
if (estimator_theta < -M_PI_2)
estimator_theta += M_PI;
else if (estimator_theta > M_PI_2)
estimator_theta -= M_PI;
} else
#endif /* IR_360 */
{
ir_top = Max(ir_top, 1);
float c = rad_of_ir*(1-z_contrast_mode)+z_contrast_mode*((float)IR_RAD_OF_IR_CONTRAST/fabs(ir_top));
estimator_phi = c * ir_roll - ir_roll_neutral;
estimator_theta = c * ir_pitch - ir_pitch_neutral;
/* infrared compensation */
#if defined IR_CORRECTION_LEFT && defined IR_CORRECTION_RIGHT
if (estimator_phi >= 0)
estimator_phi *= ir_correction_right;
else
estimator_phi *= ir_correction_left;
#endif
#if defined IR_CORRECTION_UP && defined IR_CORRECTION_DOWN
if (estimator_theta >= 0)
estimator_theta *= ir_correction_up;
else
estimator_theta *= ir_correction_down;
#endif
Bound(estimator_phi, -M_PI_2, M_PI_2);
Bound(estimator_theta, -M_PI_2, M_PI_2);
}
}
void updateParameters(){
r= sqrt(pow(pos_x-goal_x,2) + pow(pos_y-goal_y,2));
if (r<r_no_gps){
close_to_goal = 1;
v_max = v_max_close;
}
alpha = atan2(goal_y-pos_y,goal_x-pos_x);
printf("distance_goalx = %f distance_goaly = %f \n",goal_x-pos_x,goal_y-pos_y);
delta = yaw - alpha;
theta = goal_yaw - alpha;
correct_angle(delta);
correct_angle(theta);
K = -1/r*(k2*(delta-atan(-k1*theta))+(1+k1/(1+k1*k1*theta*theta))*sin(delta));
if(fabs(K)>0.15){
K=0.15*(K/fabs(K));
}
v = v_max/(1+beta*pow(abs(K),lambda));
omega = v*K;
if (fabs(omega) > 0.5){
v=0.5*v/fabs(omega);
omega=omega*0.5/fabs(omega);
}
if (r<r_threshold){
v=0;
omega = 0;
hasGoal = 0;
publishGoalReached();
}
printf("r=%f, \nalpha=%f \ndelta=%f \n theta=%f \n v=%f \n omega=%f \nK=%f\n\n",r,alpha,delta,theta,v,omega,K);
printf("v =%f omega= %f r = %f\n\n,yaw %f \n",v,omega,r,yaw);
}
void processOdometryNoGps(const nav_msgs::Odometry::ConstPtr& msg){
if(close_to_goal==0){
prev_x = msg->pose.pose.position.x;
prev_y = msg->pose.pose.position.y;
qx=msg->pose.pose.orientation.x;
qy=msg->pose.pose.orientation.y;
qz=msg->pose.pose.orientation.z;
qw=msg->pose.pose.orientation.w;
prev_yaw = atan2(2*(qx*qy + qw*qz),qw*qw + qx*qx -qy*qy - qz*qz);
}
else{
current_x = msg->pose.pose.position.x;
current_y = msg->pose.pose.position.y;
qx=msg->pose.pose.orientation.x;
qy=msg->pose.pose.orientation.y;
qz=msg->pose.pose.orientation.z;
qw=msg->pose.pose.orientation.w;
current_yaw = atan2(2*(qx*qy + qw*qz),qw*qw + qx*qx -qy*qy - qz*qz);
d_xm = current_x - prev_x;
d_ym = current_y - prev_y;
d_yaw = current_yaw - prev_yaw;
yaw = yaw + d_yaw;
correct_angle(yaw);
d_x = sqrt(d_xm*d_xm +d_ym*d_ym)*cos(yaw);
d_y = sqrt(d_xm*d_xm +d_ym*d_ym)*sin(yaw);
prev_x = current_x;
prev_y = current_y;
prev_yaw = current_yaw;
active_pose.pose.pose.position.x= pos_x = pos_x + d_x;
active_pose.pose.pose.position.y= pos_y = pos_y + d_y;
active_pose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
active_pose_pub.publish(active_pose);
if(hasGoal){
updateParameters();
publishSystemInput();
}
else{
publishGoalReached();
}
}
}