static int __open_socket(const char * sockname) { struct sockaddr_un where; int fd; if ((fd = socket(AF_UNIX, SOCK_STREAM, 0)) < 0) { d_error("%s: Counld not create unix domain socket: %s.\n",__FUNCTION__, strerror(errno)); return -1; } fcntl(fd, F_SETFD, FD_CLOEXEC); where.sun_family = AF_UNIX; if (sockname == NULL) sockname = XMLDB_DEFAULT_UNIXSOCK; snprintf(where.sun_path, sizeof(where.sun_path), "%s", sockname); if (connect(fd, (struct sockaddr *)&where, sizeof(where)) < 0) { d_error("%s: Cound not connect to unix socket: %s.\n",__FUNCTION__, sockname); close(fd); return -1; } return fd; }
static void parsePADSTags(UINT16_t type, UINT16_t len, unsigned char *data, void *extra) { PPPoEConnection * conn = (PPPoEConnection *)extra; switch (type) { case TAG_SERVICE_NAME: d_dbg("[%d]: PADS: Service-Name: '%.*s'\n", getpid(), (int) len, data); break; case TAG_SERVICE_NAME_ERROR: d_error("[%d]: PADS: Service-Name-Error: %.*s\n", getpid(), (int)len, data); exit(1); break; case TAG_AC_SYSTEM_ERROR: d_error("[%d]: PADS: System-Error: %.*s\n", getpid(), (int)len, data); exit(1); break; case TAG_GENERIC_ERROR: d_error("[%d]: PADS: Generic-Error: %.*s\n", getpid(), (int)len, data); exit(1); break; case TAG_RELAY_SESSION_ID: conn->relayId.type = htons(type); conn->relayId.length = htons(len); memcpy(conn->relayId.payload, data, len); break; } }
static size_t redirect_to_buffer(int fd, char * buff, size_t buff_size) { fd_set read_set; ssize_t size; size_t written = 0; dassert(buff && buff_size); for (;;) { FD_ZERO(&read_set); FD_SET(fd, &read_set); if (select(fd+1, &read_set, NULL, NULL, NULL) < 0) continue; if (FD_ISSET(fd, &read_set)) { size = read(fd, buff+written, buff_size - written); if (size <= 0) break; written += size; if (buff[written - 1] == '\0') break; if (buff_size >= written) { d_error("%s: no more buffer space for read !!\n", __FUNCTION__); break; } } } return written; }
/********************************************************************** * %FUNCTION: set_errmsg * %ARGUMENTS: * fmt -- printf format * ... -- format args * %RETURNS: * Nothing * %DESCRIPTION: * Sets static errmsg string ***********************************************************************/ void l2tp_set_errmsg(char const *fmt, ...) { va_list ap; va_start(ap, fmt); vsnprintf(errmsg, MAX_ERRMSG_LEN, fmt, ap); va_end(ap); errmsg[MAX_ERRMSG_LEN-1] = 0; d_error("l2tp: Error: %s\n", errmsg); }
void b_error(const emlrtStack *sp, const char_T varargin_2_data[1024], const int32_T varargin_2_size[2]) { const mxArray *y; static const int32_T iv174[2] = { 1, 41 }; const mxArray *m31; char_T cv203[41]; int32_T i; static const char_T cv204[41] = { 's', 'd', 'r', 'u', ':', 'S', 'D', 'R', 'u', 'T', 'r', 'a', 'n', 's', 'm', 'i', 't', 't', 'e', 'r', ':', 'T', 'r', 'a', 'n', 's', 'm', 'i', 't', 'U', 'n', 's', 'u', 'c', 'c', 'e', 's', 's', 'f', 'u', 'l' }; int32_T u_size[2]; int32_T i20; char_T u_data[1024]; const mxArray *b_y; emlrtStack st; st.prev = sp; st.tls = sp->tls; y = NULL; m31 = mxCreateCharArray(2, iv174); for (i = 0; i < 41; i++) { cv203[i] = cv204[i]; } emlrtInitCharArrayR2013a(sp, 41, m31, cv203); emlrtAssign(&y, m31); u_size[0] = 1; u_size[1] = varargin_2_size[1]; i = varargin_2_size[0] * varargin_2_size[1]; for (i20 = 0; i20 < i; i20++) { u_data[i20] = varargin_2_data[i20]; } b_y = NULL; m31 = mxCreateCharArray(2, u_size); emlrtInitCharArrayR2013a(sp, u_size[1], m31, (char_T *)&u_data); emlrtAssign(&b_y, m31); st.site = &acb_emlrtRSI; d_error(&st, y, b_y, &r_emlrtMCI); }
void error(const emlrtStack *sp, const char_T varargin_2_data[], const int32_T varargin_2_size[2]) { const mxArray *y; static const int32_T iv29[2] = { 1, 41 }; const mxArray *m9; char_T cv46[41]; int32_T i; static const char_T cv47[41] = { 's', 'd', 'r', 'u', ':', 'S', 'D', 'R', 'u', 'T', 'r', 'a', 'n', 's', 'm', 'i', 't', 't', 'e', 'r', ':', 'T', 'r', 'a', 'n', 's', 'm', 'i', 't', 'U', 'n', 's', 'u', 'c', 'c', 'e', 's', 's', 'f', 'u', 'l' }; int32_T u_size[2]; int32_T i11; char_T u_data[1024]; const mxArray *b_y; emlrtStack st; st.prev = sp; st.tls = sp->tls; y = NULL; m9 = emlrtCreateCharArray(2, iv29); for (i = 0; i < 41; i++) { cv46[i] = cv47[i]; } emlrtInitCharArrayR2013a(sp, 41, m9, cv46); emlrtAssign(&y, m9); u_size[0] = 1; u_size[1] = varargin_2_size[1]; i = varargin_2_size[0] * varargin_2_size[1]; for (i11 = 0; i11 < i; i11++) { u_data[i11] = varargin_2_data[i11]; } b_y = NULL; m9 = emlrtCreateCharArray(2, u_size); emlrtInitCharArrayR2013a(sp, u_size[1], m9, (char_T *)u_data); emlrtAssign(&b_y, m9); st.site = &mb_emlrtRSI; d_error(&st, y, b_y, &i_emlrtMCI); }
void error(const emlrtStack *sp, const char_T varargin_2_data[1024], const int32_T varargin_2_size[2]) { const mxArray *y; static const int32_T iv122[2] = { 1, 37 }; const mxArray *m20; char_T cv141[37]; int32_T i; static const char_T cv142[37] = { 's', 'd', 'r', 'u', ':', 'S', 'D', 'R', 'u', 'R', 'e', 'c', 'e', 'i', 'v', 'e', 'r', ':', 'R', 'e', 'c', 'e', 'i', 'v', 'e', 'U', 'n', 's', 'u', 'c', 'c', 'e', 's', 's', 'f', 'u', 'l' }; int32_T u_size[2]; int32_T i17; char_T u_data[1024]; const mxArray *b_y; emlrtStack st; st.prev = sp; st.tls = sp->tls; y = NULL; m20 = mxCreateCharArray(2, iv122); for (i = 0; i < 37; i++) { cv141[i] = cv142[i]; } emlrtInitCharArrayR2013a(sp, 37, m20, cv141); emlrtAssign(&y, m20); u_size[0] = 1; u_size[1] = varargin_2_size[1]; i = varargin_2_size[0] * varargin_2_size[1]; for (i17 = 0; i17 < i; i17++) { u_data[i17] = varargin_2_data[i17]; } b_y = NULL; m20 = mxCreateCharArray(2, u_size); emlrtInitCharArrayR2013a(sp, u_size[1], m20, (char_T *)&u_data); emlrtAssign(&b_y, m20); st.site = &acb_emlrtRSI; d_error(&st, y, b_y, &r_emlrtMCI); }
void b_error(const emlrtStack *sp, const char_T varargin_2_data[], const int32_T varargin_2_size[2]) { const mxArray *y; static const int32_T iv35[2] = { 1, 37 }; const mxArray *m13; char_T cv58[37]; int32_T i; static const char_T cv59[37] = { 's', 'd', 'r', 'u', ':', 'S', 'D', 'R', 'u', 'R', 'e', 'c', 'e', 'i', 'v', 'e', 'r', ':', 'R', 'e', 'c', 'e', 'i', 'v', 'e', 'U', 'n', 's', 'u', 'c', 'c', 'e', 's', 's', 'f', 'u', 'l' }; int32_T u_size[2]; int32_T i12; char_T u_data[1024]; const mxArray *b_y; emlrtStack st; st.prev = sp; st.tls = sp->tls; y = NULL; m13 = emlrtCreateCharArray(2, iv35); for (i = 0; i < 37; i++) { cv58[i] = cv59[i]; } emlrtInitCharArrayR2013a(sp, 37, m13, cv58); emlrtAssign(&y, m13); u_size[0] = 1; u_size[1] = varargin_2_size[1]; i = varargin_2_size[0] * varargin_2_size[1]; for (i12 = 0; i12 < i; i12++) { u_data[i12] = varargin_2_data[i12]; } b_y = NULL; m13 = emlrtCreateCharArray(2, u_size); emlrtInitCharArrayR2013a(sp, u_size[1], m13, (char_T *)u_data); emlrtAssign(&b_y, m13); st.site = &mb_emlrtRSI; d_error(&st, y, b_y, &i_emlrtMCI); }
void Circle::stateCB(const AutoNav::filter_stateConstPtr state) { if(!stateInit) {//store initial state and offset all goal commands by it initX = state->x; initY = state->y; initA = state->yaw; stateInit = true; } AutoNav::circle_control log_circle; //log all data for debugging Eigen::Vector2f error; Eigen::Vector3f d_error; Eigen::Vector2f proj_error,proj_vel; Eigen::Vector2f goal; double yawRad = state->yaw - initA; yawRad = angleFromTo(yawRad,-180,180) * PI / 180; //goal on the x and y axes in world frame goal(0) = initX + (radius*(1 - cos(yawRad))); goal(1) = initY - (radius*sin(yawRad)); //error in position in world frame error(0) = goal(0) - state->x; error(1) = goal(1) - state->y; //error in position projected in drone's frame proj_error(0) = error(0)*cos(yawRad) + error(1)*sin(yawRad); proj_error(1) = -error(0)*sin(yawRad) + error(1)*cos(yawRad); //drone's velocity projected in its own frame proj_vel(0) = (state->dx)*cos(yawRad) + (state->dy)*sin(yawRad); proj_vel(1) = -(state->dx)*sin(yawRad) + (state->dy)*cos(yawRad); //desired angular velocity at this point angVel = (-proj_vel(1)) / radius; //error in velocities d_error(0) = - proj_vel(0);//should be zero in the cross track axis d_error(1) = latVel - proj_vel(1); //should be constant in the axis tangential to circle d_error(2) = angVel - ((state->dyaw)*PI/180); //angular velocity iTerm(0) = 0; //drone's x-axis iTerm(1) += lastError(1) * (ros::Time::now()-lastTime).toSec(); //drone's y-axis iTerm(2) += lastError(2) * (ros::Time::now()-lastTime).toSec(); //yaw //cross track (normal to tangent of circle) double CTgainP = proj_error(0)*ctr.Kp; //proportional double CTgainD = d_error(0)*ctr.Kd; //derivative //along track double ATgainP = d_error(1)*atr.Kp; //proportional double ATgainI = iTerm(1) * atr.Ki; //integral double ANGgainP = d_error(2)*angular.Kp; //prop. double ANGgainI = iTerm(2) * angular.Ki; //integral lastError(1) = d_error(1); lastError(2) = d_error(2); lastTime = ros::Time::now(); geometry_msgs::Twist cmd; cmd.linear.x = CTgainP + CTgainD; cmd.linear.y = ATgainP + ATgainI; cmd.angular.z = ANGgainP + ANGgainI; //START LOGGING log_circle.yaw = state->yaw - initA; log_circle.dyaw = state->dyaw; log_circle.angVel = angVel * 180/PI; log_circle.goalX = goal(0); log_circle.goalY = goal(1); log_circle.errorX = error(0); log_circle.errorY = error(1); log_circle.PerrorX = proj_error(0); log_circle.PerrorY = proj_error(1); log_circle.PvelX = proj_vel(0); log_circle.PvelY = proj_vel(1); log_circle.VerrX = d_error(0); log_circle.VerrY = d_error(1); log_circle.VerrA = d_error(2) * 180/PI; log_circle.CTgainP = CTgainP; log_circle.CTgainD = CTgainD; log_circle.ATgainP = ATgainP; log_circle.ATgainI = ATgainI; log_circle.ANGgainP = ANGgainP; log_circle.ANGgainI = ANGgainI; log_control.publish(log_circle); //END LOGGING // ROS_INFO("Velocity commands are: %lf,%lf,%lf",cmd.linear.x,cmd.linear.y,cmd.angular.z); vel.publish(cmd); }
static void parsePADOTags(UINT16_t type, UINT16_t len, unsigned char * data, void * extra) { struct PacketCriteria *pc = (struct PacketCriteria *)extra; PPPoEConnection *conn = pc->conn; int i; d_dbg("[%d]: parsePADOTags >>>\n", getpid()); switch (type) { case TAG_AC_NAME: d_dbg("[%d]: TAG_AC_NAME\n", getpid()); d_dbg("[%d]: Access-Concentrator: [%.*s]\n", getpid(), (int)len, data); d_dbg("[%d]: acName:[%s]\n", getpid(), conn->acName); pc->seenACName = 1; if (conn->printACNames) printf("Access-Concentrator: %.*s\n", (int)len, data); if (conn->acName && len == strlen(conn->acName) && !strncmp((char *)data, conn->acName, len)) pc->acNameOK = 1; break; case TAG_SERVICE_NAME: d_dbg("[%d]: TAG_SERVICE_NAME\n", getpid()); pc->seenServiceName = 1; if (conn->printACNames && len > 0) printf(" Service-Name: %.*s\n", (int)len, data); if (conn->serviceName && len == strlen(conn->serviceName) && !strncmp((char *)data, conn->serviceName, len)) pc->serviceNameOK = 1; break; case TAG_AC_COOKIE: d_dbg("[%d]: TAG_AC_COOKIE\n", getpid()); if (conn->printACNames) { printf("Got a cookie:"); /* print first 20 bytes of cookie */ for (i=0; i<len && i<20; i++) printf(" %02x", (unsigned)data[i]); if (i < len) printf("..."); printf("\n"); } conn->cookie.type = htons(type); conn->cookie.length = htons(len); memcpy(conn->cookie.payload, data, len); break; case TAG_RELAY_SESSION_ID: d_dbg("[%d]: TAG_RELAY_SESSION_ID\n", getpid()); if (conn->printACNames) { printf("Got a Relay-ID:"); /* print first 20 bytes of relay ID */ for (i=0; i<len && i<20; i++) printf(" %02x", (unsigned) data[i]); if (i < len) printf("..."); printf("\n"); } conn->relayId.type = htons(type); conn->relayId.length = htons(len); memcpy(conn->relayId.payload, data, len); break; case TAG_SERVICE_NAME_ERROR: d_dbg("[%d]: TAG_SERVICE_NAME_ERROR\n", getpid()); if (conn->printACNames) { printf("Got a Service-Name-Error tag: %.*s\n", (int)len, data); } else { d_error("[%d]: PADO: Service-Name-Error: %.*s\n", getpid(), (int)len, data); exit(1); } break; case TAG_AC_SYSTEM_ERROR: d_dbg("[%d]: TAG_AC_SYSTEM_ERROR\n", getpid()); if (conn->printACNames) { printf("Got a System-Error tag: %.*s\n", (int)len, data); } else { d_error("[%d]: PADO: System-Error: %.*s\n", getpid(), (int)len, data); exit(1); } break; case TAG_GENERIC_ERROR: d_dbg("[%d]: TAG_GENERIC_ERROR\n", getpid()); if (conn->printACNames) { printf("Got a Generic-Error tag: %.*s\n", (int)len, data); } else { d_error("[%d]: PADO: Generic-Error: %.*s\n", getpid(), (int)len, data); exit(1); } break; } d_dbg("[%d]: parsePADOTag <<<\n", getpid()); }
static void waitForPADS(PPPoEConnection * conn, int timeout) { fd_set readable; int r; struct timeval tv; PPPoEPacket packet; int len; d_dbg("[%d]: waitForPADS: >>> \n", getpid()); do { tv.tv_sec = timeout; tv.tv_usec = 0; FD_ZERO(&readable); FD_SET(conn->discoverySocket, &readable); while (1) { r = select(conn->discoverySocket + 1, &readable, NULL, NULL, &tv); if (r >= 0 || errno != EINTR) break; } if (r < 0) { d_error("[%d]: select (waitForPADS)\n", getpid()); return; } if (r == 0) return; /* Get the packet */ receivePacket(conn->discoverySocket, &packet, &len); if (packet.code == CODE_PADS) d_dbg("[%d]: GOT a PADS!\n", getpid()); /* Check length */ if (ntohs(packet.length) + HDR_SIZE > len) { d_error("[%d]: Bogus PPPoE length field (%u)\n", getpid(), (unsigned int)ntohs(packet.length)); continue; } /* If it's not from the AC, it's not for me */ if (memcmp(packet.ethHdr.h_source, conn->peerEth, ETH_ALEN)) continue; /* If it's not for us, loop again */ if (!packetIsForMe(conn, &packet)) continue; /* Is it PADS ? */ if (packet.code == CODE_PADS) { /* Parse for goodies */ kpppoe_parsePacket(&packet, parsePADSTags, conn); conn->discoveryState = STATE_SESSION; break; } } while (conn->discoveryState != STATE_SESSION); /* Don't bother with ntohs; we'll just end up converting it back... */ conn->session = packet.session; d_info("[%d]: PPP session is %d\n", getpid(), (int)ntohs(conn->session)); #ifndef LOGNUM #ifdef ELBOX_PROGS_GPL_SYSLOGD_AP syslog(ALOG_NOTICE|LOG_NOTICE,"[Notice]PPPoE: Received PADS for %s. (Session ID: %x)", linkname, (int)ntohs(conn->session)); #else syslog(ALOG_NOTICE|LOG_NOTICE,"PPPoE: Received PADS for %s. (Session ID: %x)", linkname, (int)ntohs(conn->session)); #endif #else syslog(ALOG_NOTICE|LOG_NOTICE,"NTC:028[%s][%x]", linkname, (int)ntohs(conn->session)); #endif sts_session_id = conn->session; /* RFC 2516 says session id MUST NOT be zero of 0xffff */ if (ntohs(conn->session) == 0 || ntohs(conn->session) == 0xffff) d_info("[%d]: Access concentrator used a session value of %x -- the AC is violating RFC 2516\n", getpid(), (unsigned int) ntohs(conn->session)); d_dbg("[%d]: waitForPADS: <<< \n", getpid()); }
static void waitForPADO(PPPoEConnection * conn, int timeout) { fd_set readable; int r = -1; struct timeval tv; PPPoEPacket packet; int len; struct PacketCriteria pc; pc.conn = conn; pc.acNameOK = (conn->acName) ? 0 : 1; pc.serviceNameOK = (conn->serviceName) ? 0 : 1; pc.seenACName = 0; pc.seenServiceName = 0; do { tv.tv_sec = timeout; tv.tv_usec = 0; FD_ZERO(&readable); FD_SET(conn->discoverySocket, &readable); while (!got_sigterm) /* Modified by Kwest Wan 20080919 to terminate pppd immediately if PPPoE server don't reponse */ { d_dbg("[%d]: waitPADO select >>>>\n", getpid()); r = select(conn->discoverySocket+1, &readable, NULL, NULL, &tv); d_dbg("[%d]: waitPADO select (%d) <<<<\n", getpid(), r); if (r >= 0 || errno != EINTR) break; } if (r < 0) { d_error("[%d]: select (waitForPADS)\n", getpid()); return; } if (r == 0) return; /* Get the packet */ d_dbg("[%d]: receivePacket >>>\n", getpid()); receivePacket(conn->discoverySocket, &packet, &len); d_dbg("[%d]: receivePacket <<<\n", getpid()); /* Check length */ if (ntohs(packet.length) + HDR_SIZE > len) { d_error("[%d]: Bogus PPPoE length field (%u)\n", getpid(), (unsigned int)ntohs(packet.length)); continue; } /* If it's not for us, loop again */ if (!packetIsForMe(conn, &packet)) { d_dbg("[%d]: packet is not for me!\n", getpid()); continue; } /* Is it PADS ? */ if (packet.code == CODE_PADO) { if (NOT_UNICAST(packet.ethHdr.h_source)) { d_info("[%d]: Ignoring PADO packet from non-unicast MAC address\n", getpid()); #ifndef LOGNUM #ifdef ELBOX_PROGS_GPL_SYSLOGD_AP syslog(ALOG_NOTICE|LOG_NOTICE, "[Notice]PPPoE: Ignore PADO packet from non-unicast MAC address. (%s)", linkname); #else syslog(ALOG_NOTICE|LOG_NOTICE, "PPPoE: Ignore PADO packet from non-unicast MAC address. (%s)", linkname); #endif #else syslog(ALOG_NOTICE|LOG_NOTICE, "NTC:029[%s]", linkname); #endif continue; } kpppoe_parsePacket(&packet, parsePADOTags, &pc); if (!pc.seenACName) { d_info("[%d]: Ignoring PADO packet with no AC-Name tag\n", getpid()); #ifndef LOGNUM #ifdef ELBOX_PROGS_GPL_SYSLOGD_AP syslog(ALOG_NOTICE|LOG_NOTICE, "[Notice]PPPoE: Ignore PADO packet with no AC-Name tag. (%s)", linkname); #else syslog(ALOG_NOTICE|LOG_NOTICE, "PPPoE: Ignore PADO packet with no AC-Name tag. (%s)", linkname); #endif #else syslog(ALOG_NOTICE|LOG_NOTICE, "NTC:030[%s]", linkname); #endif continue; } if (!pc.seenServiceName) { d_info("[%d]: Ignoring PADO packet with no Service-Name tag\n", getpid()); #ifndef LOGNUM #ifdef ELBOX_PROGS_GPL_SYSLOGD_AP syslog(ALOG_NOTICE|LOG_NOTICE, "[Notice]PPPoE: Ignore PADO packet with no Service-Name tag. (%s)", linkname); #else syslog(ALOG_NOTICE|LOG_NOTICE, "PPPoE: Ignore PADO packet with no Service-Name tag. (%s)", linkname); #endif #else syslog(ALOG_NOTICE|LOG_NOTICE, "NTC:031[%s]", linkname); #endif continue; } conn->numPADOs++; if (conn->printACNames) printf("------------------------------------------------------\n"); update_statusfile("PPPoE:PADO"); d_dbg("[%d]: acNameOK:%d, serviceNameOK:%d\n", getpid(), pc.acNameOK, pc.serviceNameOK); if (pc.acNameOK && pc.serviceNameOK) { memcpy(conn->peerEth, packet.ethHdr.h_source, ETH_ALEN); if (conn->printACNames) { printf("AC-Ethernet-Address: %02x:%02x:%02x:%02x:%02x:%02x\n", (unsigned)conn->peerEth[0], (unsigned)conn->peerEth[1], (unsigned)conn->peerEth[2], (unsigned)conn->peerEth[3], (unsigned)conn->peerEth[4], (unsigned)conn->peerEth[5]); continue; } conn->discoveryState = STATE_RECEIVED_PADO; #ifndef LOGNUM #ifdef ELBOX_PROGS_GPL_SYSLOGD_AP syslog(ALOG_NOTICE|LOG_NOTICE, "[Notice]PPPoE: Received PADO for %s, from: %02x:%02x:%02x:%02x:%02x:%02x", linkname, (unsigned)conn->peerEth[0], (unsigned)conn->peerEth[1], (unsigned)conn->peerEth[2], (unsigned)conn->peerEth[3], (unsigned)conn->peerEth[4], (unsigned)conn->peerEth[5]); #else syslog(ALOG_NOTICE|LOG_NOTICE, "PPPoE: Received PADO for %s, from: %02x:%02x:%02x:%02x:%02x:%02x", linkname, (unsigned)conn->peerEth[0], (unsigned)conn->peerEth[1], (unsigned)conn->peerEth[2], (unsigned)conn->peerEth[3], (unsigned)conn->peerEth[4], (unsigned)conn->peerEth[5]); #endif #else syslog(ALOG_NOTICE|LOG_NOTICE, "NTC:026[%s][%02x:%02x:%02x:%02x:%02x:%02x]", linkname, (unsigned)conn->peerEth[0], (unsigned)conn->peerEth[1], (unsigned)conn->peerEth[2], (unsigned)conn->peerEth[3], (unsigned)conn->peerEth[4], (unsigned)conn->peerEth[5]); #endif break; } } else { d_dbg("[%d]: this is not PADO!\n", getpid()); } } while (conn->discoveryState != STATE_RECEIVED_PADO); }