monitor/monitor.cpp
// Function for correction of yaw angle (replaces the atan2 function for more controll)
Function for correction of yaw angle (replaces the atan2 function for more controll)
Function for correction of yaw angle ( replaces the atan2 function for more controll )
float wrap(float x_h, float y_h){
  float microT_tol = 0.2; 
  float angle = 0;
  if(x_h < 0) angle = M_PI-atan(y_h/x_h);
  if(x_h > 0 && y_h < 0) angle = -atan(y_h/x_h);
  if(x_h > 0 && y_h > 0) angle = 2*M_PI -atan(y_h/x_h);
  if(abs(x_h) < microT_tol && y_h < 0) angle = 0.5*M_PI;
  if(abs(x_h) < microT_tol && y_h > 0) angle = 1.5*M_PI;
  return angle;
}
float wrap ( float x_h , float y_h ) { float microT_tol = 0.2 ; float angle = 0 ; if ( x_h < 0 ) angle = M_PI - atan ( y_h / x_h ) ; if ( x_h > 0 && y_h < 0 ) angle = - atan ( y_h / x_h ) ; if ( x_h > 0 && y_h > 0 ) angle = 2 * M_PI - atan ( y_h / x_h ) ; if ( abs ( x_h ) < microT_tol && y_h < 0 ) angle = 0.5 * M_PI ; if ( abs ( x_h ) < microT_tol && y_h > 0 ) angle = 1.5 * M_PI ; return angle ; }
Function for correction of yaw angle (replaces the atan2 function for more controll)
Function for correction of yaw angle ( replaces the atan2 function for more controll )
//within +-0.2µT the value is set to 270 or 90 deg --> no devision by or near 0 of x_h in arctan function //In case of small values for x_h (no devision by 0) set the angle